Bullet Collision Detection & Physics Library: btPersistentManifold.cpp Source File

Bullet Collision Detection & Physics Library

btPersistentManifold.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include "btPersistentManifold.h"
18 #include "LinearMath/btTransform.h"
19
20
21 btScalar gContactBreakingThreshold = btScalar(0.02);
22 ContactDestroyedCallback gContactDestroyedCallback = 0;
23 ContactProcessedCallback gContactProcessedCallback = 0;
24 ContactStartedCallback gContactStartedCallback = 0;
25 ContactEndedCallback gContactEndedCallback = 0;
28 bool gContactCalcArea3Points = true;
29
30
31 btPersistentManifold::btPersistentManifold()
32 :btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
33 m_body0(0),
34 m_body1(0),
35 m_cachedPoints (0),
36 m_index1a(0)
37 {
38 }
39
40
41
42
43 #ifdef DEBUG_PERSISTENCY
44 #include <stdio.h>
45 void btPersistentManifold::DebugPersistency()
46 {
47 int i;
48 printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
49 for (i=0;i<m_cachedPoints;i++)
50 {
51 printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
52 }
53 }
54 #endif //DEBUG_PERSISTENCY
55
56 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
57 {
58
59 void* oldPtr = pt.m_userPersistentData;
60 if (oldPtr)
61 {
62 #ifdef DEBUG_PERSISTENCY
63 int i;
64 int occurance = 0;
65 for (i=0;i<m_cachedPoints;i++)
66 {
67 if (m_pointCache[i].m_userPersistentData == oldPtr)
68 {
69 occurance++;
70 if (occurance>1)
71 printf("error in clearUserCache\n");
72 }
73 }
74 btAssert(occurance<=0);
75 #endif //DEBUG_PERSISTENCY
76
77 if (pt.m_userPersistentData && gContactDestroyedCallback)
78 {
79 (*gContactDestroyedCallback)(pt.m_userPersistentData);
80 pt.m_userPersistentData = 0;
81 }
82
83 #ifdef DEBUG_PERSISTENCY
84 DebugPersistency();
85 #endif
86 }
87
88
89 }
90
91 static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
92 {
93 // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
94
95 btVector3 a[3],b[3];
96 a[0] = p0 - p1;
97 a[1] = p0 - p2;
98 a[2] = p0 - p3;
99 b[0] = p2 - p3;
100 b[1] = p1 - p3;
101 b[2] = p1 - p2;
102
103 //todo: Following 3 cross production can be easily optimized by SIMD.
104 btVector3 tmp0 = a[0].cross(b[0]);
105 btVector3 tmp1 = a[1].cross(b[1]);
106 btVector3 tmp2 = a[2].cross(b[2]);
107
108 return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
109 }
110
111 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
112 {
113 //calculate 4 possible cases areas, and take biggest area
114 //also need to keep 'deepest'
115
116 int maxPenetrationIndex = -1;
117 #define KEEP_DEEPEST_POINT 1
118 #ifdef KEEP_DEEPEST_POINT
119 btScalar maxPenetration = pt.getDistance();
120 for (int i=0;i<4;i++)
121 {
122 if (m_pointCache[i].getDistance() < maxPenetration)
123 {
124 maxPenetrationIndex = i;
125 maxPenetration = m_pointCache[i].getDistance();
126 }
127 }
128 #endif //KEEP_DEEPEST_POINT
129
130 btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
131
132 if (gContactCalcArea3Points)
133 {
134 if (maxPenetrationIndex != 0)
135 {
136 btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
137 btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
138 btVector3 cross = a0.cross(b0);
139 res0 = cross.length2();
140 }
141 if (maxPenetrationIndex != 1)
142 {
143 btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
144 btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
145 btVector3 cross = a1.cross(b1);
146 res1 = cross.length2();
147 }
148
149 if (maxPenetrationIndex != 2)
150 {
151 btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
152 btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
153 btVector3 cross = a2.cross(b2);
154 res2 = cross.length2();
155 }
156
157 if (maxPenetrationIndex != 3)
158 {
159 btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
160 btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
161 btVector3 cross = a3.cross(b3);
162 res3 = cross.length2();
163 }
164 }
165 else
166 {
167 if(maxPenetrationIndex != 0) {
168 res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
169 }
170
171 if(maxPenetrationIndex != 1) {
172 res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
173 }
174
175 if(maxPenetrationIndex != 2) {
176 res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
177 }
178
179 if(maxPenetrationIndex != 3) {
180 res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
181 }
182 }
183 btVector4 maxvec(res0,res1,res2,res3);
184 int biggestarea = maxvec.closestAxis4();
185 return biggestarea;
186
187 }
188
189
190 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
191 {
192 btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
193 int size = getNumContacts();
194 int nearestPoint = -1;
195 for( int i = 0; i < size; i++ )
196 {
197 const btManifoldPoint &mp = m_pointCache[i];
198
199 btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
200 const btScalar distToManiPoint = diffA.dot(diffA);
201 if( distToManiPoint < shortestDist )
202 {
203 shortestDist = distToManiPoint;
204 nearestPoint = i;
205 }
206 }
207 return nearestPoint;
208 }
209
210 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
211 {
212 if (!isPredictive)
213 {
214 btAssert(validContactDistance(newPoint));
215 }
216
217 int insertIndex = getNumContacts();
218 if (insertIndex == MANIFOLD_CACHE_SIZE)
219 {
220 #if MANIFOLD_CACHE_SIZE >= 4
221 //sort cache so best points come first, based on area
222 insertIndex = sortCachedPoints(newPoint);
223 #else
224 insertIndex = 0;
225 #endif
226 clearUserCache(m_pointCache[insertIndex]);
227
228 } else
229 {
230 m_cachedPoints++;
231
232
233 }
234 if (insertIndex<0)
235 insertIndex=0;
236
237 btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
238 m_pointCache[insertIndex] = newPoint;
239 return insertIndex;
240 }
241
242 btScalar btPersistentManifold::getContactBreakingThreshold() const
243 {
244 return m_contactBreakingThreshold;
245 }
246
247
248
249 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
250 {
251 int i;
252 #ifdef DEBUG_PERSISTENCY
253 printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
254 trA.getOrigin().getX(),
255 trA.getOrigin().getY(),
256 trA.getOrigin().getZ(),
257 trB.getOrigin().getX(),
258 trB.getOrigin().getY(),
259 trB.getOrigin().getZ());
260 #endif //DEBUG_PERSISTENCY
261 for (i=getNumContacts()-1;i>=0;i--)
263 {
264 btManifoldPoint &manifoldPoint = m_pointCache[i];
265 manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
266 manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
267 manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
268 manifoldPoint.m_lifeTime++;
269 }
270
272 btScalar distance2d;
273 btVector3 projectedDifference,projectedPoint;
274 for (i=getNumContacts()-1;i>=0;i--)
275 {
276
277 btManifoldPoint &manifoldPoint = m_pointCache[i];
278 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
279 if (!validContactDistance(manifoldPoint))
280 {
281 removeContactPoint(i);
282 } else
283 {
284 //todo: friction anchor may require the contact to be around a bit longer
285 //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
286 projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
287 projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
288 distance2d = projectedDifference.dot(projectedDifference);
289 if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() )
290 {
291 removeContactPoint(i);
292 } else
293 {
294 //contact point processed callback
295 if (gContactProcessedCallback)
296 (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
297 }
298 }
299 }
300 #ifdef DEBUG_PERSISTENCY
301 DebugPersistency();
302 #endif //
303 }
304
305
306
307
308
gContactStartedCallback
ContactStartedCallback gContactStartedCallback
Definition: btPersistentManifold.cpp:24
btPersistentManifold::m_body1
const btCollisionObject * m_body1
Definition: btPersistentManifold.h:70
btPersistentManifold::getContactBreakingThreshold
btScalar getContactBreakingThreshold() const
Definition: btPersistentManifold.cpp:242
btPersistentManifold::refreshContactPoints
void refreshContactPoints(const btTransform &trA, const btTransform &trB)
calculated new worldspace coordinates and depth, and reject points that exceed the collision margin ...
Definition: btPersistentManifold.cpp:249
BT_PERSISTENT_MANIFOLD_TYPE
Definition: btPersistentManifold.h:48
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:117
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btPersistentManifold::validContactDistance
bool validContactDistance(const btManifoldPoint &pt) const
Definition: btPersistentManifold.h:236
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
btPersistentManifold.h
btTransform.h
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:49
MANIFOLD_CACHE_SIZE
#define MANIFOLD_CACHE_SIZE
Definition: btPersistentManifold.h:51
btPersistentManifold::m_cachedPoints
int m_cachedPoints
Definition: btPersistentManifold.h:72
btPersistentManifold::sortCachedPoints
int sortCachedPoints(const btManifoldPoint &pt)
sort cached points so most isolated points come first
Definition: btPersistentManifold.cpp:111
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
btVector4
Definition: btVector3.h:1089
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:100
btManifoldPoint::m_userPersistentData
void * m_userPersistentData
Definition: btManifoldPoint.h:114
btManifoldPoint::m_positionWorldOnB
btVector3 m_positionWorldOnB
Definition: btManifoldPoint.h:97
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btManifoldPoint::m_localPointA
btVector3 m_localPointA
Definition: btManifoldPoint.h:95
ContactEndedCallback
void(* ContactEndedCallback)(btPersistentManifold *const &manifold)
Definition: btPersistentManifold.h:37
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btPersistentManifold::btPersistentManifold
btPersistentManifold()
Definition: btPersistentManifold.cpp:31
btManifoldPoint::m_lifeTime
int m_lifeTime
Definition: btManifoldPoint.h:138
calcArea4Points
static btScalar calcArea4Points(const btVector3 &p0, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3)
Definition: btPersistentManifold.cpp:91
btManifoldPoint::m_positionWorldOnA
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity ...
Definition: btManifoldPoint.h:99
btVector4::closestAxis4
int closestAxis4() const
Definition: btVector3.h:1197
ContactDestroyedCallback
bool(* ContactDestroyedCallback)(void *userPersistentData)
Definition: btPersistentManifold.h:34
gContactDestroyedCallback
ContactDestroyedCallback gContactDestroyedCallback
Definition: btPersistentManifold.cpp:22
gContactBreakingThreshold
btScalar gContactBreakingThreshold
maximum contact breaking and merging threshold
Definition: btPersistentManifold.cpp:21
ContactStartedCallback
void(* ContactStartedCallback)(btPersistentManifold *const &manifold)
Definition: btPersistentManifold.h:36
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
gContactEndedCallback
ContactEndedCallback gContactEndedCallback
Definition: btPersistentManifold.cpp:25
btPersistentManifold::removeContactPoint
void removeContactPoint(int index)
Definition: btPersistentManifold.h:162
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btManifoldPoint::m_distance1
btScalar m_distance1
Definition: btManifoldPoint.h:102
ContactProcessedCallback
bool(* ContactProcessedCallback)(btManifoldPoint &cp, void *body0, void *body1)
Definition: btPersistentManifold.h:35
btTypedObject
rudimentary class to provide type info
Definition: btScalar.h:777
btManifoldPoint::m_localPointB
btVector3 m_localPointB
Definition: btManifoldPoint.h:96
btPersistentManifold::clearUserCache
void clearUserCache(btManifoldPoint &pt)
Definition: btPersistentManifold.cpp:56
gContactCalcArea3Points
bool gContactCalcArea3Points
gContactCalcArea3Points will approximate the convex hull area using 3 points when setting it to false...
Definition: btPersistentManifold.cpp:28
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
btPersistentManifold::m_contactBreakingThreshold
btScalar m_contactBreakingThreshold
Definition: btPersistentManifold.h:74
btPersistentManifold::m_body0
const btCollisionObject * m_body0
this two body pointers can point to the physics rigidbody class.
Definition: btPersistentManifold.h:69
btPersistentManifold::addManifoldPoint
int addManifoldPoint(const btManifoldPoint &newPoint, bool isPredictive=false)
Definition: btPersistentManifold.cpp:210
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:146
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
gContactProcessedCallback
ContactProcessedCallback gContactProcessedCallback
Definition: btPersistentManifold.cpp:23
btPersistentManifold::m_pointCache
btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]
Definition: btPersistentManifold.h:66
btPersistentManifold::getCacheEntry
int getCacheEntry(const btManifoldPoint &newPoint) const
Definition: btPersistentManifold.cpp:190