← Back to branch summary

~scrawl-deactivatedaccount/+junk/StuntRally

« back to all changes in this revision

Viewing changes to bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

  • Committer: Jannik Heller
  • Date: 2011-01-30 12:41:18 UTC
  • Revision ID: scrawl@scrawl-desktop-20110130124118-v5euo5nkmhjqqd9s
First commit

Show diffs side-by-side

added added

removed removed

Lines of Context:
bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
 
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
 
19
static btIDebugDraw* gDebugDrawer = 0;
 
20
 
 
21
void    btSetDebugDrawer(btIDebugDraw* debugDrawer)
 
22
{
 
23
        gDebugDrawer = debugDrawer;
 
24
}
 
25
 
 
26
static void    btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color)
 
27
{
 
28
        if (gDebugDrawer)
 
29
                gDebugDrawer->drawLine(from,to,color);
 
30
}
 
31
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
 
32
 
 
33
 
 
34
static int      btGetHash(int partId, int triangleIndex)
 
35
{
 
36
        int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
 
37
        return hash;
 
38
}
 
39
 
 
40
 
 
41
 
 
42
static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)
 
43
{
 
44
        const btVector3 refAxis0  = edgeA;
 
45
        const btVector3 refAxis1  = normalA;
 
46
        const btVector3 swingAxis = normalB;
 
47
        btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 
48
        return  angle;
 
49
}
 
50
 
 
51
 
 
52
struct btConnectivityProcessor : public btTriangleCallback
 
