Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/SuperOrxoBros_HS18/src/external/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @ 12175

Last change on this file since 12175 was 12175, checked in by siramesh, 5 years ago

Super Orxo Bros (Sidharth Ramesh, Nisa Balta, Jeff Ren)
File size: 25.2 KB
Line 
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
19static btIDebugDraw* gDebugDrawer = 0;
20
21void    btSetDebugDrawer(btIDebugDraw* debugDrawer)
22{
23        gDebugDrawer = debugDrawer;
24}
25
26static 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
34static 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
42static 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
52struct 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
315void 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.
391void 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
417bool    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.
453void 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        float    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           float     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           float     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           float     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}
Note: See TracBrowser for help on using the repository browser.