Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/environment2/src/external/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @ 8474

Last change on this file since 8474 was 8351, checked in by rgrieder, 15 years ago

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

  • No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
  • In game console is not working in main menu for CEGUI 0.7
  • Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
  • And of course lots of new bugs we don't yet know about
  • Property svn:eol-style set to native
File size: 23.0 KB
Line 
1#include "btInternalEdgeUtility.h"
2
3#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
4#include "BulletCollision/CollisionShapes/btTriangleShape.h"
5#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
6#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
7#include "LinearMath/btIDebugDraw.h"
8
9
10//#define DEBUG_INTERNAL_EDGE
11
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 = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
460        btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
461        if (!triangleInfoMapPtr)
462                return;
463
464        int hash = btGetHash(partId0,index0);
465
466
467        btTriangleInfo* info = triangleInfoMapPtr->find(hash);
468        if (!info)
469                return;
470
471        btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
472       
473        const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape());
474        btVector3 v0,v1,v2;
475        tri_shape->getVertex(0,v0);
476        tri_shape->getVertex(1,v1);
477        tri_shape->getVertex(2,v2);
478
479        btVector3 center = (v0+v1+v2)*btScalar(1./3.);
480
481        btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
482        btVector3 tri_normal;
483        tri_shape->calcNormal(tri_normal);
484
485        //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
486        btVector3 nearest;
487        btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
488
489        btVector3 contact = cp.m_localPointB;
490#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
491        const btTransform& tr = colObj0->getWorldTransform();
492        btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
493#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
494
495
496
497        bool isNearEdge = false;
498
499        int numConcaveEdgeHits = 0;
500        int numConvexEdgeHits = 0;
501
502        btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
503        localContactNormalOnB.normalize();//is this necessary?
504
505        if ((info->m_edgeV0V1Angle)< SIMD_2_PI)
506        {
507#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
508                btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
509#endif
510                btScalar len = (contact-nearest).length();
511                if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
512                {
513                        btVector3 edge(v0-v1);
514                        isNearEdge = true;
515
516                        if (info->m_edgeV0V1Angle==btScalar(0))
517                        {
518                                numConcaveEdgeHits++;
519                        } else
520                        {
521
522                                bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
523                                btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
524        #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
525                                btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
526        #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
527
528                                btVector3 nA = swapFactor * tri_normal;
529
530                                btQuaternion orn(edge,info->m_edgeV0V1Angle);
531                                btVector3 computedNormalB = quatRotate(orn,tri_normal);
532                                if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
533                                        computedNormalB*=-1;
534                                btVector3 nB = swapFactor*computedNormalB;
535
536                                btScalar        NdotA = localContactNormalOnB.dot(nA);
537                                btScalar        NdotB = localContactNormalOnB.dot(nB);
538                                bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
539
540#ifdef DEBUG_INTERNAL_EDGE
541                                {
542                                       
543                                        btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
544                                }
545#endif //DEBUG_INTERNAL_EDGE
546
547
548                                if (backFacingNormal)
549                                {
550                                        numConcaveEdgeHits++;
551                                }
552                                else
553                                {
554                                        numConvexEdgeHits++;
555                                        btVector3 clampedLocalNormal;
556                                        bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
557                                        if (isClamped)
558                                        {
559                                                if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
560                                                {
561                                                        btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
562                                                        //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
563                                                        cp.m_normalWorldOnB = newNormal;
564                                                        // Reproject collision point along normal. (what about cp.m_distance1?)
565                                                        cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
566                                                        cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
567                                                       
568                                                }
569                                        }
570                                }
571                        }
572                }
573        }
574
575        btNearestPointInLineSegment(contact,v1,v2,nearest);
576#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
577        btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
578#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
579
580        if ((info->m_edgeV1V2Angle)< SIMD_2_PI)
581        {
582#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
583                btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
584#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
585
586
587
588                btScalar len = (contact-nearest).length();
589                if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
590                {
591                        isNearEdge = true;
592#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
593                        btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
594#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
595
596                        btVector3 edge(v1-v2);
597
598                        isNearEdge = true;
599
600                        if (info->m_edgeV1V2Angle == btScalar(0))
601                        {
602                                numConcaveEdgeHits++;
603                        } else
604                        {
605                                bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
606                                btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
607        #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
608                                btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
609        #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
610
611                                btVector3 nA = swapFactor * tri_normal;
612                               
613                                btQuaternion orn(edge,info->m_edgeV1V2Angle);
614                                btVector3 computedNormalB = quatRotate(orn,tri_normal);
615                                if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
616                                        computedNormalB*=-1;
617                                btVector3 nB = swapFactor*computedNormalB;
618
619#ifdef DEBUG_INTERNAL_EDGE
620                                {
621                                        btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
622                                }
623#endif //DEBUG_INTERNAL_EDGE
624
625
626                                btScalar        NdotA = localContactNormalOnB.dot(nA);
627                                btScalar        NdotB = localContactNormalOnB.dot(nB);
628                                bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
629
630                                if (backFacingNormal)
631                                {
632                                        numConcaveEdgeHits++;
633                                }
634                                else
635                                {
636                                        numConvexEdgeHits++;
637                                        btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
638                                        btVector3 clampedLocalNormal;
639                                        bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
640                                        if (isClamped)
641                                        {
642                                                if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
643                                                {
644                                                        btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
645                                                        //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
646                                                        cp.m_normalWorldOnB = newNormal;
647                                                        // Reproject collision point along normal.
648                                                        cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
649                                                        cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
650                                                }
651                                        }
652                                }
653                        }
654                }
655        }
656
657        btNearestPointInLineSegment(contact,v2,v0,nearest);
658#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
659        btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
660#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
661
662        if ((info->m_edgeV2V0Angle)< SIMD_2_PI)
663        {
664
665#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
666                btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
667#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
668
669                btScalar len = (contact-nearest).length();
670                if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
671                {
672                        isNearEdge = true;
673#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
674                        btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
675#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
676
677                        btVector3 edge(v2-v0);
678
679                        if (info->m_edgeV2V0Angle==btScalar(0))
680                        {
681                                numConcaveEdgeHits++;
682                        } else
683                        {
684
685                                bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
686                                btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
687        #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
688                                btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
689        #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
690
691                                btVector3 nA = swapFactor * tri_normal;
692                                btQuaternion orn(edge,info->m_edgeV2V0Angle);
693                                btVector3 computedNormalB = quatRotate(orn,tri_normal);
694                                if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
695                                        computedNormalB*=-1;
696                                btVector3 nB = swapFactor*computedNormalB;
697
698#ifdef DEBUG_INTERNAL_EDGE
699                                {
700                                        btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
701                                }
702#endif //DEBUG_INTERNAL_EDGE
703
704                                btScalar        NdotA = localContactNormalOnB.dot(nA);
705                                btScalar        NdotB = localContactNormalOnB.dot(nB);
706                                bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
707
708                                if (backFacingNormal)
709                                {
710                                        numConcaveEdgeHits++;
711                                }
712                                else
713                                {
714                                        numConvexEdgeHits++;
715                                        //                              printf("hitting convex edge\n");
716
717
718                                        btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
719                                        btVector3 clampedLocalNormal;
720                                        bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
721                                        if (isClamped)
722                                        {
723                                                if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
724                                                {
725                                                        btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
726                                                        //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
727                                                        cp.m_normalWorldOnB = newNormal;
728                                                        // Reproject collision point along normal.
729                                                        cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
730                                                        cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
731                                                }
732                                        }
733                                } 
734                        }
735                       
736
737                }
738        }
739
740#ifdef DEBUG_INTERNAL_EDGE
741        {
742                btVector3 color(0,1,1);
743                btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
744        }
745#endif //DEBUG_INTERNAL_EDGE
746
747        if (isNearEdge)
748        {
749
750                if (numConcaveEdgeHits>0)
751                {
752                        if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
753                        {
754                                //fix tri_normal so it pointing the same direction as the current local contact normal
755                                if (tri_normal.dot(localContactNormalOnB) < 0)
756                                {
757                                        tri_normal *= -1;
758                                }
759                                cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;
760                        } else
761                        {
762                                //modify the normal to be the triangle normal (or backfacing normal)
763                                cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing);
764                        }
765                       
766                       
767                        // Reproject collision point along normal.
768                        cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
769                        cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
770                }
771        }
772}
Note: See TracBrowser for help on using the repository browser.