src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

   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;
  24 ///gContactCalcArea3Points will approximate the convex hull area using 3 points
  25 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
  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         /// first refresh worldspace positions and distance
 260         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
 269         /// then
 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