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
btPlaneShape(const btVector3 & v,float f)40 btPlaneShape(const btVector3& v, float f)
41 :btStaticPlaneShape(v,f)
42 {
43 }
44
get_plane_equation(btVector4 & equation)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
get_plane_equation_transformed(const btTransform & trans,btVector4 & equation) const54 void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const
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
bt_begin_gim02_tri_time()73 void bt_begin_gim02_tri_time()
74 {
75 g_triangle_clock.reset();
76 }
77
bt_end_gim02_tri_time()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 const 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;
getChildShape(int index)101 virtual const btCollisionShape * getChildShape(int index)
102 {
103 return m_parent->m_gim_shape->getChildShape(index);
104 }
~ChildShapeRetriever()105 virtual ~ChildShapeRetriever() {}
106 };
107
108 class TriangleShapeRetriever:public ChildShapeRetriever
109 {
110 public:
111
getChildShape(int index)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 }
~TriangleShapeRetriever()117 virtual ~TriangleShapeRetriever() {}
118 };
119
120 class TetraShapeRetriever:public ChildShapeRetriever
121 {
122 public:
123
getChildShape(int index)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
GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape)136 GIM_ShapeRetriever(const 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
getChildShape(int index)156 const 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
getAverageTreeCollisionTime()172 float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime()
173 {
174 return btGImpactBoxSet::getAverageTreeCollisionTime();
175
176 }
177
178 //! Gets the average time in miliseconds of triangle collisions
getAverageTriangleCollisionTime()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
btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap)196 btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
197 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap)
198 {
199 m_manifoldPtr = NULL;
200 m_convex_algorithm = NULL;
201 }
202
~btGImpactCollisionAlgorithm()203 btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
204 {
205 clearCache();
206 }
207
208
209
210
211
addContactPoint(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btVector3 & point,const btVector3 & normal,btScalar distance)212 void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap,
213 const btCollisionObjectWrapper * body1Wrap,
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(body0Wrap,body1Wrap);
221 m_resultOut->addContactPoint(normal,point,distance);
222 }
223
224
shape_vs_shape_collision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btCollisionShape * shape0,const btCollisionShape * shape1)225 void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
226 const btCollisionObjectWrapper * body0Wrap,
227 const btCollisionObjectWrapper* body1Wrap,
228 const btCollisionShape * shape0,
229 const btCollisionShape * shape1)
230 {
231
232
233 {
234
235 btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap);
236 // post : checkManifold is called
237
238 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
239 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
240
241 algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut);
242
243 algor->~btCollisionAlgorithm();
244 m_dispatcher->freeCollisionAlgorithm(algor);
245 }
246
247 }
248
convex_vs_convex_collision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btCollisionShape * shape0,const btCollisionShape * shape1)249 void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
250 const btCollisionObjectWrapper* body0Wrap,
251 const btCollisionObjectWrapper* body1Wrap,
252 const btCollisionShape* shape0,
253 const btCollisionShape* shape1)
254 {
255
256 m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
257 m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
258
259 btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0);
260 btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1);
261 checkConvexAlgorithm(&ob0,&ob1);
262 m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut);
263
264
265 }
266
267
268
269
gimpact_vs_gimpact_find_pairs(const btTransform & trans0,const btTransform & trans1,const btGImpactShapeInterface * shape0,const btGImpactShapeInterface * shape1,btPairSet & pairset)270 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
271 const btTransform & trans0,
272 const btTransform & trans1,
273 const btGImpactShapeInterface * shape0,
274 const btGImpactShapeInterface * shape1,btPairSet & pairset)
275 {
276 if(shape0->hasBoxSet() && shape1->hasBoxSet())
277 {
278 btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
279 }
280 else
281 {
282 btAABB boxshape0;
283 btAABB boxshape1;
284 int i = shape0->getNumChildShapes();
285
286 while(i--)
287 {
288 shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
289
290 int j = shape1->getNumChildShapes();
291 while(j--)
292 {
293 shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
294
295 if(boxshape1.has_collision(boxshape0))
296 {
297 pairset.push_pair(i,j);
298 }
299 }
300 }
301 }
302
303
304 }
305
306
gimpact_vs_shape_find_pairs(const btTransform & trans0,const btTransform & trans1,const btGImpactShapeInterface * shape0,const btCollisionShape * shape1,btAlignedObjectArray<int> & collided_primitives)307 void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
308 const btTransform & trans0,
309 const btTransform & trans1,
310 const btGImpactShapeInterface * shape0,
311 const btCollisionShape * shape1,
312 btAlignedObjectArray<int> & collided_primitives)
313 {
314
315 btAABB boxshape;
316
317
318 if(shape0->hasBoxSet())
319 {
320 btTransform trans1to0 = trans0.inverse();
321 trans1to0 *= trans1;
322
323 shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
324
325 shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
326 }
327 else
328 {
329 shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
330
331 btAABB boxshape0;
332 int i = shape0->getNumChildShapes();
333
334 while(i--)
335 {
336 shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
337
338 if(boxshape.has_collision(boxshape0))
339 {
340 collided_primitives.push_back(i);
341 }
342 }
343
344 }
345
346 }
347
348
collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btGImpactMeshShapePart * shape0,const btGImpactMeshShapePart * shape1,const int * pairs,int pair_count)349 void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap,
350 const btCollisionObjectWrapper * body1Wrap,
351 const btGImpactMeshShapePart * shape0,
352 const btGImpactMeshShapePart * shape1,
353 const int * pairs, int pair_count)
354 {
355 btTriangleShapeEx tri0;
356 btTriangleShapeEx tri1;
357
358 shape0->lockChildShapes();
359 shape1->lockChildShapes();
360
361 const int * pair_pointer = pairs;
362
363 while(pair_count--)
364 {
365
366 m_triface0 = *(pair_pointer);
367 m_triface1 = *(pair_pointer+1);
368 pair_pointer+=2;
369
370
371
372 shape0->getBulletTriangle(m_triface0,tri0);
373 shape1->getBulletTriangle(m_triface1,tri1);
374
375
376 //collide two convex shapes
377 if(tri0.overlap_test_conservative(tri1))
378 {
379 convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1);
380 }
381
382 }
383
384 shape0->unlockChildShapes();
385 shape1->unlockChildShapes();
386 }
387
collide_sat_triangles(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btGImpactMeshShapePart * shape0,const btGImpactMeshShapePart * shape1,const int * pairs,int pair_count)388 void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
389 const btCollisionObjectWrapper* body1Wrap,
390 const btGImpactMeshShapePart * shape0,
391 const btGImpactMeshShapePart * shape1,
392 const int * pairs, int pair_count)
393 {
394 btTransform orgtrans0 = body0Wrap->getWorldTransform();
395 btTransform orgtrans1 = body1Wrap->getWorldTransform();
396
397 btPrimitiveTriangle ptri0;
398 btPrimitiveTriangle ptri1;
399 GIM_TRIANGLE_CONTACT contact_data;
400
401 shape0->lockChildShapes();
402 shape1->lockChildShapes();
403
404 const int * pair_pointer = pairs;
405
406 while(pair_count--)
407 {
408
409 m_triface0 = *(pair_pointer);
410 m_triface1 = *(pair_pointer+1);
411 pair_pointer+=2;
412
413
414 shape0->getPrimitiveTriangle(m_triface0,ptri0);
415 shape1->getPrimitiveTriangle(m_triface1,ptri1);
416
417 #ifdef TRI_COLLISION_PROFILING
418 bt_begin_gim02_tri_time();
419 #endif
420
421 ptri0.applyTransform(orgtrans0);
422 ptri1.applyTransform(orgtrans1);
423
424
425 //build planes
426 ptri0.buildTriPlane();
427 ptri1.buildTriPlane();
428 // test conservative
429
430
431
432 if(ptri0.overlap_test_conservative(ptri1))
433 {
434 if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
435 {
436
437 int j = contact_data.m_point_count;
438 while(j--)
439 {
440
441 addContactPoint(body0Wrap, body1Wrap,
442 contact_data.m_points[j],
443 contact_data.m_separating_normal,
444 -contact_data.m_penetration_depth);
445 }
446 }
447 }
448
449 #ifdef TRI_COLLISION_PROFILING
450 bt_end_gim02_tri_time();
451 #endif
452
453 }
454
455 shape0->unlockChildShapes();
456 shape1->unlockChildShapes();
457
458 }
459
460
gimpact_vs_gimpact(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btGImpactShapeInterface * shape0,const btGImpactShapeInterface * shape1)461 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
462 const btCollisionObjectWrapper* body0Wrap,
463 const btCollisionObjectWrapper * body1Wrap,
464 const btGImpactShapeInterface * shape0,
465 const btGImpactShapeInterface * shape1)
466 {
467
468 if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
469 {
470 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
471 m_part0 = meshshape0->getMeshPartCount();
472
473 while(m_part0--)
474 {
475 gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1);
476 }
477
478 return;
479 }
480
481 if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
482 {
483 const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1);
484 m_part1 = meshshape1->getMeshPartCount();
485
486 while(m_part1--)
487 {
488
489 gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1));
490
491 }
492
493 return;
494 }
495
496
497 btTransform orgtrans0 = body0Wrap->getWorldTransform();
498 btTransform orgtrans1 = body1Wrap->getWorldTransform();
499
500 btPairSet pairset;
501
502 gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
503
504 if(pairset.size()== 0) return;
505
506 if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
507 shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
508 {
509 const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0);
510 const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1);
511 //specialized function
512 #ifdef BULLET_TRIANGLE_COLLISION
513 collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
514 #else
515 collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
516 #endif
517
518 return;
519 }
520
521 //general function
522
523 shape0->lockChildShapes();
524 shape1->lockChildShapes();
525
526 GIM_ShapeRetriever retriever0(shape0);
527 GIM_ShapeRetriever retriever1(shape1);
528
529 bool child_has_transform0 = shape0->childrenHasTransform();
530 bool child_has_transform1 = shape1->childrenHasTransform();
531
532 int i = pairset.size();
533 while(i--)
534 {
535 GIM_PAIR * pair = &pairset[i];
536 m_triface0 = pair->m_index1;
537 m_triface1 = pair->m_index2;
538 const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
539 const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
540
541 btTransform tr0 = body0Wrap->getWorldTransform();
542 btTransform tr1 = body1Wrap->getWorldTransform();
543
544 if(child_has_transform0)
545 {
546 tr0 = orgtrans0*shape0->getChildTransform(m_triface0);
547 }
548
549 if(child_has_transform1)
550 {
551 tr1 = orgtrans1*shape1->getChildTransform(m_triface1);
552 }
553
554 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0);
555 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1);
556
557 //collide two convex shapes
558 convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1);
559 }
560
561 shape0->unlockChildShapes();
562 shape1->unlockChildShapes();
563 }
564
gimpact_vs_shape(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btGImpactShapeInterface * shape0,const btCollisionShape * shape1,bool swapped)565 void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
566 const btCollisionObjectWrapper * body1Wrap,
567 const btGImpactShapeInterface * shape0,
568 const btCollisionShape * shape1,bool swapped)
569 {
570 if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
571 {
572 const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
573 int& part = swapped ? m_part1 : m_part0;
574 part = meshshape0->getMeshPartCount();
575
576 while(part--)
577 {
578
579 gimpact_vs_shape(body0Wrap,
580 body1Wrap,
581 meshshape0->getMeshPart(part),
582 shape1,swapped);
583
584 }
585
586 return;
587 }
588
589 #ifdef GIMPACT_VS_PLANE_COLLISION
590 if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
591 shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
592 {
593 const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0);
594 const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1);
595 gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped);
596 return;
597 }
598
599 #endif
600
601
602
603 if(shape1->isCompound())
604 {
605 const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1);
606 gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped);
607 return;
608 }
609 else if(shape1->isConcave())
610 {
611 const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1);
612 gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped);
613 return;
614 }
615
616
617 btTransform orgtrans0 = body0Wrap->getWorldTransform();
618
619 btTransform orgtrans1 = body1Wrap->getWorldTransform();
620
621 btAlignedObjectArray<int> collided_results;
622
623 gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
624
625 if(collided_results.size() == 0) return;
626
627
628 shape0->lockChildShapes();
629
630 GIM_ShapeRetriever retriever0(shape0);
631
632
633 bool child_has_transform0 = shape0->childrenHasTransform();
634
635
636 int i = collided_results.size();
637
638 while(i--)
639 {
640 int child_index = collided_results[i];
641 if(swapped)
642 m_triface1 = child_index;
643 else
644 m_triface0 = child_index;
645
646 const btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
647
648 btTransform tr0 = body0Wrap->getWorldTransform();
649
650 if(child_has_transform0)
651 {
652 tr0 = orgtrans0*shape0->getChildTransform(child_index);
653 }
654
655 btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0);
656 const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap();
657
658 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject())
659 {
660 m_resultOut->setBody0Wrap(&ob0);
661 } else
662 {
663 m_resultOut->setBody1Wrap(&ob0);
664 }
665
666 //collide two shapes
667 if(swapped)
668 {
669
670 shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0);
671 }
672 else
673 {
674
675 shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1);
676 }
677 m_resultOut->setBody0Wrap(prevObj0);
678
679 }
680
681 shape0->unlockChildShapes();
682
683 }
684
gimpact_vs_compoundshape(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btGImpactShapeInterface * shape0,const btCompoundShape * shape1,bool swapped)685 void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap,
686 const btCollisionObjectWrapper* body1Wrap,
687 const btGImpactShapeInterface * shape0,
688 const btCompoundShape * shape1,bool swapped)
689 {
690 btTransform orgtrans1 = body1Wrap->getWorldTransform();
691
692 int i = shape1->getNumChildShapes();
693 while(i--)
694 {
695
696 const btCollisionShape * colshape1 = shape1->getChildShape(i);
697 btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
698
699 btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i);
700
701 const btCollisionObjectWrapper* tmp = 0;
702 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject())
703 {
704 tmp = m_resultOut->getBody0Wrap();
705 m_resultOut->setBody0Wrap(&ob1);
706 } else
707 {
708 tmp = m_resultOut->getBody1Wrap();
709 m_resultOut->setBody1Wrap(&ob1);
710 }
711 //collide child shape
712 gimpact_vs_shape(body0Wrap, &ob1,
713 shape0,colshape1,swapped);
714
715 if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject())
716 {
717 m_resultOut->setBody0Wrap(tmp);
718 } else
719 {
720 m_resultOut->setBody1Wrap(tmp);
721 }
722 }
723 }
724
gimpacttrimeshpart_vs_plane_collision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btGImpactMeshShapePart * shape0,const btStaticPlaneShape * shape1,bool swapped)725 void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
726 const btCollisionObjectWrapper * body0Wrap,
727 const btCollisionObjectWrapper * body1Wrap,
728 const btGImpactMeshShapePart * shape0,
729 const btStaticPlaneShape * shape1,bool swapped)
730 {
731
732
733 btTransform orgtrans0 = body0Wrap->getWorldTransform();
734 btTransform orgtrans1 = body1Wrap->getWorldTransform();
735
736 const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1);
737 btVector4 plane;
738 planeshape->get_plane_equation_transformed(orgtrans1,plane);
739
740 //test box against plane
741
742 btAABB tribox;
743 shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
744 tribox.increment_margin(planeshape->getMargin());
745
746 if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
747
748 shape0->lockChildShapes();
749
750 btScalar margin = shape0->getMargin() + planeshape->getMargin();
751
752 btVector3 vertex;
753 int vi = shape0->getVertexCount();
754 while(vi--)
755 {
756 shape0->getVertex(vi,vertex);
757 vertex = orgtrans0(vertex);
758
759 btScalar distance = vertex.dot(plane) - plane[3] - margin;
760
761 if(distance<0.0)//add contact
762 {
763 if(swapped)
764 {
765 addContactPoint(body1Wrap, body0Wrap,
766 vertex,
767 -plane,
768 distance);
769 }
770 else
771 {
772 addContactPoint(body0Wrap, body1Wrap,
773 vertex,
774 plane,
775 distance);
776 }
777 }
778 }
779
780 shape0->unlockChildShapes();
781 }
782
783
784
785
786 class btGImpactTriangleCallback: public btTriangleCallback
787 {
788 public:
789 btGImpactCollisionAlgorithm * algorithm;
790 const btCollisionObjectWrapper * body0Wrap;
791 const btCollisionObjectWrapper * body1Wrap;
792 const btGImpactShapeInterface * gimpactshape0;
793 bool swapped;
794 btScalar margin;
795
processTriangle(btVector3 * triangle,int partId,int triangleIndex)796 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
797 {
798 btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
799 tri1.setMargin(margin);
800 if(swapped)
801 {
802 algorithm->setPart0(partId);
803 algorithm->setFace0(triangleIndex);
804 }
805 else
806 {
807 algorithm->setPart1(partId);
808 algorithm->setFace1(triangleIndex);
809 }
810
811 btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex);
812 const btCollisionObjectWrapper * tmp = 0;
813
814 if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject())
815 {
816 tmp = algorithm->internalGetResultOut()->getBody0Wrap();
817 algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap);
818 } else
819 {
820 tmp = algorithm->internalGetResultOut()->getBody1Wrap();
821 algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap);
822 }
823
824 algorithm->gimpact_vs_shape(
825 body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped);
826
827 if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject())
828 {
829 algorithm->internalGetResultOut()->setBody0Wrap(tmp);
830 } else
831 {
832 algorithm->internalGetResultOut()->setBody1Wrap(tmp);
833 }
834
835 }
836 };
837
838
839
840
gimpact_vs_concave(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btGImpactShapeInterface * shape0,const btConcaveShape * shape1,bool swapped)841 void btGImpactCollisionAlgorithm::gimpact_vs_concave(
842 const btCollisionObjectWrapper* body0Wrap,
843 const btCollisionObjectWrapper * body1Wrap,
844 const btGImpactShapeInterface * shape0,
845 const btConcaveShape * shape1,bool swapped)
846 {
847 //create the callback
848 btGImpactTriangleCallback tricallback;
849 tricallback.algorithm = this;
850 tricallback.body0Wrap = body0Wrap;
851 tricallback.body1Wrap = body1Wrap;
852 tricallback.gimpactshape0 = shape0;
853 tricallback.swapped = swapped;
854 tricallback.margin = shape1->getMargin();
855
856 //getting the trimesh AABB
857 btTransform gimpactInConcaveSpace;
858
859 gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform();
860
861 btVector3 minAABB,maxAABB;
862 shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
863
864 shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
865
866 }
867
868
869
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)870 void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
871 {
872 clearCache();
873
874 m_resultOut = resultOut;
875 m_dispatchInfo = &dispatchInfo;
876 const btGImpactShapeInterface * gimpactshape0;
877 const btGImpactShapeInterface * gimpactshape1;
878
879 if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
880 {
881 gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape());
882
883 if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
884 {
885 gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
886
887 gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1);
888 }
889 else
890 {
891 gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false);
892 }
893
894 }
895 else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
896 {
897 gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
898
899 gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true);
900 }
901 }
902
903
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)904 btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
905 {
906 return 1.f;
907
908 }
909
910 ///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
911
912
913
914 //! Use this function for register the algorithm externally
registerAlgorithm(btCollisionDispatcher * dispatcher)915 void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
916 {
917
918 static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf;
919
920 int i;
921
922 for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
923 {
924 dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf);
925 }
926
927 for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
928 {
929 dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf);
930 }
931
932 }
933