857 lines
24 KiB
C++
857 lines
24 KiB
C++
/*
|
||
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 "btGImpactCollisionAlgorithm.h"
|
||
#include "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) const
|
||
{
|
||
const btVector3 normal = trans.getBasis() * m_planeNormal;
|
||
equation[0] = normal[0];
|
||
equation[1] = normal[1];
|
||
equation[2] = normal[2];
|
||
equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal));
|
||
}
|
||
};
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
#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:
|
||
const btGImpactShapeInterface* m_gim_shape;
|
||
btTriangleShapeEx m_trishape;
|
||
btTetrahedronShapeEx m_tetrashape;
|
||
|
||
public:
|
||
class ChildShapeRetriever
|
||
{
|
||
public:
|
||
GIM_ShapeRetriever* m_parent;
|
||
virtual const 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(const 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;
|
||
}
|
||
|
||
const 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, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
|
||
: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap)
|
||
{
|
||
m_manifoldPtr = NULL;
|
||
m_convex_algorithm = NULL;
|
||
}
|
||
|
||
btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
|
||
{
|
||
clearCache();
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btVector3& point,
|
||
const btVector3& normal,
|
||
btScalar distance)
|
||
{
|
||
m_resultOut->setShapeIdentifiersA(m_part0, m_triface0);
|
||
m_resultOut->setShapeIdentifiersB(m_part1, m_triface1);
|
||
checkManifold(body0Wrap, body1Wrap);
|
||
m_resultOut->addContactPoint(normal, point, distance);
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
|
||
const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btCollisionShape* shape0,
|
||
const btCollisionShape* shape1)
|
||
{
|
||
{
|
||
btCollisionAlgorithm* algor = newAlgorithm(body0Wrap, body1Wrap);
|
||
// post : checkManifold is called
|
||
|
||
m_resultOut->setShapeIdentifiersA(m_part0, m_triface0);
|
||
m_resultOut->setShapeIdentifiersB(m_part1, m_triface1);
|
||
|
||
algor->processCollision(body0Wrap, body1Wrap, *m_dispatchInfo, m_resultOut);
|
||
|
||
algor->~btCollisionAlgorithm();
|
||
m_dispatcher->freeCollisionAlgorithm(algor);
|
||
}
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
|
||
const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btCollisionShape* shape0,
|
||
const btCollisionShape* shape1)
|
||
{
|
||
m_resultOut->setShapeIdentifiersA(m_part0, m_triface0);
|
||
m_resultOut->setShapeIdentifiersB(m_part1, m_triface1);
|
||
|
||
btCollisionObjectWrapper ob0(body0Wrap, shape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0);
|
||
btCollisionObjectWrapper ob1(body1Wrap, shape1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), m_part1, m_triface1);
|
||
checkConvexAlgorithm(&ob0, &ob1);
|
||
m_convex_algorithm->processCollision(&ob0, &ob1, *m_dispatchInfo, m_resultOut);
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
|
||
const btTransform& trans0,
|
||
const btTransform& trans1,
|
||
const btGImpactShapeInterface* shape0,
|
||
const 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,
|
||
const btGImpactShapeInterface* shape0,
|
||
const 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(const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btGImpactMeshShapePart* shape0,
|
||
const 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(body0Wrap, body1Wrap, &tri0, &tri1);
|
||
}
|
||
}
|
||
|
||
shape0->unlockChildShapes();
|
||
shape1->unlockChildShapes();
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btGImpactMeshShapePart* shape0,
|
||
const btGImpactMeshShapePart* shape1,
|
||
const int* pairs, int pair_count)
|
||
{
|
||
btTransform orgtrans0 = body0Wrap->getWorldTransform();
|
||
btTransform orgtrans1 = body1Wrap->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(body0Wrap, body1Wrap,
|
||
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(
|
||
const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btGImpactShapeInterface* shape0,
|
||
const btGImpactShapeInterface* shape1)
|
||
{
|
||
if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE)
|
||
{
|
||
const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0);
|
||
m_part0 = meshshape0->getMeshPartCount();
|
||
|
||
while (m_part0--)
|
||
{
|
||
gimpact_vs_gimpact(body0Wrap, body1Wrap, meshshape0->getMeshPart(m_part0), shape1);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
if (shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE)
|
||
{
|
||
const btGImpactMeshShape* meshshape1 = static_cast<const btGImpactMeshShape*>(shape1);
|
||
m_part1 = meshshape1->getMeshPartCount();
|
||
|
||
while (m_part1--)
|
||
{
|
||
gimpact_vs_gimpact(body0Wrap, body1Wrap, shape0, meshshape1->getMeshPart(m_part1));
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
btTransform orgtrans0 = body0Wrap->getWorldTransform();
|
||
btTransform orgtrans1 = body1Wrap->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)
|
||
{
|
||
const btGImpactMeshShapePart* shapepart0 = static_cast<const btGImpactMeshShapePart*>(shape0);
|
||
const btGImpactMeshShapePart* shapepart1 = static_cast<const btGImpactMeshShapePart*>(shape1);
|
||
//specialized function
|
||
#ifdef BULLET_TRIANGLE_COLLISION
|
||
collide_gjk_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size());
|
||
#else
|
||
collide_sat_triangles(body0Wrap, body1Wrap, 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;
|
||
const btCollisionShape* colshape0 = retriever0.getChildShape(m_triface0);
|
||
const btCollisionShape* colshape1 = retriever1.getChildShape(m_triface1);
|
||
|
||
btTransform tr0 = body0Wrap->getWorldTransform();
|
||
btTransform tr1 = body1Wrap->getWorldTransform();
|
||
|
||
if (child_has_transform0)
|
||
{
|
||
tr0 = orgtrans0 * shape0->getChildTransform(m_triface0);
|
||
}
|
||
|
||
if (child_has_transform1)
|
||
{
|
||
tr1 = orgtrans1 * shape1->getChildTransform(m_triface1);
|
||
}
|
||
|
||
btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), tr0, m_part0, m_triface0);
|
||
btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), tr1, m_part1, m_triface1);
|
||
|
||
//collide two convex shapes
|
||
convex_vs_convex_collision(&ob0, &ob1, colshape0, colshape1);
|
||
}
|
||
|
||
shape0->unlockChildShapes();
|
||
shape1->unlockChildShapes();
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btGImpactShapeInterface* shape0,
|
||
const btCollisionShape* shape1, bool swapped)
|
||
{
|
||
if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE)
|
||
{
|
||
const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0);
|
||
int& part = swapped ? m_part1 : m_part0;
|
||
part = meshshape0->getMeshPartCount();
|
||
|
||
while (part--)
|
||
{
|
||
gimpact_vs_shape(body0Wrap,
|
||
body1Wrap,
|
||
meshshape0->getMeshPart(part),
|
||
shape1, swapped);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
#ifdef GIMPACT_VS_PLANE_COLLISION
|
||
if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
|
||
shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
|
||
{
|
||
const btGImpactMeshShapePart* shapepart = static_cast<const btGImpactMeshShapePart*>(shape0);
|
||
const btStaticPlaneShape* planeshape = static_cast<const btStaticPlaneShape*>(shape1);
|
||
gimpacttrimeshpart_vs_plane_collision(body0Wrap, body1Wrap, shapepart, planeshape, swapped);
|
||
return;
|
||
}
|
||
|
||
#endif
|
||
|
||
if (shape1->isCompound())
|
||
{
|
||
const btCompoundShape* compoundshape = static_cast<const btCompoundShape*>(shape1);
|
||
gimpact_vs_compoundshape(body0Wrap, body1Wrap, shape0, compoundshape, swapped);
|
||
return;
|
||
}
|
||
else if (shape1->isConcave())
|
||
{
|
||
const btConcaveShape* concaveshape = static_cast<const btConcaveShape*>(shape1);
|
||
gimpact_vs_concave(body0Wrap, body1Wrap, shape0, concaveshape, swapped);
|
||
return;
|
||
}
|
||
|
||
btTransform orgtrans0 = body0Wrap->getWorldTransform();
|
||
|
||
btTransform orgtrans1 = body1Wrap->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;
|
||
|
||
const btCollisionShape* colshape0 = retriever0.getChildShape(child_index);
|
||
|
||
btTransform tr0 = body0Wrap->getWorldTransform();
|
||
|
||
if (child_has_transform0)
|
||
{
|
||
tr0 = orgtrans0 * shape0->getChildTransform(child_index);
|
||
}
|
||
|
||
btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0);
|
||
const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap();
|
||
|
||
if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject())
|
||
{
|
||
m_resultOut->setBody0Wrap(&ob0);
|
||
}
|
||
else
|
||
{
|
||
m_resultOut->setBody1Wrap(&ob0);
|
||
}
|
||
|
||
//collide two shapes
|
||
if (swapped)
|
||
{
|
||
shape_vs_shape_collision(body1Wrap, &ob0, shape1, colshape0);
|
||
}
|
||
else
|
||
{
|
||
shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1);
|
||
}
|
||
m_resultOut->setBody0Wrap(prevObj0);
|
||
}
|
||
|
||
shape0->unlockChildShapes();
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btGImpactShapeInterface* shape0,
|
||
const btCompoundShape* shape1, bool swapped)
|
||
{
|
||
btTransform orgtrans1 = body1Wrap->getWorldTransform();
|
||
|
||
int i = shape1->getNumChildShapes();
|
||
while (i--)
|
||
{
|
||
const btCollisionShape* colshape1 = shape1->getChildShape(i);
|
||
btTransform childtrans1 = orgtrans1 * shape1->getChildTransform(i);
|
||
|
||
btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), childtrans1, -1, i);
|
||
|
||
const btCollisionObjectWrapper* tmp = 0;
|
||
if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject())
|
||
{
|
||
tmp = m_resultOut->getBody0Wrap();
|
||
m_resultOut->setBody0Wrap(&ob1);
|
||
}
|
||
else
|
||
{
|
||
tmp = m_resultOut->getBody1Wrap();
|
||
m_resultOut->setBody1Wrap(&ob1);
|
||
}
|
||
//collide child shape
|
||
gimpact_vs_shape(body0Wrap, &ob1,
|
||
shape0, colshape1, swapped);
|
||
|
||
if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject())
|
||
{
|
||
m_resultOut->setBody0Wrap(tmp);
|
||
}
|
||
else
|
||
{
|
||
m_resultOut->setBody1Wrap(tmp);
|
||
}
|
||
}
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
|
||
const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btGImpactMeshShapePart* shape0,
|
||
const btStaticPlaneShape* shape1, bool swapped)
|
||
{
|
||
btTransform orgtrans0 = body0Wrap->getWorldTransform();
|
||
btTransform orgtrans1 = body1Wrap->getWorldTransform();
|
||
|
||
const btPlaneShape* planeshape = static_cast<const 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(body1Wrap, body0Wrap,
|
||
vertex,
|
||
-plane,
|
||
distance);
|
||
}
|
||
else
|
||
{
|
||
addContactPoint(body0Wrap, body1Wrap,
|
||
vertex,
|
||
plane,
|
||
distance);
|
||
}
|
||
}
|
||
}
|
||
|
||
shape0->unlockChildShapes();
|
||
}
|
||
|
||
class btGImpactTriangleCallback : public btTriangleCallback
|
||
{
|
||
public:
|
||
btGImpactCollisionAlgorithm* algorithm;
|
||
const btCollisionObjectWrapper* body0Wrap;
|
||
const btCollisionObjectWrapper* body1Wrap;
|
||
const 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);
|
||
}
|
||
|
||
btCollisionObjectWrapper ob1Wrap(body1Wrap, &tri1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), partId, triangleIndex);
|
||
const btCollisionObjectWrapper* tmp = 0;
|
||
|
||
if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject())
|
||
{
|
||
tmp = algorithm->internalGetResultOut()->getBody0Wrap();
|
||
algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap);
|
||
}
|
||
else
|
||
{
|
||
tmp = algorithm->internalGetResultOut()->getBody1Wrap();
|
||
algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap);
|
||
}
|
||
|
||
algorithm->gimpact_vs_shape(
|
||
body0Wrap, &ob1Wrap, gimpactshape0, &tri1, swapped);
|
||
|
||
if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject())
|
||
{
|
||
algorithm->internalGetResultOut()->setBody0Wrap(tmp);
|
||
}
|
||
else
|
||
{
|
||
algorithm->internalGetResultOut()->setBody1Wrap(tmp);
|
||
}
|
||
}
|
||
};
|
||
|
||
void btGImpactCollisionAlgorithm::gimpact_vs_concave(
|
||
const btCollisionObjectWrapper* body0Wrap,
|
||
const btCollisionObjectWrapper* body1Wrap,
|
||
const btGImpactShapeInterface* shape0,
|
||
const btConcaveShape* shape1, bool swapped)
|
||
{
|
||
//create the callback
|
||
btGImpactTriangleCallback tricallback;
|
||
tricallback.algorithm = this;
|
||
tricallback.body0Wrap = body0Wrap;
|
||
tricallback.body1Wrap = body1Wrap;
|
||
tricallback.gimpactshape0 = shape0;
|
||
tricallback.swapped = swapped;
|
||
tricallback.margin = shape1->getMargin();
|
||
|
||
//getting the trimesh AABB
|
||
btTransform gimpactInConcaveSpace;
|
||
|
||
gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform();
|
||
|
||
btVector3 minAABB, maxAABB;
|
||
shape0->getAabb(gimpactInConcaveSpace, minAABB, maxAABB);
|
||
|
||
shape1->processAllTriangles(&tricallback, minAABB, maxAABB);
|
||
}
|
||
|
||
void btGImpactCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
|
||
{
|
||
clearCache();
|
||
|
||
m_resultOut = resultOut;
|
||
m_dispatchInfo = &dispatchInfo;
|
||
const btGImpactShapeInterface* gimpactshape0;
|
||
const btGImpactShapeInterface* gimpactshape1;
|
||
|
||
if (body0Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
|
||
{
|
||
gimpactshape0 = static_cast<const btGImpactShapeInterface*>(body0Wrap->getCollisionShape());
|
||
|
||
if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
|
||
{
|
||
gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape());
|
||
|
||
gimpact_vs_gimpact(body0Wrap, body1Wrap, gimpactshape0, gimpactshape1);
|
||
}
|
||
else
|
||
{
|
||
gimpact_vs_shape(body0Wrap, body1Wrap, gimpactshape0, body1Wrap->getCollisionShape(), false);
|
||
}
|
||
}
|
||
else if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
|
||
{
|
||
gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape());
|
||
|
||
gimpact_vs_shape(body1Wrap, body0Wrap, gimpactshape1, body0Wrap->getCollisionShape(), true);
|
||
}
|
||
|
||
// Ensure that gContactProcessedCallback is called for concave shapes.
|
||
if (getLastManifold())
|
||
{
|
||
m_resultOut->refreshContactPoints();
|
||
}
|
||
}
|
||
|
||
btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
|
||
{
|
||
return 1.f;
|
||
}
|
||
|
||
///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
|
||
|
||
//! Use this function for register the algorithm externally
|
||
void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher* dispatcher)
|
||
{
|
||
static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf;
|
||
|
||
int i;
|
||
|
||
for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
|
||
{
|
||
dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE, i, &s_gimpact_cf);
|
||
}
|
||
|
||
for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
|
||
{
|
||
dispatcher->registerCollisionCreateFunc(i, GIMPACT_SHAPE_PROXYTYPE, &s_gimpact_cf);
|
||
}
|
||
}
|