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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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;
26 bool gContactCalcArea3Points = true;
27
28
29 btPersistentManifold::btPersistentManifold()
30 :btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
31 m_body0(0),
32 m_body1(0),
33 m_cachedPoints (0),
34 m_index1a(0)
35 {
36 }
37
38
39
40
41 #ifdef DEBUG_PERSISTENCY
42 #include <stdio.h>
43 void btPersistentManifold::DebugPersistency()
44 {
45 int i;
46 printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
47 for (i=0;i<m_cachedPoints;i++)
48 {
49 printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
50 }
51 }
52 #endif //DEBUG_PERSISTENCY
53
54 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
55 {
56
57 void* oldPtr = pt.m_userPersistentData;
58 if (oldPtr)
59 {
60 #ifdef DEBUG_PERSISTENCY
61 int i;
62 int occurance = 0;
63 for (i=0;i<m_cachedPoints;i++)
64 {
65 if (m_pointCache[i].m_userPersistentData == oldPtr)
66 {
67 occurance++;
68 if (occurance>1)
69 printf("error in clearUserCache\n");
70 }
71 }
72 btAssert(occurance<=0);
73 #endif //DEBUG_PERSISTENCY
74
75 if (pt.m_userPersistentData && gContactDestroyedCallback)
76 {
77 (*gContactDestroyedCallback)(pt.m_userPersistentData);
78 pt.m_userPersistentData = 0;
79 }
80
81 #ifdef DEBUG_PERSISTENCY
82 DebugPersistency();
83 #endif
84 }
85
86
87 }
88
89 static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
90 {
91 // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
92
93 btVector3 a[3],b[3];
94 a[0] = p0 - p1;
95 a[1] = p0 - p2;
96 a[2] = p0 - p3;
97 b[0] = p2 - p3;
98 b[1] = p1 - p3;
99 b[2] = p1 - p2;
100
101 //todo: Following 3 cross production can be easily optimized by SIMD.
102 btVector3 tmp0 = a[0].cross(b[0]);
103 btVector3 tmp1 = a[1].cross(b[1]);
104 btVector3 tmp2 = a[2].cross(b[2]);
105
106 return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
107 }
108
109 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
110 {
111 //calculate 4 possible cases areas, and take biggest area
112 //also need to keep 'deepest'
113
114 int maxPenetrationIndex = -1;
115 #define KEEP_DEEPEST_POINT 1
116 #ifdef KEEP_DEEPEST_POINT
117 btScalar maxPenetration = pt.getDistance();
118 for (int i=0;i<4;i++)
119 {
120 if (m_pointCache[i].getDistance() < maxPenetration)
121 {
122 maxPenetrationIndex = i;
123 maxPenetration = m_pointCache[i].getDistance();
124 }
125 }
126 #endif //KEEP_DEEPEST_POINT
127
128 btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
129
130 if (gContactCalcArea3Points)
131 {
132 if (maxPenetrationIndex != 0)
133 {
134 btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
135 btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
136 btVector3 cross = a0.cross(b0);
137 res0 = cross.length2();
138 }
139 if (maxPenetrationIndex != 1)
140 {
141 btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
142 btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
143 btVector3 cross = a1.cross(b1);
144 res1 = cross.length2();
145 }
146
147 if (maxPenetrationIndex != 2)
148 {
149 btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
150 btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
151 btVector3 cross = a2.cross(b2);
152 res2 = cross.length2();
153 }
154
155 if (maxPenetrationIndex != 3)
156 {
157 btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
158 btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
159 btVector3 cross = a3.cross(b3);
160 res3 = cross.length2();
161 }
162 }
163 else
164 {
165 if(maxPenetrationIndex != 0) {
166 res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
167 }
168
169 if(maxPenetrationIndex != 1) {
170 res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
171 }
172
173 if(maxPenetrationIndex != 2) {
174 res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
175 }
176
177 if(maxPenetrationIndex != 3) {
178 res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
179 }
180 }
181 btVector4 maxvec(res0,res1,res2,res3);
182 int biggestarea = maxvec.closestAxis4();
183 return biggestarea;
184
185 }
186
187
188 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
189 {
190 btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
191 int size = getNumContacts();
192 int nearestPoint = -1;
193 for( int i = 0; i < size; i++ )
194 {
195 const btManifoldPoint &mp = m_pointCache[i];
196
197 btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
198 const btScalar distToManiPoint = diffA.dot(diffA);
199 if( distToManiPoint < shortestDist )
200 {
201 shortestDist = distToManiPoint;
202 nearestPoint = i;
203 }
204 }
205 return nearestPoint;
206 }
207
208 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
209 {
210 if (!isPredictive)
211 {
212 btAssert(validContactDistance(newPoint));
213 }
214
215 int insertIndex = getNumContacts();
216 if (insertIndex == MANIFOLD_CACHE_SIZE)
217 {
218 #if MANIFOLD_CACHE_SIZE >= 4
219 //sort cache so best points come first, based on area
220 insertIndex = sortCachedPoints(newPoint);
221 #else
222 insertIndex = 0;
223 #endif
224 clearUserCache(m_pointCache[insertIndex]);
225
226 } else
227 {
228 m_cachedPoints++;
229
230
231 }
232 if (insertIndex<0)
233 insertIndex=0;
234
235 btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
236 m_pointCache[insertIndex] = newPoint;
237 return insertIndex;
238 }
239
240 btScalar btPersistentManifold::getContactBreakingThreshold() const
241 {
242 return m_contactBreakingThreshold;
243 }
244
245
246
247 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
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
275 btManifoldPoint &manifoldPoint = m_pointCache[i];
276 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
277 if (!validContactDistance(manifoldPoint))
278 {
279 removeContactPoint(i);
280 } else
281 {
282 //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
283 projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
284 projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
285 distance2d = projectedDifference.dot(projectedDifference);
286 if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() )
287 {
288 removeContactPoint(i);
289 } else
290 {
291 //contact point processed callback
292 if (gContactProcessedCallback)
293 (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
294 }
295 }
296 }
297 #ifdef DEBUG_PERSISTENCY
298 DebugPersistency();
299 #endif //
300 }
301
302
303
304
305
btPersistentManifold::getCacheEntry
int getCacheEntry(const btManifoldPoint &newPoint) const
Definition: btPersistentManifold.cpp:188
btPersistentManifold::m_body1
const btCollisionObject * m_body1
Definition: btPersistentManifold.h:62
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:247
BT_PERSISTENT_MANIFOLD_TYPE
Definition: btPersistentManifold.h:40
btPersistentManifold::getContactBreakingThreshold
btScalar getContactBreakingThreshold() const
Definition: btPersistentManifold.cpp:240
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btPersistentManifold.h
btTransform.h
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:42
MANIFOLD_CACHE_SIZE
#define MANIFOLD_CACHE_SIZE
Definition: btPersistentManifold.h:43
btPersistentManifold::m_cachedPoints
int m_cachedPoints
Definition: btPersistentManifold.h:64
btPersistentManifold::sortCachedPoints
int sortCachedPoints(const btManifoldPoint &pt)
sort cached points so most isolated points come first
Definition: btPersistentManifold.cpp:109
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVector4
Definition: btVector3.h:1077
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:90
btManifoldPoint::m_userPersistentData
void * m_userPersistentData
Definition: btManifoldPoint.h:103
btManifoldPoint::m_positionWorldOnB
btVector3 m_positionWorldOnB
Definition: btManifoldPoint.h:87
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btManifoldPoint::m_localPointA
btVector3 m_localPointA
Definition: btManifoldPoint.h:85
btPersistentManifold::btPersistentManifold
btPersistentManifold()
Definition: btPersistentManifold.cpp:29
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btManifoldPoint::m_lifeTime
int m_lifeTime
Definition: btManifoldPoint.h:114
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
calcArea4Points
static btScalar calcArea4Points(const btVector3 &p0, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3)
Definition: btPersistentManifold.cpp:89
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btManifoldPoint::m_positionWorldOnA
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity ...
Definition: btManifoldPoint.h:89
ContactDestroyedCallback
bool(* ContactDestroyedCallback)(void *userPersistentData)
Definition: btPersistentManifold.h:31
gContactDestroyedCallback
ContactDestroyedCallback gContactDestroyedCallback
Definition: btPersistentManifold.cpp:22
gContactBreakingThreshold
btScalar gContactBreakingThreshold
maximum contact breaking and merging threshold
Definition: btPersistentManifold.cpp:21
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btPersistentManifold::removeContactPoint
void removeContactPoint(int index)
Definition: btPersistentManifold.h:154
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:92
ContactProcessedCallback
bool(* ContactProcessedCallback)(btManifoldPoint &cp, void *body0, void *body1)
Definition: btPersistentManifold.h:32
btTypedObject
rudimentary class to provide type info
Definition: btScalar.h:744
btManifoldPoint::m_localPointB
btVector3 m_localPointB
Definition: btManifoldPoint.h:86
btVector4::closestAxis4
int closestAxis4() const
Definition: btVector3.h:1186
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:109
btPersistentManifold::clearUserCache
void clearUserCache(btManifoldPoint &pt)
Definition: btPersistentManifold.cpp:54
gContactCalcArea3Points
bool gContactCalcArea3Points
gContactCalcArea3Points will approximate the convex hull area using 3 points when setting it to false...
Definition: btPersistentManifold.cpp:26
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:845
btPersistentManifold::m_contactBreakingThreshold
btScalar m_contactBreakingThreshold
Definition: btPersistentManifold.h:66
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:122
btPersistentManifold::m_body0
const btCollisionObject * m_body0
this two body pointers can point to the physics rigidbody class.
Definition: btPersistentManifold.h:61
btPersistentManifold::addManifoldPoint
int addManifoldPoint(const btManifoldPoint &newPoint, bool isPredictive=false)
Definition: btPersistentManifold.cpp:208
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btPersistentManifold::validContactDistance
bool validContactDistance(const btManifoldPoint &pt) const
Definition: btPersistentManifold.h:213
gContactProcessedCallback
ContactProcessedCallback gContactProcessedCallback
Definition: btPersistentManifold.cpp:23
btPersistentManifold::m_pointCache
btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]
Definition: btPersistentManifold.h:58