← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
 
1
/*
 
2
This source file is part of GIMPACT Library.
 
3
 
 
4
For the latest info, see http://gimpact.sourceforge.net/
 
5
 
 
6
Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
 
7
email: projectileman@yahoo.com
 
8
 
 
9
 
 
10
This software is provided 'as-is', without any express or implied warranty.
 
11
In no event will the authors be held liable for any damages arising from the use of this software.
 
12
Permission is granted to anyone to use this software for any purpose,
 
13
including commercial applications, and to alter it and redistribute it freely,
 
14
subject to the following restrictions:
 
15
 
 
16
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 
17
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 
18
3. This notice may not be removed or altered from any source distribution.
 
19
*/
 
20
/*
 
21
Author: Francisco Len N�jera
 
22
Concave-Concave Collision
 
23
 
 
24
*/
 
25
 
 
26
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
 
27
#include "LinearMath/btIDebugDraw.h"
 
28
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
29
#include "BulletCollision/CollisionShapes/btBoxShape.h"
 
30
#include "btGImpactCollisionAlgorithm.h"
 
31
#include "btContactProcessing.h"
 
32
#include "LinearMath/btQuickprof.h"
 
33
 
 
34
 
 
35
//! Class for accessing the plane equation
 
36
class btPlaneShape : public btStaticPlaneShape
 
37
{
 
38
public:
 
39
 
 
40
        btPlaneShape(const btVector3& v, float f)
 
41
                :btStaticPlaneShape(v,f)
 
42
        {
 
43
        }
 
44
 
 
45
        void get_plane_equation(btVector4 &equation)
 
46
        {
 
47
                equation[0] = m_planeNormal[0];
 
48
                equation[1] = m_planeNormal[1];
 
49
                equation[2] = m_planeNormal[2];
 
50
                equation[3] = m_planeConstant;
 
51
        }
 
52
 
 
53
 
 
54
        void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation)
 
55
        {
 
56
                equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal);
 
57
                equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal);
 
58
                equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal);
 
59
                equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant;
 
60
        }
 
61
};
 
62
 
 
63
 
 
64
 
 
65
//////////////////////////////////////////////////////////////////////////////////////////////
 
66
#ifdef TRI_COLLISION_PROFILING
 
67
 
 
68
btClock g_triangle_clock;
 
69
 
 
70
float g_accum_triangle_collision_time = 0;
 
71
int g_count_triangle_collision = 0;
 
72
 
 
73
void bt_begin_gim02_tri_time()
 
74
{
 
75
        g_triangle_clock.reset();
 
76
}
 
77
 
 
78
void bt_end_gim02_tri_time()
 
79
{
 
80
        g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds();
 
81
        g_count_triangle_collision++;
 
82
}
 
83
#endif //TRI_COLLISION_PROFILING
 
84
//! Retrieving shapes shapes
 
85
/*!
 
86
Declared here due of insuficent space on Pool allocators
 
87
*/
 
88
//!@{
 
89
class GIM_ShapeRetriever
 
90
{
 
91
public:
 
92
        btGImpactShapeInterface * m_gim_shape;
 
93
        btTriangleShapeEx m_trishape;
 
94
        btTetrahedronShapeEx m_tetrashape;
 
95
 
 
96
public:
 
97
        class ChildShapeRetriever
 
98
        {
 
99
        public:
 
100
                GIM_ShapeRetriever * m_parent;
 
101
                virtual btCollisionShape * getChildShape(int index)
 
102
                {
 
103
                        return m_parent->m_gim_shape->getChildShape(index);
 
104
                }
 
105
                virtual ~ChildShapeRetriever() {}
 
106
        };
 
107
 
 
108
        class TriangleShapeRetriever:public ChildShapeRetriever
 
109
        {
 
110
        public:
 
111
 
 
112
                virtual btCollisionShape * getChildShape(int index)
 
113
                {
 
114
                        m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape);
 
115
                        return &m_parent->m_trishape;
 
116
                }
 
117
                virtual ~TriangleShapeRetriever() {}
 
118
        };
 
119
 
 
120
        class TetraShapeRetriever:public ChildShapeRetriever
 
