lolengine
/
lol
spegling av https://github.com/lolengine/lol
1
0
Förgrening 0
Kod Ärenden 0 Släpp 0 Wiki Aktiviteter
Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.
1246 Incheckningar
5 Grenar
77 MiB
 
 
 
Träd: 2a5d9ed4d8
Grenar Taggar
${ item.name }
Skapa branchen ${ searchTerm }
från '2a5d9ed4d8'
${ noResults }
lol/src/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

843 rader
25 KiB
Rå Blame Historik 

  1. #include "btInternalEdgeUtility.h"

  2. 

  3. #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"

  4. #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"

  5. #include "BulletCollision/CollisionShapes/btTriangleShape.h"

  6. #include "BulletCollision/CollisionDispatch/btCollisionObject.h"

  7. #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"

  8. #include "LinearMath/btIDebugDraw.h"

  9. 

  10. 

  11. //#define DEBUG_INTERNAL_EDGE

  12. 

  13. #ifdef DEBUG_INTERNAL_EDGE

  14. #include <stdio.h>

  15. #endif //DEBUG_INTERNAL_EDGE

  16. 

  17. 

  18. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  19. static btIDebugDraw* gDebugDrawer = 0;

  20. 

  21. void	btSetDebugDrawer(btIDebugDraw* debugDrawer)

  22. {

  23. 	gDebugDrawer = debugDrawer;

  24. }

  25. 

  26. static void    btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color)

  27. {

  28. 	if (gDebugDrawer)

  29. 		gDebugDrawer->drawLine(from,to,color);

  30. }

  31. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  32. 

  33. 

  34. static int	btGetHash(int partId, int triangleIndex)

  35. {

  36. 	int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;

  37. 	return hash;

  38. }

  39. 

  40. 

  41. 

  42. static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)

  43. {

  44. 	const btVector3 refAxis0  = edgeA;

  45. 	const btVector3 refAxis1  = normalA;

  46. 	const btVector3 swingAxis = normalB;

  47. 	btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));

  48. 	return  angle;

  49. }

  50. 

  51. 

  52. struct btConnectivityProcessor : public btTriangleCallback

  53. {

  54. 	int				m_partIdA;

  55. 	int				m_triangleIndexA;

  56. 	btVector3*		m_triangleVerticesA;

  57. 	btTriangleInfoMap*	m_triangleInfoMap;

  58. 

  59. 

  60. 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)

  61. 	{

  62. 		//skip self-collisions

  63. 		if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))

  64. 			return;

  65. 

  66. 		//skip duplicates (disabled for now)

  67. 		//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))

  68. 		//	return;

  69. 

  70. 		//search for shared vertices and edges

  71. 		int numshared = 0;

  72. 		int sharedVertsA[3]={-1,-1,-1};

  73. 		int sharedVertsB[3]={-1,-1,-1};

  74. 

  75. 		///skip degenerate triangles

  76. 		btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();

  77. 		if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)

  78. 			return;

  79. 

  80. 

  81. 		btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();

  82. 		///skip degenerate triangles

  83. 		if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)

  84. 			return;

  85. 

  86. #if 0

  87. 		printf("triangle A[0]	=	(%f,%f,%f)\ntriangle A[1]	=	(%f,%f,%f)\ntriangle A[2]	=	(%f,%f,%f)\n",

  88. 			m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),

  89. 			m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),

  90. 			m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());

  91. 

  92. 		printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);

  93. 		printf("triangle B[0]	=	(%f,%f,%f)\ntriangle B[1]	=	(%f,%f,%f)\ntriangle B[2]	=	(%f,%f,%f)\n",

  94. 			triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),

  95. 			triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),

  96. 			triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());

  97. #endif

  98. 

  99. 		for (int i=0;i<3;i++)

  100. 		{

  101. 			for (int j=0;j<3;j++)

  102. 			{

  103. 				if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)

  104. 				{

  105. 					sharedVertsA[numshared] = i;

  106. 					sharedVertsB[numshared] = j;

  107. 					numshared++;

  108. 					///degenerate case

  109. 					if(numshared >= 3)

  110. 						return;

  111. 				}

  112. 			}

  113. 			///degenerate case

  114. 			if(numshared >= 3)

  115. 				return;

  116. 		}

  117. 		switch (numshared)

  118. 		{

  119. 		case 0:

  120. 			{

  121. 				break;

  122. 			}

  123. 		case 1:

  124. 			{

  125. 				//shared vertex

  126. 				break;

  127. 			}

  128. 		case 2:

  129. 			{

  130. 				//shared edge

  131. 				//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct

  132. 				if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)

  133. 				{

  134. 					sharedVertsA[0] = 2;

  135. 					sharedVertsA[1] = 0;

  136. 					int tmp = sharedVertsB[1];

  137. 					sharedVertsB[1] = sharedVertsB[0];

  138. 					sharedVertsB[0] = tmp;

  139. 				}

  140. 

  141. 				int hash = btGetHash(m_partIdA,m_triangleIndexA);

  142. 

  143. 				btTriangleInfo* info = m_triangleInfoMap->find(hash);

  144. 				if (!info)

  145. 				{

  146. 					btTriangleInfo tmp;

  147. 					m_triangleInfoMap->insert(hash,tmp);

  148. 					info = m_triangleInfoMap->find(hash);

  149. 				}

  150. 

  151. 				int sumvertsA = sharedVertsA[0]+sharedVertsA[1];

  152. 				int otherIndexA = 3-sumvertsA;

  153. 

  154. 				

  155. 				btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);

  156. 

  157. 				btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);

  158. 				int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);

  159. 

  160. 				btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);

  161. 				//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);

  162. 

  163. 				btVector3 normalA;

  164. 				btVector3 normalB;

  165. 				tA.calcNormal(normalA);

  166. 				tB.calcNormal(normalB);

  167. 				edge.normalize();

  168. 				btVector3 edgeCrossA = edge.cross(normalA).normalize();

  169. 

  170. 				{

  171. 					btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];

  172. 					if (edgeCrossA.dot(tmp) < 0)

  173. 					{

  174. 						edgeCrossA*=-1;

  175. 					}

  176. 				}

  177. 

  178. 				btVector3 edgeCrossB = edge.cross(normalB).normalize();

  179. 

  180. 				{

  181. 					btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];

  182. 					if (edgeCrossB.dot(tmp) < 0)

  183. 					{

  184. 						edgeCrossB*=-1;

  185. 					}

  186. 				}

  187. 

  188. 				btScalar	angle2 = 0;

  189. 				btScalar	ang4 = 0.f;

  190. 

  191. 

  192. 				btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);

  193. 				btScalar len2 = calculatedEdge.length2();

  194. 

  195. 				btScalar correctedAngle(0);

  196. 				btVector3 calculatedNormalB = normalA;

  197. 				bool isConvex = false;

  198. 

  199. 				if (len2<m_triangleInfoMap->m_planarEpsilon)

  200. 				{

  201. 					angle2 = 0.f;

  202. 					ang4 = 0.f;

  203. 				} else

  204. 				{

  205. 

  206. 					calculatedEdge.normalize();

  207. 					btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);

  208. 					calculatedNormalA.normalize();

  209. 					angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);

  210. 					ang4 = SIMD_PI-angle2;

  211. 					btScalar dotA = normalA.dot(edgeCrossB);

  212. 					///@todo: check if we need some epsilon, due to floating point imprecision

  213. 					isConvex = (dotA<0.);

  214. 

  215. 					correctedAngle = isConvex ? ang4 : -ang4;

  216. 					btQuaternion orn2(calculatedEdge,-correctedAngle);

  217. 					calculatedNormalB = btMatrix3x3(orn2)*normalA;

  218. 

  219. 

  220. 				}

  221. 

  222. 				

  223. 

  224. 				

  225. 							

  226. 				//alternatively use 

  227. 				//btVector3 calculatedNormalB2 = quatRotate(orn,normalA);

  228. 

  229. 

  230. 				switch (sumvertsA)

  231. 				{

  232. 				case 1:

  233. 					{

  234. 						btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];

  235. 						btQuaternion orn(edge,-correctedAngle);

  236. 						btVector3 computedNormalB = quatRotate(orn,normalA);

  237. 						btScalar bla = computedNormalB.dot(normalB);

  238. 						if (bla<0)

  239. 						{

  240. 							computedNormalB*=-1;

  241. 							info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;

  242. 						}

  243. #ifdef DEBUG_INTERNAL_EDGE

  244. 						if ((computedNormalB-normalB).length()>0.0001)

  245. 						{

  246. 							printf("warning: normals not identical\n");

  247. 						}

  248. #endif//DEBUG_INTERNAL_EDGE

  249. 

  250. 						info->m_edgeV0V1Angle = -correctedAngle;

  251. 

  252. 						if (isConvex)

  253. 							info->m_flags |= TRI_INFO_V0V1_CONVEX;

  254. 						break;

  255. 					}

  256. 				case 2:

  257. 					{

  258. 						btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];

  259. 						btQuaternion orn(edge,-correctedAngle);

  260. 						btVector3 computedNormalB = quatRotate(orn,normalA);

  261. 						if (computedNormalB.dot(normalB)<0)

  262. 						{

  263. 							computedNormalB*=-1;

  264. 							info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;

  265. 						}

  266. 

  267. #ifdef DEBUG_INTERNAL_EDGE

  268. 						if ((computedNormalB-normalB).length()>0.0001)

  269. 						{

  270. 							printf("warning: normals not identical\n");

  271. 						}

  272. #endif //DEBUG_INTERNAL_EDGE

  273. 						info->m_edgeV2V0Angle = -correctedAngle;

  274. 						if (isConvex)

  275. 							info->m_flags |= TRI_INFO_V2V0_CONVEX;

  276. 						break;	

  277. 					}

  278. 				case 3:

  279. 					{

  280. 						btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];

  281. 						btQuaternion orn(edge,-correctedAngle);

  282. 						btVector3 computedNormalB = quatRotate(orn,normalA);

  283. 						if (computedNormalB.dot(normalB)<0)

  284. 						{

  285. 							info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;

  286. 							computedNormalB*=-1;

  287. 						}

  288. #ifdef DEBUG_INTERNAL_EDGE

  289. 						if ((computedNormalB-normalB).length()>0.0001)

  290. 						{

  291. 							printf("warning: normals not identical\n");

  292. 						}

  293. #endif //DEBUG_INTERNAL_EDGE

  294. 						info->m_edgeV1V2Angle = -correctedAngle;

  295. 

  296. 						if (isConvex)

  297. 							info->m_flags |= TRI_INFO_V1V2_CONVEX;

  298. 						break;

  299. 					}

  300. 				}

  301. 

  302. 				break;

  303. 			}

  304. 		default:

  305. 			{

  306. 				//				printf("warning: duplicate triangle\n");

  307. 			}

  308. 

  309. 		}

  310. 	}

  311. };

  312. /////////////////////////////////////////////////////////

  313. /////////////////////////////////////////////////////////

  314. 

  315. void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)

  316. {

  317. 	//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!

  318. 	if (trimeshShape->getTriangleInfoMap())

  319. 		return;

  320. 

  321. 	trimeshShape->setTriangleInfoMap(triangleInfoMap);

  322. 

  323. 	btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();

  324. 	const btVector3& meshScaling = meshInterface->getScaling();

  325. 

  326. 	for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)

  327. 	{

  328. 		const unsigned char *vertexbase = 0;

  329. 		int numverts = 0;

  330. 		PHY_ScalarType type = PHY_INTEGER;

  331. 		int stride = 0;

  332. 		const unsigned char *indexbase = 0;

  333. 		int indexstride = 0;

  334. 		int numfaces = 0;

  335. 		PHY_ScalarType indicestype = PHY_INTEGER;

  336. 		//PHY_ScalarType indexType=0;

  337. 

  338. 		btVector3 triangleVerts[3];

  339. 		meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,partId);

  340. 		btVector3 aabbMin,aabbMax;

  341. 

  342. 		for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)

  343. 		{

  344. 			unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);

  345. 

  346. 			for (int j=2;j>=0;j--)

  347. 			{

  348. 

  349. 				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];

  350. 				if (type == PHY_FLOAT)

  351. 				{

  352. 					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);

  353. 					triangleVerts[j] = btVector3(

  354. 						graphicsbase[0]*meshScaling.getX(),

  355. 						graphicsbase[1]*meshScaling.getY(),

  356. 						graphicsbase[2]*meshScaling.getZ());

  357. 				}

  358. 				else

  359. 				{

  360. 					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);

  361. 					triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));

  362. 				}

  363. 			}

  364. 			aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));

  365. 			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 

  366. 			aabbMin.setMin(triangleVerts[0]);

  367. 			aabbMax.setMax(triangleVerts[0]);

  368. 			aabbMin.setMin(triangleVerts[1]);

  369. 			aabbMax.setMax(triangleVerts[1]);

  370. 			aabbMin.setMin(triangleVerts[2]);

  371. 			aabbMax.setMax(triangleVerts[2]);

  372. 

  373. 			btConnectivityProcessor connectivityProcessor;

  374. 			connectivityProcessor.m_partIdA = partId;

  375. 			connectivityProcessor.m_triangleIndexA = triangleIndex;

  376. 			connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];

  377. 			connectivityProcessor.m_triangleInfoMap  = triangleInfoMap;

  378. 

  379. 			trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);

  380. 		}

  381. 

  382. 	}

  383. 

  384. }

  385. 

  386. 

  387. 

  388. 

  389. // Given a point and a line segment (defined by two points), compute the closest point

  390. // in the line.  Cap the point at the endpoints of the line segment.

  391. void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)

  392. {

  393. 	btVector3 lineDelta     = line1 - line0;

  394. 

  395. 	// Handle degenerate lines

  396. 	if ( lineDelta.fuzzyZero())

  397. 	{

  398. 		nearestPoint = line0;

  399. 	}

  400. 	else

  401. 	{

  402. 		btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);

  403. 

  404. 		// Clamp the point to conform to the segment's endpoints

  405. 		if ( delta < 0 )

  406. 			delta = 0;

  407. 		else if ( delta > 1 )

  408. 			delta = 1;

  409. 

  410. 		nearestPoint = line0 + lineDelta*delta;

  411. 	}

  412. }

  413. 

  414. 

  415. 

  416. 

  417. bool	btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)

  418. {

  419. 	btVector3 tri_normal = tri_normal_org;

  420. 	//we only have a local triangle normal, not a local contact normal -> only normal in world space...

  421. 	//either compute the current angle all in local space, or all in world space

  422. 

  423. 	btVector3 edgeCross = edge.cross(tri_normal).normalize();

  424. 	btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);

  425. 

  426. 	if (correctedEdgeAngle<0)

  427. 	{

  428. 		if (curAngle < correctedEdgeAngle)

  429. 		{

  430. 			btScalar diffAngle = correctedEdgeAngle-curAngle;

  431. 			btQuaternion rotation(edge,diffAngle );

  432. 			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;

  433. 			return true;

  434. 		}

  435. 	}

  436. 

  437. 	if (correctedEdgeAngle>=0)

  438. 	{

  439. 		if (curAngle > correctedEdgeAngle)

  440. 		{

  441. 			btScalar diffAngle = correctedEdgeAngle-curAngle;

  442. 			btQuaternion rotation(edge,diffAngle );

  443. 			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;

  444. 			return true;

  445. 		}

  446. 	}

  447. 	return false;

  448. }

  449. 

  450. 

  451. 

  452. /// Changes a btManifoldPoint collision normal to the normal from the mesh.

  453. void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags)

  454. {

  455. 	//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);

  456. 	if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)

  457. 		return;

  458. 

  459. 	btBvhTriangleMeshShape* trimesh = 0;

  460. 	

  461. 	if( colObj0->getRootCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )

  462. 	   trimesh = ((btScaledBvhTriangleMeshShape*)colObj0->getRootCollisionShape())->getChildShape();

  463.    else	   

  464. 	   trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();

  465. 	   

  466.    	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();

  467. 	if (!triangleInfoMapPtr)

  468. 		return;

  469. 

  470. 	int hash = btGetHash(partId0,index0);

  471. 

  472. 

  473. 	btTriangleInfo* info = triangleInfoMapPtr->find(hash);

  474. 	if (!info)

  475. 		return;

  476. 

  477. 	btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;

  478. 	

  479. 	const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape());

  480. 	btVector3 v0,v1,v2;

  481. 	tri_shape->getVertex(0,v0);

  482. 	tri_shape->getVertex(1,v1);

  483. 	tri_shape->getVertex(2,v2);

  484. 

  485. 	btVector3 center = (v0+v1+v2)*btScalar(1./3.);

  486. 

  487. 	btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);

  488. 	btVector3 tri_normal;

  489. 	tri_shape->calcNormal(tri_normal);

  490. 

  491. 	//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);

  492. 	btVector3 nearest;

  493. 	btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);

  494. 

  495. 	btVector3 contact = cp.m_localPointB;

  496. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  497. 	const btTransform& tr = colObj0->getWorldTransform();

  498. 	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);

  499. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  500. 

  501. 

  502. 

  503. 	bool isNearEdge = false;

  504. 

  505. 	int numConcaveEdgeHits = 0;

  506. 	int numConvexEdgeHits = 0;

  507. 

  508. 	btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;

  509. 	localContactNormalOnB.normalize();//is this necessary?

  510. 	

  511. 	// Get closest edge

  512. 	int      bestedge=-1;

  513. 	btScalar    disttobestedge=BT_LARGE_FLOAT;

  514. 	//

  515. 	// Edge 0 -> 1

  516. 	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)

  517. 	{	

  518. 	   btVector3 nearest;

  519. 	   btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );

  520. 	   btScalar     len=(contact-nearest).length();

  521. 	   //

  522. 	   if( len < disttobestedge )

  523. 	   {

  524. 	      bestedge=0;

  525. 	      disttobestedge=len;

  526.       }	      

  527.    }	   

  528. 	// Edge 1 -> 2

  529. 	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)

  530. 	{	

  531. 	   btVector3 nearest;

  532. 	   btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );

  533. 	   btScalar     len=(contact-nearest).length();

  534. 	   //

  535. 	   if( len < disttobestedge )

  536. 	   {

  537. 	      bestedge=1;

  538. 	      disttobestedge=len;

  539.       }	      

  540.    }	   

  541. 	// Edge 2 -> 0

  542. 	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)

  543. 	{	

  544. 	   btVector3 nearest;

  545. 	   btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );

  546. 	   btScalar     len=(contact-nearest).length();

  547. 	   //

  548. 	   if( len < disttobestedge )

  549. 	   {

  550. 	      bestedge=2;

  551. 	      disttobestedge=len;

  552.       }	      

  553.    }   	      	

  554. 	

  555. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  556.    btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);

  557.    btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );

  558. #endif   

  559. 	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)

  560. 	{

  561. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  562. 		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);

  563. #endif

  564. 		btScalar len = (contact-nearest).length();

  565. 		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)

  566. 		if( bestedge==0 )

  567. 		{

  568. 			btVector3 edge(v0-v1);

  569. 			isNearEdge = true;

  570. 

  571. 			if (info->m_edgeV0V1Angle==btScalar(0))

  572. 			{

  573. 				numConcaveEdgeHits++;

  574. 			} else

  575. 			{

  576. 

  577. 				bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);

  578. 				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);

  579. 	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  580. 				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);

  581. 	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  582. 

  583. 				btVector3 nA = swapFactor * tri_normal;

  584. 

  585. 				btQuaternion orn(edge,info->m_edgeV0V1Angle);

  586. 				btVector3 computedNormalB = quatRotate(orn,tri_normal);

  587. 				if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)

  588. 					computedNormalB*=-1;

  589. 				btVector3 nB = swapFactor*computedNormalB;

  590. 

  591. 				btScalar	NdotA = localContactNormalOnB.dot(nA);

  592. 				btScalar	NdotB = localContactNormalOnB.dot(nB);

  593. 				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

  594. 

  595. #ifdef DEBUG_INTERNAL_EDGE

  596. 				{

  597. 					

  598. 					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);

  599. 				}

  600. #endif //DEBUG_INTERNAL_EDGE

  601. 

  602. 

  603. 				if (backFacingNormal)

  604. 				{

  605. 					numConcaveEdgeHits++;

  606. 				}

  607. 				else

  608. 				{

  609. 					numConvexEdgeHits++;

  610. 					btVector3 clampedLocalNormal;

  611. 					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);

  612. 					if (isClamped)

  613. 					{

  614. 						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))

  615. 						{

  616. 							btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;

  617. 							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);

  618. 							cp.m_normalWorldOnB = newNormal;

  619. 							// Reproject collision point along normal. (what about cp.m_distance1?)

  620. 							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;

  621. 							cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);

  622. 							

  623. 						}

  624. 					}

  625. 				}

  626. 			}

  627. 		}

  628. 	}

  629. 

  630. 	btNearestPointInLineSegment(contact,v1,v2,nearest);

  631. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  632. 	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);

  633. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  634. 

  635. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  636.    btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );

  637. #endif   

  638. 

  639. 	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)

  640. 	{

  641. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  642. 		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);

  643. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  644. 

  645. 

  646. 

  647. 		btScalar len = (contact-nearest).length();

  648. 		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)

  649. 		if( bestedge==1 )

  650. 		{

  651. 			isNearEdge = true;

  652. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  653. 			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);

  654. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  655. 

  656. 			btVector3 edge(v1-v2);

  657. 

  658. 			isNearEdge = true;

  659. 

  660. 			if (info->m_edgeV1V2Angle == btScalar(0))

  661. 			{

  662. 				numConcaveEdgeHits++;

  663. 			} else

  664. 			{

  665. 				bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;

  666. 				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);

  667. 	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  668. 				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);

  669. 	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  670. 

  671. 				btVector3 nA = swapFactor * tri_normal;

  672. 				

  673. 				btQuaternion orn(edge,info->m_edgeV1V2Angle);

  674. 				btVector3 computedNormalB = quatRotate(orn,tri_normal);

  675. 				if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)

  676. 					computedNormalB*=-1;

  677. 				btVector3 nB = swapFactor*computedNormalB;

  678. 

  679. #ifdef DEBUG_INTERNAL_EDGE

  680. 				{

  681. 					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);

  682. 				}

  683. #endif //DEBUG_INTERNAL_EDGE

  684. 

  685. 

  686. 				btScalar	NdotA = localContactNormalOnB.dot(nA);

  687. 				btScalar	NdotB = localContactNormalOnB.dot(nB);

  688. 				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

  689. 

  690. 				if (backFacingNormal)

  691. 				{

  692. 					numConcaveEdgeHits++;

  693. 				}

  694. 				else

  695. 				{

  696. 					numConvexEdgeHits++;

  697. 					btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;

  698. 					btVector3 clampedLocalNormal;

  699. 					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);

  700. 					if (isClamped)

  701. 					{

  702. 						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))

  703. 						{

  704. 							btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;

  705. 							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);

  706. 							cp.m_normalWorldOnB = newNormal;

  707. 							// Reproject collision point along normal.

  708. 							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;

  709. 							cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);

  710. 						}

  711. 					}

  712. 				}

  713. 			}

  714. 		}

  715. 	}

  716. 

  717. 	btNearestPointInLineSegment(contact,v2,v0,nearest);

  718. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  719. 	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);

  720. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  721. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  722.    btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );

  723. #endif   

  724. 

  725. 	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)

  726. 	{

  727. 

  728. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  729. 		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);

  730. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  731. 

  732. 		btScalar len = (contact-nearest).length();

  733. 		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)

  734. 		if( bestedge==2 )

  735. 		{

  736. 			isNearEdge = true;

  737. #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  738. 			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);

  739. #endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  740. 

  741. 			btVector3 edge(v2-v0);

  742. 

  743. 			if (info->m_edgeV2V0Angle==btScalar(0))

  744. 			{

  745. 				numConcaveEdgeHits++;

  746. 			} else

  747. 			{

  748. 

  749. 				bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;

  750. 				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);

  751. 	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW

  752. 				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);

  753. 	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

  754. 

  755. 				btVector3 nA = swapFactor * tri_normal;

  756. 				btQuaternion orn(edge,info->m_edgeV2V0Angle);

  757. 				btVector3 computedNormalB = quatRotate(orn,tri_normal);

  758. 				if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)

  759. 					computedNormalB*=-1;

  760. 				btVector3 nB = swapFactor*computedNormalB;

  761. 

  762. #ifdef DEBUG_INTERNAL_EDGE

  763. 				{

  764. 					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);

  765. 				}

  766. #endif //DEBUG_INTERNAL_EDGE

  767. 

  768. 				btScalar	NdotA = localContactNormalOnB.dot(nA);

  769. 				btScalar	NdotB = localContactNormalOnB.dot(nB);

  770. 				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

  771. 

  772. 				if (backFacingNormal)

  773. 				{

  774. 					numConcaveEdgeHits++;

  775. 				}

  776. 				else

  777. 				{

  778. 					numConvexEdgeHits++;

  779. 					//				printf("hitting convex edge\n");

  780. 

  781. 

  782. 					btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;

  783. 					btVector3 clampedLocalNormal;

  784. 					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);

  785. 					if (isClamped)

  786. 					{

  787. 						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))

  788. 						{

  789. 							btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;

  790. 							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);

  791. 							cp.m_normalWorldOnB = newNormal;

  792. 							// Reproject collision point along normal.

  793. 							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;

  794. 							cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);

  795. 						}

  796. 					}

  797. 				} 

  798. 			}

  799. 			

  800. 

  801. 		}

  802. 	}

  803. 

  804. #ifdef DEBUG_INTERNAL_EDGE

  805. 	{

  806. 		btVector3 color(0,1,1);

  807. 		btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);

  808. 	}

  809. #endif //DEBUG_INTERNAL_EDGE

  810. 

  811. 	if (isNearEdge)

  812. 	{

  813. 

  814. 		if (numConcaveEdgeHits>0)

  815. 		{

  816. 			if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)

  817. 			{

  818. 				//fix tri_normal so it pointing the same direction as the current local contact normal

  819. 				if (tri_normal.dot(localContactNormalOnB) < 0)

  820. 				{

  821. 					tri_normal *= -1;

  822. 				}

  823. 				cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;

  824. 			} else

  825. 			{

  826. 				btVector3 newNormal = tri_normal *frontFacing;

  827. 				//if the tri_normal is pointing opposite direction as the current local contact normal, skip it

  828. 				btScalar d = newNormal.dot(localContactNormalOnB) ;

  829. 				if (d< 0)

  830. 				{

  831. 					return;

  832. 				}

  833. 				//modify the normal to be the triangle normal (or backfacing normal)

  834. 				cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *newNormal;

  835. 			}

  836. 						

  837. 			// Reproject collision point along normal.

  838. 			cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;

  839. 			cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);

  840. 		}

  841. 	}

  842. }