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"
20 
21 #ifdef BT_USE_DOUBLE_PRECISION
22 #define btCollisionObjectData btCollisionObjectDoubleData
23 #else
24 #define btCollisionObjectData btCollisionObjectFloatData
25 #endif
26 
35 
36 
39 m_body0(0),
40 m_body1(0),
41 m_cachedPoints (0),
42 m_companionIdA(0),
43 m_companionIdB(0),
44 m_index1a(0)
45 {
46 }
47 
48 
49 
50 
51 #ifdef DEBUG_PERSISTENCY
52 #include <stdio.h>
53 void btPersistentManifold::DebugPersistency()
54 {
55  int i;
56  printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
57  for (i=0;i<m_cachedPoints;i++)
58  {
59  printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
60  }
61 }
62 #endif //DEBUG_PERSISTENCY
63 
65 {
66 
67  void* oldPtr = pt.m_userPersistentData;
68  if (oldPtr)
69  {
70 #ifdef DEBUG_PERSISTENCY
71  int i;
72  int occurance = 0;
73  for (i=0;i<m_cachedPoints;i++)
74  {
75  if (m_pointCache[i].m_userPersistentData == oldPtr)
76  {
77  occurance++;
78  if (occurance>1)
79  printf("error in clearUserCache\n");
80  }
81  }
82  btAssert(occurance<=0);
83 #endif //DEBUG_PERSISTENCY
84 
86  {
87  (*gContactDestroyedCallback)(pt.m_userPersistentData);
88  pt.m_userPersistentData = 0;
89  }
90 
91 #ifdef DEBUG_PERSISTENCY
92  DebugPersistency();
93 #endif
94  }
95 
96 
97 }
98 
99 static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
100 {
101  // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
102 
103  btVector3 a[3],b[3];
104  a[0] = p0 - p1;
105  a[1] = p0 - p2;
106  a[2] = p0 - p3;
107  b[0] = p2 - p3;
108  b[1] = p1 - p3;
109  b[2] = p1 - p2;
110 
111  //todo: Following 3 cross production can be easily optimized by SIMD.
112  btVector3 tmp0 = a[0].cross(b[0]);
113  btVector3 tmp1 = a[1].cross(b[1]);
114  btVector3 tmp2 = a[2].cross(b[2]);
115 
116  return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
117 }
118 
120 {
121  //calculate 4 possible cases areas, and take biggest area
122  //also need to keep 'deepest'
123 
124  int maxPenetrationIndex = -1;
125 #define KEEP_DEEPEST_POINT 1
126 #ifdef KEEP_DEEPEST_POINT
127  btScalar maxPenetration = pt.getDistance();
128  for (int i=0;i<4;i++)
129  {
130  if (m_pointCache[i].getDistance() < maxPenetration)
131  {
132  maxPenetrationIndex = i;
133  maxPenetration = m_pointCache[i].getDistance();
134  }
135  }
136 #endif //KEEP_DEEPEST_POINT
137 
138  btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
139 
141  {
142  if (maxPenetrationIndex != 0)
143  {
146  btVector3 cross = a0.cross(b0);
147  res0 = cross.length2();
148  }
149  if (maxPenetrationIndex != 1)
150  {
153  btVector3 cross = a1.cross(b1);
154  res1 = cross.length2();
155  }
156 
157  if (maxPenetrationIndex != 2)
158  {
161  btVector3 cross = a2.cross(b2);
162  res2 = cross.length2();
163  }
164 
165  if (maxPenetrationIndex != 3)
166  {
169  btVector3 cross = a3.cross(b3);
170  res3 = cross.length2();
171  }
172  }
173  else
174  {
175  if(maxPenetrationIndex != 0) {
177  }
178 
179  if(maxPenetrationIndex != 1) {
181  }
182 
183  if(maxPenetrationIndex != 2) {
185  }
186 
187  if(maxPenetrationIndex != 3) {
189  }
190  }
191  btVector4 maxvec(res0,res1,res2,res3);
192  int biggestarea = maxvec.closestAxis4();
193  return biggestarea;
194 
195 }
196 
197 
199 {
201  int size = getNumContacts();
202  int nearestPoint = -1;
203  for( int i = 0; i < size; i++ )
204  {
205  const btManifoldPoint &mp = m_pointCache[i];
206 
207  btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
208  const btScalar distToManiPoint = diffA.dot(diffA);
209  if( distToManiPoint < shortestDist )
210  {
211  shortestDist = distToManiPoint;
212  nearestPoint = i;
213  }
214  }
215  return nearestPoint;
216 }
217 
218 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
219 {
220  if (!isPredictive)
221  {
222  btAssert(validContactDistance(newPoint));
223  }
224 
225  int insertIndex = getNumContacts();
226  if (insertIndex == MANIFOLD_CACHE_SIZE)
227  {
228 #if MANIFOLD_CACHE_SIZE >= 4
229  //sort cache so best points come first, based on area
230  insertIndex = sortCachedPoints(newPoint);
231 #else
232  insertIndex = 0;
233 #endif
234  clearUserCache(m_pointCache[insertIndex]);
235 
236  } else
237  {
238  m_cachedPoints++;
239 
240 
241  }
242  if (insertIndex<0)
243  insertIndex=0;
244 
245  btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
246  m_pointCache[insertIndex] = newPoint;
247  return insertIndex;
248 }
249 
251 {
253 }
254 
255 
256 
258 {
259  int i;
260 #ifdef DEBUG_PERSISTENCY
261  printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
262  trA.getOrigin().getX(),
263  trA.getOrigin().getY(),
264  trA.getOrigin().getZ(),
265  trB.getOrigin().getX(),
266  trB.getOrigin().getY(),
267  trB.getOrigin().getZ());
268 #endif //DEBUG_PERSISTENCY
269  for (i=getNumContacts()-1;i>=0;i--)
271  {
272  btManifoldPoint &manifoldPoint = m_pointCache[i];
273  manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
274  manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
275  manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
276  manifoldPoint.m_lifeTime++;
277  }
278 
280  btScalar distance2d;
281  btVector3 projectedDifference,projectedPoint;
282  for (i=getNumContacts()-1;i>=0;i--)
283  {
284 
285  btManifoldPoint &manifoldPoint = m_pointCache[i];
286  //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
287  if (!validContactDistance(manifoldPoint))
288  {
290  } else
291  {
292  //todo: friction anchor may require the contact to be around a bit longer
293  //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
294  projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
295  projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
296  distance2d = projectedDifference.dot(projectedDifference);
298  {
300  } else
301  {
302  //contact point processed callback
304  (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
305  }
306  }
307  }
308 #ifdef DEBUG_PERSISTENCY
309  DebugPersistency();
310 #endif //
311 }
312 
313 
315 {
316  return sizeof(btPersistentManifoldData);
317 }
318 
319 const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
320 {
321  btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
322  memset(dataOut, 0, sizeof(btPersistentManifoldData));
323 
324  dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
325  dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
326  dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
327  dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
328  dataOut->m_numCachedPoints = manifold->getNumContacts();
329  dataOut->m_companionIdA = manifold->m_companionIdA;
330  dataOut->m_companionIdB = manifold->m_companionIdB;
331  dataOut->m_index1a = manifold->m_index1a;
332  dataOut->m_objectType = manifold->m_objectType;
333 
334  for (int i = 0; i < this->getNumContacts(); i++)
335  {
336  const btManifoldPoint& pt = manifold->getContactPoint(i);
337  dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
338  dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
339  dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
340  pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
341  pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
342  pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
343  dataOut->m_pointCacheDistance[i] = pt.m_distance1;
344  dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
345  dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
346  dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
347  dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
348  dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
349  dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
350  dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
351  dataOut->m_pointCacheIndex0[i] = pt.m_index0;
352  dataOut->m_pointCacheIndex1[i] = pt.m_index1;
353  dataOut->m_pointCachePartId0[i] = pt.m_partId0;
354  dataOut->m_pointCachePartId1[i] = pt.m_partId1;
355  pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
356  pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
357  dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
358  pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
359  pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
360  dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
361  dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
362  dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
363  dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
364  dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
365  }
367 }
368 
370 {
373  m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
374  m_companionIdA = manifoldDataPtr->m_companionIdA;
375  m_companionIdB = manifoldDataPtr->m_companionIdB;
376  //m_index1a = manifoldDataPtr->m_index1a;
377  m_objectType = manifoldDataPtr->m_objectType;
378 
379  for (int i = 0; i < this->getNumContacts(); i++)
380  {
381  btManifoldPoint& pt = m_pointCache[i];
382 
383  pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
389  pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
392  pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
393  pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
394  pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
395  pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
396  pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
397  pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
398  pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
399  pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
400  pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
403  pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
409  pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
410  pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
411  }
412 
413 }
414 
416 {
419  m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
420  m_companionIdA = manifoldDataPtr->m_companionIdA;
421  m_companionIdB = manifoldDataPtr->m_companionIdB;
422  //m_index1a = manifoldDataPtr->m_index1a;
423  m_objectType = manifoldDataPtr->m_objectType;
424 
425  for (int i = 0; i < this->getNumContacts(); i++)
426  {
427  btManifoldPoint& pt = m_pointCache[i];
428 
429  pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
432  pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
433  pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
435  pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
438  pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
439  pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
440  pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
441  pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
442  pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
443  pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
444  pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
445  pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
446  pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
449  pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
455  pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
456  pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
457  }
458 
459 }
ContactStartedCallback gContactStartedCallback
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
int getCacheEntry(const btManifoldPoint &newPoint) const
const btCollisionObject * m_body1
btVector3DoubleData m_pointCachePositionWorldOnB[4]
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1345
btScalar m_combinedContactStiffness1
btVector3 m_lateralFrictionDir1
void refreshContactPoints(const btTransform &trA, const btTransform &trB)
calculated new worldspace coordinates and depth, and reject points that exceed the collision margin ...
btScalar getContactBreakingThreshold() const
btScalar m_appliedImpulseLateral1
btScalar m_combinedRestitution
virtual void * getUniquePointer(void *oldPtr)=0
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btScalar m_appliedImpulse
#define btAssert(x)
Definition: btScalar.h:131
btVector3DoubleData m_pointCacheLocalPointB[4]
const char * serialize(const class btPersistentManifold *manifold, void *dataBuffer, class btSerializer *serializer) const
btScalar m_frictionCFM
ManifoldContactPoint collects and maintains persistent contactpoints.
const btCollisionObject * getBody0() const
btScalar m_contactMotion1
#define MANIFOLD_CACHE_SIZE
int sortCachedPoints(const btManifoldPoint &pt)
sort cached points so most isolated points come first
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
btVector3DoubleData m_pointCacheNormalWorldOnB[4]
#define btCollisionObjectData
btScalar m_combinedRollingFriction
btVector3DoubleData m_pointCachePositionWorldOnA[4]
#define btPersistentManifoldData
btVector3 m_normalWorldOnB
void * m_userPersistentData
btVector3 m_positionWorldOnB
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVector3FloatData m_pointCacheLocalPointB[4]
btVector3 m_localPointA
btScalar m_appliedImpulseLateral2
void(* ContactEndedCallback)(btPersistentManifold *const &manifold)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1352
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
static btScalar calcArea4Points(const btVector3 &p0, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3)
void deSerialize(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1367
btVector3FloatData m_pointCachePositionWorldOnB[4]
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
btScalar getContactProcessingThreshold() const
btVector3DoubleData m_pointCacheLateralFrictionDir1[4]
btVector3FloatData m_pointCacheNormalWorldOnB[4]
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity ...
const btManifoldPoint & getContactPoint(int index) const
bool(* ContactDestroyedCallback)(void *userPersistentData)
ContactDestroyedCallback gContactDestroyedCallback
btScalar gContactBreakingThreshold
maximum contact breaking and merging threshold
void(* ContactStartedCallback)(btPersistentManifold *const &manifold)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
ContactEndedCallback gContactEndedCallback
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
void removeContactPoint(int index)
int calculateSerializeBufferSize() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3FloatData m_pointCacheLocalPointA[4]
bool(* ContactProcessedCallback)(btManifoldPoint &cp, void *body0, void *body1)
btScalar m_combinedContactDamping1
rudimentary class to provide type info
Definition: btScalar.h:777
btVector3FloatData m_pointCacheLateralFrictionDir2[4]
btVector3 m_localPointB
int closestAxis4() const
Definition: btVector3.h:1198
#define btPersistentManifoldDataName
void clearUserCache(btManifoldPoint &pt)
bool gContactCalcArea3Points
gContactCalcArea3Points will approximate the convex hull area using 3 points when setting it to false...
btScalar m_contactMotion2
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
btScalar m_combinedFriction
btVector3DoubleData m_pointCacheLateralFrictionDir2[4]
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:898
void deSerialize(const struct btPersistentManifoldDoubleData *manifoldDataPtr)
btVector3 m_lateralFrictionDir2
btVector3DoubleData m_pointCacheLocalPointA[4]
btScalar getDistance() const
const btCollisionObject * m_body0
this two body pointers can point to the physics rigidbody class.
const btCollisionObject * getBody1() const
int addManifoldPoint(const btManifoldPoint &newPoint, bool isPredictive=false)
btVector3FloatData m_pointCacheLateralFrictionDir1[4]
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar m_combinedSpinningFriction
btVector3FloatData m_pointCachePositionWorldOnA[4]
bool validContactDistance(const btManifoldPoint &pt) const
ContactProcessedCallback gContactProcessedCallback
btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]
int m_objectType
Definition: btScalar.h:783