00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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
00112
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
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
00277 if (!validContactDistance(manifoldPoint))
00278 {
00279 removeContactPoint(i);
00280 } else
00281 {
00282
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
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