diff --git a/Extras/GIMPACT/include/GIMPACT/gim_geometry.h b/Extras/GIMPACT/include/GIMPACT/gim_geometry.h index e91bbcfea..7fe3c13e0 100644 --- a/Extras/GIMPACT/include/GIMPACT/gim_geometry.h +++ b/Extras/GIMPACT/include/GIMPACT/gim_geometry.h @@ -1042,6 +1042,16 @@ Last column is added as the position p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \ }\ +//! Transform a plane +#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\ +{ \ + pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1] + m[0][2]*plane[2];\ + pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1] + m[1][2]*plane[2];\ + pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1] + m[2][2]*plane[2];\ + pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1] + m[2][3]*pout[2] + plane[3];\ +}\ + + /** inverse transpose of matrix times vector * diff --git a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp b/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp index a768fd5ee..5e0163eb9 100644 --- a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp +++ b/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp @@ -24,9 +24,44 @@ Concave-Concave Collision #include "btConcaveConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "btGIMPACTMeshShape.h" -#include "../Extras/GIMPACT/include/GIMPACT/gimpact.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" +#include "GIMPACT/gimpact.h" + +//! Class for accessing the plane ecuation +class btPlaneShape : public btStaticPlaneShape +{ +public: + void get_plane_equation(float equation[4]) + { + 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,float equation[4]) + { + /*mat4f plane_trans; + IDENTIFY_MATRIX_4X4(plane_trans); + COPY_MATRIX_3X3(plane_trans,trans.getBasis()); + MAT_SET_TRANSLATION(plane_trans,trans.getOrigin()); + + float ptemp[4] + //vec4f ptemp; + get_plane_equation(ptemp); + + MAT_TRANSFORM_PLANE_4X4(equation,plane_trans,ptemp);*/ + + 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; + } +}; + btConcaveConcaveCollisionAlgorithm::btConcaveConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) : btCollisionAlgorithm(ci) { @@ -61,70 +96,371 @@ void process_gimpact_contacts(GIM_CONTACT * pcontacts, btConcaveConcaveCollisionAlgorithm * algorithm, btCollisionObject* body0, btCollisionObject* body1, - btManifoldResult* resultOut) + btManifoldResult* resultOut, bool swapped = false) { int i, ci = MANIFOLD_CACHE_SIZE;//Max point size btPersistentManifold * current_mainfold = 0; + btCollisionObject* pbody0 = swapped?body1:body0; + btCollisionObject* pbody1 = swapped?body0:body1; + + float csign = swapped?-1.f:1.f; + + btVector3 cpoint; + btVector3 cnormal; + for(i=0;i=MANIFOLD_CACHE_SIZE) { - current_mainfold = algorithm->newContactMainfold(body0,body1); + current_mainfold = algorithm->newContactMainfold(pbody0,pbody1); resultOut->setPersistentManifold(current_mainfold); ci=0; } - - btVector3 cpoint(pcontacts->m_point[0],pcontacts->m_point[1],pcontacts->m_point[2]); + + cpoint.setValue(pcontacts->m_point[0],pcontacts->m_point[1],pcontacts->m_point[2]); //Normal points to body0 - btVector3 cnormal(pcontacts->m_normal[0],pcontacts->m_normal[1],pcontacts->m_normal[2]); - + cnormal.setValue(csign*pcontacts->m_normal[0],csign*pcontacts->m_normal[1],csign*pcontacts->m_normal[2]); + resultOut->addContactPoint(cnormal,cpoint,-pcontacts->m_depth); pcontacts++; ci++; } - } + +void process_gimpact_plane_contacts(vec4f * pcontacts, + vec4f planenormal, + int contact_count, + btConcaveConcaveCollisionAlgorithm * algorithm, + btCollisionObject* body0, + btCollisionObject* body1, + btManifoldResult* resultOut, bool swapped = false) +{ + int i, ci = MANIFOLD_CACHE_SIZE;//Max point size + btPersistentManifold * current_mainfold = 0; + + btCollisionObject* pbody0 = swapped?body1:body0; + btCollisionObject* pbody1 = swapped?body0:body1; + + float csign = swapped?-1.f:1.f; + + btVector3 cpoint; + btVector3 cnormal; + + for(i=0;i=MANIFOLD_CACHE_SIZE) + { + current_mainfold = algorithm->newContactMainfold(pbody0,pbody1); + resultOut->setPersistentManifold(current_mainfold); + ci=0; + } + + cpoint.setValue(pcontacts[i][0],pcontacts[i][1],pcontacts[i][2]); + //Normal points to body0 + cnormal.setValue(csign*planenormal[0],csign*planenormal[1],csign*planenormal[2]); + + resultOut->addContactPoint(cnormal,cpoint,-pcontacts[i][3]); + + ci++; + } +} + +class CONCAVE_TRIANGLE_TOKEN +{ +public: + GIM_TRIANGLE_DATA m_tridata; + int partId; + int triangleIndex; + + CONCAVE_TRIANGLE_TOKEN() + { + m_tridata.m_has_planes = 0; + partId = 0; + triangleIndex = 0; + } + + CONCAVE_TRIANGLE_TOKEN(const CONCAVE_TRIANGLE_TOKEN & token) + { + m_tridata.m_has_planes = 0; + VEC_COPY(m_tridata.m_vertices[0],token.m_tridata.m_vertices[0]); + VEC_COPY(m_tridata.m_vertices[1],token.m_tridata.m_vertices[1]); + VEC_COPY(m_tridata.m_vertices[2],token.m_tridata.m_vertices[2]); + partId = token.partId; + triangleIndex = token.triangleIndex; + } + +}; + + + + +void bt_gimpact_gimpact_collision(btConcaveConcaveCollisionAlgorithm * algorithm, + btCollisionObject* body0, + btCollisionObject* body1, + btManifoldResult* resultOut) +{ + btGIMPACTMeshShape* tri0b = static_cast( body0->getCollisionShape()); + btGIMPACTMeshShape* tri1b = static_cast( body1->getCollisionShape()); + + tri0b->prepareMeshes(body0->getWorldTransform()); + tri1b->prepareMeshes(body1->getWorldTransform()); + + size_t i,j; + size_t parts0 = tri0b->m_gim_trimesh_parts.size(); + size_t parts1 = tri1b->m_gim_trimesh_parts.size(); + + GIM_TRIMESH * trimesh0; + GIM_TRIMESH * trimesh1; + GDYNAMIC_ARRAY contacts; + GIM_CONTACT * pcontacts; + for(i=0;im_gim_trimesh_parts[i]; + trimesh1 = (GIM_TRIMESH * )tri1b->m_gim_trimesh_parts[j]; + + GIM_CREATE_CONTACT_LIST(contacts); + + gim_trimesh_trimesh_collision(trimesh0,trimesh1,&contacts); + + if(contacts.m_size>0) + { + pcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,contacts); + process_gimpact_contacts(pcontacts,contacts.m_size,algorithm,body0,body1,resultOut); + } + GIM_DYNARRAY_DESTROY(contacts); + } + } +} + +void bt_gimpact_plane_collision(btConcaveConcaveCollisionAlgorithm * algorithm, + btCollisionObject* tribody0, + btCollisionObject* planebody1, + btManifoldResult* resultOut,bool swapped) +{ + btGIMPACTMeshShape* tri0b = static_cast( tribody0->getCollisionShape()); + btPlaneShape * plane1b = static_cast( planebody1->getCollisionShape()); + + tri0b->prepareMeshes(tribody0->getWorldTransform()); + + ////////////////////////////////Getting plane//////////////////////////////////// + + vec4f pnormal; + plane1b->get_plane_equation_transformed(planebody1->getWorldTransform(),pnormal); + + ////////////////////////////////End Getting plane//////////////////////////////////// + + size_t i; + size_t parts0 = tri0b->m_gim_trimesh_parts.size(); + + + GIM_TRIMESH * trimesh0; + + GDYNAMIC_ARRAY contacts; + vec4f * pcontacts; + for(i=0;im_gim_trimesh_parts[i]; + + GIM_CREATE_TRIMESHPLANE_CONTACTS(contacts); + + gim_trimesh_plane_collision(trimesh0,pnormal,&contacts); + + if(contacts.m_size>0) + { + pcontacts = GIM_DYNARRAY_POINTER(vec4f,contacts); + + process_gimpact_plane_contacts(pcontacts,pnormal, + contacts.m_size,algorithm,tribody0,planebody1,resultOut,swapped); + } + GIM_DYNARRAY_DESTROY(contacts); + } +} + + + +///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. +class btConcaveTriangleCallback : public btTriangleCallback +{ + +public: + btCollisionObject* m_body; + mat4f m_transform; + std::vector m_triangles; + + btConcaveTriangleCallback(btCollisionObject* body) + { + m_body = body; + IDENTIFY_MATRIX_4X4(m_transform); + COPY_MATRIX_3X3(m_transform,body->getWorldTransform().getBasis()); + MAT_SET_TRANSLATION(m_transform,body->getWorldTransform().getOrigin()); + m_triangles.reserve(100); + } + + void setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) + {} + + virtual ~btConcaveTriangleCallback(){ + } + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + CONCAVE_TRIANGLE_TOKEN token; + + token.m_tridata.m_has_planes = 0; + token.partId = partId; + token.triangleIndex = triangleIndex; + //Copy vertices + MAT_DOT_VEC_3X4(token.m_tridata.m_vertices[0],m_transform,triangle[0]); + MAT_DOT_VEC_3X4(token.m_tridata.m_vertices[1],m_transform,triangle[1]); + MAT_DOT_VEC_3X4(token.m_tridata.m_vertices[2],m_transform,triangle[2]); + + m_triangles.push_back(token); + } + + void clearCache(){} +}; + + + +void bt_concave_concave_collision(btConcaveConcaveCollisionAlgorithm * algorithm, + btCollisionObject* tribody0, + btCollisionObject* tribody1, + btManifoldResult* resultOut) +{ + ConcaveShape* tri0b = static_cast( tribody0->getCollisionShape()); + ConcaveShape* tri1b = static_cast( tribody1->getCollisionShape()); + + //Get First AABB + btVector3 aabbMin0,aabbMax0; + tri0b->getAabb(tribody0->getWorldTransform(),aabbMin0,aabbMax0); + + //Get Second AABB + btVector3 aabbMin1,aabbMax1; + tri1b->getAabb(tribody1->getWorldTransform(),aabbMin1,aabbMax1); + + //Transform boxes to local spaces + aabb3f aabb0 = { + aabbMin0[0],aabbMax0[0], + aabbMin0[1],aabbMax0[1], + aabbMin0[2],aabbMax0[2], + }; + + aabb3f aabb1 = { + aabbMin1[0],aabbMax1[0], + aabbMin1[1],aabbMax1[1], + aabbMin1[2],aabbMax1[2], + }; + + mat4f transform; + IDENTIFY_MATRIX_4X4(transform); + + // body0 inverse transform + btTransform transinv = tribody0->getWorldTransform().inverse(); + COPY_MATRIX_3X3(transform,transinv.getBasis()); + MAT_SET_TRANSLATION(transform,transinv.getOrigin()); + + //Transform box1 to body0 space + AABB_TRANSFORM(aabb1,aabb1,transform); + + AABB_GET_MIN(aabb1,aabbMin1); + AABB_GET_MAX(aabb1,aabbMax1); + + btConcaveTriangleCallback callback0(tribody0); + tri0b->processAllTriangles(&callback0,aabbMin1,aabbMax1); + + if(callback0.m_triangles.size()==0) return; + // body1 inverse transform + transinv = tribody1->getWorldTransform().inverse(); + COPY_MATRIX_3X3(transform,transinv.getBasis()); + MAT_SET_TRANSLATION(transform,transinv.getOrigin()); + + //Transform box0 to body1 space + AABB_TRANSFORM(aabb0,aabb0,transform); + + AABB_GET_MIN(aabb0,aabbMin0); + AABB_GET_MAX(aabb0,aabbMax0); + + btConcaveTriangleCallback callback1(tribody1); + tri1b->processAllTriangles(&callback1,aabbMin0,aabbMax0); + if(callback1.m_triangles.size()==0) return; + + ////////////////////////////////Collide triangles//////////////////////////////////// + + //dummy contacts + GDYNAMIC_ARRAY dummycontacts; + GIM_CREATE_CONTACT_LIST(dummycontacts); + + //Auxiliary triangle data + GIM_TRIANGLE_CONTACT_DATA tri_contact_data; + + size_t i,j,ci; + int colresult; + + for(i=0;igetCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE && body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) { - btGIMPACTMeshShape* tri0b = static_cast( body0->getCollisionShape()); - btGIMPACTMeshShape* tri1b = static_cast( body1->getCollisionShape()); - - tri0b->prepareMeshes(body0->getWorldTransform()); - tri1b->prepareMeshes(body1->getWorldTransform()); - - size_t i,j; - size_t parts0 = tri0b->m_gim_trimesh_parts.size(); - size_t parts1 = tri1b->m_gim_trimesh_parts.size(); - - GIM_TRIMESH * trimesh0; - GIM_TRIMESH * trimesh1; - GDYNAMIC_ARRAY contacts; - GIM_CONTACT * pcontacts; - for(i=0;im_gim_trimesh_parts[i]; - trimesh1 = (GIM_TRIMESH * )tri1b->m_gim_trimesh_parts[j]; - - GIM_CREATE_CONTACT_LIST(contacts); - - gim_trimesh_trimesh_collision(trimesh0,trimesh1,&contacts); - - if(contacts.m_size>0) - { - pcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,contacts); - process_gimpact_contacts(pcontacts,contacts.m_size,this,body0,body1,resultOut); - } - GIM_DYNARRAY_DESTROY(contacts); - } - } + bt_gimpact_gimpact_collision(this,body0,body1,resultOut); + } + else if (body0->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE&& body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + { + bt_gimpact_plane_collision(this,body1,body0,resultOut,true); + } + else if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE&& body1->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) + { + bt_gimpact_plane_collision(this,body0,body1,resultOut,false); + } + else if(body0->getCollisionShape()->isConcave() && body1->getCollisionShape()->isConcave() ) + { + bt_concave_concave_collision(this,body0,body1,resultOut); } } @@ -134,3 +470,19 @@ float btConcaveConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObjec return 1.f; } + +///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// + + +//! Use this function for register the algorithm externally +void btConcaveConcaveCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) +{ + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE ,new btConcaveConcaveCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,STATIC_PLANE_PROXYTYPE ,new btConcaveConcaveCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(STATIC_PLANE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE ,new btConcaveConcaveCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,TRIANGLE_MESH_SHAPE_PROXYTYPE,new btConcaveConcaveCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(TRIANGLE_MESH_SHAPE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE,new btConcaveConcaveCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(STATIC_PLANE_PROXYTYPE,TRIANGLE_MESH_SHAPE_PROXYTYPE,new btConcaveConcaveCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(TRIANGLE_MESH_SHAPE_PROXYTYPE,STATIC_PLANE_PROXYTYPE,new btConcaveConcaveCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(TRIANGLE_MESH_SHAPE_PROXYTYPE,TRIANGLE_MESH_SHAPE_PROXYTYPE,new btConcaveConcaveCollisionAlgorithm::CreateFunc); +} diff --git a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h b/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h index 29ddc5632..2cb740bd2 100644 --- a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h +++ b/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h @@ -24,6 +24,8 @@ subject to the following restrictions: class btDispatcher; #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" + /// btConcaveConcaveCollisionAlgorithm supports collision between btBvhTriangleMeshShape shapes @@ -53,8 +55,11 @@ public: } }; + //! Use this function for register the algorithm externally + static void registerAlgorithm(btCollisionDispatcher * dispatcher); }; + #endif //BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/Extras/GIMPACTBullet/btGIMPACTMeshShape.cpp b/Extras/GIMPACTBullet/btGIMPACTMeshShape.cpp index 16cd409af..aca4a7e57 100644 --- a/Extras/GIMPACTBullet/btGIMPACTMeshShape.cpp +++ b/Extras/GIMPACTBullet/btGIMPACTMeshShape.cpp @@ -24,9 +24,9 @@ int g_gimpact_references = 0; void increase_gimpact_reference() { g_gimpact_references++; - + if(g_gimpact_references >1 ) return; - + gimpact_init(); } @@ -34,18 +34,18 @@ void increase_gimpact_reference() void decrease_gimpact_reference() { if(g_gimpact_references <=0 ) return; - g_gimpact_references--; - if(g_gimpact_references >0 ) return; + g_gimpact_references--; + if(g_gimpact_references >0 ) return; gimpact_terminate(); } /////////////////////////// btGIMPACTMeshData class///////////////////////////////////////////////////////////// void btGIMPACTMeshData::clearMeshParts() -{ +{ for(size_t i = 0;im_meshdata = meshdata; @@ -187,19 +187,21 @@ void btGIMPACTMeshShape::processMeshParts(btGIMPACTMeshData * meshdata) } btGIMPACTMeshShape::btGIMPACTMeshShape(btGIMPACTMeshData * meshdata) -{ +{ + m_scale.setValue(1.0f,1.0f,1.0f); processMeshParts(meshdata); + } btGIMPACTMeshShape::~btGIMPACTMeshShape() -{ - clearMeshParts(); +{ + clearMeshParts(); } void btGIMPACTMeshShape::prepareMeshes(const btTransform & trans) const -{ +{ mat4f gim_trans; - IDENTIFY_MATRIX_4X4(gim_trans); + IDENTIFY_MATRIX_4X4(gim_trans); COPY_MATRIX_3X3(gim_trans,trans.getBasis()); btVector3 scaling = getLocalScaling(); @@ -243,7 +245,7 @@ void btGIMPACTMeshShape::getAabb(const btTransform& t,btVector3& aabbMin,btVecto aabbMax[0] = global_box.maxX; aabbMax[1] = global_box.maxY; aabbMax[2] = global_box.maxZ; - + } void btGIMPACTMeshShape::setLocalScaling(const btVector3& scaling) @@ -251,171 +253,11 @@ void btGIMPACTMeshShape::setLocalScaling(const btVector3& scaling) m_scale = scaling; } -const btVector3& btGIMPACTMeshShape::getLocalScaling() const +const btVector3& btGIMPACTMeshShape::getLocalScaling() const { return m_scale ; } -#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube - -void bt_calc_inertia_gim_trimesh(GIM_TRIMESH * trimesh, vec3f T) -{ - unsigned int triangles = gim_trimesh_get_triangle_count(trimesh); - - float nx, ny, nz; - unsigned int i, A, B, C; - // face integrals - float Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; - - // projection integrals - float P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; - - float T0 = 0; - - gim_trimesh_locks_work_data(trimesh); - - - for( i = 0; i < triangles; i++ ) - { - vec3f v0, v1, v2; - - gim_trimesh_get_triangle_vertices(trimesh,i,v0,v1,v2); - - - vec3f n, a, b; - - VEC_DIFF(a,v1,v0); - VEC_DIFF(b,v2,v0); - - VEC_CROSS(n,b,a); - - nx = fabs(n[0]); - ny = fabs(n[1]); - nz = fabs(n[2]); - - if( nx > ny && nx > nz ) - C = 0; - else - C = (ny > nz) ? 1 : 2; - - A = (C + 1) % 3; - B = (A + 1) % 3; - - // calculate face integrals - { - float w; - float k1, k2, k3, k4; - - //compProjectionIntegrals(f); - { - float a0, a1, da; - float b0, b1, db; - float a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; - float a1_2, a1_3, b1_2, b1_3; - float C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; - float Cab, Kab, Caab, Kaab, Cabb, Kabb; - - P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; - - for( int j = 0; j < 3; j++) - { - switch(j) - { - case 0: - a0 = v0[A]; - b0 = v0[B]; - a1 = v1[A]; - b1 = v1[B]; - break; - case 1: - a0 = v1[A]; - b0 = v1[B]; - a1 = v2[A]; - b1 = v2[B]; - break; - case 2: - a0 = v2[A]; - b0 = v2[B]; - a1 = v0[A]; - b1 = v0[B]; - break; - } - da = a1 - a0; - db = b1 - b0; - a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; - b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; - a1_2 = a1 * a1; a1_3 = a1_2 * a1; - b1_2 = b1 * b1; b1_3 = b1_2 * b1; - - C1 = a1 + a0; - Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; - Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; - Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; - Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; - Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; - Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; - - P1 += db*C1; - Pa += db*Ca; - Paa += db*Caa; - Paaa += db*Caaa; - Pb += da*Cb; - Pbb += da*Cbb; - Pbbb += da*Cbbb; - Pab += db*(b1*Cab + b0*Kab); - Paab += db*(b1*Caab + b0*Kaab); - Pabb += da*(a1*Cabb + a0*Kabb); - } - - P1 /= 2.0; - Pa /= 6.0; - Paa /= 12.0; - Paaa /= 20.0; - Pb /= -6.0; - Pbb /= -12.0; - Pbbb /= -20.0; - Pab /= 24.0; - Paab /= 60.0; - Pabb /= -60.0; - } - - w = - VEC_DOT(n, v0); - - k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; - - Fa = k1 * Pa; - Fb = k1 * Pb; - Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); - - Faa = k1 * Paa; - Fbb = k1 * Pbb; - Fcc = k3 * (gim_sqrt(n[A])*Paa + 2*n[A]*n[B]*Pab + gim_sqrt(n[B])*Pbb + - w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); - - Faaa = k1 * Paaa; - Fbbb = k1 * Pbbb; - Fccc = -k4 * (CUBE(n[A])*Paaa + 3*gim_sqrt(n[A])*n[B]*Paab - + 3*n[A]*gim_sqrt(n[B])*Pabb + CUBE(n[B])*Pbbb - + 3*w*(gim_sqrt(n[A])*Paa + 2*n[A]*n[B]*Pab + gim_sqrt(n[B])*Pbb) - + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); - - Faab = k1 * Paab; - Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); - Fcca = k3 * (gim_sqrt(n[A])*Paaa + 2*n[A]*n[B]*Paab + gim_sqrt(n[B])*Pabb - + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); - } - - T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc)); - - - T[A] += n[A] * Faaa; - T[B] += n[B] * Fbbb; - T[C] += n[C] * Fccc; - } - - gim_trimesh_unlocks_work_data(trimesh); - -} void btGIMPACTMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { @@ -427,7 +269,7 @@ void btGIMPACTMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) getAabb(t,aabbMin,aabbMax); - //not yet, return box inertia + //not yet, return box inertia btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; @@ -441,44 +283,18 @@ void btGIMPACTMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); - /* - //prepareMeshes(t); - inertia.setValue(0.0f,0.0f,0.0f); - vec3f T; - VEC_ZERO(T); - - //Process Every triangle - GIM_TRIMESH * ptrimesh; - - for(size_t i = 0;i