btPersistentManifold.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btPersistentManifold.h"
00018 #include "LinearMath/btTransform.h"
00019 
00020 
00021 btScalar                                        gContactBreakingThreshold = btScalar(0.02);
00022 ContactDestroyedCallback        gContactDestroyedCallback = 0;
00023 ContactProcessedCallback        gContactProcessedCallback = 0;
00026 bool                                            gContactCalcArea3Points = true;
00027 
00028 
00029 btPersistentManifold::btPersistentManifold()
00030 :btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
00031 m_body0(0),
00032 m_body1(0),
00033 m_cachedPoints (0),
00034 m_index1a(0)
00035 {
00036 }
00037 
00038 
00039 
00040 
00041 #ifdef DEBUG_PERSISTENCY
00042 #include <stdio.h>
00043 void    btPersistentManifold::DebugPersistency()
00044 {
00045         int i;
00046         printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
00047         for (i=0;i<m_cachedPoints;i++)
00048         {
00049                 printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
00050         }
00051 }
00052 #endif //DEBUG_PERSISTENCY
00053 
00054 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
00055 {
00056 
00057         void* oldPtr = pt.m_userPersistentData;
00058         if (oldPtr)
00059         {
00060 #ifdef DEBUG_PERSISTENCY
00061                 int i;
00062                 int occurance = 0;
00063                 for (i=0;i<m_cachedPoints;i++)
00064                 {
00065                         if (m_pointCache[i].m_userPersistentData == oldPtr)
00066                         {
00067                                 occurance++;
00068                                 if (occurance>1)
00069                                         printf("error in clearUserCache\n");
00070                         }
00071                 }
00072                 btAssert(occurance<=0);
00073 #endif //DEBUG_PERSISTENCY
00074 
00075                 if (pt.m_userPersistentData && gContactDestroyedCallback)
00076                 {
00077                         (*gContactDestroyedCallback)(pt.m_userPersistentData);
00078                         pt.m_userPersistentData = 0;
00079                 }
00080                 
00081 #ifdef DEBUG_PERSISTENCY
00082                 DebugPersistency();
00083 #endif
00084         }
00085 
00086         
00087 }
00088 
00089 static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
00090 {
00091         // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
00092 
00093         btVector3 a[3],b[3];
00094         a[0] = p0 - p1;
00095         a[1] = p0 - p2;
00096         a[2] = p0 - p3;
00097         b[0] = p2 - p3;
00098         b[1] = p1 - p3;
00099         b[2] = p1 - p2;
00100 
00101         //todo: Following 3 cross production can be easily optimized by SIMD.
00102         btVector3 tmp0 = a[0].cross(b[0]);
00103         btVector3 tmp1 = a[1].cross(b[1]);
00104         btVector3 tmp2 = a[2].cross(b[2]);
00105 
00106         return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
00107 }
00108 
00109 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
00110 {
00111                 //calculate 4 possible cases areas, and take biggest area
00112                 //also need to keep 'deepest'
00113                 
00114                 int maxPenetrationIndex = -1;
00115 #define KEEP_DEEPEST_POINT 1
00116 #ifdef KEEP_DEEPEST_POINT
00117                 btScalar maxPenetration = pt.getDistance();
00118                 for (int i=0;i<4;i++)
00119                 {
00120                         if (m_pointCache[i].getDistance() < maxPenetration)
00121                         {
00122                                 maxPenetrationIndex = i;
00123                                 maxPenetration = m_pointCache[i].getDistance();
00124                         }
00125                 }
00126 #endif //KEEP_DEEPEST_POINT
00127                 
00128                 btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
00129 
00130         if (gContactCalcArea3Points)
00131         {
00132                 if (maxPenetrationIndex != 0)
00133                 {
00134                         btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
00135                         btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
00136                         btVector3 cross = a0.cross(b0);
00137                         res0 = cross.length2();
00138                 }
00139                 if (maxPenetrationIndex != 1)
00140                 {
00141                         btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
00142                         btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
00143                         btVector3 cross = a1.cross(b1);
00144                         res1 = cross.length2();
00145                 }
00146 
00147                 if (maxPenetrationIndex != 2)
00148                 {
00149                         btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
00150                         btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
00151                         btVector3 cross = a2.cross(b2);
00152                         res2 = cross.length2();
00153                 }
00154 
00155                 if (maxPenetrationIndex != 3)
00156                 {
00157                         btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
00158                         btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
00159                         btVector3 cross = a3.cross(b3);
00160                         res3 = cross.length2();
00161                 }
00162         } 
00163         else
00164         {
00165                 if(maxPenetrationIndex != 0) {
00166                         res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
00167                 }
00168 
00169                 if(maxPenetrationIndex != 1) {
00170                         res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
00171                 }
00172 
00173                 if(maxPenetrationIndex != 2) {
00174                         res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
00175                 }
00176 
00177                 if(maxPenetrationIndex != 3) {
00178                         res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
00179                 }
00180         }
00181         btVector4 maxvec(res0,res1,res2,res3);
00182         int biggestarea = maxvec.closestAxis4();
00183         return biggestarea;
00184         
00185 }
00186 
00187 
00188 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
00189 {
00190         btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
00191         int size = getNumContacts();
00192         int nearestPoint = -1;
00193         for( int i = 0; i < size; i++ )
00194         {
00195                 const btManifoldPoint &mp = m_pointCache[i];
00196 
00197                 btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
00198                 const btScalar distToManiPoint = diffA.dot(diffA);
00199                 if( distToManiPoint < shortestDist )
00200                 {
00201                         shortestDist = distToManiPoint;
00202                         nearestPoint = i;
00203                 }
00204         }
00205         return nearestPoint;
00206 }
00207 
00208 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
00209 {
00210         if (!isPredictive)
00211         {
00212                 btAssert(validContactDistance(newPoint));
00213         }
00214         
00215         int insertIndex = getNumContacts();
00216         if (insertIndex == MANIFOLD_CACHE_SIZE)
00217         {
00218 #if MANIFOLD_CACHE_SIZE >= 4
00219                 //sort cache so best points come first, based on area
00220                 insertIndex = sortCachedPoints(newPoint);
00221 #else
00222                 insertIndex = 0;
00223 #endif
00224                 clearUserCache(m_pointCache[insertIndex]);
00225                 
00226         } else
00227         {
00228                 m_cachedPoints++;
00229 
00230                 
00231         }
00232         if (insertIndex<0)
00233                 insertIndex=0;
00234 
00235         btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
00236         m_pointCache[insertIndex] = newPoint;
00237         return insertIndex;
00238 }
00239 
00240 btScalar        btPersistentManifold::getContactBreakingThreshold() const
00241 {
00242         return m_contactBreakingThreshold;
00243 }
00244 
00245 
00246 
00247 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
00248 {
00249         int i;
00250 #ifdef DEBUG_PERSISTENCY
00251         printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
00252                 trA.getOrigin().getX(),
00253                 trA.getOrigin().getY(),
00254                 trA.getOrigin().getZ(),
00255                 trB.getOrigin().getX(),
00256                 trB.getOrigin().getY(),
00257                 trB.getOrigin().getZ());
00258 #endif //DEBUG_PERSISTENCY
00259 
00260         for (i=getNumContacts()-1;i>=0;i--)
00261         {
00262                 btManifoldPoint &manifoldPoint = m_pointCache[i];
00263                 manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
00264                 manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
00265                 manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
00266                 manifoldPoint.m_lifeTime++;
00267         }
00268 
00270         btScalar distance2d;
00271         btVector3 projectedDifference,projectedPoint;
00272         for (i=getNumContacts()-1;i>=0;i--)
00273         {
00274                 
00275                 btManifoldPoint &manifoldPoint = m_pointCache[i];
00276                 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
00277                 if (!validContactDistance(manifoldPoint))
00278                 {
00279                         removeContactPoint(i);
00280                 } else
00281                 {
00282                         //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
00283                         projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
00284                         projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
00285                         distance2d = projectedDifference.dot(projectedDifference);
00286                         if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
00287                         {
00288                                 removeContactPoint(i);
00289                         } else
00290                         {
00291                                 //contact point processed callback
00292                                 if (gContactProcessedCallback)
00293                                         (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
00294                         }
00295                 }
00296         }
00297 #ifdef DEBUG_PERSISTENCY
00298         DebugPersistency();
00299 #endif //
00300 }
00301 
00302 
00303 
00304 
00305