| /* |
| This source file is part of GIMPACT Library. |
| |
| For the latest info, see http://gimpact.sourceforge.net/ |
| |
| Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. |
| email: projectileman@yahoo.com |
| |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| /* |
| Author: Francisco Len Nßjera |
| Concave-Concave Collision |
| |
| */ |
| |
| #include "BulletCollision/CollisionDispatch/btManifoldResult.h" |
| #include "LinearMath/btIDebugDraw.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionObject.h" |
| #include "BulletCollision/CollisionShapes/btBoxShape.h" |
| #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h" |
| #include "BulletCollision/Gimpact/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) |
| { |
| } |
| |
| void get_plane_equation(btVector4 &equation) |
| { |
| equation[0] = m_planeNormal[0]; |
| equation[1] = m_planeNormal[1]; |
| equation[2] = m_planeNormal[2]; |
| equation[3] = m_planeConstant; |
| } |
| |
| |
| void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) |
| { |
| 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; |
| } |
| }; |
| |
| |
| |
| ////////////////////////////////////////////////////////////////////////////////////////////// |
| #ifdef TRI_COLLISION_PROFILING |
| |
| btClock g_triangle_clock; |
| |
| float g_accum_triangle_collision_time = 0; |
| int g_count_triangle_collision = 0; |
| |
| void bt_begin_gim02_tri_time() |
| { |
| g_triangle_clock.reset(); |
| } |
| |
| void bt_end_gim02_tri_time() |
| { |
| g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); |
| g_count_triangle_collision++; |
| } |
| #endif //TRI_COLLISION_PROFILING |
| //! Retrieving shapes shapes |
| /*! |
| Declared here due of insuficent space on Pool allocators |
| */ |
| //!@{ |
| class GIM_ShapeRetriever |
| { |
| public: |
| btGImpactShapeInterface * m_gim_shape; |
| btTriangleShapeEx m_trishape; |
| btTetrahedronShapeEx m_tetrashape; |
| |
| public: |
| class ChildShapeRetriever |
| { |
| public: |
| GIM_ShapeRetriever * m_parent; |
| virtual btCollisionShape * getChildShape(int index) |
| { |
| return m_parent->m_gim_shape->getChildShape(index); |
| } |
| virtual ~ChildShapeRetriever() {} |
| }; |
| |
| class TriangleShapeRetriever:public ChildShapeRetriever |
| { |
| public: |
| |
| virtual btCollisionShape * getChildShape(int index) |
| { |
| m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); |
| return &m_parent->m_trishape; |
| } |
| virtual ~TriangleShapeRetriever() {} |
| }; |
| |
| class TetraShapeRetriever:public ChildShapeRetriever |
| { |
| public: |
| |
| virtual btCollisionShape * getChildShape(int index) |
| { |
| 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; |
| |
| GIM_ShapeRetriever(btGImpactShapeInterface * gim_shape) |
| { |
| m_gim_shape = gim_shape; |
| //select retriever |
| if(m_gim_shape->needsRetrieveTriangles()) |
| { |
| m_current_retriever = &m_tri_retriever; |
| } |
| else if(m_gim_shape->needsRetrieveTetrahedrons()) |
| { |
| m_current_retriever = &m_tetra_retriever; |
| } |
| else |
| { |
| m_current_retriever = &m_child_retriever; |
| } |
| |
| m_current_retriever->m_parent = this; |
| } |
| |
| 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; |
| |
| float avgtime = g_accum_triangle_collision_time; |
| avgtime /= (float)g_count_triangle_collision; |
| |
| g_accum_triangle_collision_time = 0; |
| g_count_triangle_collision = 0; |
| |
| return avgtime; |
| } |
| |
| #endif //TRI_COLLISION_PROFILING |
| |
| |
| |
| btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) |
| : btActivatingCollisionAlgorithm(ci,body0,body1) |
| { |
| m_manifoldPtr = NULL; |
| m_convex_algorithm = NULL; |
| } |
| |
| btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() |
| { |
| clearCache(); |
| } |
| |
| |
| |
| |
| |
| void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0, |
| btCollisionObject * body1, |
| const btVector3 & point, |
| const btVector3 & normal, |
| btScalar distance) |
| { |
| m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); |
| m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); |
| checkManifold(body0,body1); |
| m_resultOut->addContactPoint(normal,point,distance); |
| } |
| |
| |
| void btGImpactCollisionAlgorithm::shape_vs_shape_collision( |
| btCollisionObject * body0, |
| btCollisionObject * body1, |
| btCollisionShape * shape0, |
| btCollisionShape * shape1) |
| { |
| |
| btCollisionShape* tmpShape0 = body0->getCollisionShape(); |
| btCollisionShape* tmpShape1 = body1->getCollisionShape(); |
| |
| body0->internalSetTemporaryCollisionShape(shape0); |
| body1->internalSetTemporaryCollisionShape(shape1); |
| |
| { |
| btCollisionAlgorithm* algor = newAlgorithm(body0,body1); |
| // post : checkManifold is called |
| |
| m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); |
| m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); |
| |
| algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut); |
| |
| algor->~btCollisionAlgorithm(); |
| m_dispatcher->freeCollisionAlgorithm(algor); |
| } |
| |
| body0->internalSetTemporaryCollisionShape(tmpShape0); |
| body1->internalSetTemporaryCollisionShape(tmpShape1); |
| } |
| |
| void btGImpactCollisionAlgorithm::convex_vs_convex_collision( |
| btCollisionObject * body0, |
| btCollisionObject * body1, |
| btCollisionShape * shape0, |
| btCollisionShape * shape1) |
| { |
| |
| btCollisionShape* tmpShape0 = body0->getCollisionShape(); |
| btCollisionShape* tmpShape1 = body1->getCollisionShape(); |
| |
| body0->internalSetTemporaryCollisionShape(shape0); |
| body1->internalSetTemporaryCollisionShape(shape1); |
| |
| |
| m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); |
| m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); |
| |
| checkConvexAlgorithm(body0,body1); |
| m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut); |
| |
| body0->internalSetTemporaryCollisionShape(tmpShape0); |
| body1->internalSetTemporaryCollisionShape(tmpShape1); |
| |
| } |
| |
| |
| |
| |
| void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( |
| const btTransform & trans0, |
| const btTransform & trans1, |
| btGImpactShapeInterface * shape0, |
| btGImpactShapeInterface * shape1,btPairSet & pairset) |
| { |
| if(shape0->hasBoxSet() && shape1->hasBoxSet()) |
| { |
| btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); |
| } |
| else |
| { |
| btAABB boxshape0; |
| btAABB boxshape1; |
| int i = shape0->getNumChildShapes(); |
| |
| while(i--) |
| { |
| shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); |
| |
| int j = shape1->getNumChildShapes(); |
| while(j--) |
| { |
| shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); |
| |
| if(boxshape1.has_collision(boxshape0)) |
| { |
| pairset.push_pair(i,j); |
| } |
| } |
| } |
| } |
| |
| |
| } |
| |
| |
| void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( |
| const btTransform & trans0, |
| const btTransform & trans1, |
| btGImpactShapeInterface * shape0, |
| btCollisionShape * shape1, |
| btAlignedObjectArray<int> & collided_primitives) |
| { |
| |
| btAABB boxshape; |
| |
| |
| if(shape0->hasBoxSet()) |
| { |
| btTransform trans1to0 = trans0.inverse(); |
| trans1to0 *= trans1; |
| |
| 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); |
| |
| btAABB boxshape0; |
| int i = shape0->getNumChildShapes(); |
| |
| while(i--) |
| { |
| shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); |
| |
| if(boxshape.has_collision(boxshape0)) |
| { |
| collided_primitives.push_back(i); |
| } |
| } |
| |
| } |
| |
| } |
| |
| |
| void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0, |
| btCollisionObject * body1, |
| btGImpactMeshShapePart * shape0, |
| btGImpactMeshShapePart * shape1, |
| const int * pairs, int pair_count) |
| { |
| btTriangleShapeEx tri0; |
| btTriangleShapeEx tri1; |
| |
| shape0->lockChildShapes(); |
| shape1->lockChildShapes(); |
| |
| const int * pair_pointer = pairs; |
| |
| 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); |
| |
| |
| //collide two convex shapes |
| if(tri0.overlap_test_conservative(tri1)) |
| { |
| convex_vs_convex_collision(body0,body1,&tri0,&tri1); |
| } |
| |
| } |
| |
| shape0->unlockChildShapes(); |
| shape1->unlockChildShapes(); |
| } |
| |
| void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0, |
| btCollisionObject * body1, |
| btGImpactMeshShapePart * shape0, |
| btGImpactMeshShapePart * shape1, |
| const int * pairs, int pair_count) |
| { |
| btTransform orgtrans0 = body0->getWorldTransform(); |
| btTransform orgtrans1 = body1->getWorldTransform(); |
| |
| btPrimitiveTriangle ptri0; |
| btPrimitiveTriangle ptri1; |
| GIM_TRIANGLE_CONTACT contact_data; |
| |
| shape0->lockChildShapes(); |
| shape1->lockChildShapes(); |
| |
| const int * pair_pointer = pairs; |
| |
| while(pair_count--) |
| { |
| |
| m_triface0 = *(pair_pointer); |
| m_triface1 = *(pair_pointer+1); |
| pair_pointer+=2; |
| |
| |
| shape0->getPrimitiveTriangle(m_triface0,ptri0); |
| shape1->getPrimitiveTriangle(m_triface1,ptri1); |
| |
| #ifdef TRI_COLLISION_PROFILING |
| bt_begin_gim02_tri_time(); |
| #endif |
| |
| ptri0.applyTransform(orgtrans0); |
| ptri1.applyTransform(orgtrans1); |
| |
| |
| //build planes |
| ptri0.buildTriPlane(); |
| ptri1.buildTriPlane(); |
| // test conservative |
| |
| |
| |
| if(ptri0.overlap_test_conservative(ptri1)) |
| { |
| if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) |
| { |
| |
| int j = contact_data.m_point_count; |
| while(j--) |
| { |
| |
| addContactPoint(body0, body1, |
| contact_data.m_points[j], |
| contact_data.m_separating_normal, |
| -contact_data.m_penetration_depth); |
| } |
| } |
| } |
| |
| #ifdef TRI_COLLISION_PROFILING |
| bt_end_gim02_tri_time(); |
| #endif |
| |
| } |
| |
| shape0->unlockChildShapes(); |
| shape1->unlockChildShapes(); |
| |
| } |
| |
| |
| void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( |
| btCollisionObject * body0, |
| btCollisionObject * body1, |
| btGImpactShapeInterface * shape0, |
| btGImpactShapeInterface * shape1) |
| { |
| |
| if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) |
| { |
| btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0); |
| m_part0 = meshshape0->getMeshPartCount(); |
| |
| while(m_part0--) |
| { |
| gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1); |
| } |
| |
| return; |
| } |
| |
| if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) |
| { |
| btGImpactMeshShape * meshshape1 = static_cast<btGImpactMeshShape *>(shape1); |
| m_part1 = meshshape1->getMeshPartCount(); |
| |
| while(m_part1--) |
| { |
| |
| gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1)); |
| |
| } |
| |
| return; |
| } |
| |
| |
| btTransform orgtrans0 = body0->getWorldTransform(); |
| btTransform orgtrans1 = body1->getWorldTransform(); |
| |
| btPairSet pairset; |
| |
| gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); |
| |
| if(pairset.size()== 0) return; |
| |
| if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && |
| shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) |
| { |
| btGImpactMeshShapePart * shapepart0 = static_cast<btGImpactMeshShapePart * >(shape0); |
| btGImpactMeshShapePart * shapepart1 = static_cast<btGImpactMeshShapePart * >(shape1); |
| //specialized function |
| #ifdef BULLET_TRIANGLE_COLLISION |
| collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); |
| #else |
| collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); |
| #endif |
| |
| return; |
| } |
| |
| //general function |
| |
| shape0->lockChildShapes(); |
| shape1->lockChildShapes(); |
| |
| GIM_ShapeRetriever retriever0(shape0); |
| GIM_ShapeRetriever retriever1(shape1); |
| |
| bool child_has_transform0 = shape0->childrenHasTransform(); |
| bool child_has_transform1 = shape1->childrenHasTransform(); |
| |
| int i = pairset.size(); |
| while(i--) |
| { |
| GIM_PAIR * pair = &pairset[i]; |
| m_triface0 = pair->m_index1; |
| m_triface1 = pair->m_index2; |
| btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); |
| btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); |
| |
| if(child_has_transform0) |
| { |
| body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0)); |
| } |
| |
| if(child_has_transform1) |
| { |
| body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1)); |
| } |
| |
| //collide two convex shapes |
| convex_vs_convex_collision(body0,body1,colshape0,colshape1); |
| |
| |
| if(child_has_transform0) |
| { |
| body0->setWorldTransform(orgtrans0); |
| } |
| |
| if(child_has_transform1) |
| { |
| body1->setWorldTransform(orgtrans1); |
| } |
| |
| } |
| |
| shape0->unlockChildShapes(); |
| shape1->unlockChildShapes(); |
| } |
| |
| void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0, |
| btCollisionObject * body1, |
| btGImpactShapeInterface * shape0, |
| btCollisionShape * shape1,bool swapped) |
| { |
| if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) |
| { |
| btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0); |
| int& part = swapped ? m_part1 : m_part0; |
| part = meshshape0->getMeshPartCount(); |
| |
| while(part--) |
| { |
| |
| gimpact_vs_shape(body0, |
| body1, |
| meshshape0->getMeshPart(part), |
| shape1,swapped); |
| |
| } |
| |
| return; |
| } |
| |
| #ifdef GIMPACT_VS_PLANE_COLLISION |
| if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && |
| shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) |
| { |
| btGImpactMeshShapePart * shapepart = static_cast<btGImpactMeshShapePart *>(shape0); |
| btStaticPlaneShape * planeshape = static_cast<btStaticPlaneShape * >(shape1); |
| gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped); |
| return; |
| } |
| |
| #endif |
| |
| |
| |
| if(shape1->isCompound()) |
| { |
| btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1); |
| gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped); |
| return; |
| } |
| else if(shape1->isConcave()) |
| { |
| btConcaveShape * concaveshape = static_cast<btConcaveShape *>(shape1); |
| gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped); |
| return; |
| } |
| |
| |
| btTransform orgtrans0 = body0->getWorldTransform(); |
| |
| btTransform orgtrans1 = body1->getWorldTransform(); |
| |
| btAlignedObjectArray<int> collided_results; |
| |
| 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--) |
| { |
| int child_index = collided_results[i]; |
| if(swapped) |
| m_triface1 = child_index; |
| else |
| m_triface0 = child_index; |
| |
| btCollisionShape * colshape0 = retriever0.getChildShape(child_index); |
| |
| if(child_has_transform0) |
| { |
| body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index)); |
| } |
| |
| //collide two shapes |
| if(swapped) |
| { |
| shape_vs_shape_collision(body1,body0,shape1,colshape0); |
| } |
| else |
| { |
| shape_vs_shape_collision(body0,body1,colshape0,shape1); |
| } |
| |
| //restore transforms |
| if(child_has_transform0) |
| { |
| body0->setWorldTransform(orgtrans0); |
| } |
| |
| } |
| |
| shape0->unlockChildShapes(); |
| |
| } |
| |
| void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0, |
| btCollisionObject * body1, |
| btGImpactShapeInterface * shape0, |
| btCompoundShape * shape1,bool swapped) |
| { |
| btTransform orgtrans1 = body1->getWorldTransform(); |
| |
| int i = shape1->getNumChildShapes(); |
| while(i--) |
| { |
| |
| btCollisionShape * colshape1 = shape1->getChildShape(i); |
| btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); |
| |
| body1->setWorldTransform(childtrans1); |
| |
| //collide child shape |
| gimpact_vs_shape(body0, body1, |
| shape0,colshape1,swapped); |
| |
| |
| //restore transforms |
| body1->setWorldTransform(orgtrans1); |
| } |
| } |
| |
| void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( |
| btCollisionObject * body0, |
| btCollisionObject * body1, |
| btGImpactMeshShapePart * shape0, |
| btStaticPlaneShape * shape1,bool swapped) |
| { |
| |
| |
| btTransform orgtrans0 = body0->getWorldTransform(); |
| btTransform orgtrans1 = body1->getWorldTransform(); |
| |
| btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1); |
| btVector4 plane; |
| planeshape->get_plane_equation_transformed(orgtrans1,plane); |
| |
| //test box against plane |
| |
| btAABB tribox; |
| shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); |
| tribox.increment_margin(planeshape->getMargin()); |
| |
| if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; |
| |
| shape0->lockChildShapes(); |
| |
| btScalar margin = shape0->getMargin() + planeshape->getMargin(); |
| |
| btVector3 vertex; |
| int vi = shape0->getVertexCount(); |
| while(vi--) |
| { |
| shape0->getVertex(vi,vertex); |
| vertex = orgtrans0(vertex); |
| |
| btScalar distance = vertex.dot(plane) - plane[3] - margin; |
| |
| if(distance<0.0)//add contact |
| { |
| if(swapped) |
| { |
| addContactPoint(body1, body0, |
| vertex, |
| -plane, |
| distance); |
| } |
| else |
| { |
| addContactPoint(body0, body1, |
| vertex, |
| plane, |
| distance); |
| } |
| } |
| } |
| |
| shape0->unlockChildShapes(); |
| } |
| |
| |
| |
| |
| class btGImpactTriangleCallback: public btTriangleCallback |
| { |
| public: |
| btGImpactCollisionAlgorithm * algorithm; |
| btCollisionObject * body0; |
| btCollisionObject * body1; |
| btGImpactShapeInterface * gimpactshape0; |
| bool swapped; |
| btScalar margin; |
| |
| virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) |
| { |
| 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); |
| } |
| algorithm->gimpact_vs_shape( |
| body0,body1,gimpactshape0,&tri1,swapped); |
| } |
| }; |
| |
| |
| |
| |
| void btGImpactCollisionAlgorithm::gimpact_vs_concave( |
| btCollisionObject * body0, |
| btCollisionObject * body1, |
| btGImpactShapeInterface * shape0, |
| btConcaveShape * shape1,bool swapped) |
| { |
| //create the callback |
| btGImpactTriangleCallback tricallback; |
| tricallback.algorithm = this; |
| tricallback.body0 = body0; |
| tricallback.body1 = body1; |
| tricallback.gimpactshape0 = shape0; |
| tricallback.swapped = swapped; |
| tricallback.margin = shape1->getMargin(); |
| |
| //getting the trimesh AABB |
| btTransform gimpactInConcaveSpace; |
| |
| gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform(); |
| |
| btVector3 minAABB,maxAABB; |
| shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); |
| |
| shape1->processAllTriangles(&tricallback,minAABB,maxAABB); |
| |
| } |
| |
| |
| |
| void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) |
| { |
| clearCache(); |
| |
| m_resultOut = resultOut; |
| m_dispatchInfo = &dispatchInfo; |
| btGImpactShapeInterface * gimpactshape0; |
| btGImpactShapeInterface * gimpactshape1; |
| |
| if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) |
| { |
| gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape()); |
| |
| if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) |
| { |
| gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape()); |
| |
| gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1); |
| } |
| else |
| { |
| gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false); |
| } |
| |
| } |
| else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) |
| { |
| gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape()); |
| |
| gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true); |
| } |
| } |
| |
| |
| btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) |
| { |
| return 1.f; |
| |
| } |
| |
| ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// |
| |
| btGImpactCollisionAlgorithm::CreateFunc g_gimpact_cf; |
| |
| //! Use this function for register the algorithm externally |
| void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) |
| { |
| |
| int i; |
| |
| for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) |
| { |
| dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&g_gimpact_cf); |
| } |
| |
| for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) |
| { |
| dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&g_gimpact_cf); |
| } |
| |
| } |