| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| |
| #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" |
| #include "LinearMath/btTransform.h" |
| |
| |
| btScalar gContactBreakingThreshold = btScalar(0.02); |
| ContactDestroyedCallback gContactDestroyedCallback = 0; |
| ContactProcessedCallback gContactProcessedCallback = 0; |
| |
| |
| |
| btPersistentManifold::btPersistentManifold() |
| :btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), |
| m_body0(0), |
| m_body1(0), |
| m_cachedPoints (0), |
| m_index1a(0) |
| { |
| } |
| |
| |
| |
| |
| #ifdef DEBUG_PERSISTENCY |
| #include <stdio.h> |
| void btPersistentManifold::DebugPersistency() |
| { |
| int i; |
| printf("DebugPersistency : numPoints %d\n",m_cachedPoints); |
| for (i=0;i<m_cachedPoints;i++) |
| { |
| printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData); |
| } |
| } |
| #endif //DEBUG_PERSISTENCY |
| |
| void btPersistentManifold::clearUserCache(btManifoldPoint& pt) |
| { |
| |
| void* oldPtr = pt.m_userPersistentData; |
| if (oldPtr) |
| { |
| #ifdef DEBUG_PERSISTENCY |
| int i; |
| int occurance = 0; |
| for (i=0;i<m_cachedPoints;i++) |
| { |
| if (m_pointCache[i].m_userPersistentData == oldPtr) |
| { |
| occurance++; |
| if (occurance>1) |
| printf("error in clearUserCache\n"); |
| } |
| } |
| btAssert(occurance<=0); |
| #endif //DEBUG_PERSISTENCY |
| |
| if (pt.m_userPersistentData && gContactDestroyedCallback) |
| { |
| (*gContactDestroyedCallback)(pt.m_userPersistentData); |
| pt.m_userPersistentData = 0; |
| } |
| |
| #ifdef DEBUG_PERSISTENCY |
| DebugPersistency(); |
| #endif |
| } |
| |
| |
| } |
| |
| |
| int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) |
| { |
| |
| //calculate 4 possible cases areas, and take biggest area |
| //also need to keep 'deepest' |
| |
| int maxPenetrationIndex = -1; |
| #define KEEP_DEEPEST_POINT 1 |
| #ifdef KEEP_DEEPEST_POINT |
| btScalar maxPenetration = pt.getDistance(); |
| for (int i=0;i<4;i++) |
| { |
| if (m_pointCache[i].getDistance() < maxPenetration) |
| { |
| maxPenetrationIndex = i; |
| maxPenetration = m_pointCache[i].getDistance(); |
| } |
| } |
| #endif //KEEP_DEEPEST_POINT |
| |
| btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); |
| if (maxPenetrationIndex != 0) |
| { |
| btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; |
| btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; |
| btVector3 cross = a0.cross(b0); |
| res0 = cross.length2(); |
| } |
| if (maxPenetrationIndex != 1) |
| { |
| btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; |
| btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; |
| btVector3 cross = a1.cross(b1); |
| res1 = cross.length2(); |
| } |
| |
| if (maxPenetrationIndex != 2) |
| { |
| btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; |
| btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; |
| btVector3 cross = a2.cross(b2); |
| res2 = cross.length2(); |
| } |
| |
| if (maxPenetrationIndex != 3) |
| { |
| btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; |
| btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; |
| btVector3 cross = a3.cross(b3); |
| res3 = cross.length2(); |
| } |
| |
| btVector4 maxvec(res0,res1,res2,res3); |
| int biggestarea = maxvec.closestAxis4(); |
| return biggestarea; |
| } |
| |
| |
| int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const |
| { |
| btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); |
| int size = getNumContacts(); |
| int nearestPoint = -1; |
| for( int i = 0; i < size; i++ ) |
| { |
| const btManifoldPoint &mp = m_pointCache[i]; |
| |
| btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; |
| const btScalar distToManiPoint = diffA.dot(diffA); |
| if( distToManiPoint < shortestDist ) |
| { |
| shortestDist = distToManiPoint; |
| nearestPoint = i; |
| } |
| } |
| return nearestPoint; |
| } |
| |
| int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) |
| { |
| btAssert(validContactDistance(newPoint)); |
| |
| int insertIndex = getNumContacts(); |
| if (insertIndex == MANIFOLD_CACHE_SIZE) |
| { |
| #if MANIFOLD_CACHE_SIZE >= 4 |
| //sort cache so best points come first, based on area |
| insertIndex = sortCachedPoints(newPoint); |
| #else |
| insertIndex = 0; |
| #endif |
| clearUserCache(m_pointCache[insertIndex]); |
| |
| } else |
| { |
| m_cachedPoints++; |
| |
| |
| } |
| if (insertIndex<0) |
| insertIndex=0; |
| |
| btAssert(m_pointCache[insertIndex].m_userPersistentData==0); |
| m_pointCache[insertIndex] = newPoint; |
| return insertIndex; |
| } |
| |
| btScalar btPersistentManifold::getContactBreakingThreshold() const |
| { |
| return m_contactBreakingThreshold; |
| } |
| |
| |
| |
| void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) |
| { |
| int i; |
| #ifdef DEBUG_PERSISTENCY |
| printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n", |
| trA.getOrigin().getX(), |
| trA.getOrigin().getY(), |
| trA.getOrigin().getZ(), |
| trB.getOrigin().getX(), |
| trB.getOrigin().getY(), |
| trB.getOrigin().getZ()); |
| #endif //DEBUG_PERSISTENCY |
| /// first refresh worldspace positions and distance |
| for (i=getNumContacts()-1;i>=0;i--) |
| { |
| btManifoldPoint &manifoldPoint = m_pointCache[i]; |
| manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA ); |
| manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB ); |
| manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB); |
| manifoldPoint.m_lifeTime++; |
| } |
| |
| /// then |
| btScalar distance2d; |
| btVector3 projectedDifference,projectedPoint; |
| for (i=getNumContacts()-1;i>=0;i--) |
| { |
| |
| btManifoldPoint &manifoldPoint = m_pointCache[i]; |
| //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) |
| if (!validContactDistance(manifoldPoint)) |
| { |
| removeContactPoint(i); |
| } else |
| { |
| //contact also becomes invalid when relative movement orthogonal to normal exceeds margin |
| projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; |
| projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; |
| distance2d = projectedDifference.dot(projectedDifference); |
| if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() ) |
| { |
| removeContactPoint(i); |
| } else |
| { |
| //contact point processed callback |
| if (gContactProcessedCallback) |
| (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1); |
| } |
| } |
| } |
| #ifdef DEBUG_PERSISTENCY |
| DebugPersistency(); |
| #endif // |
| } |
| |
| |
| |
| |
| |