diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp | 650 |
1 files changed, 292 insertions, 358 deletions
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp index 2e87475e393..73e3db10107 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ /* -Author: Francisco Len Nßjera +Author: Francisco Leon Najera Concave-Concave Collision */ @@ -31,18 +31,16 @@ Concave-Concave Collision #include "btContactProcessing.h" #include "LinearMath/btQuickprof.h" - //! Class for accessing the plane equation class btPlaneShape : public btStaticPlaneShape { public: - btPlaneShape(const btVector3& v, float f) - :btStaticPlaneShape(v,f) + : btStaticPlaneShape(v, f) { } - void get_plane_equation(btVector4 &equation) + void get_plane_equation(btVector4& equation) { equation[0] = m_planeNormal[0]; equation[1] = m_planeNormal[1]; @@ -50,18 +48,16 @@ public: equation[3] = m_planeConstant; } - - void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const + void get_plane_equation_transformed(const btTransform& trans, btVector4& equation) const { - equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal); - equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal); - equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal); - equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant; + const btVector3 normal = trans.getBasis() * m_planeNormal; + equation[0] = normal[0]; + equation[1] = normal[1]; + equation[2] = normal[2]; + equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal)); } }; - - ////////////////////////////////////////////////////////////////////////////////////////////// #ifdef TRI_COLLISION_PROFILING @@ -80,7 +76,7 @@ void bt_end_gim02_tri_time() g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); g_count_triangle_collision++; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING //! Retrieving shapes shapes /*! Declared here due of insuficent space on Pool allocators @@ -89,7 +85,7 @@ Declared here due of insuficent space on Pool allocators class GIM_ShapeRetriever { public: - const btGImpactShapeInterface * m_gim_shape; + const btGImpactShapeInterface* m_gim_shape; btTriangleShapeEx m_trishape; btTetrahedronShapeEx m_tetrashape; @@ -97,51 +93,50 @@ public: class ChildShapeRetriever { public: - GIM_ShapeRetriever * m_parent; - virtual const btCollisionShape * getChildShape(int index) + GIM_ShapeRetriever* m_parent; + virtual const btCollisionShape* getChildShape(int index) { return m_parent->m_gim_shape->getChildShape(index); } virtual ~ChildShapeRetriever() {} }; - class TriangleShapeRetriever:public ChildShapeRetriever + class TriangleShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); + m_parent->m_gim_shape->getBulletTriangle(index, m_parent->m_trishape); return &m_parent->m_trishape; } virtual ~TriangleShapeRetriever() {} }; - class TetraShapeRetriever:public ChildShapeRetriever + class TetraShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape); + m_parent->m_gim_shape->getBulletTetrahedron(index, m_parent->m_tetrashape); return &m_parent->m_tetrashape; } }; + public: ChildShapeRetriever m_child_retriever; TriangleShapeRetriever m_tri_retriever; - TetraShapeRetriever m_tetra_retriever; - ChildShapeRetriever * m_current_retriever; + TetraShapeRetriever m_tetra_retriever; + ChildShapeRetriever* m_current_retriever; - GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape) + GIM_ShapeRetriever(const btGImpactShapeInterface* gim_shape) { m_gim_shape = gim_shape; //select retriever - if(m_gim_shape->needsRetrieveTriangles()) + if (m_gim_shape->needsRetrieveTriangles()) { m_current_retriever = &m_tri_retriever; } - else if(m_gim_shape->needsRetrieveTetrahedrons()) + else if (m_gim_shape->needsRetrieveTetrahedrons()) { m_current_retriever = &m_tetra_retriever; } @@ -153,32 +148,26 @@ public: m_current_retriever->m_parent = this; } - const btCollisionShape * getChildShape(int index) + const btCollisionShape* getChildShape(int index) { return m_current_retriever->getChildShape(index); } - - }; - - //!@} - #ifdef TRI_COLLISION_PROFILING //! Gets the average time in miliseconds of tree collisions float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime() { return btGImpactBoxSet::getAverageTreeCollisionTime(); - } //! Gets the average time in miliseconds of triangle collisions float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() { - if(g_count_triangle_collision == 0) return 0; + if (g_count_triangle_collision == 0) return 0; float avgtime = g_accum_triangle_collision_time; avgtime /= (float)g_count_triangle_collision; @@ -189,12 +178,10 @@ float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() return avgtime; } -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING - -btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap) +btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap) { m_manifoldPtr = NULL; m_convex_algorithm = NULL; @@ -205,77 +192,62 @@ btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() clearCache(); } - - - - -void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btVector3 & point, - const btVector3 & normal, - btScalar distance) +void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btVector3& point, + const btVector3& normal, + btScalar distance) { - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - checkManifold(body0Wrap,body1Wrap); - m_resultOut->addContactPoint(normal,point,distance); + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + checkManifold(body0Wrap, body1Wrap); + m_resultOut->addContactPoint(normal, point, distance); } - void btGImpactCollisionAlgorithm::shape_vs_shape_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape * shape0, - const btCollisionShape * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { - - { - - btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap); + btCollisionAlgorithm* algor = newAlgorithm(body0Wrap, body1Wrap); // post : checkManifold is called - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut); - + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + + algor->processCollision(body0Wrap, body1Wrap, *m_dispatchInfo, m_resultOut); + algor->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algor); } - } void btGImpactCollisionAlgorithm::convex_vs_convex_collision( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape* shape0, - const btCollisionShape* shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1); - checkConvexAlgorithm(&ob0,&ob1); - m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut); - - + btCollisionObjectWrapper ob0(body0Wrap, shape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, shape1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), m_part1, m_triface1); + checkConvexAlgorithm(&ob0, &ob1); + m_convex_algorithm->processCollision(&ob0, &ob1, *m_dispatchInfo, m_resultOut); } - - - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1,btPairSet & pairset) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1, btPairSet& pairset) { - if(shape0->hasBoxSet() && shape1->hasBoxSet()) + if (shape0->hasBoxSet() && shape1->hasBoxSet()) { - btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); + btGImpactBoxSet::find_collision(shape0->getBoxSet(), trans0, shape1->getBoxSet(), trans1, pairset); } else { @@ -283,74 +255,66 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( btAABB boxshape1; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); int j = shape1->getNumChildShapes(); - while(j--) + while (j--) { - shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); + shape1->getChildAabb(i, trans1, boxshape1.m_min, boxshape1.m_max); - if(boxshape1.has_collision(boxshape0)) + if (boxshape1.has_collision(boxshape0)) { - pairset.push_pair(i,j); + pairset.push_pair(i, j); } } } } - - } - void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray<int> & collided_primitives) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, + btAlignedObjectArray<int>& collided_primitives) { - btAABB boxshape; - - if(shape0->hasBoxSet()) + if (shape0->hasBoxSet()) { btTransform trans1to0 = trans0.inverse(); trans1to0 *= trans1; - shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); + shape1->getAabb(trans1to0, boxshape.m_min, boxshape.m_max); shape0->getBoxSet()->boxQuery(boxshape, collided_primitives); } else { - shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max); + shape1->getAabb(trans1, boxshape.m_min, boxshape.m_max); btAABB boxshape0; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); - if(boxshape.has_collision(boxshape0)) + if (boxshape.has_collision(boxshape0)) { collided_primitives.push_back(i); } } - } - } - -void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) +void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTriangleShapeEx tri0; btTriangleShapeEx tri1; @@ -358,27 +322,22 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + while (pair_count--) { - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; - - - - shape0->getBulletTriangle(m_triface0,tri0); - shape1->getBulletTriangle(m_triface1,tri1); + m_triface1 = *(pair_pointer + 1); + pair_pointer += 2; + shape0->getBulletTriangle(m_triface0, tri0); + shape1->getBulletTriangle(m_triface1, tri1); //collide two convex shapes - if(tri0.overlap_test_conservative(tri1)) + if (tri0.overlap_test_conservative(tri1)) { - convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1); + convex_vs_convex_collision(body0Wrap, body1Wrap, &tri0, &tri1); } - } shape0->unlockChildShapes(); @@ -386,10 +345,10 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW } void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); @@ -401,119 +360,105 @@ void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + while (pair_count--) { - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; - + m_triface1 = *(pair_pointer + 1); + pair_pointer += 2; - shape0->getPrimitiveTriangle(m_triface0,ptri0); - shape1->getPrimitiveTriangle(m_triface1,ptri1); + shape0->getPrimitiveTriangle(m_triface0, ptri0); + shape1->getPrimitiveTriangle(m_triface1, ptri1); - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_begin_gim02_tri_time(); - #endif +#endif ptri0.applyTransform(orgtrans0); ptri1.applyTransform(orgtrans1); - //build planes ptri0.buildTriPlane(); ptri1.buildTriPlane(); // test conservative - - - if(ptri0.overlap_test_conservative(ptri1)) + if (ptri0.overlap_test_conservative(ptri1)) { - if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) + if (ptri0.find_triangle_collision_clip_method(ptri1, contact_data)) { - int j = contact_data.m_point_count; - while(j--) + while (j--) { - addContactPoint(body0Wrap, body1Wrap, - contact_data.m_points[j], - contact_data.m_separating_normal, - -contact_data.m_penetration_depth); + contact_data.m_points[j], + contact_data.m_separating_normal, + -contact_data.m_penetration_depth); } } } - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_end_gim02_tri_time(); - #endif - +#endif } shape0->unlockChildShapes(); shape1->unlockChildShapes(); - } - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1) { - - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); + const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0); m_part0 = meshshape0->getMeshPartCount(); - while(m_part0--) + while (m_part0--) { - gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, meshshape0->getMeshPart(m_part0), shape1); } return; } - if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1); + const btGImpactMeshShape* meshshape1 = static_cast<const btGImpactMeshShape*>(shape1); m_part1 = meshshape1->getMeshPartCount(); - while(m_part1--) + while (m_part1--) { - - gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1)); - + gimpact_vs_gimpact(body0Wrap, body1Wrap, shape0, meshshape1->getMeshPart(m_part1)); } return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btPairSet pairset; - gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); + gimpact_vs_gimpact_find_pairs(orgtrans0, orgtrans1, shape0, shape1, pairset); - if(pairset.size()== 0) return; + if (pairset.size() == 0) return; - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) { - const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0); - const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1); - //specialized function - #ifdef BULLET_TRIANGLE_COLLISION - collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #else - collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #endif + const btGImpactMeshShapePart* shapepart0 = static_cast<const btGImpactMeshShapePart*>(shape0); + const btGImpactMeshShapePart* shapepart1 = static_cast<const btGImpactMeshShapePart*>(shape1); +//specialized function +#ifdef BULLET_TRIANGLE_COLLISION + collide_gjk_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#else + collide_sat_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#endif return; } @@ -530,32 +475,32 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( bool child_has_transform1 = shape1->childrenHasTransform(); int i = pairset.size(); - while(i--) + while (i--) { - GIM_PAIR * pair = &pairset[i]; + GIM_PAIR* pair = &pairset[i]; m_triface0 = pair->m_index1; m_triface1 = pair->m_index2; - const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); - const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); + const btCollisionShape* colshape0 = retriever0.getChildShape(m_triface0); + const btCollisionShape* colshape1 = retriever1.getChildShape(m_triface1); btTransform tr0 = body0Wrap->getWorldTransform(); btTransform tr1 = body1Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(m_triface0); + tr0 = orgtrans0 * shape0->getChildTransform(m_triface0); } - if(child_has_transform1) + if (child_has_transform1) { - tr1 = orgtrans1*shape1->getChildTransform(m_triface1); + tr1 = orgtrans1 * shape1->getChildTransform(m_triface1); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1); + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), tr0, m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), tr1, m_part1, m_triface1); //collide two convex shapes - convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1); + convex_vs_convex_collision(&ob0, &ob1, colshape0, colshape1); } shape0->unlockChildShapes(); @@ -563,159 +508,159 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( } void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, bool swapped) { - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); + const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0); int& part = swapped ? m_part1 : m_part0; part = meshshape0->getMeshPartCount(); - while(part--) + while (part--) { - gimpact_vs_shape(body0Wrap, - body1Wrap, - meshshape0->getMeshPart(part), - shape1,swapped); - + body1Wrap, + meshshape0->getMeshPart(part), + shape1, swapped); } return; } - #ifdef GIMPACT_VS_PLANE_COLLISION - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && +#ifdef GIMPACT_VS_PLANE_COLLISION + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) { - const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0); - const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1); - gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped); + const btGImpactMeshShapePart* shapepart = static_cast<const btGImpactMeshShapePart*>(shape0); + const btStaticPlaneShape* planeshape = static_cast<const btStaticPlaneShape*>(shape1); + gimpacttrimeshpart_vs_plane_collision(body0Wrap, body1Wrap, shapepart, planeshape, swapped); return; } - #endif - +#endif - - if(shape1->isCompound()) + if (shape1->isCompound()) { - const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1); - gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped); + const btCompoundShape* compoundshape = static_cast<const btCompoundShape*>(shape1); + gimpact_vs_compoundshape(body0Wrap, body1Wrap, shape0, compoundshape, swapped); return; } - else if(shape1->isConcave()) + else if (shape1->isConcave()) { - const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1); - gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped); + const btConcaveShape* concaveshape = static_cast<const btConcaveShape*>(shape1); + gimpact_vs_concave(body0Wrap, body1Wrap, shape0, concaveshape, swapped); return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btAlignedObjectArray<int> collided_results; - gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results); - - if(collided_results.size() == 0) return; + gimpact_vs_shape_find_pairs(orgtrans0, orgtrans1, shape0, shape1, collided_results); + if (collided_results.size() == 0) return; shape0->lockChildShapes(); GIM_ShapeRetriever retriever0(shape0); - bool child_has_transform0 = shape0->childrenHasTransform(); - int i = collided_results.size(); - while(i--) + while (i--) { int child_index = collided_results[i]; - if(swapped) - m_triface1 = child_index; - else - m_triface0 = child_index; + if (swapped) + m_triface1 = child_index; + else + m_triface0 = child_index; - const btCollisionShape * colshape0 = retriever0.getChildShape(child_index); + const btCollisionShape* colshape0 = retriever0.getChildShape(child_index); btTransform tr0 = body0Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(child_index); + tr0 = orgtrans0 * shape0->getChildTransform(child_index); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); - const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap(); - - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject()) + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); + const btCollisionObjectWrapper* prevObj; + + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject()) { + prevObj = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&ob0); - } else + } + else { + prevObj = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&ob0); } //collide two shapes - if(swapped) + if (swapped) { - - shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0); + shape_vs_shape_collision(body1Wrap, &ob0, shape1, colshape0); } else { - - shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1); + shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1); } - m_resultOut->setBody0Wrap(prevObj0); + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject()) + { + m_resultOut->setBody0Wrap(prevObj); + } + else + { + m_resultOut->setBody1Wrap(prevObj); + } } shape0->unlockChildShapes(); - } void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCompoundShape* shape1, bool swapped) { btTransform orgtrans1 = body1Wrap->getWorldTransform(); int i = shape1->getNumChildShapes(); - while(i--) + while (i--) { + const btCollisionShape* colshape1 = shape1->getChildShape(i); + btTransform childtrans1 = orgtrans1 * shape1->getChildTransform(i); - const btCollisionShape * colshape1 = shape1->getChildShape(i); - btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), childtrans1, -1, i); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i); - const btCollisionObjectWrapper* tmp = 0; - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { tmp = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&ob1); - } else + } + else { tmp = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&ob1); } //collide child shape gimpact_vs_shape(body0Wrap, &ob1, - shape0,colshape1,swapped); + shape0, colshape1, swapped); - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { m_resultOut->setBody0Wrap(tmp); - } else + } + else { m_resultOut->setBody1Wrap(tmp); } @@ -723,27 +668,25 @@ void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObje } void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btStaticPlaneShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btStaticPlaneShape* shape1, bool swapped) { - - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); - const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1); + const btPlaneShape* planeshape = static_cast<const btPlaneShape*>(shape1); btVector4 plane; - planeshape->get_plane_equation_transformed(orgtrans1,plane); + planeshape->get_plane_equation_transformed(orgtrans1, plane); //test box against plane btAABB tribox; - shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); + shape0->getAabb(orgtrans0, tribox.m_min, tribox.m_max); tribox.increment_margin(planeshape->getMargin()); - if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; + if (tribox.plane_classify(plane) != BT_CONST_COLLIDE_PLANE) return; shape0->lockChildShapes(); @@ -751,28 +694,28 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( btVector3 vertex; int vi = shape0->getVertexCount(); - while(vi--) + while (vi--) { - shape0->getVertex(vi,vertex); + shape0->getVertex(vi, vertex); vertex = orgtrans0(vertex); btScalar distance = vertex.dot(plane) - plane[3] - margin; - if(distance<0.0)//add contact + if (distance < 0.0) //add contact { - if(swapped) + if (swapped) { addContactPoint(body1Wrap, body0Wrap, - vertex, - -plane, - distance); + vertex, + -plane, + distance); } else { addContactPoint(body0Wrap, body1Wrap, - vertex, - plane, - distance); + vertex, + plane, + distance); } } } @@ -780,69 +723,64 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( shape0->unlockChildShapes(); } - - - -class btGImpactTriangleCallback: public btTriangleCallback +class btGImpactTriangleCallback : public btTriangleCallback { public: - btGImpactCollisionAlgorithm * algorithm; - const btCollisionObjectWrapper * body0Wrap; - const btCollisionObjectWrapper * body1Wrap; - const btGImpactShapeInterface * gimpactshape0; + btGImpactCollisionAlgorithm* algorithm; + const btCollisionObjectWrapper* body0Wrap; + const btCollisionObjectWrapper* body1Wrap; + const btGImpactShapeInterface* gimpactshape0; bool swapped; btScalar margin; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); + btTriangleShapeEx tri1(triangle[0], triangle[1], triangle[2]); tri1.setMargin(margin); - if(swapped) - { - algorithm->setPart0(partId); - algorithm->setFace0(triangleIndex); - } - else - { - algorithm->setPart1(partId); - algorithm->setFace1(triangleIndex); - } - - btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex); - const btCollisionObjectWrapper * tmp = 0; - - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + if (swapped) + { + algorithm->setPart0(partId); + algorithm->setFace0(triangleIndex); + } + else + { + algorithm->setPart1(partId); + algorithm->setFace1(triangleIndex); + } + + btCollisionObjectWrapper ob1Wrap(body1Wrap, &tri1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), partId, triangleIndex); + const btCollisionObjectWrapper* tmp = 0; + + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { tmp = algorithm->internalGetResultOut()->getBody0Wrap(); algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap); - } else + } + else { tmp = algorithm->internalGetResultOut()->getBody1Wrap(); algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap); } - + algorithm->gimpact_vs_shape( - body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped); + body0Wrap, &ob1Wrap, gimpactshape0, &tri1, swapped); - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { algorithm->internalGetResultOut()->setBody0Wrap(tmp); - } else + } + else { algorithm->internalGetResultOut()->setBody1Wrap(tmp); } - } }; - - - void btGImpactCollisionAlgorithm::gimpact_vs_concave( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btConcaveShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btConcaveShape* shape1, bool swapped) { //create the callback btGImpactTriangleCallback tricallback; @@ -858,75 +796,71 @@ void btGImpactCollisionAlgorithm::gimpact_vs_concave( gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform(); - btVector3 minAABB,maxAABB; - shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); - - shape1->processAllTriangles(&tricallback,minAABB,maxAABB); + btVector3 minAABB, maxAABB; + shape0->getAabb(gimpactInConcaveSpace, minAABB, maxAABB); + shape1->processAllTriangles(&tricallback, minAABB, maxAABB); } - - -void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btGImpactCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - clearCache(); + clearCache(); - m_resultOut = resultOut; + m_resultOut = resultOut; m_dispatchInfo = &dispatchInfo; - const btGImpactShapeInterface * gimpactshape0; - const btGImpactShapeInterface * gimpactshape1; + const btGImpactShapeInterface* gimpactshape0; + const btGImpactShapeInterface* gimpactshape1; - if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) + if (body0Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape()); + gimpactshape0 = static_cast<const btGImpactShapeInterface*>(body0Wrap->getCollisionShape()); - if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape()); - gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, gimpactshape0, gimpactshape1); } else { - gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false); + gimpact_vs_shape(body0Wrap, body1Wrap, gimpactshape0, body1Wrap->getCollisionShape(), false); } - } - else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + else if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape()); - gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true); + gimpact_vs_shape(body1Wrap, body0Wrap, gimpactshape1, body0Wrap->getCollisionShape(), true); } -} + // Ensure that gContactProcessedCallback is called for concave shapes. + if (getLastManifold()) + { + m_resultOut->refreshContactPoints(); + } +} -btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { return 1.f; - } ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// - - //! Use this function for register the algorithm externally -void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) +void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher* dispatcher) { - static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf; int i; - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE, i, &s_gimpact_cf); } - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(i, GIMPACT_SHAPE_PROXYTYPE, &s_gimpact_cf); } - } |