mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-16 14:40:05 +00:00
added GIMPACT update from Francisco. Thanks!
This commit is contained in:
ejcoumans
parent
b07bb88a2d
commit
509ed8f634
@ -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
|
||||
*
|
||||
|
||||
@ -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<contact_count;i++)
|
||||
{
|
||||
if(ci>=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<contact_count;i++)
|
||||
{
|
||||
if(ci>=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<btGIMPACTMeshShape*>( body0->getCollisionShape());
|
||||
btGIMPACTMeshShape* tri1b = static_cast<btGIMPACTMeshShape*>( 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;i<parts0;i++)
|
||||
{
|
||||
for(j=0;j<parts1;j++)
|
||||
{
|
||||
trimesh0 = (GIM_TRIMESH * )tri0b->m_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<btGIMPACTMeshShape*>( tribody0->getCollisionShape());
|
||||
btPlaneShape * plane1b = static_cast<btPlaneShape *>( 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;i<parts0;i++)
|
||||
{
|
||||
trimesh0 = (GIM_TRIMESH * )tri0b->m_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<CONCAVE_TRIANGLE_TOKEN> 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<ConcaveShape*>( tribody0->getCollisionShape());
|
||||
ConcaveShape* tri1b = static_cast<ConcaveShape*>( 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;i<callback0.m_triangles.size();i++)
|
||||
{
|
||||
for(j=0;j<callback1.m_triangles.size();j++)
|
||||
{
|
||||
|
||||
//collide triangles
|
||||
colresult = gim_triangle_triangle_collision(
|
||||
&callback0.m_triangles[i].m_tridata,
|
||||
&callback1.m_triangles[j].m_tridata,&tri_contact_data);
|
||||
if(colresult == 1)
|
||||
{
|
||||
//Add contacts
|
||||
for (ci=0;ci<tri_contact_data.m_point_count ;ci++ )
|
||||
{
|
||||
GIM_PUSH_CONTACT(dummycontacts, tri_contact_data.m_points[ci],tri_contact_data.m_separating_normal ,tri_contact_data.m_penetration_depth,tribody0, tribody1, callback0.m_triangles[i].triangleIndex, callback1.m_triangles[j].triangleIndex);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(dummycontacts.m_size == 0) //reject
|
||||
{
|
||||
GIM_DYNARRAY_DESTROY(dummycontacts);
|
||||
return;
|
||||
}
|
||||
|
||||
//dummy contacts
|
||||
GDYNAMIC_ARRAY contacts;
|
||||
GIM_CREATE_CONTACT_LIST(contacts);
|
||||
|
||||
//merge contacts
|
||||
gim_merge_contacts(&dummycontacts,&contacts);
|
||||
|
||||
GIM_CONTACT * pcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,contacts);
|
||||
process_gimpact_contacts(pcontacts,contacts.m_size,algorithm,tribody0,tribody1,resultOut);
|
||||
|
||||
//Terminate
|
||||
GIM_DYNARRAY_DESTROY(dummycontacts);
|
||||
GIM_DYNARRAY_DESTROY(contacts);
|
||||
}
|
||||
|
||||
|
||||
void btConcaveConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
||||
{
|
||||
clearCache();
|
||||
if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE && body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
|
||||
{
|
||||
btGIMPACTMeshShape* tri0b = static_cast<btGIMPACTMeshShape*>( body0->getCollisionShape());
|
||||
btGIMPACTMeshShape* tri1b = static_cast<btGIMPACTMeshShape*>( 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;i<parts0;i++)
|
||||
{
|
||||
for(j=0;j<parts1;j++)
|
||||
{
|
||||
trimesh0 = (GIM_TRIMESH * )tri0b->m_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);
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;i<m_meshes.size();i++)
|
||||
{
|
||||
gim_trimesh_data_dec_ref(m_meshes[i]);
|
||||
gim_trimesh_data_dec_ref(m_meshes[i]);
|
||||
}
|
||||
m_meshes.clear();
|
||||
}
|
||||
@ -95,7 +95,7 @@ void btGIMPACTMeshData::addMeshPart(btStridingMeshInterface* meshInterface, int
|
||||
|
||||
//Create shared buffer for indices
|
||||
|
||||
gim_create_shared_buffer_from_data(
|
||||
gim_create_shared_buffer_from_data(
|
||||
vertexbase, numverts*vertexStride,
|
||||
&buffer_vertex_array.m_buffer_id);
|
||||
|
||||
@ -170,7 +170,7 @@ void btGIMPACTMeshShape::clearMeshParts()
|
||||
}
|
||||
|
||||
void btGIMPACTMeshShape::processMeshParts(btGIMPACTMeshData * meshdata)
|
||||
{
|
||||
{
|
||||
clearMeshParts();
|
||||
this->m_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<m_gim_trimesh_parts.size();i++)
|
||||
{
|
||||
ptrimesh = (GIM_TRIMESH * )m_gim_trimesh_parts[i];
|
||||
bt_calc_inertia_gim_trimesh(ptrimesh,T);
|
||||
}
|
||||
|
||||
T[0] /= 3; T[1] /= 3; T[2] /= 3;
|
||||
|
||||
inertia[0] = (T[1] + T[2]);
|
||||
inertia[0] *= mass/(aabbMax[0] - aabbMin[0]);
|
||||
|
||||
inertia[1] = (T[2] + T[0]);
|
||||
inertia[1] *= mass/(aabbMax[1] - aabbMin[1]);
|
||||
|
||||
inertia[2] = (T[0] + T[1]);
|
||||
inertia[2] *= mass/(aabbMax[2] - aabbMin[2]);*/
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGIMPACTMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
|
||||
{
|
||||
{
|
||||
|
||||
/*btTransform t;
|
||||
t.setIdentity();
|
||||
prepareMeshes(t);*/
|
||||
|
||||
GDYNAMIC_ARRAY collision_result;
|
||||
GDYNAMIC_ARRAY collision_result;
|
||||
|
||||
GIM_TRIMESH * ptrimesh;
|
||||
size_t i,j;
|
||||
@ -513,7 +329,7 @@ void btGIMPACTMeshShape::processAllTriangles(btTriangleCallback* callback,const
|
||||
for(j=0;j<collision_result.m_size;j++)
|
||||
{
|
||||
gim_trimesh_get_triangle_vertices_local(ptrimesh,boxesresult[j],trivec[0],trivec[1],trivec[2]);
|
||||
|
||||
|
||||
btrivec[0].setValue(trivec[0][0]*scalevec[0],trivec[0][1]*scalevec[1],trivec[0][2]*scalevec[2]);
|
||||
btrivec[1].setValue(trivec[1][0]*scalevec[0],trivec[1][1]*scalevec[1],trivec[1][2]*scalevec[2]);
|
||||
btrivec[2].setValue(trivec[2][0]*scalevec[0],trivec[2][1]*scalevec[1],trivec[2][2]*scalevec[2]);
|
||||
@ -524,8 +340,8 @@ void btGIMPACTMeshShape::processAllTriangles(btTriangleCallback* callback,const
|
||||
///unlocks
|
||||
gim_trimesh_unlocks_work_data(ptrimesh);
|
||||
|
||||
|
||||
GIM_DYNARRAY_DESTROY(collision_result);
|
||||
|
||||
GIM_DYNARRAY_DESTROY(collision_result);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user