53
{
 
54
        int                             m_partIdA;
 
55
        int                             m_triangleIndexA;
 
56
        btVector3*              m_triangleVerticesA;
 
57
        btTriangleInfoMap*      m_triangleInfoMap;
 
58
 
 
59
 
 
60
        virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 
61
        {
 
62
                //skip self-collisions
 
63
                if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
 
64
                        return;
 
65
 
 
66
                //skip duplicates (disabled for now)
 
67
                //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
 
68
                //      return;
 
69
 
 
70
                //search for shared vertices and edges
 
71
                int numshared = 0;
 
72
                int sharedVertsA[3]={-1,-1,-1};
 
73
                int sharedVertsB[3]={-1,-1,-1};
 
74
 
 
75
                ///skip degenerate triangles
 
76
                btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
 
77
                if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
 
78
                        return;
 
79
 
 
80
 
 
81
                btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
 
82
                ///skip degenerate triangles
 
83
                if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
 
84
                        return;
 
85
 
 
86
#if 0
 
87
                printf("triangle A[0]   =       (%f,%f,%f)\ntriangle A[1]       =       (%f,%f,%f)\ntriangle A[2]       =       (%f,%f,%f)\n",
 
88
                        m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
 
89
                        m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
 
90
                        m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
 
91
 
 
92
                printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
 
93
                printf("triangle B[0]   =       (%f,%f,%f)\ntriangle B[1]       =       (%f,%f,%f)\ntriangle B[2]       =       (%f,%f,%f)\n",
 
94
                        triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
 
95
                        triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
 
96
                        triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
 
97
#endif
 
98
 
 
99
                for (int i=0;i<3;i++)
 
100
                {
 
101
                        for (int j=0;j<3;j++)
 
102
                        {
 
103
                                if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
 
104
                                {
 
105
                                        sharedVertsA[numshared] = i;
 
106
                                        sharedVertsB[numshared] = j;
 
107
                                        numshared++;
 
108
                                        ///degenerate case
 
109
                                        if(numshared >= 3)
 
110
                                                return;
 
111
                                }
 
112
                        }
 
113
                        ///degenerate case
 
114
                        if(numshared >= 3)
 
115
                                return;
 
116
                }
 
117
                switch (numshared)
 
118
                {
 
119
                case 0:
 
120
                        {
 
121
                                break;
 
122
                        }
 
123
                case 1:
 
124
                        {
 
125
                                //shared vertex
 
126
                                break;
 
127
                        }
 
128
                case 2:
 
129
                        {
 
130
                                //shared edge
 
131
                                //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
 
132
                                if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
 
133
                                {
 
134
                                        sharedVertsA[0] = 2;
 
135
                                        sharedVertsA[1] = 0;
 
136
                                        int tmp = sharedVertsB[1];
 
137
                                        sharedVertsB[1] = sharedVertsB[0];
 
138
                                        sharedVertsB[0] = tmp;
 
139
                                }
 
140
 
 
141
                                int hash = btGetHash(m_partIdA,m_triangleIndexA);
 
142
 
 
143
                                btTriangleInfo* info = m_triangleInfoMap->find(hash);
 
144
                                if (!info)
 
145
                                {
 
146
                                        btTriangleInfo tmp;
 
147
                                        m_triangleInfoMap->insert(hash,tmp);
 
148
                                        info = m_triangleInfoMap->find(hash);
 
149
                                }
 
150
 
 
151
                                int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
 
152
                                int otherIndexA = 3-sumvertsA;
 
153
 
 
154
                                
 
155
                                btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);
 
156
 
 
157
                                btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
 
158
                                int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);
 
159
 
 
160
                                btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
 
161
                                //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
 
162
 
 
163
                                btVector3 normalA;
 
164
                                btVector3 normalB;
 
165
                                tA.calcNormal(normalA);
 
166
                                tB.calcNormal(normalB);
 
167
                                edge.normalize();
 
168
                                btVector3 edgeCrossA = edge.cross(normalA).normalize();
 
169
 
 
170
                                {
 
171
                                        btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
 
172
                                        if (edgeCrossA.dot(tmp) < 0)
 
173
                                        {
 
174
                                                edgeCrossA*=-1;
 
175
                                        }
 
176
                                }
 
177
 
 
178
                                btVector3 edgeCrossB = edge.cross(normalB).normalize();
 
179
 
 
180
                                {
 
181
                                        btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
 
182
                                        if (edgeCrossB.dot(tmp) < 0)
 
183
                                        {
 
184
                                                edgeCrossB*=-1;
 
185
                                        }
 
186
                                }
 
187
 
 
188
                                btScalar        angle2 = 0;
 
189
                                btScalar        ang4 = 0.f;
 
190
 
 
191
 
 
192
                                btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
 
193
                                btScalar len2 = calculatedEdge.length2();
 
194
 
 
195
                                btScalar correctedAngle(0);
 
196
                                btVector3 calculatedNormalB = normalA;
 
197
                                bool isConvex = false;
 
198
 
 
199
                                if (len2<m_triangleInfoMap->m_planarEpsilon)
 
200
                                {
 
201
                                        angle2 = 0.f;
 
202
                                        ang4 = 0.f;
 
203
                                } else
 
204
                                {
 
205
 
 
206
                                        calculatedEdge.normalize();
 
207
                                        btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
 
208
                                        calculatedNormalA.normalize();
 
209
                                        angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
 
210
                                        ang4 = SIMD_PI-angle2;
 
211
                                        btScalar dotA = normalA.dot(edgeCrossB);
 
212
                                        ///@todo: check if we need some epsilon, due to floating point imprecision
 
213
                                        isConvex = (dotA<0.);
 
214
 
 
215
                                        correctedAngle = isConvex ? ang4 : -ang4;
 
216
                                        btQuaternion orn2(calculatedEdge,-correctedAngle);
 
217
                                        calculatedNormalB = btMatrix3x3(orn2)*normalA;
 
218
 
 
219
 
 
220
                                }
 
221
 
 
222
                                
 
223
 
 
224
                                
 
225
                                                        
 
226
                                //alternatively use 
 
227
                                //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
 
228
 
 
229
 
 
230
                                switch (sumvertsA)
 
231
                                {
 
232
                                case 1:
 
233
                                        {
 
234
                                                btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
 
235
                                                btQuaternion orn(edge,-correctedAngle);
 
236
                                                btVector3 computedNormalB = quatRotate(orn,normalA);
 
237
                                                btScalar bla = computedNormalB.dot(normalB);
 
238
                                                if (bla<0)
 
239
                                                {
 
240
                                                        computedNormalB*=-1;
 
241
                                                        info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
 
242
                                                }
 
243
#ifdef DEBUG_INTERNAL_EDGE
 
244
                                                if ((computedNormalB-normalB).length()>0.0001)
 
245
                                                {
 
246
                                                        printf("warning: normals not identical\n");
 
247
                                                }
 
248
#endif//DEBUG_INTERNAL_EDGE
 
249
 
 
250
                                                info->m_edgeV0V1Angle = -correctedAngle;
 
251
 
 
252
                                                if (isConvex)
 
253
                                                        info->m_flags |= TRI_INFO_V0V1_CONVEX;
 
254
                                                break;
 
255
                                        }
 
256
                                case 2:
 
257
                                        {
 
258
                                                btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
 
259
                                                btQuaternion orn(edge,-correctedAngle);
 
260
                                                btVector3 computedNormalB = quatRotate(orn,normalA);
 
261
                                                if (computedNormalB.dot(normalB)<0)
 
262
                                                {
 
263
                                                        computedNormalB*=-1;
 
264
                                                        info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
 
265
                                                }
 
266
 
 
267
#ifdef DEBUG_INTERNAL_EDGE
 
268
                                                if ((computedNormalB-normalB).length()>0.0001)
 
269
                                                {
 
270
                                                        printf("warning: normals not identical\n");
 
271
                                                }
 
272
#endif //DEBUG_INTERNAL_EDGE
 
273
                                                info->m_edgeV2V0Angle = -correctedAngle;
 
274
                                                if (isConvex)
 
275
                                                        info->m_flags |= TRI_INFO_V2V0_CONVEX;
 
276
                                                break;  
 
277
                                        }
 
278
                                case 3:
 
279
                                        {
 
280
                                                btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
 
281
                                                btQuaternion orn(edge,-correctedAngle);
 
282
                                                btVector3 computedNormalB = quatRotate(orn,normalA);
 
283
                                                if (computedNormalB.dot(normalB)<0)
 
284
                                                {
 
285
                                                        info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
 
286
                                                        computedNormalB*=-1;
 
287
                                                }
 
288
#ifdef DEBUG_INTERNAL_EDGE
 
289
                                                if ((computedNormalB-normalB).length()>0.0001)
 
290
                                                {
 
291
                                                        printf("warning: normals not identical\n");
 
292
                                                }
 
293
#endif //DEBUG_INTERNAL_EDGE
 
294
                                                info->m_edgeV1V2Angle = -correctedAngle;
 
295
 
 
296
                                                if (isConvex)
 
297
                                                        info->m_flags |= TRI_INFO_V1V2_CONVEX;
 
298
                                                break;
 
299
                                        }
 
300
                                }
 
301
 
 
302
                                break;
 
303
                        }
 
304
                default:
 
305
                        {
 
306
                                //                              printf("warning: duplicate triangle\n");
 
307
                        }
 
308
 
 
309
                }
 
310
        }
 
311
};
 