121
        {
 
122
        public:
 
123
 
 
124
                virtual btCollisionShape * getChildShape(int index)
 
125
                {
 
126
                        m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape);
 
127
                        return &m_parent->m_tetrashape;
 
128
                }
 
129
        };
 
130
public:
 
131
        ChildShapeRetriever m_child_retriever;
 
132
        TriangleShapeRetriever m_tri_retriever;
 
133
        TetraShapeRetriever  m_tetra_retriever;
 
134
        ChildShapeRetriever * m_current_retriever;
 
135
 
 
136
        GIM_ShapeRetriever(btGImpactShapeInterface * gim_shape)
 
137
        {
 
138
                m_gim_shape = gim_shape;
 
139
                //select retriever
 
140
                if(m_gim_shape->needsRetrieveTriangles())
 
141
                {
 
142
                        m_current_retriever = &m_tri_retriever;
 
143
                }
 
144
                else if(m_gim_shape->needsRetrieveTetrahedrons())
 
145
                {
 
146
                        m_current_retriever = &m_tetra_retriever;
 
147
                }
 
148
                else
 
149
                {
 
150
                        m_current_retriever = &m_child_retriever;
 
151
                }
 
152
 
 
153
                m_current_retriever->m_parent = this;
 
154
        }
 
155
 
 
156
        btCollisionShape * getChildShape(int index)
 
157
        {
 
158
                return m_current_retriever->getChildShape(index);
 
159
        }
 
160
 
 
161
 
 
162
};
 
163
 
 
164
 
 
165
 
 
166
//!@}
 
167
 
 
168
 
 
169
#ifdef TRI_COLLISION_PROFILING
 
170
 
 
171
//! Gets the average time in miliseconds of tree collisions
 
172
float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime()
 
173
{
 
174
        return btGImpactBoxSet::getAverageTreeCollisionTime();
 
175
 
 
176
}
 
177
 
 
178
//! Gets the average time in miliseconds of triangle collisions
 
179
float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime()
 
180
{
 
181
        if(g_count_triangle_collision == 0) return 0;
 
182
 
 
183
        float avgtime = g_accum_triangle_collision_time;
 
184
        avgtime /= (float)g_count_triangle_collision;
 
185
 
 
186
        g_accum_triangle_collision_time = 0;
 
187
        g_count_triangle_collision = 0;
 
188
 
 
189
        return avgtime;
 
190
}
 
191
 
 
192
#endif //TRI_COLLISION_PROFILING
 
193
 
 
194
 
 
195
 
 
196
btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 
197
: btActivatingCollisionAlgorithm(ci,body0,body1)
 
198
{
 
199
        m_manifoldPtr = NULL;
 
200
        m_convex_algorithm = NULL;
 
201
}
 
202
 
 
203
btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
 
204
{
 
205
        clearCache();
 
206
}
 
207
 
 
208
 
 
209
 
 
210
 
 
211
 
 
212
void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0,
 
213
                                btCollisionObject * body1,
 
214
                                const btVector3 & point,
 
215
                                const btVector3 & normal,
 
216
                                btScalar distance)
 
217
{
 
218
        m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
 
219
        m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
 
220
        checkManifold(body0,body1);
 
221
        m_resultOut->addContactPoint(normal,point,distance);
 
222
}
 
223
 
 
224
 
 
225
void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
 
226
                                          btCollisionObject * body0,
 
227
                                          btCollisionObject * body1,
 
228
                                          btCollisionShape * shape0,
 
229
                                          btCollisionShape * shape1)
 
230
{
 
231
 
 
232
        btCollisionShape* tmpShape0 = body0->getCollisionShape();
 
233
        btCollisionShape* tmpShape1 = body1->getCollisionShape();
 
234
        
 
235
        body0->internalSetTemporaryCollisionShape(shape0);
 
236
        body1->internalSetTemporaryCollisionShape(shape1);
 
237
 
 
238
        {
 
239
                btCollisionAlgorithm* algor = newAlgorithm(body0,body1);
 
240
                // post :       checkManifold is called
 
241
 
 
242
                m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
 
243
                m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
 
244
 
 
245
                algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
 
246
 
 
247
                algor->~btCollisionAlgorithm();
 
248
                m_dispatcher->freeCollisionAlgorithm(algor);
 
249
        }
 
250
 
 
251
        body0->internalSetTemporaryCollisionShape(tmpShape0);
 
252
        body1->internalSetTemporaryCollisionShape(tmpShape1);
 
253
}
 
