bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btInternalEdgeUtility.cpp
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 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
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 btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
454 {
455         //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
456         if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
457                 return;
458
459         btBvhTriangleMeshShape* trimesh = 0;
460         
461         if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
462            trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
463    else    
464            trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
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*>(colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->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 = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
840                 }
841         }
842 }