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
259  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_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
329  dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
330  pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
331  pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
332  pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
333  dataOut->m_pointCacheDistance[i] = pt.m_distance1;
334  dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
335  dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
336  dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
337  dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
338  dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
339  dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
340  dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
341  dataOut->m_pointCacheIndex0[i] = pt.m_index0;
342  dataOut->m_pointCacheIndex1[i] = pt.m_index1;
343  dataOut->m_pointCachePartId0[i] = pt.m_partId0;
344  dataOut->m_pointCachePartId1[i] = pt.m_partId1;
345  pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
346  pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
347  dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
348  pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
349  pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
350  dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
351  dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
352  dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
353  dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
354  dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
355  }
357 }
358 
360 {
363  m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
364  m_companionIdA = manifoldDataPtr->m_companionIdA;
365  m_companionIdB = manifoldDataPtr->m_companionIdB;
366  //m_index1a = manifoldDataPtr->m_index1a;
367  m_objectType = manifoldDataPtr->m_objectType;
368 
369  for (int i = 0; i < this->getNumContacts(); i++)
370  {
371  btManifoldPoint& pt = m_pointCache[i];
372 
373  pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
379  pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
382  pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
383  pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
384  pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
385  pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
386  pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
387  pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
388  pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
389  pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
390  pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
393  pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
399  pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
400  pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
401  }
402 }
403 
405 {
408  m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
409  m_companionIdA = manifoldDataPtr->m_companionIdA;
410  m_companionIdB = manifoldDataPtr->m_companionIdB;
411  //m_index1a = manifoldDataPtr->m_index1a;
412  m_objectType = manifoldDataPtr->m_objectType;
413 
414  for (int i = 0; i < this->getNumContacts(); i++)
415  {
416  btManifoldPoint& pt = m_pointCache[i];
417 
418  pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
421  pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
422  pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
424  pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
427  pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
428  pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
429  pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
430  pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
431  pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
432  pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
433  pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
434  pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
435  pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
438  pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
444  pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
445  pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
446  }
447 }
ContactStartedCallback gContactStartedCallback
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btCollisionObject * m_body1
btScalar getContactBreakingThreshold() const
btVector3DoubleData m_pointCachePositionWorldOnB[4]
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1311
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 m_appliedImpulseLateral1
btScalar m_combinedRestitution
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
void(* ContactEndedCallback)(btPersistentManifold *const &manifold)
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:133
btVector3DoubleData m_pointCacheLocalPointB[4]
bool validContactDistance(const btManifoldPoint &pt) const
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btScalar m_frictionCFM
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_contactMotion1
#define MANIFOLD_CACHE_SIZE
int sortCachedPoints(const btManifoldPoint &pt)
sort cached points so most isolated points come first
const btManifoldPoint & getContactPoint(int index) const
btVector3DoubleData m_pointCacheNormalWorldOnB[4]
#define btCollisionObjectData
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
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:113
btVector3FloatData m_pointCacheLocalPointB[4]
btVector3 m_localPointA
const btCollisionObject * getBody0() const
btScalar m_appliedImpulseLateral2
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
void(* ContactStartedCallback)(btPersistentManifold *const &manifold)
static btScalar calcArea4Points(const btVector3 &p0, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3)
void deSerialize(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1330
btVector3FloatData m_pointCachePositionWorldOnB[4]
btVector3DoubleData m_pointCacheLateralFrictionDir1[4]
btVector3FloatData m_pointCacheNormalWorldOnB[4]
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity ...
int closestAxis4() const
Definition: btVector3.h:1174
btScalar getContactProcessingThreshold() const
int calculateSerializeBufferSize() const
bool(* ContactDestroyedCallback)(void *userPersistentData)
ContactDestroyedCallback gContactDestroyedCallback
btScalar gContactBreakingThreshold
maximum contact breaking and merging threshold
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
ContactEndedCallback gContactEndedCallback
void removeContactPoint(int index)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1317
btVector3FloatData m_pointCacheLocalPointA[4]
bool(* ContactProcessedCallback)(btManifoldPoint &cp, void *body0, void *body1)
btScalar m_combinedContactDamping1
rudimentary class to provide type info
Definition: btScalar.h:779
btVector3FloatData m_pointCacheLateralFrictionDir2[4]
btVector3 m_localPointB
#define btPersistentManifoldDataName
const btCollisionObject * getBody1() const
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:27
btScalar m_combinedFriction
btVector3DoubleData m_pointCacheLateralFrictionDir2[4]
const char * serialize(const class btPersistentManifold *manifold, void *dataBuffer, class btSerializer *serializer) const
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
void deSerialize(const struct btPersistentManifoldDoubleData *manifoldDataPtr)
btVector3 m_lateralFrictionDir2
btVector3DoubleData m_pointCacheLocalPointA[4]
const btCollisionObject * m_body0
this two body pointers can point to the physics rigidbody class.
int addManifoldPoint(const btManifoldPoint &newPoint, bool isPredictive=false)
btScalar getDistance() const
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btVector3FloatData m_pointCacheLateralFrictionDir1[4]
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btScalar m_combinedSpinningFriction
btVector3FloatData m_pointCachePositionWorldOnA[4]
ContactProcessedCallback gContactProcessedCallback
btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]
int getCacheEntry(const btManifoldPoint &newPoint) const
int m_objectType
Definition: btScalar.h:785