254
 
 
255
void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
 
256
                                          btCollisionObject * body0,
 
257
                                          btCollisionObject * body1,
 
258
                                          btCollisionShape * shape0,
 
259
                                          btCollisionShape * shape1)
 
260
{
 
261
 
 
262
        btCollisionShape* tmpShape0 = body0->getCollisionShape();
 
263
        btCollisionShape* tmpShape1 = body1->getCollisionShape();
 
264
        
 
265
        body0->internalSetTemporaryCollisionShape(shape0);
 
266
        body1->internalSetTemporaryCollisionShape(shape1);
 
267
 
 
268
 
 
269
        m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
 
270
        m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
 
271
 
 
272
        checkConvexAlgorithm(body0,body1);
 
273
        m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
 
274
 
 
275
        body0->internalSetTemporaryCollisionShape(tmpShape0);
 
276
        body1->internalSetTemporaryCollisionShape(tmpShape1);
 
277
 
 
278
}
 
279
 
 
280
 
 
281
 
 
282
 
 
283
void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
 
284
                                          const btTransform & trans0,
 
285
                                          const btTransform & trans1,
 
286
                                          btGImpactShapeInterface * shape0,
 
287
                                          btGImpactShapeInterface * shape1,btPairSet & pairset)
 
288
{
 
289
        if(shape0->hasBoxSet() && shape1->hasBoxSet())
 
290
        {
 
291
                btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
 
292
        }
 
293
        else
 
294
        {
 
295
                btAABB boxshape0;
 
296
                btAABB boxshape1;
 
297
                int i = shape0->getNumChildShapes();
 
298
 
 
299
                while(i--)
 
300
                {
 
301
                        shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
 
302
 
 
303
                        int j = shape1->getNumChildShapes();
 
304
                        while(j--)
 
305
                        {
 
306
                                shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
 
307
 
 
308
                                if(boxshape1.has_collision(boxshape0))
 
309
                                {
 
310
                                        pairset.push_pair(i,j);
 
311
                                }
 
312
                        }
 
313
                }
 
314
        }
 
315
 
 
316
 
 
317
}
 
318
 
 
319
 
 
320
void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
 
321
                                          const btTransform & trans0,
 
322
                                          const btTransform & trans1,
 
323
                                          btGImpactShapeInterface * shape0,
 
324
                                          btCollisionShape * shape1,
 
325
                                          btAlignedObjectArray<int> & collided_primitives)
 
326
{
 
327
 
 
328
        btAABB boxshape;
 
329
 
 
330
 
 
331
        if(shape0->hasBoxSet())
 
332
        {
 
333
                btTransform trans1to0 = trans0.inverse();
 
334
                trans1to0 *= trans1;
 
335
 
 
336
                shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
 
337
 
 
338
                shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
 
339
        }
 
340
        else
 
341
        {
 
342
                shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
 
343
 
 
344
                btAABB boxshape0;
 
345
                int i = shape0->getNumChildShapes();
 
346
 
 
347
                while(i--)
 
348
                {
 
349
                        shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
 
350
 
 
351
                        if(boxshape.has_collision(boxshape0))
 
352
                        {
 
353
                                collided_primitives.push_back(i);
 
354
                        }
 
355
                }
 
356
 
 
357
        }
 
358
 
 
359
}
 
360
 
 
361
 
 
362
void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0,
 
363
                                  btCollisionObject * body1,
 
364
                                  btGImpactMeshShapePart * shape0,
 
365
                                  btGImpactMeshShapePart * shape1,
 
366
                                  const int * pairs, int pair_count)
 
