diff options
Diffstat (limited to 'extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp')
| -rw-r--r-- | extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp | 182 |
1 files changed, 182 insertions, 0 deletions
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp new file mode 100644 index 00000000000..9085ffa0ef0 --- /dev/null +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp @@ -0,0 +1,182 @@ +/* +* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com +* +* Permission to use, copy, modify, distribute and sell this software +* and its documentation for any purpose is hereby granted without fee, +* provided that the above copyright notice appear in all copies. +* Erwin Coumans makes no representations about the suitability +* of this software for any purpose. +* It is provided "as is" without express or implied warranty. +*/ + + +#include "PersistentManifold.h" +#include "SimdTransform.h" +#include <assert.h> + + +PersistentManifold::PersistentManifold() +:m_body0(0), +m_body1(0), +m_cachedPoints (0), +m_index1(0) +{ +} + + +void PersistentManifold::ClearManifold() +{ + m_cachedPoints = 0; +} + + + + + +int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt) +{ + + //calculate 4 possible cases areas, and take biggest area + + SimdScalar res0,res1,res2,res3; + + { + SimdVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; + SimdVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + SimdVector3 cross = a0.cross(b0); + res0 = cross.length2(); + } + { + SimdVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; + SimdVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + SimdVector3 cross = a1.cross(b1); + res1 = cross.length2(); + } + { + SimdVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; + SimdVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; + SimdVector3 cross = a2.cross(b2); + res2 = cross.length2(); + } + { + SimdVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; + SimdVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; + SimdVector3 cross = a3.cross(b3); + res3 = cross.length2(); + } + + SimdVector4 maxvec(res0,res1,res2,res3); + int biggestarea = maxvec.closestAxis4(); + + return biggestarea; + + +} + + + +int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const +{ + SimdScalar shortestDist = GetManifoldMargin() * GetManifoldMargin(); + int size = GetNumContacts(); + int nearestPoint = -1; + for( int i = 0; i < size; i++ ) + { + const ManifoldPoint &mp = m_pointCache[i]; + + SimdVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + const SimdScalar distToManiPoint = diffA.dot(diffA); + if( distToManiPoint < shortestDist ) + { + shortestDist = distToManiPoint; + nearestPoint = i; + } + } + return nearestPoint; +} + +void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint) +{ + assert(ValidContactDistance(newPoint)); + + int insertIndex = GetNumContacts(); + if (insertIndex == MANIFOLD_CACHE_SIZE) + { + //sort cache so best points come first + insertIndex = SortCachedPoints(newPoint); + } else + { + m_cachedPoints++; + } + ReplaceContactPoint(newPoint,insertIndex); +} + + +void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB) +{ + int i; + + /// first refresh worldspace positions and distance + for (i=GetNumContacts()-1;i>=0;i--) + { + ManifoldPoint &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); + } + + /// then + SimdScalar distance2d; + SimdVector3 projectedDifference,projectedPoint; + for (i=GetNumContacts()-1;i>=0;i--) + { + + ManifoldPoint &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 > GetManifoldMargin()*GetManifoldMargin() ) + { + RemoveContactPoint(i); + } + } + } +} + + +//todo: remove this treshold +float gPenetrationDistanceCheck = -0.05f; + +float PersistentManifold::GetCollisionImpulse() const +{ + float averageImpulse = 0.f; + if (GetNumContacts() > 0) + { + float totalImpulse = 0.f; + + //return the sum of the applied impulses on the box + for (int i=0;i<GetNumContacts();i++) + { + const ManifoldPoint& cp = GetContactPoint(i); + //avoid conflic noice + if ( cp.GetDistance() <gPenetrationDistanceCheck) + return 0.f; + + totalImpulse += cp.m_appliedImpulse; + + } + averageImpulse = totalImpulse / ((float)GetNumContacts()); + + } + return averageImpulse; + +} + + |