312
/////////////////////////////////////////////////////////
 
313
/////////////////////////////////////////////////////////
 
314
 
 
315
void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
 
316
{
 
317
        //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
 
318
        if (trimeshShape->getTriangleInfoMap())
 
319
                return;
 
320
 
 
321
        trimeshShape->setTriangleInfoMap(triangleInfoMap);
 
322
 
 
323
        btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
 
324
        const btVector3& meshScaling = meshInterface->getScaling();
 
325
 
 
326
        for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
 
327
        {
 
328
                const unsigned char *vertexbase = 0;
 
329
                int numverts = 0;
 
330
                PHY_ScalarType type = PHY_INTEGER;
 
331
                int stride = 0;
 
332
                const unsigned char *indexbase = 0;
 
333
                int indexstride = 0;
 
334
                int numfaces = 0;
 
335
                PHY_ScalarType indicestype = PHY_INTEGER;
 
336
                //PHY_ScalarType indexType=0;
 
337
 
 
338
                btVector3 triangleVerts[3];
 
339
                meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,   type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
 
340
                btVector3 aabbMin,aabbMax;
 
341
 
 
342
                for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
 
343
                {
 
344
                        unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
 
345
 
 
346
                        for (int j=2;j>=0;j--)
 
347
                        {
 
348
 
 
349
                                int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
 
350
                                if (type == PHY_FLOAT)
 
351
                                {
 
352
                                        float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
 
353
                                        triangleVerts[j] = btVector3(
 
354
                                                graphicsbase[0]*meshScaling.getX(),
 
355
                                                graphicsbase[1]*meshScaling.getY(),
 
356
                                                graphicsbase[2]*meshScaling.getZ());
 
357
                                }
 
358
                                else
 
359
                                {
 
360
                                        double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
 
361
                                        triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
 
362
                                }
 
363
                        }
 
364
                        aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
 
365
                        aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
 
366
                        aabbMin.setMin(triangleVerts[0]);
 
367
                        aabbMax.setMax(triangleVerts[0]);
 
368
                        aabbMin.setMin(triangleVerts[1]);
 
369
                        aabbMax.setMax(triangleVerts[1]);
 
370
                        aabbMin.setMin(triangleVerts[2]);
 
371
                        aabbMax.setMax(triangleVerts[2]);
 
372
 
 
373
                        btConnectivityProcessor connectivityProcessor;
 
374
                        connectivityProcessor.m_partIdA = partId;
 
375
                        connectivityProcessor.m_triangleIndexA = triangleIndex;
 
376
                        connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
 
377
                        connectivityProcessor.m_triangleInfoMap  = triangleInfoMap;
 
378
 
 
379
                        trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
 
380
                }
 
381
 
 
382
        }
 
383
 
 
384
}
 
