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 #include "btPersistentManifold.h"
17 #include "LinearMath/btTransform.h"
19 
20 #ifdef BT_USE_DOUBLE_PRECISION
21 #define btCollisionObjectData btCollisionObjectDoubleData
22 #else
23 #define btCollisionObjectData btCollisionObjectFloatData
24 #endif
25 
34 
37  m_body0(0),
38  m_body1(0),
39  m_cachedPoints(0),
40  m_companionIdA(0),
41  m_companionIdB(0),
42  m_index1a(0)
43 {
44 }
45 
46 #ifdef DEBUG_PERSISTENCY
47 #include <stdio.h>
48 void btPersistentManifold::DebugPersistency()
49 {
50  int i;
51  printf("DebugPersistency : numPoints %d\n", m_cachedPoints);
52  for (i = 0; i < m_cachedPoints; i++)
53  {
54  printf("m_pointCache[%d].m_userPersistentData = %x\n", i, m_pointCache[i].m_userPersistentData);
55  }
56 }
57 #endif //DEBUG_PERSISTENCY
58 
60 {
61  void* oldPtr = pt.m_userPersistentData;
62  if (oldPtr)
63  {
64 #ifdef DEBUG_PERSISTENCY
65  int i;
66  int occurance = 0;
67  for (i = 0; i < m_cachedPoints; i++)
68  {
69  if (m_pointCache[i].m_userPersistentData == oldPtr)
70  {
71  occurance++;
72  if (occurance > 1)
73  printf("error in clearUserCache\n");
74  }
75  }
76  btAssert(occurance <= 0);
77 #endif //DEBUG_PERSISTENCY
78 
80  {
81  (*gContactDestroyedCallback)(pt.m_userPersistentData);
82  pt.m_userPersistentData = 0;
83  }
84 
85 #ifdef DEBUG_PERSISTENCY
86  DebugPersistency();
87 #endif
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 
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 
133  {
134  if (maxPenetrationIndex != 0)
135  {
138  btVector3 cross = a0.cross(b0);
139  res0 = cross.length2();
140  }
141  if (maxPenetrationIndex != 1)
142  {
145  btVector3 cross = a1.cross(b1);
146  res1 = cross.length2();
147  }
148 
149  if (maxPenetrationIndex != 2)
150  {
153  btVector3 cross = a2.cross(b2);
154  res2 = cross.length2();
155  }
156 
157  if (maxPenetrationIndex != 3)
158  {
161  btVector3 cross = a3.cross(b3);
162  res3 = cross.length2();
163  }
164  }
165  else
166  {
167  if (maxPenetrationIndex != 0)
168  {
170  }
171 
172  if (maxPenetrationIndex != 1)
173  {
175  }
176 
177  if (maxPenetrationIndex != 2)
178  {
180  }
181 
182  if (maxPenetrationIndex != 3)
183  {
185  }
186  }
187  btVector4 maxvec(res0, res1, res2, res3);
188  int biggestarea = maxvec.closestAxis4();
189  return biggestarea;
190 }
191 
193 {
195  int size = getNumContacts();
196  int nearestPoint = -1;
197  for (int i = 0; i < size; i++)
198  {
199  const btManifoldPoint& mp = m_pointCache[i];
200 
201  btVector3 diffA = mp.m_localPointA - newPoint.m_localPointA;
202  const btScalar distToManiPoint = diffA.dot(diffA);
203  if (distToManiPoint < shortestDist)
204  {
205  shortestDist = distToManiPoint;
206  nearestPoint = i;
207  }
208  }
209  return nearestPoint;
210 }
211 
212 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
213 {
214  if (!isPredictive)
215  {
216  btAssert(validContactDistance(newPoint));
217  }
218 
219  int insertIndex = getNumContacts();
220  if (insertIndex == MANIFOLD_CACHE_SIZE)
221  {
222 #if MANIFOLD_CACHE_SIZE >= 4
223  //sort cache so best points come first, based on area
224  insertIndex = sortCachedPoints(newPoint);
225 #else
226  insertIndex = 0;
227 #endif
228  clearUserCache(m_pointCache[insertIndex]);
229  }
230  else
231  {
232  m_cachedPoints++;
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 
243 {
245 }
246 
248 {
249  int i;
250 #ifdef DEBUG_PERSISTENCY
251  printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
252  trA.getOrigin().getX(),
253  trA.getOrigin().getY(),
254  trA.getOrigin().getZ(),
255  trB.getOrigin().getX(),
256  trB.getOrigin().getY(),
257  trB.getOrigin().getZ());
258 #endif //DEBUG_PERSISTENCY
260  for (i = getNumContacts() - 1; i >= 0; i--)
261  {
262  btManifoldPoint& manifoldPoint = m_pointCache[i];
263  manifoldPoint.m_positionWorldOnA = trA(manifoldPoint.m_localPointA);
264  manifoldPoint.m_positionWorldOnB = trB(manifoldPoint.m_localPointB);
265  manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
266  manifoldPoint.m_lifeTime++;
267  }
268 
270  btScalar distance2d;
271  btVector3 projectedDifference, projectedPoint;
272  for (i = getNumContacts() - 1; i >= 0; i--)
273  {
274  btManifoldPoint& manifoldPoint = m_pointCache[i];
275  //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
276  if (!validContactDistance(manifoldPoint))
277  {
279  }
280  else
281  {
282  //todo: friction anchor may require the contact to be around a bit longer
283  //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
284  projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
285  projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
286  distance2d = projectedDifference.dot(projectedDifference);
288  {
290  }
291  else
292  {
293  //contact point processed callback
295  (*gContactProcessedCallback)(manifoldPoint, (void*)m_body0, (void*)m_body1);
296  }
297  }
298  }
299 #ifdef DEBUG_PERSISTENCY
300  DebugPersistency();
301 #endif //
302 }
303 
305 {
306  return sizeof(btPersistentManifoldData);
307 }
308 
309 const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
310 {
311  btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
312  memset(dataOut, 0, sizeof(btPersistentManifoldData));
313 
314  dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
315  dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
316  dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
317  dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
318  dataOut->m_numCachedPoints = manifold->getNumContacts();
319  dataOut->m_companionIdA = manifold->m_companionIdA;
320  dataOut->m_companionIdB = manifold->m_companionIdB;
321  dataOut->m_index1a = manifold->m_index1a;
322  dataOut->m_objectType = manifold->m_objectType;
323 
324  for (int i = 0; i < this->getNumContacts(); i++)
325  {
326  const btManifoldPoint& pt = manifold->getContactPoint(i);
327  dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
328  dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
329  dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
330  dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
331  pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
332  pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
333  pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
334  dataOut->m_pointCacheDistance[i] = pt.m_distance1;
335  dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
336  dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
337  dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
338  dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
339  dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
340  dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
341  dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
342  dataOut->m_pointCacheIndex0[i] = pt.m_index0;
343  dataOut->m_pointCacheIndex1[i] = pt.m_index1;
344  dataOut->m_pointCachePartId0[i] = pt.m_partId0;
345  dataOut->m_pointCachePartId1[i] = pt.m_partId1;
346  pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
347  pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
348  dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
349  pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
350  pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
351  dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
352  dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
353  dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
354  dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
355  dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
356  }
358 }
359 
361 {
364  m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
365  m_companionIdA = manifoldDataPtr->m_companionIdA;
366  m_companionIdB = manifoldDataPtr->m_companionIdB;
367  //m_index1a = manifoldDataPtr->m_index1a;
368  m_objectType = manifoldDataPtr->m_objectType;
369 
370  for (int i = 0; i < this->getNumContacts(); i++)
371  {
372  btManifoldPoint& pt = m_pointCache[i];
373 
374  pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
375  pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
381  pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
384  pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
385  pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
386  pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
387  pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
388  pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
389  pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
390  pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
391  pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
392  pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
395  pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
401  pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
402  pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
403  }
404 }
405 
407 {
410  m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
411  m_companionIdA = manifoldDataPtr->m_companionIdA;
412  m_companionIdB = manifoldDataPtr->m_companionIdB;
413  //m_index1a = manifoldDataPtr->m_index1a;
414  m_objectType = manifoldDataPtr->m_objectType;
415 
416  for (int i = 0; i < this->getNumContacts(); i++)
417  {
418  btManifoldPoint& pt = m_pointCache[i];
419 
420  pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
421  pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
424  pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
425  pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
427  pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
430  pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
431  pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
432  pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
433  pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
434  pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
435  pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
436  pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
437  pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
438  pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
441  pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
447  pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
448  pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
449  }
450 }
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
bool gContactCalcArea3Points
gContactCalcArea3Points will approximate the convex hull area using 3 points when setting it to false...
#define btCollisionObjectData
ContactEndedCallback gContactEndedCallback
static btScalar calcArea4Points(const btVector3 &p0, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3)
ContactStartedCallback gContactStartedCallback
ContactProcessedCallback gContactProcessedCallback
ContactDestroyedCallback gContactDestroyedCallback
btScalar gContactBreakingThreshold
maximum contact breaking and merging threshold
void(* ContactEndedCallback)(btPersistentManifold *const &manifold)
void(* ContactStartedCallback)(btPersistentManifold *const &manifold)
bool(* ContactDestroyedCallback)(void *userPersistentData)
#define btPersistentManifoldDataName
#define btPersistentManifoldData
@ BT_PERSISTENT_MANIFOLD_TYPE
#define MANIFOLD_CACHE_SIZE
bool(* ContactProcessedCallback)(btManifoldPoint &cp, void *body0, void *body1)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:153
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_frictionCFM
btScalar m_combinedSpinningFriction
btScalar m_combinedRollingFriction
btScalar getDistance() const
btScalar m_combinedContactStiffness1
btScalar m_combinedRestitution
btVector3 m_localPointA
btVector3 m_lateralFrictionDir2
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
btVector3 m_localPointB
btScalar m_combinedContactDamping1
btScalar m_appliedImpulseLateral2
btScalar m_appliedImpulse
void * m_userPersistentData
btScalar m_appliedImpulseLateral1
btVector3 m_normalWorldOnB
btScalar m_combinedFriction
btVector3 m_positionWorldOnB
btScalar m_contactMotion2
btVector3 m_lateralFrictionDir1
btScalar m_contactMotion1
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btCollisionObject * m_body1
const btCollisionObject * getBody1() const
bool validContactDistance(const btManifoldPoint &pt) const
const char * serialize(const class btPersistentManifold *manifold, void *dataBuffer, class btSerializer *serializer) const
int getCacheEntry(const btManifoldPoint &newPoint) const
const btCollisionObject * getBody0() const
void refreshContactPoints(const btTransform &trA, const btTransform &trB)
calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
int calculateSerializeBufferSize() const
int sortCachedPoints(const btManifoldPoint &pt)
sort cached points so most isolated points come first
void clearUserCache(btManifoldPoint &pt)
void removeContactPoint(int index)
void deSerialize(const struct btPersistentManifoldDoubleData *manifoldDataPtr)
int addManifoldPoint(const btManifoldPoint &newPoint, bool isPredictive=false)
btScalar getContactBreakingThreshold() const
const btManifoldPoint & getContactPoint(int index) const
btScalar getContactProcessingThreshold() const
const btCollisionObject * m_body0
this two body pointers can point to the physics rigidbody class.
btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]
virtual void * getUniquePointer(void *oldPtr)=0
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
void deSerialize(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1330
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1311
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1317
int closestAxis4() const
Definition: btVector3.h:1174
btVector3DoubleData m_pointCacheLateralFrictionDir1[4]
btVector3DoubleData m_pointCachePositionWorldOnB[4]
btVector3DoubleData m_pointCacheLocalPointB[4]
btVector3DoubleData m_pointCachePositionWorldOnA[4]
btVector3DoubleData m_pointCacheNormalWorldOnB[4]
btVector3DoubleData m_pointCacheLateralFrictionDir2[4]
btVector3DoubleData m_pointCacheLocalPointA[4]
btVector3FloatData m_pointCacheNormalWorldOnB[4]
btVector3FloatData m_pointCacheLocalPointB[4]
btVector3FloatData m_pointCachePositionWorldOnB[4]
btVector3FloatData m_pointCacheLocalPointA[4]
btVector3FloatData m_pointCachePositionWorldOnA[4]
btVector3FloatData m_pointCacheLateralFrictionDir1[4]
btVector3FloatData m_pointCacheLateralFrictionDir2[4]
rudimentary class to provide type info
Definition: btScalar.h:800
int m_objectType
Definition: btScalar.h:805