Back to index

supertuxkart  0.5+dfsg1
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 #include <assert.h>
00020 
00021 btScalar                                  gContactBreakingThreshold = btScalar(0.02);
00022 ContactDestroyedCallback    gContactDestroyedCallback = 0;
00023 ContactProcessedCallback    gContactProcessedCallback = 0;
00024 
00025 
00026 
00027 btPersistentManifold::btPersistentManifold()
00028 :m_body0(0),
00029 m_body1(0),
00030 m_cachedPoints (0),
00031 m_index1a(0)
00032 {
00033 }
00034 
00035 
00036 
00037 
00038 #ifdef DEBUG_PERSISTENCY
00039 #include <stdio.h>
00040 void   btPersistentManifold::DebugPersistency()
00041 {
00042        int i;
00043        printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
00044        for (i=0;i<m_cachedPoints;i++)
00045        {
00046               printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
00047        }
00048 }
00049 #endif //DEBUG_PERSISTENCY
00050 
00051 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
00052 {
00053 
00054        void* oldPtr = pt.m_userPersistentData;
00055        if (oldPtr)
00056        {
00057 #ifdef DEBUG_PERSISTENCY
00058               int i;
00059               int occurance = 0;
00060               for (i=0;i<m_cachedPoints;i++)
00061               {
00062                      if (m_pointCache[i].m_userPersistentData == oldPtr)
00063                      {
00064                             occurance++;
00065                             if (occurance>1)
00066                                    printf("error in clearUserCache\n");
00067                      }
00068               }
00069               assert(occurance<=0);
00070 #endif //DEBUG_PERSISTENCY
00071 
00072               if (pt.m_userPersistentData && gContactDestroyedCallback)
00073               {
00074                      (*gContactDestroyedCallback)(pt.m_userPersistentData);
00075                      pt.m_userPersistentData = 0;
00076               }
00077               
00078 #ifdef DEBUG_PERSISTENCY
00079               DebugPersistency();
00080 #endif
00081        }
00082 
00083        
00084 }
00085 
00086 
00087 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
00088 {
00089 
00090               //calculate 4 possible cases areas, and take biggest area
00091               //also need to keep 'deepest'
00092               
00093               int maxPenetrationIndex = -1;
00094 #define KEEP_DEEPEST_POINT 1
00095 #ifdef KEEP_DEEPEST_POINT
00096               btScalar maxPenetration = pt.getDistance();
00097               for (int i=0;i<4;i++)
00098               {
00099                      if (m_pointCache[i].getDistance() < maxPenetration)
00100                      {
00101                             maxPenetrationIndex = i;
00102                             maxPenetration = m_pointCache[i].getDistance();
00103                      }
00104               }
00105 #endif //KEEP_DEEPEST_POINT
00106               
00107               btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
00108               if (maxPenetrationIndex != 0)
00109               {
00110                      btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
00111                      btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
00112                      btVector3 cross = a0.cross(b0);
00113                      res0 = cross.length2();
00114               }
00115               if (maxPenetrationIndex != 1)
00116               {
00117                      btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
00118                      btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
00119                      btVector3 cross = a1.cross(b1);
00120                      res1 = cross.length2();
00121               }
00122 
00123               if (maxPenetrationIndex != 2)
00124               {
00125                      btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
00126                      btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
00127                      btVector3 cross = a2.cross(b2);
00128                      res2 = cross.length2();
00129               }
00130 
00131               if (maxPenetrationIndex != 3)
00132               {
00133                      btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
00134                      btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
00135                      btVector3 cross = a3.cross(b3);
00136                      res3 = cross.length2();
00137               }
00138 
00139               btVector4 maxvec(res0,res1,res2,res3);
00140               int biggestarea = maxvec.closestAxis4();
00141               return biggestarea;
00142 }
00143 
00144 
00145 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
00146 {
00147        btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
00148        int size = getNumContacts();
00149        int nearestPoint = -1;
00150        for( int i = 0; i < size; i++ )
00151        {
00152               const btManifoldPoint &mp = m_pointCache[i];
00153 
00154               btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
00155               const btScalar distToManiPoint = diffA.dot(diffA);
00156               if( distToManiPoint < shortestDist )
00157               {
00158                      shortestDist = distToManiPoint;
00159                      nearestPoint = i;
00160               }
00161        }
00162        return nearestPoint;
00163 }
00164 
00165 void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
00166 {
00167        assert(validContactDistance(newPoint));
00168 
00169        int insertIndex = getNumContacts();
00170        if (insertIndex == MANIFOLD_CACHE_SIZE)
00171        {
00172 #if MANIFOLD_CACHE_SIZE >= 4
00173               //sort cache so best points come first, based on area
00174               insertIndex = sortCachedPoints(newPoint);
00175 #else
00176               insertIndex = 0;
00177 #endif
00178 
00179               
00180        } else
00181        {
00182               m_cachedPoints++;
00183 
00184               
00185        }
00186        replaceContactPoint(newPoint,insertIndex);
00187 }
00188 
00189 btScalar      btPersistentManifold::getContactBreakingThreshold() const
00190 {
00191        return gContactBreakingThreshold;
00192 }
00193 
00194 
00195 
00196 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
00197 {
00198        int i;
00199 #ifdef DEBUG_PERSISTENCY
00200        printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
00201               trA.getOrigin().getX(),
00202               trA.getOrigin().getY(),
00203               trA.getOrigin().getZ(),
00204               trB.getOrigin().getX(),
00205               trB.getOrigin().getY(),
00206               trB.getOrigin().getZ());
00207 #endif //DEBUG_PERSISTENCY
00208 
00209        for (i=getNumContacts()-1;i>=0;i--)
00210        {
00211               btManifoldPoint &manifoldPoint = m_pointCache[i];
00212               manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
00213               manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
00214               manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
00215               manifoldPoint.m_lifeTime++;
00216        }
00217 
00219        btScalar distance2d;
00220        btVector3 projectedDifference,projectedPoint;
00221        for (i=getNumContacts()-1;i>=0;i--)
00222        {
00223               
00224               btManifoldPoint &manifoldPoint = m_pointCache[i];
00225               //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
00226               if (!validContactDistance(manifoldPoint))
00227               {
00228                      removeContactPoint(i);
00229               } else
00230               {
00231                      //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
00232                      projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
00233                      projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
00234                      distance2d = projectedDifference.dot(projectedDifference);
00235                      if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
00236                      {
00237                             removeContactPoint(i);
00238                      } else
00239                      {
00240                             //contact point processed callback
00241                             if (gContactProcessedCallback)
00242                                    (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
00243                      }
00244               }
00245        }
00246 #ifdef DEBUG_PERSISTENCY
00247        DebugPersistency();
00248 #endif //
00249 }
00250 
00251 
00252 
00253 
00254