385
 
 
386
 
 
387
 
 
388
 
 
389
// Given a point and a line segment (defined by two points), compute the closest point
 
390
// in the line.  Cap the point at the endpoints of the line segment.
 
391
void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
 
392
{
 
393
        btVector3 lineDelta     = line1 - line0;
 
394
 
 
395
        // Handle degenerate lines
 
396
        if ( lineDelta.fuzzyZero())
 
397
        {
 
398
                nearestPoint = line0;
 
399
        }
 
400
        else
 
401
        {
 
402
                btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
 
403
 
 
404
                // Clamp the point to conform to the segment's endpoints
 
405
                if ( delta < 0 )
 
406
                        delta = 0;
 
407
                else if ( delta > 1 )
 
408
                        delta = 1;
 
409
 
 
410
                nearestPoint = line0 + lineDelta*delta;
 
411
        }
 
412
}
 
413
 
 
414
 
 
415
 
 
416
 
 
417
bool    btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
 
418
{
 
419
        btVector3 tri_normal = tri_normal_org;
 
420
        //we only have a local triangle normal, not a local contact normal -> only normal in world space...
 
421
        //either compute the current angle all in local space, or all in world space
 
422
 
 
423
        btVector3 edgeCross = edge.cross(tri_normal).normalize();
 
424
        btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);
 
425
 
 
426
        if (correctedEdgeAngle<0)
 
427
        {
 
428
                if (curAngle < correctedEdgeAngle)
 
429
                {
 
430
                        btScalar diffAngle = correctedEdgeAngle-curAngle;
 
431
                        btQuaternion rotation(edge,diffAngle );
 
432
                        clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
 
433
                        return true;
 
434
                }
 
435
        }
 
436
 
 
437
        if (correctedEdgeAngle>=0)
 
438
        {
 
439
                if (curAngle > correctedEdgeAngle)
 
440
                {
 
441
                        btScalar diffAngle = correctedEdgeAngle-curAngle;
 
442
                        btQuaternion rotation(edge,diffAngle );
 
443
                        clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
 
444
                        return true;
 
445
                }
 
446
        }
 
447
        return false;
 
448
}
 
449
 
 
450
 
 
451
 
 
452
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
 
453
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags)
 
454
{
 
455
        //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
 
456
        if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
 
457
                return;
 
458
 
 
459
        btBvhTriangleMeshShape* trimesh = (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
}