367
{
 
368
        btTriangleShapeEx tri0;
 
369
        btTriangleShapeEx tri1;
 
370
 
 
371
        shape0->lockChildShapes();
 
372
        shape1->lockChildShapes();
 
373
 
 
374
        const int * pair_pointer = pairs;
 
375
 
 
376
        while(pair_count--)
 
377
        {
 
378
 
 
379
                m_triface0 = *(pair_pointer);
 
380
                m_triface1 = *(pair_pointer+1);
 
381
                pair_pointer+=2;
 
382
 
 
383
 
 
384
 
 
385
                shape0->getBulletTriangle(m_triface0,tri0);
 
386
                shape1->getBulletTriangle(m_triface1,tri1);
 
387
 
 
388
 
 
389
                //collide two convex shapes
 
390
                if(tri0.overlap_test_conservative(tri1))
 
391
                {
 
392
                        convex_vs_convex_collision(body0,body1,&tri0,&tri1);
 
393
                }
 
394
 
 
395
        }
 
396
 
 
397
        shape0->unlockChildShapes();
 
398
        shape1->unlockChildShapes();
 
399
}
 
400
 
 
401
void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0,
 
402
                                          btCollisionObject * body1,
 
403
                                          btGImpactMeshShapePart * shape0,
 
404
                                          btGImpactMeshShapePart * shape1,
 
405
                                          const int * pairs, int pair_count)
 
406
{
 
407
        btTransform orgtrans0 = body0->getWorldTransform();
 
408
        btTransform orgtrans1 = body1->getWorldTransform();
 
409
 
 
410
        btPrimitiveTriangle ptri0;
 
411
        btPrimitiveTriangle ptri1;
 
412
        GIM_TRIANGLE_CONTACT contact_data;
 
413
 
 
414
        shape0->lockChildShapes();
 
415
        shape1->lockChildShapes();
 
416
 
 
417
        const int * pair_pointer = pairs;
 
418
 
 
419
        while(pair_count--)
 
420
        {
 
421
 
 
422
                m_triface0 = *(pair_pointer);
 
423
                m_triface1 = *(pair_pointer+1);
 
424
                pair_pointer+=2;
 
425
 
 
426
 
 
427
                shape0->getPrimitiveTriangle(m_triface0,ptri0);
 
428
                shape1->getPrimitiveTriangle(m_triface1,ptri1);
 
429
 
 
430
                #ifdef TRI_COLLISION_PROFILING
 
431
                bt_begin_gim02_tri_time();
 
432
                #endif
 
433
 
 
434
                ptri0.applyTransform(orgtrans0);
 
435
                ptri1.applyTransform(orgtrans1);
 
436
 
 
437
 
 
438
                //build planes
 
439
                ptri0.buildTriPlane();
 
440
                ptri1.buildTriPlane();
 
441
                // test conservative
 
442
 
 
443
 
 
444
 
 
445
                if(ptri0.overlap_test_conservative(ptri1))
 
446
                {
 
447
                        if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
 
448
                        {
 
449
 
 
450
                                int j = contact_data.m_point_count;
 
451
                                while(j--)
 
452
                                {
 
453
 
 
454
                                        addContactPoint(body0, body1,
 
455
                                                                contact_data.m_points[j],
 
456
                                                                contact_data.m_separating_normal,
 
457
                                                                -contact_data.m_penetration_depth);
 
458
                                }
 
459
                        }
 
460
                }
 
461
 
 
462
                #ifdef TRI_COLLISION_PROFILING
 
463
                bt_end_gim02_tri_time();
 
464
                #endif
 
465
 
 
466
        }
 
467
 
 
468
        shape0->unlockChildShapes();
 
469
        shape1->unlockChildShapes();
 
470
 
 
471
}
 
472
 
 
473
 
 
474
void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
 
475
                                                btCollisionObject * body0,
 
476
                                                btCollisionObject * body1,
 
477
                                                btGImpactShapeInterface * shape0,
 
478
                                                btGImpactShapeInterface * shape1)
 
479
{
 
480
 
 
481
        if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
 
482
        {
 
483
                btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
 
484
                m_part0 = meshshape0->getMeshPartCount();
 
485
 
 
486
                while(m_part0--)
 
487
                {
 
488
                        gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1);
 
489
                }
 
490
 
 
491
                return;
 
492
        }
 
493
 
 
494
        if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
 
495
        {
 
496
                btGImpactMeshShape * meshshape1 = static_cast<btGImpactMeshShape *>(shape1);
 
497
                m_part1 = meshshape1->getMeshPartCount();
 
498
 
 
499
                while(m_part1--)
 
500
                {
 
501
 
 
502
                        gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1));
 
