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 ContactStartedCallback gContactStartedCallback = 0; 25 ContactEndedCallback gContactEndedCallback = 0; 26 ///gContactCalcArea3Points will approximate the convex hull area using 3 points 27 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower 28 bool gContactCalcArea3Points = true; 29 30 31 btPersistentManifold::btPersistentManifold() 32 :btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), 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 56 void btPersistentManifold::clearUserCache(btManifoldPoint& pt) 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 77 if (pt.m_userPersistentData && gContactDestroyedCallback) 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 111 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 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 132 if (gContactCalcArea3Points) 133 { 134 if (maxPenetrationIndex != 0) 135 { 136 btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; 137 btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; 138 btVector3 cross = a0.cross(b0); 139 res0 = cross.length2(); 140 } 141 if (maxPenetrationIndex != 1) 142 { 143 btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; 144 btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; 145 btVector3 cross = a1.cross(b1); 146 res1 = cross.length2(); 147 } 148 149 if (maxPenetrationIndex != 2) 150 { 151 btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; 152 btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; 153 btVector3 cross = a2.cross(b2); 154 res2 = cross.length2(); 155 } 156 157 if (maxPenetrationIndex != 3) 158 { 159 btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; 160 btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; 161 btVector3 cross = a3.cross(b3); 162 res3 = cross.length2(); 163 } 164 } 165 else 166 { 167 if(maxPenetrationIndex != 0) { 168 res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); 169 } 170 171 if(maxPenetrationIndex != 1) { 172 res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); 173 } 174 175 if(maxPenetrationIndex != 2) { 176 res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); 177 } 178 179 if(maxPenetrationIndex != 3) { 180 res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); 181 } 182 } 183 btVector4 maxvec(res0,res1,res2,res3); 184 int biggestarea = maxvec.closestAxis4(); 185 return biggestarea; 186 187 } 188 189 190 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const 191 { 192 btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); 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 242 btScalar btPersistentManifold::getContactBreakingThreshold() const 243 { 244 return m_contactBreakingThreshold; 245 } 246 247 248 249 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) 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 /// first refresh worldspace positions and distance 262 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 271 /// then 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 { 281 removeContactPoint(i); 282 } else 283 { 284 //contact also becomes invalid when relative movement orthogonal to normal exceeds margin 285 projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; 286 projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; 287 distance2d = projectedDifference.dot(projectedDifference); 288 if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() ) 289 { 290 removeContactPoint(i); 291 } else 292 { 293 //contact point processed callback 294 if (gContactProcessedCallback) 295 (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1); 296 } 297 } 298 } 299 #ifdef DEBUG_PERSISTENCY 300 DebugPersistency(); 301 #endif // 302 } 303 304 305 306 307 308