dali-physics/third-party/bullet3/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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
  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