503
 
 
504
                }
 
505
 
 
506
                return;
 
507
        }
 
508
 
 
509
 
 
510
        btTransform orgtrans0 = body0->getWorldTransform();
 
511
        btTransform orgtrans1 = body1->getWorldTransform();
 
512
 
 
513
        btPairSet pairset;
 
514
 
 
515
        gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
 
516
 
 
517
        if(pairset.size()== 0) return;
 
518
 
 
519
        if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
 
520
                shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
 
521
        {
 
522
                btGImpactMeshShapePart * shapepart0 = static_cast<btGImpactMeshShapePart * >(shape0);
 
523
                btGImpactMeshShapePart * shapepart1 = static_cast<btGImpactMeshShapePart * >(shape1);
 
524
                //specialized function
 
525
                #ifdef BULLET_TRIANGLE_COLLISION
 
526
                collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
 
527
                #else
 
528
                collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
 
529
                #endif
 
530
 
 
531
                return;
 
532
        }
 
533
 
 
534
        //general function
 
535
 
 
536
        shape0->lockChildShapes();
 
537
        shape1->lockChildShapes();
 
538
 
 
539
        GIM_ShapeRetriever retriever0(shape0);
 
540
        GIM_ShapeRetriever retriever1(shape1);
 
541
 
 
542
        bool child_has_transform0 = shape0->childrenHasTransform();
 
543
        bool child_has_transform1 = shape1->childrenHasTransform();
 
544
 
 
545
        int i = pairset.size();
 
546
        while(i--)
 
547
        {
 
548
                GIM_PAIR * pair = &pairset[i];
 
549
                m_triface0 = pair->m_index1;
 
550
                m_triface1 = pair->m_index2;
 
551
                btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
 
552
                btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
 
553
 
 
554
                if(child_has_transform0)
 
555
                {
 
556
                        body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0));
 
557
                }
 
558
 
 
559
                if(child_has_transform1)
 
560
                {
 
561
                        body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1));
 
562
                }
 
563
 
 
564
                //collide two convex shapes
 
565
                convex_vs_convex_collision(body0,body1,colshape0,colshape1);
 
566
 
 
567
 
 
568
                if(child_has_transform0)
 
569
                {
 
570
                        body0->setWorldTransform(orgtrans0);
 
571
                }
 
572
 
 
573
                if(child_has_transform1)
 
574
                {
 
575
                        body1->setWorldTransform(orgtrans1);
 
576
                }
 
577
 
 
578
        }
 
579
 
 
580
        shape0->unlockChildShapes();
 
581
        shape1->unlockChildShapes();
 
582
}
 
583
 
 
584
void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0,
 
585
                                  btCollisionObject * body1,
 
586
                                  btGImpactShapeInterface * shape0,
 
587
                                  btCollisionShape * shape1,bool swapped)
 
588
{
 
589
        if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
 
590
        {
 
591
                btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
 
592
                int& part = swapped ? m_part1 : m_part0;
 
593
                part = meshshape0->getMeshPartCount();
 
594
 
 
595
                while(part--)
 
596
                {
 
597
 
 
598
                        gimpact_vs_shape(body0,
 
599
                                  body1,
 
600
                                  meshshape0->getMeshPart(part),
 
601
                                  shape1,swapped);
 
602
 
 
603
                }
 
604
 
 
605
                return;
 
606
        }
 
607
 
 
608
        #ifdef GIMPACT_VS_PLANE_COLLISION
 
609
        if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
 
610
                shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
 
611
        {
 
612
                btGImpactMeshShapePart * shapepart = static_cast<btGImpactMeshShapePart *>(shape0);
 
613
                btStaticPlaneShape * planeshape = static_cast<btStaticPlaneShape * >(shape1);
 
614
                gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped);
 
615
                return;
 
616
        }
 
617
 
 
618
        #endif
 
619
 
 
620
 
 
621
 
 
622
        if(shape1->isCompound())
 
623
        {
 
624
                btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1);
 
625
                gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped);
 
626
                return;
 
627
        }
 
