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 
29 
30 
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 
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 
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 
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) {
169  }
170 
171  if(maxPenetrationIndex != 1) {
173  }
174 
175  if(maxPenetrationIndex != 2) {
177  }
178 
179  if(maxPenetrationIndex != 3) {
181  }
182  }
183  btVector4 maxvec(res0,res1,res2,res3);
184  int biggestarea = maxvec.closestAxis4();
185  return biggestarea;
186 
187 }
188 
189 
191 {
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 
243 {
245 }
246 
247 
248 
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  {
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);
290  {
292  } else
293  {
294  //contact point processed callback
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 
ContactStartedCallback gContactStartedCallback
const btCollisionObject * m_body1
btScalar getContactBreakingThreshold() const
void refreshContactPoints(const btTransform &trA, const btTransform &trB)
calculated new worldspace coordinates and depth, and reject points that exceed the collision margin ...
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
#define btAssert(x)
Definition: btScalar.h:131
bool validContactDistance(const btManifoldPoint &pt) const
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
ManifoldContactPoint collects and maintains persistent contactpoints.
#define MANIFOLD_CACHE_SIZE
int sortCachedPoints(const btManifoldPoint &pt)
sort cached points so most isolated points come first
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
btVector3 m_normalWorldOnB
void * m_userPersistentData
btVector3 m_positionWorldOnB
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVector3 m_localPointA
void(* ContactEndedCallback)(btPersistentManifold *const &manifold)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
static btScalar calcArea4Points(const btVector3 &p0, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3)
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity ...
int closestAxis4() const
Definition: btVector3.h:1197
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
void removeContactPoint(int index)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
bool(* ContactProcessedCallback)(btManifoldPoint &cp, void *body0, void *body1)
rudimentary class to provide type info
Definition: btScalar.h:777
btVector3 m_localPointB
void clearUserCache(btManifoldPoint &pt)
bool gContactCalcArea3Points
gContactCalcArea3Points will approximate the convex hull area using 3 points when setting it to false...
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
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:573
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
ContactProcessedCallback gContactProcessedCallback
btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]
int getCacheEntry(const btManifoldPoint &newPoint) const