054d672592
- added heap sort and binary search/linear search to btAlignedObjectArray - fixed wrong cast, thanks Hamstray, http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1015
495 lines
16 KiB
C++
495 lines
16 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
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 León Nájera
|
|
Concave-Concave Collision
|
|
|
|
*/
|
|
|
|
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
|
|
#include "LinearMath/btIDebugDraw.h"
|
|
#include "btConcaveConcaveCollisionAlgorithm.h"
|
|
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
|
#include "btGIMPACTMeshShape.h"
|
|
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
|
|
#include "GIMPACT/gimpact.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(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)
|
|
{
|
|
}
|
|
|
|
btConcaveConcaveCollisionAlgorithm::~btConcaveConcaveCollisionAlgorithm()
|
|
{
|
|
clearCache();
|
|
}
|
|
|
|
void btConcaveConcaveCollisionAlgorithm::clearCache()
|
|
{
|
|
btPersistentManifold* mainfold;
|
|
for (size_t i=0;i<this->m_mainfoldsPtr.size() ; i++)
|
|
{
|
|
mainfold = m_mainfoldsPtr[i];
|
|
m_dispatcher->releaseManifold(mainfold);
|
|
}
|
|
m_mainfoldsPtr.clear();
|
|
}
|
|
|
|
btPersistentManifold* btConcaveConcaveCollisionAlgorithm::newContactMainfold(btCollisionObject* body0,btCollisionObject* body1)
|
|
{
|
|
btPersistentManifold* newmainfold;
|
|
newmainfold = m_dispatcher->getNewManifold(body0,body1);
|
|
m_mainfoldsPtr.push_back(newmainfold);
|
|
return newmainfold;
|
|
}
|
|
|
|
void process_gimpact_contacts(GIM_CONTACT * pcontacts,
|
|
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->m_point[0],pcontacts->m_point[1],pcontacts->m_point[2]);
|
|
//Normal points to body0
|
|
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;
|
|
btAlignedObjectArray<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)
|
|
{
|
|
btConcaveShape* tri0b = static_cast<btConcaveShape*>( tribody0->getCollisionShape());
|
|
btConcaveShape* tri1b = static_cast<btConcaveShape*>( 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 )
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
|
|
btScalar btConcaveConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
|
{
|
|
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);
|
|
}
|