628
        else if(shape1->isConcave())
 
629
        {
 
630
                btConcaveShape * concaveshape = static_cast<btConcaveShape *>(shape1);
 
631
                gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped);
 
632
                return;
 
633
        }
 
634
 
 
635
 
 
636
        btTransform orgtrans0 = body0->getWorldTransform();
 
637
 
 
638
        btTransform orgtrans1 = body1->getWorldTransform();
 
639
 
 
640
        btAlignedObjectArray<int> collided_results;
 
641
 
 
642
        gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
 
643
 
 
644
        if(collided_results.size() == 0) return;
 
645
 
 
646
 
 
647
        shape0->lockChildShapes();
 
648
 
 
649
        GIM_ShapeRetriever retriever0(shape0);
 
650
 
 
651
 
 
652
        bool child_has_transform0 = shape0->childrenHasTransform();
 
653
 
 
654
 
 
655
        int i = collided_results.size();
 
656
 
 
657
        while(i--)
 
658
        {
 
659
                int child_index = collided_results[i];
 
660
        if(swapped)
 
661
                m_triface1 = child_index;
 
662
        else
 
663
            m_triface0 = child_index;
 
664
 
 
665
                btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
 
666
 
 
667
                if(child_has_transform0)
 
668
                {
 
669
                        body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index));
 
670
                }
 
671
 
 
672
                //collide two shapes
 
673
                if(swapped)
 
674
                {
 
675
                        shape_vs_shape_collision(body1,body0,shape1,colshape0);
 
676
                }
 
677
                else
 
678
                {
 
679
                        shape_vs_shape_collision(body0,body1,colshape0,shape1);
 
680
                }
 
681
 
 
682
                //restore transforms
 
683
                if(child_has_transform0)
 
684
                {
 
685
                        body0->setWorldTransform(orgtrans0);
 
686
                }
 
687
 
 
688
        }
 
689
 
 
690
        shape0->unlockChildShapes();
 
691
 
 
692
}
 
693
 
 
694
void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0,
 
695
                                  btCollisionObject * body1,
 
696
                                  btGImpactShapeInterface * shape0,
 
697
                                  btCompoundShape * shape1,bool swapped)
 
698
{
 
699
        btTransform orgtrans1 = body1->getWorldTransform();
 
700
 
 
701
        int i = shape1->getNumChildShapes();
 
702
        while(i--)
 
703
        {
 
704
 
 
705
                btCollisionShape * colshape1 = shape1->getChildShape(i);
 
706
                btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
 
707
 
 
708
                body1->setWorldTransform(childtrans1);
 
709
 
 
710
                //collide child shape
 
711
                gimpact_vs_shape(body0, body1,
 
712
                                          shape0,colshape1,swapped);
 
713
 
 
714
 
 
715
                //restore transforms
 
716
                body1->setWorldTransform(orgtrans1);
 
717
        }
 
718
}
 
719
 
 
720
void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
 
721
                                          btCollisionObject * body0,
 
722
                                          btCollisionObject * body1,
 
723
                                          btGImpactMeshShapePart * shape0,
 
724
                                          btStaticPlaneShape * shape1,bool swapped)
 
725
{
 
726
 
 
727
 
 
728
        btTransform orgtrans0 = body0->getWorldTransform();
 
729
        btTransform orgtrans1 = body1->getWorldTransform();
 
730
 
 
731
        btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1);
 
732
        btVector4 plane;
 
733
        planeshape->get_plane_equation_transformed(orgtrans1,plane);
 
734
 
 
735
        //test box against plane
 
736
 
 
737
        btAABB tribox;
 
738
        shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
 
739
        tribox.increment_margin(planeshape->getMargin());
 
740
 
 
741
        if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
 
742
 
 
743
        shape0->lockChildShapes();
 
744
 
 
745
        btScalar margin = shape0->getMargin() + planeshape->getMargin();
 
746
 
 
747
        btVector3 vertex;
 
748
        int vi = shape0->getVertexCount();
 
749
        while(vi--)
 
