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 #ifndef BT_PERSISTENT_MANIFOLD_H
17 #define BT_PERSISTENT_MANIFOLD_H
18
19 #include "LinearMath/btVector3.h"
20 #include "LinearMath/btTransform.h"
21 #include "btManifoldPoint.h"
22 class btCollisionObject;
23 #include "LinearMath/btAlignedAllocator.h"
24
25 struct btCollisionResult;
26 struct btCollisionObjectDoubleData;
27 struct btCollisionObjectFloatData;
28
29 ///maximum contact breaking and merging threshold
30 extern btScalar gContactBreakingThreshold;
31
32 #ifndef SWIG
33 class btPersistentManifold;
34
35 typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
36 typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp, void* body0, void* body1);
37 typedef void (*ContactStartedCallback)(btPersistentManifold* const& manifold);
38 typedef void (*ContactEndedCallback)(btPersistentManifold* const& manifold);
39 extern ContactDestroyedCallback gContactDestroyedCallback;
40 extern ContactProcessedCallback gContactProcessedCallback;
41 extern ContactStartedCallback gContactStartedCallback;
42 extern ContactEndedCallback gContactEndedCallback;
43 #endif //SWIG
44
45 //the enum starts at 1024 to avoid type conflicts with btTypedConstraint
46 enum btContactManifoldTypes
47 {
48 MIN_CONTACT_MANIFOLD_TYPE = 1024,
49 BT_PERSISTENT_MANIFOLD_TYPE
50 };
51
52 #define MANIFOLD_CACHE_SIZE 4
53
54 ///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
55 ///Those contact points are created by the collision narrow phase.
56 ///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
57 ///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
58 ///reduces the cache to 4 points, when more then 4 points are added, using following rules:
59 ///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
60 ///note that some pairs of objects might have more then one contact manifold.
61
62 //ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
ATTRIBUTE_ALIGNED16(class)63 ATTRIBUTE_ALIGNED16(class)
64 btPersistentManifold : public btTypedObject
65 {
66 btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
67
68 /// this two body pointers can point to the physics rigidbody class.
69 const btCollisionObject* m_body0;
70 const btCollisionObject* m_body1;
71
72 int m_cachedPoints;
73
74 btScalar m_contactBreakingThreshold;
75 btScalar m_contactProcessingThreshold;
76
77 /// sort cached points so most isolated points come first
78 int sortCachedPoints(const btManifoldPoint& pt);
79
80 int findContactPoint(const btManifoldPoint* unUsed, int numUnused, const btManifoldPoint& pt);
81
82 public:
83 BT_DECLARE_ALIGNED_ALLOCATOR();
84
85 int m_companionIdA;
86 int m_companionIdB;
87
88 int m_index1a;
89
90 btPersistentManifold();
91
92 btPersistentManifold(const btCollisionObject* body0, const btCollisionObject* body1, int, btScalar contactBreakingThreshold, btScalar contactProcessingThreshold)
93 : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
94 m_body0(body0),
95 m_body1(body1),
96 m_cachedPoints(0),
97 m_contactBreakingThreshold(contactBreakingThreshold),
98 m_contactProcessingThreshold(contactProcessingThreshold),
99 m_companionIdA(0),
100 m_companionIdB(0),
101 m_index1a(0)
102 {
103 }
104
105 SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0; }
106 SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1; }
107
108 void setBodies(const btCollisionObject* body0, const btCollisionObject* body1)
109 {
110 m_body0 = body0;
111 m_body1 = body1;
112 }
113
114 void clearUserCache(btManifoldPoint & pt);
115
116 #ifdef DEBUG_PERSISTENCY
117 void DebugPersistency();
118 #endif //
119
120 SIMD_FORCE_INLINE int getNumContacts() const
121 {
122 return m_cachedPoints;
123 }
124 /// the setNumContacts API is usually not used, except when you gather/fill all contacts manually
125 void setNumContacts(int cachedPoints)
126 {
127 m_cachedPoints = cachedPoints;
128 }
129
130 SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
131 {
132 btAssert(index < m_cachedPoints);
133 return m_pointCache[index];
134 }
135
136 SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
137 {
138 btAssert(index < m_cachedPoints);
139 return m_pointCache[index];
140 }
141
142 ///@todo: get this margin from the current physics / collision environment
143 btScalar getContactBreakingThreshold() const;
144
145 btScalar getContactProcessingThreshold() const
146 {
147 return m_contactProcessingThreshold;
148 }
149
150 void setContactBreakingThreshold(btScalar contactBreakingThreshold)
151 {
152 m_contactBreakingThreshold = contactBreakingThreshold;
153 }
154
155 void setContactProcessingThreshold(btScalar contactProcessingThreshold)
156 {
157 m_contactProcessingThreshold = contactProcessingThreshold;
158 }
159
160 int getCacheEntry(const btManifoldPoint& newPoint) const;
161
162 int addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive = false);
163
164 void removeContactPoint(int index)
165 {
166 clearUserCache(m_pointCache[index]);
167
168 int lastUsedIndex = getNumContacts() - 1;
169 // m_pointCache[index] = m_pointCache[lastUsedIndex];
170 if (index != lastUsedIndex)
171 {
172 m_pointCache[index] = m_pointCache[lastUsedIndex];
173 //get rid of duplicated userPersistentData pointer
174 m_pointCache[lastUsedIndex].m_userPersistentData = 0;
175 m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
176 m_pointCache[lastUsedIndex].m_prevRHS = 0.f;
177 m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
178 m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
179 m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
180 m_pointCache[lastUsedIndex].m_lifeTime = 0;
181 }
182
183 btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0);
184 m_cachedPoints--;
185
186 if (gContactEndedCallback && m_cachedPoints == 0)
187 {
188 gContactEndedCallback(this);
189 }
190 }
191 void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex)
192 {
193 btAssert(validContactDistance(newPoint));
194
195 #define MAINTAIN_PERSISTENCY 1
196 #ifdef MAINTAIN_PERSISTENCY
197 int lifeTime = m_pointCache[insertIndex].getLifeTime();
198 btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
199 btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS;
200 btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
201 btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
202
203 bool replacePoint = true;
204 ///we keep existing contact points for friction anchors
205 ///if the friction force is within the Coulomb friction cone
206 if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
207 {
208 // printf("appliedImpulse=%f\n", appliedImpulse);
209 // printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1);
210 // printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2);
211 // printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction);
212 btScalar mu = m_pointCache[insertIndex].m_combinedFriction;
213 btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7
214 btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2;
215 btScalar b = eps + mu * appliedImpulse;
216 b = b * b;
217 replacePoint = (a) > (b);
218 }
219
220 if (replacePoint)
221 {
222 btAssert(lifeTime >= 0);
223 void* cache = m_pointCache[insertIndex].m_userPersistentData;
224
225 m_pointCache[insertIndex] = newPoint;
226 m_pointCache[insertIndex].m_userPersistentData = cache;
227 m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
228 m_pointCache[insertIndex].m_prevRHS = prevRHS;
229 m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
230 m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
231 }
232
233 m_pointCache[insertIndex].m_lifeTime = lifeTime;
234 #else
235 clearUserCache(m_pointCache[insertIndex]);
236 m_pointCache[insertIndex] = newPoint;
237
238 #endif
239 }
240
241 bool validContactDistance(const btManifoldPoint& pt) const
242 {
243 return pt.m_distance1 <= getContactBreakingThreshold();
244 }
245 /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
246 void refreshContactPoints(const btTransform& trA, const btTransform& trB);
247
248 SIMD_FORCE_INLINE void clearManifold()
249 {
250 int i;
251 for (i = 0; i < m_cachedPoints; i++)
252 {
253 clearUserCache(m_pointCache[i]);
254 }
255
256 if (gContactEndedCallback && m_cachedPoints)
257 {
258 gContactEndedCallback(this);
259 }
260 m_cachedPoints = 0;
261 }
262
263 int calculateSerializeBufferSize() const;
264 const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
265 void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
266 void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
267 };
268
269 // clang-format off
270
271 struct btPersistentManifoldDoubleData
272 {
273 btVector3DoubleData m_pointCacheLocalPointA[4];
274 btVector3DoubleData m_pointCacheLocalPointB[4];
275 btVector3DoubleData m_pointCachePositionWorldOnA[4];
276 btVector3DoubleData m_pointCachePositionWorldOnB[4];
277 btVector3DoubleData m_pointCacheNormalWorldOnB[4];
278 btVector3DoubleData m_pointCacheLateralFrictionDir1[4];
279 btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
280 double m_pointCacheDistance[4];
281 double m_pointCacheAppliedImpulse[4];
282 double m_pointCachePrevRHS[4];
283 double m_pointCacheCombinedFriction[4];
284 double m_pointCacheCombinedRollingFriction[4];
285 double m_pointCacheCombinedSpinningFriction[4];
286 double m_pointCacheCombinedRestitution[4];
287 int m_pointCachePartId0[4];
288 int m_pointCachePartId1[4];
289 int m_pointCacheIndex0[4];
290 int m_pointCacheIndex1[4];
291 int m_pointCacheContactPointFlags[4];
292 double m_pointCacheAppliedImpulseLateral1[4];
293 double m_pointCacheAppliedImpulseLateral2[4];
294 double m_pointCacheContactMotion1[4];
295 double m_pointCacheContactMotion2[4];
296 double m_pointCacheContactCFM[4];
297 double m_pointCacheCombinedContactStiffness1[4];
298 double m_pointCacheContactERP[4];
299 double m_pointCacheCombinedContactDamping1[4];
300 double m_pointCacheFrictionCFM[4];
301 int m_pointCacheLifeTime[4];
302
303 int m_numCachedPoints;
304 int m_companionIdA;
305 int m_companionIdB;
306 int m_index1a;
307
308 int m_objectType;
309 double m_contactBreakingThreshold;
310 double m_contactProcessingThreshold;
311 int m_padding;
312
313 btCollisionObjectDoubleData *m_body0;
314 btCollisionObjectDoubleData *m_body1;
315 };
316
317
318 struct btPersistentManifoldFloatData
319 {
320 btVector3FloatData m_pointCacheLocalPointA[4];
321 btVector3FloatData m_pointCacheLocalPointB[4];
322 btVector3FloatData m_pointCachePositionWorldOnA[4];
323 btVector3FloatData m_pointCachePositionWorldOnB[4];
324 btVector3FloatData m_pointCacheNormalWorldOnB[4];
325 btVector3FloatData m_pointCacheLateralFrictionDir1[4];
326 btVector3FloatData m_pointCacheLateralFrictionDir2[4];
327 float m_pointCacheDistance[4];
328 float m_pointCacheAppliedImpulse[4];
329 float m_pointCachePrevRHS[4];
330 float m_pointCacheCombinedFriction[4];
331 float m_pointCacheCombinedRollingFriction[4];
332 float m_pointCacheCombinedSpinningFriction[4];
333 float m_pointCacheCombinedRestitution[4];
334 int m_pointCachePartId0[4];
335 int m_pointCachePartId1[4];
336 int m_pointCacheIndex0[4];
337 int m_pointCacheIndex1[4];
338 int m_pointCacheContactPointFlags[4];
339 float m_pointCacheAppliedImpulseLateral1[4];
340 float m_pointCacheAppliedImpulseLateral2[4];
341 float m_pointCacheContactMotion1[4];
342 float m_pointCacheContactMotion2[4];
343 float m_pointCacheContactCFM[4];
344 float m_pointCacheCombinedContactStiffness1[4];
345 float m_pointCacheContactERP[4];
346 float m_pointCacheCombinedContactDamping1[4];
347 float m_pointCacheFrictionCFM[4];
348 int m_pointCacheLifeTime[4];
349
350 int m_numCachedPoints;
351 int m_companionIdA;
352 int m_companionIdB;
353 int m_index1a;
354
355 int m_objectType;
356 float m_contactBreakingThreshold;
357 float m_contactProcessingThreshold;
358 int m_padding;
359
360 btCollisionObjectFloatData *m_body0;
361 btCollisionObjectFloatData *m_body1;
362 };
363
364 // clang-format on
365
366 #ifdef BT_USE_DOUBLE_PRECISION
367 #define btPersistentManifoldData btPersistentManifoldDoubleData
368 #define btPersistentManifoldDataName "btPersistentManifoldDoubleData"
369 #else
370 #define btPersistentManifoldData btPersistentManifoldFloatData
371 #define btPersistentManifoldDataName "btPersistentManifoldFloatData"
372 #endif //BT_USE_DOUBLE_PRECISION
373
374 #endif //BT_PERSISTENT_MANIFOLD_H
375