Home | History | Annotate | Download | only in NarrowPhaseCollision
      1 /*
      2 Bullet Continuous Collision Detection and Physics Library
      3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
      4 
      5 This software is provided 'as-is', without any express or implied warranty.
      6 In no event will the authors be held liable for any damages arising from the use of this software.
      7 Permission is granted to anyone to use this software for any purpose,
      8 including commercial applications, and to alter it and redistribute it freely,
      9 subject to the following restrictions:
     10 
     11 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.
     12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
     13 3. This notice may not be removed or altered from any source distribution.
     14 */
     15 
     16 
     17 #include "btPersistentManifold.h"
     18 #include "LinearMath/btTransform.h"
     19 
     20 
     21 btScalar					gContactBreakingThreshold = btScalar(0.02);
     22 ContactDestroyedCallback	gContactDestroyedCallback = 0;
     23 ContactProcessedCallback	gContactProcessedCallback = 0;
     24 ContactStartedCallback		gContactStartedCallback = 0;
     25 ContactEndedCallback		gContactEndedCallback = 0;
     26 ///gContactCalcArea3Points will approximate the convex hull area using 3 points
     27 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
     28 bool						gContactCalcArea3Points = true;
     29 
     30 
     31 btPersistentManifold::btPersistentManifold()
     32 :btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
     33 m_body0(0),
     34 m_body1(0),
     35 m_cachedPoints (0),
     36 m_index1a(0)
     37 {
     38 }
     39 
     40 
     41 
     42 
     43 #ifdef DEBUG_PERSISTENCY
     44 #include <stdio.h>
     45 void	btPersistentManifold::DebugPersistency()
     46 {
     47 	int i;
     48 	printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
     49 	for (i=0;i<m_cachedPoints;i++)
     50 	{
     51 		printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
     52 	}
     53 }
     54 #endif //DEBUG_PERSISTENCY
     55 
     56 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
     57 {
     58 
     59 	void* oldPtr = pt.m_userPersistentData;
     60 	if (oldPtr)
     61 	{
     62 #ifdef DEBUG_PERSISTENCY
     63 		int i;
     64 		int occurance = 0;
     65 		for (i=0;i<m_cachedPoints;i++)
     66 		{
     67 			if (m_pointCache[i].m_userPersistentData == oldPtr)
     68 			{
     69 				occurance++;
     70 				if (occurance>1)
     71 					printf("error in clearUserCache\n");
     72 			}
     73 		}
     74 		btAssert(occurance<=0);
     75 #endif //DEBUG_PERSISTENCY
     76 
     77 		if (pt.m_userPersistentData && gContactDestroyedCallback)
     78 		{
     79 			(*gContactDestroyedCallback)(pt.m_userPersistentData);
     80 			pt.m_userPersistentData = 0;
     81 		}
     82 
     83 #ifdef DEBUG_PERSISTENCY
     84 		DebugPersistency();
     85 #endif
     86 	}
     87 
     88 
     89 }
     90 
     91 static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
     92 {
     93 	// It calculates possible 3 area constructed from random 4 points and returns the biggest one.
     94 
     95 	btVector3 a[3],b[3];
     96 	a[0] = p0 - p1;
     97 	a[1] = p0 - p2;
     98 	a[2] = p0 - p3;
     99 	b[0] = p2 - p3;
    100 	b[1] = p1 - p3;
    101 	b[2] = p1 - p2;
    102 
    103 	//todo: Following 3 cross production can be easily optimized by SIMD.
    104 	btVector3 tmp0 = a[0].cross(b[0]);
    105 	btVector3 tmp1 = a[1].cross(b[1]);
    106 	btVector3 tmp2 = a[2].cross(b[2]);
    107 
    108 	return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
    109 }
    110 
    111 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
    112 {
    113 		//calculate 4 possible cases areas, and take biggest area
    114 		//also need to keep 'deepest'
    115 
    116 		int maxPenetrationIndex = -1;
    117 #define KEEP_DEEPEST_POINT 1
    118 #ifdef KEEP_DEEPEST_POINT
    119 		btScalar maxPenetration = pt.getDistance();
    120 		for (int i=0;i<4;i++)
    121 		{
    122 			if (m_pointCache[i].getDistance() < maxPenetration)
    123 			{
    124 				maxPenetrationIndex = i;
    125 				maxPenetration = m_pointCache[i].getDistance();
    126 			}
    127 		}
    128 #endif //KEEP_DEEPEST_POINT
    129 
    130 		btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
    131 
    132 	if (gContactCalcArea3Points)
    133 	{
    134 		if (maxPenetrationIndex != 0)
    135 		{
    136 			btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
    137 			btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
    138 			btVector3 cross = a0.cross(b0);
    139 			res0 = cross.length2();
    140 		}
    141 		if (maxPenetrationIndex != 1)
    142 		{
    143 			btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
    144 			btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
    145 			btVector3 cross = a1.cross(b1);
    146 			res1 = cross.length2();
    147 		}
    148 
    149 		if (maxPenetrationIndex != 2)
    150 		{
    151 			btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
    152 			btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
    153 			btVector3 cross = a2.cross(b2);
    154 			res2 = cross.length2();
    155 		}
    156 
    157 		if (maxPenetrationIndex != 3)
    158 		{
    159 			btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
    160 			btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
    161 			btVector3 cross = a3.cross(b3);
    162 			res3 = cross.length2();
    163 		}
    164 	}
    165 	else
    166 	{
    167 		if(maxPenetrationIndex != 0) {
    168 			res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
    169 		}
    170 
    171 		if(maxPenetrationIndex != 1) {
    172 			res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
    173 		}
    174 
    175 		if(maxPenetrationIndex != 2) {
    176 			res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
    177 		}
    178 
    179 		if(maxPenetrationIndex != 3) {
    180 			res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
    181 		}
    182 	}
    183 	btVector4 maxvec(res0,res1,res2,res3);
    184 	int biggestarea = maxvec.closestAxis4();
    185 	return biggestarea;
    186 
    187 }
    188 
    189 
    190 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
    191 {
    192 	btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
    193 	int size = getNumContacts();
    194 	int nearestPoint = -1;
    195 	for( int i = 0; i < size; i++ )
    196 	{
    197 		const btManifoldPoint &mp = m_pointCache[i];
    198 
    199 		btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
    200 		const btScalar distToManiPoint = diffA.dot(diffA);
    201 		if( distToManiPoint < shortestDist )
    202 		{
    203 			shortestDist = distToManiPoint;
    204 			nearestPoint = i;
    205 		}
    206 	}
    207 	return nearestPoint;
    208 }
    209 
    210 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
    211 {
    212 	if (!isPredictive)
    213 	{
    214 		btAssert(validContactDistance(newPoint));
    215 	}
    216 
    217 	int insertIndex = getNumContacts();
    218 	if (insertIndex == MANIFOLD_CACHE_SIZE)
    219 	{
    220 #if MANIFOLD_CACHE_SIZE >= 4
    221 		//sort cache so best points come first, based on area
    222 		insertIndex = sortCachedPoints(newPoint);
    223 #else
    224 		insertIndex = 0;
    225 #endif
    226 		clearUserCache(m_pointCache[insertIndex]);
    227 
    228 	} else
    229 	{
    230 		m_cachedPoints++;
    231 
    232 
    233 	}
    234 	if (insertIndex<0)
    235 		insertIndex=0;
    236 
    237 	btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
    238 	m_pointCache[insertIndex] = newPoint;
    239 	return insertIndex;
    240 }
    241 
    242 btScalar	btPersistentManifold::getContactBreakingThreshold() const
    243 {
    244 	return m_contactBreakingThreshold;
    245 }
    246 
    247 
    248 
    249 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
    250 {
    251 	int i;
    252 #ifdef DEBUG_PERSISTENCY
    253 	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
    254 		trA.getOrigin().getX(),
    255 		trA.getOrigin().getY(),
    256 		trA.getOrigin().getZ(),
    257 		trB.getOrigin().getX(),
    258 		trB.getOrigin().getY(),
    259 		trB.getOrigin().getZ());
    260 #endif //DEBUG_PERSISTENCY
    261 	/// first refresh worldspace positions and distance
    262 	for (i=getNumContacts()-1;i>=0;i--)
    263 	{
    264 		btManifoldPoint &manifoldPoint = m_pointCache[i];
    265 		manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
    266 		manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
    267 		manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
    268 		manifoldPoint.m_lifeTime++;
    269 	}
    270 
    271 	/// then
    272 	btScalar distance2d;
    273 	btVector3 projectedDifference,projectedPoint;
    274 	for (i=getNumContacts()-1;i>=0;i--)
    275 	{
    276 
    277 		btManifoldPoint &manifoldPoint = m_pointCache[i];
    278 		//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
    279 		if (!validContactDistance(manifoldPoint))
    280 		{
    281 			removeContactPoint(i);
    282 		} else
    283 		{
    284 			//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
    285 			projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
    286 			projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
    287 			distance2d = projectedDifference.dot(projectedDifference);
    288 			if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
    289 			{
    290 				removeContactPoint(i);
    291 			} else
    292 			{
    293 				//contact point processed callback
    294 				if (gContactProcessedCallback)
    295 					(*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
    296 			}
    297 		}
    298 	}
    299 #ifdef DEBUG_PERSISTENCY
    300 	DebugPersistency();
    301 #endif //
    302 }
    303 
    304 
    305 
    306 
    307 
    308