750
        {
 
751
                shape0->getVertex(vi,vertex);
 
752
                vertex = orgtrans0(vertex);
 
753
 
 
754
                btScalar distance = vertex.dot(plane) - plane[3] - margin;
 
755
 
 
756
                if(distance<0.0)//add contact
 
757
                {
 
758
                        if(swapped)
 
759
                        {
 
760
                                addContactPoint(body1, body0,
 
761
                                        vertex,
 
762
                                        -plane,
 
763
                                        distance);
 
764
                        }
 
765
                        else
 
766
                        {
 
767
                                addContactPoint(body0, body1,
 
768
                                        vertex,
 
769
                                        plane,
 
770
                                        distance);
 
771
                        }
 
772
                }
 
773
        }
 
774
 
 
775
        shape0->unlockChildShapes();
 
776
}
 
777
 
 
778
 
 
779
 
 
780
 
 
781
class btGImpactTriangleCallback: public btTriangleCallback
 
782
{
 
783
public:
 
784
        btGImpactCollisionAlgorithm * algorithm;
 
785
        btCollisionObject * body0;
 
786
        btCollisionObject * body1;
 
787
        btGImpactShapeInterface * gimpactshape0;
 
788
        bool swapped;
 
789
        btScalar margin;
 
790
 
 
791
        virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 
792
        {
 
793
                btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
 
794
                tri1.setMargin(margin);
 
795
        if(swapped)
 
796
        {
 
797
            algorithm->setPart0(partId);
 
798
            algorithm->setFace0(triangleIndex);
 
799
        }
 
800
        else
 
801
        {
 
802
            algorithm->setPart1(partId);
 
803
            algorithm->setFace1(triangleIndex);
 
804
        }
 
805
                algorithm->gimpact_vs_shape(
 
806
                                                        body0,body1,gimpactshape0,&tri1,swapped);
 
807
        }
 
808
};
 
809
 
 
810
 
 
811
 
 
812
 
 
813
void btGImpactCollisionAlgorithm::gimpact_vs_concave(
 
814
                                  btCollisionObject * body0,
 
815
                                  btCollisionObject * body1,
 
816
                                  btGImpactShapeInterface * shape0,
 
817
                                  btConcaveShape * shape1,bool swapped)
 
818
{
 
819
        //create the callback
 
820
        btGImpactTriangleCallback tricallback;
 
821
        tricallback.algorithm = this;
 
822
        tricallback.body0 = body0;
 
823
        tricallback.body1 = body1;
 
824
        tricallback.gimpactshape0 = shape0;
 
825
        tricallback.swapped = swapped;
 
826
        tricallback.margin = shape1->getMargin();
 
827
 
 
828
        //getting the trimesh AABB
 
829
        btTransform gimpactInConcaveSpace;
 
830
 
 
831
        gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform();
 
832
 
 
833
        btVector3 minAABB,maxAABB;
 
834
        shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
 
835
 
 
836
        shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
 
837
 
 
838
}
 
839
 
 
840
 
 
841
 
 
842
void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 
843
{
 
844
    clearCache();
 
845
 
 
846
    m_resultOut = resultOut;
 
847
        m_dispatchInfo = &dispatchInfo;
 
848
    btGImpactShapeInterface * gimpactshape0;
 
849
    btGImpactShapeInterface * gimpactshape1;
 
850
 
 
851
        if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
 
852
        {
 
853
                gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape());
 
854
 
 
855
                if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
 
856
                {
 
857
                        gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
 
858
 
 
859
                        gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1);
 
860
                }
 
861
                else
 
862
                {
 
863
                        gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false);
 
864
                }
 
865
 
 
866
        }
 
867
        else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
 
868
        {
 
869
                gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
 
870
 
 
871
                gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true);
 
872
        }
 
873
}
 
874
 
 
875
 
 
876
btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 
877
{
 
878
        return 1.f;
 
879
 
 
880
}
 
881
 
 
882
///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
 
883
 
 
884
 
 
885
 
 
886
//! Use this function for register the algorithm externally
 
887
void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
 
888
{
 
889
 
 
890
        static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf;
 
891
 
 
892
        int i;
 
893
 
 
894
        for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
 
895
        {
 
896
                dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf);
 
897
        }
 
898
 
 
899
        for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
 
900
        {
 
901
                dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf);
 
902
        }
 
903
 
 
904
}