added GIMPACT update from Francisco. Thanks!

This commit is contained in:
ejcoumans
2006-11-17 02:20:37 +00:00
parent b07bb88a2d
commit 509ed8f634
4 changed files with 431 additions and 248 deletions

View File

@@ -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);
}