upgrade to GIMPACT 0.2
This commit is contained in:
ejcoumans
924
Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp
Executable file
924
Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp
Executable file
@@ -0,0 +1,924 @@
|
||||
/*
|
||||
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<4C>n N<>jera
|
||||
Concave-Concave Collision
|
||||
|
||||
*/
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
#include "GIMPACT/Bullet/btGImpactCollisionAlgorithm.h"
|
||||
#include "GIMPACT/core/gim_contact.h"
|
||||
#include "BulletCollision/CollisionShapes/btBoxShape.h"
|
||||
|
||||
#define BULLET_TRIANGLE_COLLISION 1
|
||||
#define TREE_PRIMITIVE_VS_BOX true
|
||||
#define GIMPACT_VS_PLANE_COLLISION 1
|
||||
|
||||
|
||||
//! 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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
|
||||
: btCollisionAlgorithm(ci)
|
||||
{
|
||||
m_manifoldPtr = NULL;
|
||||
m_convex_algorithm = NULL;
|
||||
}
|
||||
|
||||
btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
|
||||
{
|
||||
clearCache();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpactcompound_find_pairs(
|
||||
const btTransform & trans0,
|
||||
const btTransform & trans1,
|
||||
btGImpactCompoundShape * shape0,
|
||||
btGImpactCompoundShape * shape1,gim_pair_set & pairset) const
|
||||
{
|
||||
GIM_TREE_TREE_COLLIDER<btGImpactCompoundShape::BoxSetClass,btGImpactCompoundShape::BoxSetClass> collider;
|
||||
collider.find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset,TREE_PRIMITIVE_VS_BOX);
|
||||
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_gimpacttrimeshpart_find_pairs(
|
||||
const btTransform & trans0,
|
||||
const btTransform & trans1,
|
||||
btGImpactMeshShapePart * shape0,
|
||||
btGImpactMeshShapePart * shape1,gim_pair_set & pairset) const
|
||||
{
|
||||
GIM_TREE_TREE_COLLIDER<btGImpactMeshShapePart::BoxSetClass,btGImpactMeshShapePart::BoxSetClass> collider;
|
||||
|
||||
collider.find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset,TREE_PRIMITIVE_VS_BOX);
|
||||
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpacttrimeshpart_find_pairs(
|
||||
const btTransform & trans0,
|
||||
const btTransform & trans1,
|
||||
btGImpactCompoundShape * shape0,
|
||||
btGImpactMeshShapePart * shape1,gim_pair_set & pairset) const
|
||||
{
|
||||
GIM_TREE_TREE_COLLIDER<btGImpactCompoundShape::BoxSetClass,btGImpactMeshShapePart::BoxSetClass> collider;
|
||||
collider.find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset,TREE_PRIMITIVE_VS_BOX);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btCollisionShape * shape0,
|
||||
btCollisionShape * shape1,bool swapped)
|
||||
{
|
||||
|
||||
btCollisionShape * orgshape0 = body0->getCollisionShape();
|
||||
btCollisionShape * orgshape1 = body1->getCollisionShape();
|
||||
|
||||
body0->setCollisionShape(shape0);
|
||||
body1->setCollisionShape(shape1);
|
||||
|
||||
|
||||
if(swapped)
|
||||
{
|
||||
btCollisionAlgorithm* algorswapped = newAlgorithm(body1,body0);
|
||||
|
||||
m_resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
m_resultOut->setShapeIdentifiers(m_part1,m_triface1,m_part0,m_triface0);
|
||||
|
||||
algorswapped->processCollision(body1,body0,*m_dispatchInfo,m_resultOut);
|
||||
|
||||
delete algorswapped;
|
||||
}
|
||||
else
|
||||
{
|
||||
btCollisionAlgorithm* algor = newAlgorithm(body0,body1);
|
||||
|
||||
m_resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
|
||||
|
||||
algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
|
||||
|
||||
delete algor;
|
||||
}
|
||||
|
||||
body0->setCollisionShape(orgshape0);
|
||||
body1->setCollisionShape(orgshape1);
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btCollisionShape * shape0,
|
||||
btCollisionShape * shape1)
|
||||
{
|
||||
//shape_vs_shape_collision(body0,body1,shape0,shape1,false);
|
||||
//return;
|
||||
|
||||
btCollisionShape * orgshape0 = body0->getCollisionShape();
|
||||
btCollisionShape * orgshape1 = body1->getCollisionShape();
|
||||
|
||||
body0->setCollisionShape(shape0);
|
||||
body1->setCollisionShape(shape1);
|
||||
|
||||
|
||||
m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
|
||||
|
||||
checkConvexAlgorithm(body0,body1);
|
||||
m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
|
||||
|
||||
body0->setCollisionShape(orgshape0);
|
||||
body1->setCollisionShape(orgshape1);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_shape_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShape * shape0,
|
||||
btCollisionShape * shape1,bool swapped)
|
||||
{
|
||||
GUINT i = shape0->getMeshPartCount();
|
||||
while(i--)
|
||||
{
|
||||
btGImpactMeshShapePart * part = shape0->getMeshPart(i);
|
||||
gimpacttrimeshpart_vs_shape_collision(body0,body1,part,shape1,swapped);
|
||||
}
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_gimpacttrimesh(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShape * shape0,
|
||||
btGImpactMeshShape * shape1)
|
||||
{
|
||||
GUINT i = shape0->getMeshPartCount();
|
||||
while(i--)
|
||||
{
|
||||
btGImpactMeshShapePart * part0 = shape0->getMeshPart(i);
|
||||
|
||||
GUINT j = shape1->getMeshPartCount();
|
||||
while(j--)
|
||||
{
|
||||
btGImpactMeshShapePart * part1 = shape1->getMeshPart(j);
|
||||
gimpacttrimeshpart_vs_gimpacttrimeshpart_collision(body0,body1,part0,part1,false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_gimpactcompound(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShape * shape0,
|
||||
btGImpactCompoundShape * shape1,bool swapped)
|
||||
{
|
||||
GUINT i = shape0->getMeshPartCount();
|
||||
while(i--)
|
||||
{
|
||||
btGImpactMeshShapePart * part = shape0->getMeshPart(i);
|
||||
gimpactcompound_vs_gimpacttrimeshpart_collision(body1,body0,shape1,part,!swapped);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_trimeshpart(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShape * shape0,
|
||||
btGImpactMeshShapePart * shape1,bool swapped)
|
||||
{
|
||||
GUINT i = shape0->getMeshPartCount();
|
||||
while(i--)
|
||||
{
|
||||
btGImpactMeshShapePart * part = shape0->getMeshPart(i);
|
||||
gimpacttrimeshpart_vs_gimpacttrimeshpart_collision(body0,body1,part,shape1,swapped);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpactcompound_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactCompoundShape * shape0,
|
||||
btGImpactCompoundShape * shape1)
|
||||
{
|
||||
btTransform orgtrans0 = body0->getWorldTransform();
|
||||
btTransform orgtrans1 = body1->getWorldTransform();
|
||||
|
||||
gim_pair_set pairset;
|
||||
|
||||
gimpactcompound_vs_gimpactcompound_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
|
||||
|
||||
if(pairset.size()== 0) return;
|
||||
|
||||
// btCollisionShape * orgshape0 = body0->getCollisionShape();
|
||||
// btCollisionShape * orgshape1 = body1->getCollisionShape();
|
||||
|
||||
GUINT i = pairset.size();
|
||||
while(i--)
|
||||
{
|
||||
const GIM_PAIR & pair = pairset[i];
|
||||
btCollisionShape * colshape0 = shape0->getChildShape(pair.m_index1);
|
||||
btCollisionShape * colshape1 = shape1->getChildShape(pair.m_index2);
|
||||
|
||||
btTransform childtrans0 = orgtrans0*shape0->getChildTransform(pair.m_index1);
|
||||
btTransform childtrans1 = orgtrans1*shape1->getChildTransform(pair.m_index2);
|
||||
|
||||
body0->setWorldTransform(childtrans0);
|
||||
body1->setWorldTransform(childtrans1);
|
||||
|
||||
|
||||
//collide two shapes
|
||||
shape_vs_shape_collision(body0,body1,colshape0,colshape1,false);
|
||||
|
||||
//restore transforms
|
||||
// body0->setCollisionShape(orgshape0);
|
||||
// body1->setCollisionShape(orgshape1);
|
||||
|
||||
body0->setWorldTransform(orgtrans0);
|
||||
body1->setWorldTransform(orgtrans1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpacttrimeshpart_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactCompoundShape * shape0,
|
||||
btGImpactMeshShapePart * shape1,bool swapped)
|
||||
{
|
||||
//lock trimesh
|
||||
shape1->lock();
|
||||
|
||||
btTransform orgtrans0 = body0->getWorldTransform();
|
||||
btTransform orgtrans1 = body1->getWorldTransform();
|
||||
|
||||
gim_pair_set pairset;
|
||||
|
||||
gimpactcompound_vs_gimpacttrimeshpart_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
|
||||
|
||||
if(pairset.size()== 0)
|
||||
{
|
||||
//unlock trimesh
|
||||
shape1->unlock();
|
||||
return;
|
||||
}
|
||||
|
||||
// btCollisionShape * orgshape0 = body0->getCollisionShape();
|
||||
// btCollisionShape * orgshape1 = body1->getCollisionShape();
|
||||
|
||||
|
||||
m_part1 = shape1->getPart();
|
||||
m_part0 = -1;
|
||||
m_triface0 = -1;
|
||||
|
||||
btTriangleShapeEx bullet_triangle(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f));
|
||||
|
||||
|
||||
GUINT i = pairset.size();
|
||||
|
||||
while(i--)
|
||||
{
|
||||
const GIM_PAIR & pair = pairset[i];
|
||||
btCollisionShape * colshape0 = shape0->getChildShape(pair.m_index1);
|
||||
btTransform childtrans0 = orgtrans0*shape0->getChildTransform(pair.m_index1);
|
||||
body0->setWorldTransform(childtrans0);
|
||||
|
||||
shape1->getBulletTriangle(pair.m_index2,bullet_triangle);
|
||||
m_triface1 = pair.m_index2;
|
||||
|
||||
//collide two shapes
|
||||
shape_vs_shape_collision(body0,body1,colshape0,&bullet_triangle,swapped);
|
||||
|
||||
//restore transforms
|
||||
// body0->setCollisionShape(orgshape0);
|
||||
// body1->setCollisionShape(orgshape1);
|
||||
body1->setWorldTransform(orgtrans1);
|
||||
}
|
||||
|
||||
//unlock trimesh
|
||||
shape1->unlock();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpactcompound_vs_shape_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactCompoundShape * shape0,
|
||||
btCollisionShape * shape1,bool swapped)
|
||||
{
|
||||
btTransform orgtrans0 = body0->getWorldTransform();
|
||||
|
||||
btTransform trans1to0 = orgtrans0.inverse();
|
||||
trans1to0 *= body1->getWorldTransform();
|
||||
|
||||
GIM_AABB boxshape;
|
||||
shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
|
||||
gim_array<GUINT> collided_results;
|
||||
shape0->getBoxSet()->boxQuery(boxshape, collided_results);
|
||||
|
||||
if(collided_results.size() == 0) return;
|
||||
|
||||
|
||||
|
||||
GUINT i = collided_results.size();
|
||||
|
||||
while(i--)
|
||||
{
|
||||
btCollisionShape * colshape0 = shape0->getChildShape(collided_results[i]);
|
||||
btTransform childtrans0 = orgtrans0*shape0->getChildTransform(collided_results[i]);
|
||||
|
||||
body0->setWorldTransform(childtrans0);
|
||||
|
||||
//collide two shapes
|
||||
shape_vs_shape_collision(body0,body1,colshape0,shape1,swapped);
|
||||
|
||||
//restore transforms
|
||||
// body0->setCollisionShape(orgshape0);
|
||||
body0->setWorldTransform(orgtrans0);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_gimpacttrimeshpart_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShapePart * shape0,
|
||||
btGImpactMeshShapePart * shape1,bool swapped)
|
||||
{
|
||||
shape0->lock();
|
||||
shape1->lock();
|
||||
|
||||
btGImpactMeshShapePart * trishape0;
|
||||
btGImpactMeshShapePart * trishape1;
|
||||
btCollisionObject * tribody0;
|
||||
btCollisionObject * tribody1;
|
||||
|
||||
if(swapped)
|
||||
{
|
||||
trishape0 = shape1;
|
||||
trishape1 = shape0;
|
||||
tribody0 = body1;
|
||||
tribody1 = body0;
|
||||
}
|
||||
else
|
||||
{
|
||||
trishape0 = shape0;
|
||||
trishape1 = shape1;
|
||||
tribody0 = body0;
|
||||
tribody1 = body1;
|
||||
}
|
||||
|
||||
btTransform orgtrans0 = tribody0->getWorldTransform();
|
||||
btTransform orgtrans1 = tribody1->getWorldTransform();
|
||||
|
||||
gim_pair_set pairset;
|
||||
|
||||
gimpacttrimeshpart_vs_gimpacttrimeshpart_find_pairs(orgtrans0,orgtrans1,trishape0,trishape1,pairset);
|
||||
|
||||
if(pairset.size()== 0)
|
||||
{
|
||||
shape0->unlock();
|
||||
shape1->unlock();
|
||||
return;
|
||||
}
|
||||
|
||||
m_part0 = trishape0->getPart();
|
||||
m_part1 = trishape1->getPart();
|
||||
|
||||
#ifdef BULLET_TRIANGLE_COLLISION
|
||||
|
||||
btTriangleShapeEx tri0(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f));
|
||||
btTriangleShapeEx tri1(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f));
|
||||
GUINT i = pairset.size();
|
||||
while(i--)
|
||||
{
|
||||
const GIM_PAIR & pair = pairset[i];
|
||||
m_triface0 = pair.m_index1;
|
||||
m_triface1 = pair.m_index2;
|
||||
|
||||
trishape0->getBulletTriangle(pair.m_index1,tri0);
|
||||
trishape1->getBulletTriangle(pair.m_index2,tri1);
|
||||
|
||||
|
||||
convex_vs_convex_collision(
|
||||
tribody0,
|
||||
tribody1,
|
||||
&tri0,
|
||||
&tri1);
|
||||
|
||||
}
|
||||
//unlock
|
||||
shape0->unlock();
|
||||
shape1->unlock();
|
||||
|
||||
#else
|
||||
gim_contact_array tempcontacts;
|
||||
GIM_TRIANGLE tri0;
|
||||
GIM_TRIANGLE tri1;
|
||||
GIM_TRIANGLE_CONTACT_DATA contact_data;
|
||||
GUINT i = pairset.size();
|
||||
while(i--)
|
||||
{
|
||||
const GIM_PAIR & pair = pairset[i];
|
||||
|
||||
trishape0->getTriangle(pair.m_index1,tri0);
|
||||
trishape1->getTriangle(pair.m_index2,tri1);
|
||||
|
||||
tri0.apply_transform(orgtrans0);
|
||||
tri1.apply_transform(orgtrans1);
|
||||
|
||||
if(tri0.collide_triangle(tri1,contact_data))
|
||||
{
|
||||
tempcontacts.push_triangle_contacts(contact_data,pair.m_index1,pair.m_index2);
|
||||
}
|
||||
}
|
||||
//unlock
|
||||
shape0->unlock();
|
||||
shape1->unlock();
|
||||
|
||||
if(tempcontacts.size()==0) return;
|
||||
|
||||
//sort contacts
|
||||
gim_contact_array contacts;
|
||||
contacts.merge_contacts(tempcontacts,true);
|
||||
// put contacts
|
||||
m_part0 = trishape0->getPart();
|
||||
m_part1 = trishape1->getPart();
|
||||
i = contacts.size();
|
||||
while(i--)
|
||||
{
|
||||
GIM_CONTACT * pcontact = &contacts[i];
|
||||
|
||||
m_triface0 = pcontact->m_feature1;
|
||||
m_triface1 = pcontact->m_feature2;
|
||||
|
||||
addContactPoint(tribody0, tribody1,
|
||||
pcontact->m_point,
|
||||
pcontact->m_normal,
|
||||
-pcontact->m_depth);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShapePart * shape0,
|
||||
btStaticPlaneShape * shape1,bool swapped)
|
||||
{
|
||||
|
||||
|
||||
btTransform orgtrans0 = body0->getWorldTransform();
|
||||
btTransform orgtrans1 = body1->getWorldTransform();
|
||||
|
||||
btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1);
|
||||
btVector4 plane;
|
||||
planeshape->get_plane_equation_transformed(orgtrans1,plane);
|
||||
|
||||
//test box against plane
|
||||
|
||||
GIM_AABB tribox;
|
||||
shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
|
||||
tribox.increment_margin(planeshape->getMargin());
|
||||
|
||||
if( tribox.plane_classify(plane)!= G_COLLIDE_PLANE) return;
|
||||
|
||||
shape0->lock();
|
||||
|
||||
GREAL margin = shape0->getMargin() + planeshape->getMargin();
|
||||
|
||||
btVector3 vertex;
|
||||
GUINT vi = shape0->getVertexCount();
|
||||
while(vi--)
|
||||
{
|
||||
shape0->getVertex(vi,vertex);
|
||||
vertex = orgtrans0(vertex);
|
||||
|
||||
GREAL distance = vertex.dot(plane) - plane[3] - margin;
|
||||
|
||||
if(distance<0.0)//add contact
|
||||
{
|
||||
if(swapped)
|
||||
{
|
||||
addContactPoint(body1, body0,
|
||||
vertex,
|
||||
-plane,
|
||||
distance);
|
||||
}
|
||||
else
|
||||
{
|
||||
addContactPoint(body0, body1,
|
||||
vertex,
|
||||
plane,
|
||||
distance);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
shape0->unlock();
|
||||
}
|
||||
|
||||
|
||||
class btGImpactTriangleCallback: public btTriangleCallback
|
||||
{
|
||||
public:
|
||||
btGImpactCollisionAlgorithm * algorithm;
|
||||
btCollisionObject * body0;
|
||||
btCollisionObject * body1;
|
||||
btGImpactMeshShapePart * 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);
|
||||
algorithm->gimpacttrimeshpart_vs_shape_collision(
|
||||
body0,body1,gimpactshape0,&tri1,swapped);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_concave_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShapePart * shape0,
|
||||
btConcaveShape * shape1,bool swapped)
|
||||
{
|
||||
|
||||
//create the callback
|
||||
btGImpactTriangleCallback tricallback;
|
||||
tricallback.algorithm = this;
|
||||
tricallback.body0 = body0;
|
||||
tricallback.body1 = body1;
|
||||
tricallback.gimpactshape0 = shape0;
|
||||
tricallback.swapped = swapped;
|
||||
tricallback.margin = shape1->getMargin();
|
||||
|
||||
|
||||
|
||||
//getting the trimesh AABB
|
||||
btTransform gimpactInConcaveSpace;
|
||||
gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform();
|
||||
btVector3 minAABB,maxAABB;
|
||||
shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
|
||||
|
||||
shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_shape_collision(
|
||||
btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactMeshShapePart * shape0,
|
||||
btCollisionShape * shape1,bool swapped)
|
||||
{
|
||||
#ifdef GIMPACT_VS_PLANE_COLLISION
|
||||
if(shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
|
||||
{
|
||||
btStaticPlaneShape * plane1 = static_cast<btStaticPlaneShape * >(shape1);
|
||||
gimpacttrimeshpart_vs_plane_collision(body0,body1,shape0,plane1,swapped);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
if(shape1->isConcave())
|
||||
{
|
||||
btConcaveShape * concave1 = static_cast<btConcaveShape * >(shape1);
|
||||
gimpacttrimeshpart_vs_concave_collision(body0,body1,shape0,concave1,swapped);
|
||||
return;
|
||||
}
|
||||
|
||||
btTransform trans1to0 = body0->getWorldTransform().inverse();
|
||||
trans1to0 *= body1->getWorldTransform();
|
||||
|
||||
//lock
|
||||
shape0->lock();
|
||||
|
||||
GIM_AABB boxshape;
|
||||
shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
|
||||
gim_array<GUINT> collided_results(32);
|
||||
shape0->getBoxSet()->boxQuery(boxshape, collided_results);
|
||||
|
||||
if(collided_results.size() == 0)
|
||||
{
|
||||
shape0->unlock();
|
||||
return;
|
||||
}
|
||||
|
||||
btTriangleShapeEx bullet_triangle(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f));
|
||||
|
||||
|
||||
m_part0 = shape0->getPart();
|
||||
m_part1 = -1;
|
||||
m_triface1 = -1;
|
||||
|
||||
GUINT i = collided_results.size();
|
||||
|
||||
if(shape1->isConvex())
|
||||
{
|
||||
if(swapped)
|
||||
{
|
||||
while(i--)
|
||||
{
|
||||
m_triface0 = collided_results[i];
|
||||
shape0->getBulletTriangle(collided_results[i],bullet_triangle);
|
||||
//collide two shapes
|
||||
convex_vs_convex_collision(body1,body0,shape1,&bullet_triangle);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
while(i--)
|
||||
{
|
||||
m_triface0 = collided_results[i];
|
||||
shape0->getBulletTriangle(collided_results[i],bullet_triangle);
|
||||
//collide two shapes
|
||||
convex_vs_convex_collision(body0,body1,&bullet_triangle,shape1);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
while(i--)
|
||||
{
|
||||
m_triface0 = collided_results[i];
|
||||
shape0->getBulletTriangle(collided_results[i],bullet_triangle);
|
||||
//collide two shapes
|
||||
shape_vs_shape_collision(body0,body1,&bullet_triangle,shape1,swapped);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
shape0->unlock();
|
||||
}
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactShapeInterface * shape0,
|
||||
btCompoundShape * shape1,bool swapped)
|
||||
{
|
||||
btTransform orgtrans1 = body1->getWorldTransform();
|
||||
|
||||
int i = shape1->getNumChildShapes();
|
||||
while(i--)
|
||||
{
|
||||
|
||||
btCollisionShape * colshape1 = shape1->getChildShape(i);
|
||||
btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
|
||||
|
||||
body1->setWorldTransform(childtrans1);
|
||||
|
||||
//collide child shape
|
||||
gimpact_vs_shape(body0, body1,
|
||||
shape0,colshape1,swapped);
|
||||
|
||||
|
||||
//restore transforms
|
||||
body1->setWorldTransform(orgtrans1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactShapeInterface * shape0,
|
||||
btCollisionShape * shape1,bool swapped)
|
||||
{
|
||||
if(shape1->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
|
||||
{
|
||||
btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1);
|
||||
gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped);
|
||||
return;
|
||||
}
|
||||
|
||||
eGIMPACT_SHAPE_TYPE shapetype0 = shape0->getGImpactShapeType();
|
||||
if(shapetype0 == CONST_GIMPACT_TRIMESH_SHAPE)
|
||||
{
|
||||
btGImpactMeshShape * trimesh0 = static_cast<btGImpactMeshShape *>(shape0);
|
||||
gimpacttrimesh_vs_shape_collision(body0,body1,trimesh0,shape1,swapped);
|
||||
}
|
||||
else if(shapetype0 == CONST_GIMPACT_TRIMESH_SHAPE_PART)
|
||||
{
|
||||
btGImpactMeshShapePart * trimeshpart0 = static_cast<btGImpactMeshShapePart *>(shape0);
|
||||
gimpacttrimeshpart_vs_shape_collision(body0,body1,trimeshpart0,shape1,swapped);
|
||||
}
|
||||
else// compound
|
||||
{
|
||||
btGImpactCompoundShape * compound0 = static_cast<btGImpactCompoundShape*>(shape0);
|
||||
gimpactcompound_vs_shape_collision(body0,body1,compound0,shape1,swapped);
|
||||
}
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(btCollisionObject * body0,
|
||||
btCollisionObject * body1,
|
||||
btGImpactShapeInterface * shape0,
|
||||
btGImpactShapeInterface * shape1)
|
||||
{
|
||||
|
||||
eGIMPACT_SHAPE_TYPE shapetype0 = shape0->getGImpactShapeType();
|
||||
eGIMPACT_SHAPE_TYPE shapetype1 = shape1->getGImpactShapeType();
|
||||
|
||||
btGImpactMeshShape * trimesh0;
|
||||
btGImpactMeshShape * trimesh1;
|
||||
btGImpactMeshShapePart * trimeshpart0;
|
||||
btGImpactMeshShapePart * trimeshpart1;
|
||||
btGImpactCompoundShape * compound0;
|
||||
btGImpactCompoundShape * compound1;
|
||||
|
||||
|
||||
if(shapetype0 == CONST_GIMPACT_TRIMESH_SHAPE)
|
||||
{
|
||||
if(shapetype1 == CONST_GIMPACT_TRIMESH_SHAPE) // trimesh vs trimesh
|
||||
{
|
||||
trimesh0 = static_cast<btGImpactMeshShape *>(shape0);
|
||||
trimesh1 = static_cast<btGImpactMeshShape *>(shape1);
|
||||
gimpacttrimesh_vs_gimpacttrimesh(body0,body1,trimesh0,trimesh1);
|
||||
}
|
||||
else
|
||||
{
|
||||
if(shapetype1 == CONST_GIMPACT_COMPOUND_SHAPE) // trimesh vs compound
|
||||
{
|
||||
trimesh0 = static_cast<btGImpactMeshShape *>(shape0);
|
||||
compound1 = static_cast<btGImpactCompoundShape *>(shape1);
|
||||
gimpacttrimesh_vs_gimpactcompound(body0,body1,trimesh0,compound1,false);
|
||||
}
|
||||
else // trimesh vs trimesh part
|
||||
{
|
||||
trimesh0 = static_cast<btGImpactMeshShape *>(shape0);
|
||||
trimeshpart1 = static_cast<btGImpactMeshShapePart *>(shape1);
|
||||
gimpacttrimesh_vs_trimeshpart(body0,body1,trimesh0,trimeshpart1,false);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(shapetype1 == CONST_GIMPACT_TRIMESH_SHAPE)
|
||||
{
|
||||
if(shapetype0 == CONST_GIMPACT_COMPOUND_SHAPE) // compound vs trimesh
|
||||
{
|
||||
compound0 = static_cast<btGImpactCompoundShape *>(shape0);
|
||||
trimesh1 = static_cast<btGImpactMeshShape *>(shape1);
|
||||
gimpacttrimesh_vs_gimpactcompound(body1,body0,trimesh1,compound0,true);
|
||||
}
|
||||
else // trimesh part vs trimesh
|
||||
{
|
||||
trimeshpart0 = static_cast<btGImpactMeshShapePart *>(shape0);
|
||||
trimesh1 = static_cast<btGImpactMeshShape *>(shape1);
|
||||
gimpacttrimesh_vs_trimeshpart(body1,body0,trimesh1,trimeshpart0,true);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(shapetype0 == CONST_GIMPACT_COMPOUND_SHAPE)
|
||||
{
|
||||
if(shapetype1 == CONST_GIMPACT_COMPOUND_SHAPE) // compound vs compound
|
||||
{
|
||||
compound0 = static_cast<btGImpactCompoundShape *>(shape0);
|
||||
compound1 = static_cast<btGImpactCompoundShape *>(shape1);
|
||||
gimpactcompound_vs_gimpactcompound_collision(body0,body1,compound0,compound1);
|
||||
}
|
||||
else // compound vs trimesh part
|
||||
{
|
||||
compound0 = static_cast<btGImpactCompoundShape *>(shape0);
|
||||
trimeshpart1 = static_cast<btGImpactMeshShapePart *>(shape1);
|
||||
gimpactcompound_vs_gimpacttrimeshpart_collision(body0,body1,compound0,trimeshpart1,false);
|
||||
}
|
||||
}
|
||||
else if(shapetype1 == CONST_GIMPACT_COMPOUND_SHAPE) // trimesh part vs compound
|
||||
{
|
||||
compound1 = static_cast<btGImpactCompoundShape *>(shape1);
|
||||
trimeshpart0 = static_cast<btGImpactMeshShapePart *>(shape0);
|
||||
gimpactcompound_vs_gimpacttrimeshpart_collision(body1,body0,compound1,trimeshpart0,true);
|
||||
}
|
||||
else // trimesh part vs trimesh part
|
||||
{
|
||||
trimeshpart0 = static_cast<btGImpactMeshShapePart *>(shape0);
|
||||
trimeshpart1 = static_cast<btGImpactMeshShapePart *>(shape1);
|
||||
|
||||
gimpacttrimeshpart_vs_gimpacttrimeshpart_collision(body0,body1,trimeshpart0,trimeshpart1,false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
||||
{
|
||||
clearCache();
|
||||
|
||||
m_resultOut = resultOut;
|
||||
m_dispatchInfo = &dispatchInfo;
|
||||
|
||||
|
||||
btGImpactShapeInterface * gimpactshape0;
|
||||
btGImpactShapeInterface * gimpactshape1;
|
||||
if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
|
||||
{
|
||||
if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
|
||||
{
|
||||
gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape());
|
||||
gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
|
||||
gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1);
|
||||
}
|
||||
else
|
||||
{
|
||||
gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape());
|
||||
gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false);
|
||||
}
|
||||
}
|
||||
else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
|
||||
{
|
||||
gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
|
||||
gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
for (GUINT i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
|
||||
{
|
||||
dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,new btGImpactCollisionAlgorithm::CreateFunc);
|
||||
}
|
||||
|
||||
for (GUINT i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
|
||||
{
|
||||
dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,new btGImpactCollisionAlgorithm::CreateFunc);
|
||||
}
|
||||
|
||||
/*
|
||||
dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE,new btGImpactCollisionAlgorithm::CreateFunc);
|
||||
dispatcher->registerCollisionCreateFunc(STATIC_PLANE_PROXYTYPE ,GIMPACT_SHAPE_PROXYTYPE,new btGImpactCollisionAlgorithm::CreateFunc);
|
||||
dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,STATIC_PLANE_PROXYTYPE ,new btGImpactCollisionAlgorithm::CreateFunc);*/
|
||||
|
||||
|
||||
}
|
||||
194
Extras/GIMPACT/src/Bullet/btGImpactShape.cpp
Executable file
194
Extras/GIMPACT/src/Bullet/btGImpactShape.cpp
Executable file
@@ -0,0 +1,194 @@
|
||||
/*
|
||||
-----------------------------------------------------------------------------
|
||||
This source file is part of GIMPACT Library.
|
||||
|
||||
For the latest info, see http://gimpact.sourceforge.net/
|
||||
|
||||
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
|
||||
email: projectileman@yahoo.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of EITHER:
|
||||
(1) The GNU Lesser General Public License as published by the Free
|
||||
Software Foundation; either version 2.1 of the License, or (at
|
||||
your option) any later version. The text of the GNU Lesser
|
||||
General Public License is included with this library in the
|
||||
file GIMPACT-LICENSE-LGPL.TXT.
|
||||
(2) The BSD-style license that is included with this library in
|
||||
the file GIMPACT-LICENSE-BSD.TXT.
|
||||
(3) The zlib/libpng license that is included with this library in
|
||||
the file GIMPACT-LICENSE-ZLIB.TXT.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
||||
GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
||||
|
||||
-----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "GIMPACT/Bullet/btGImpactShape.h"
|
||||
#include "GIMPACT/Bullet/btGImpactMassUtil.h"
|
||||
|
||||
|
||||
#define CALC_EXACT_INERTIA 1
|
||||
|
||||
void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
{
|
||||
|
||||
#ifdef CALC_EXACT_INERTIA
|
||||
inertia.setValue(0.f,0.f,0.f);
|
||||
|
||||
GUINT i = this->getNumChildShapes();
|
||||
GREAL shapemass = mass/btScalar(i);
|
||||
|
||||
while(i--)
|
||||
{
|
||||
btVector3 temp_inertia;
|
||||
m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
|
||||
inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
// Calc box inertia
|
||||
|
||||
btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
|
||||
btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
|
||||
btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
|
||||
const btScalar x2 = lx*lx;
|
||||
const btScalar y2 = ly*ly;
|
||||
const btScalar z2 = lz*lz;
|
||||
const btScalar scaledmass = mass * btScalar(0.08333333);
|
||||
|
||||
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
{
|
||||
lock();
|
||||
|
||||
|
||||
#ifdef CALC_EXACT_INERTIA
|
||||
inertia.setValue(0.f,0.f,0.f);
|
||||
|
||||
GUINT i = this->getVertexCount();
|
||||
GREAL pointmass = mass/btScalar(i);
|
||||
|
||||
while(i--)
|
||||
{
|
||||
btVector3 pointintertia;
|
||||
this->getVertex(i,pointintertia);
|
||||
pointintertia = gim_get_point_inertia(pointintertia,pointmass);
|
||||
inertia+=pointintertia;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
// Calc box inertia
|
||||
|
||||
btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
|
||||
btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
|
||||
btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
|
||||
const btScalar x2 = lx*lx;
|
||||
const btScalar y2 = ly*ly;
|
||||
const btScalar z2 = lz*lz;
|
||||
const btScalar scaledmass = mass * btScalar(0.08333333);
|
||||
|
||||
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
|
||||
|
||||
#endif
|
||||
|
||||
unlock();
|
||||
}
|
||||
|
||||
void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
{
|
||||
|
||||
#ifdef CALC_EXACT_INERTIA
|
||||
inertia.setValue(0.f,0.f,0.f);
|
||||
|
||||
GUINT i = this->getMeshPartCount();
|
||||
GREAL partmass = mass/btScalar(i);
|
||||
|
||||
while(i--)
|
||||
{
|
||||
btVector3 partinertia;
|
||||
getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
|
||||
inertia+=partinertia;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
// Calc box inertia
|
||||
|
||||
btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
|
||||
btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
|
||||
btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
|
||||
const btScalar x2 = lx*lx;
|
||||
const btScalar y2 = ly*ly;
|
||||
const btScalar z2 = lz*lz;
|
||||
const btScalar scaledmass = mass * btScalar(0.08333333);
|
||||
|
||||
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGImpactCompoundShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
|
||||
{
|
||||
}
|
||||
|
||||
void btGImpactMeshShapePart::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
|
||||
{
|
||||
lock();
|
||||
GIM_AABB box;
|
||||
box.m_min = aabbMin;
|
||||
box.m_max = aabbMax;
|
||||
|
||||
gim_array<GUINT> collided;
|
||||
m_box_set.boxQuery(box,collided);
|
||||
|
||||
if(collided.size()==0)
|
||||
{
|
||||
unlock();
|
||||
return;
|
||||
}
|
||||
|
||||
int part = (int)getPart();
|
||||
GIM_TRIANGLE triangle;
|
||||
GUINT i = collided.size();
|
||||
while(i--)
|
||||
{
|
||||
this->getTriangle(collided[i],triangle);
|
||||
callback->processTriangle(triangle.m_vertices,part,collided[i]);
|
||||
}
|
||||
unlock();
|
||||
|
||||
}
|
||||
|
||||
void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
|
||||
{
|
||||
GUINT i = m_mesh_parts.size();
|
||||
while(i--)
|
||||
{
|
||||
m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax);
|
||||
}
|
||||
}
|
||||
182
Extras/GIMPACT/src/core/gim_box_set.cpp
Executable file
182
Extras/GIMPACT/src/core/gim_box_set.cpp
Executable file
@@ -0,0 +1,182 @@
|
||||
|
||||
/*
|
||||
-----------------------------------------------------------------------------
|
||||
This source file is part of GIMPACT Library.
|
||||
|
||||
For the latest info, see http://gimpact.sourceforge.net/
|
||||
|
||||
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
|
||||
email: projectileman@yahoo.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of EITHER:
|
||||
(1) The GNU Lesser General Public License as published by the Free
|
||||
Software Foundation; either version 2.1 of the License, or (at
|
||||
your option) any later version. The text of the GNU Lesser
|
||||
General Public License is included with this library in the
|
||||
file GIMPACT-LICENSE-LGPL.TXT.
|
||||
(2) The BSD-style license that is included with this library in
|
||||
the file GIMPACT-LICENSE-BSD.TXT.
|
||||
(3) The zlib/libpng license that is included with this library in
|
||||
the file GIMPACT-LICENSE-ZLIB.TXT.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
||||
GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
||||
|
||||
-----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "GIMPACT/core/gim_box_set.h"
|
||||
|
||||
|
||||
GUINT GIM_BOX_TREE::_calc_splitting_axis(
|
||||
gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex)
|
||||
{
|
||||
GUINT i;
|
||||
|
||||
btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
GUINT numIndices = endIndex-startIndex;
|
||||
|
||||
for (i=startIndex;i<endIndex;i++)
|
||||
{
|
||||
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
|
||||
primitive_boxes[i].m_bound.m_min);
|
||||
means+=center;
|
||||
}
|
||||
means *= (btScalar(1.)/(btScalar)numIndices);
|
||||
|
||||
for (i=startIndex;i<endIndex;i++)
|
||||
{
|
||||
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
|
||||
primitive_boxes[i].m_bound.m_min);
|
||||
btVector3 diff2 = center-means;
|
||||
diff2 = diff2 * diff2;
|
||||
variance += diff2;
|
||||
}
|
||||
variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
|
||||
|
||||
return variance.maxAxis();
|
||||
}
|
||||
|
||||
|
||||
GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
|
||||
gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,
|
||||
GUINT endIndex, GUINT splitAxis)
|
||||
{
|
||||
GUINT i;
|
||||
GUINT splitIndex =startIndex;
|
||||
GUINT numIndices = endIndex - startIndex;
|
||||
|
||||
// average of centers
|
||||
btScalar splitValue = 0.0f;
|
||||
for (i=startIndex;i<endIndex;i++)
|
||||
{
|
||||
splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
|
||||
primitive_boxes[i].m_bound.m_min[splitAxis]);
|
||||
}
|
||||
splitValue /= (btScalar)numIndices;
|
||||
|
||||
//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
|
||||
for (i=startIndex;i<endIndex;i++)
|
||||
{
|
||||
btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
|
||||
primitive_boxes[i].m_bound.m_min[splitAxis]);
|
||||
if (center > splitValue)
|
||||
{
|
||||
//swap
|
||||
primitive_boxes.swap(i,splitIndex);
|
||||
splitIndex++;
|
||||
}
|
||||
}
|
||||
|
||||
//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
|
||||
//otherwise the tree-building might fail due to stack-overflows in certain cases.
|
||||
//unbalanced1 is unsafe: it can cause stack overflows
|
||||
//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
|
||||
|
||||
//unbalanced2 should work too: always use center (perfect balanced trees)
|
||||
//bool unbalanced2 = true;
|
||||
|
||||
//this should be safe too:
|
||||
GUINT rangeBalancedIndices = numIndices/3;
|
||||
bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
|
||||
|
||||
if (unbalanced)
|
||||
{
|
||||
splitIndex = startIndex+ (numIndices>>1);
|
||||
}
|
||||
|
||||
bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
|
||||
btAssert(!unbal);
|
||||
|
||||
return splitIndex;
|
||||
}
|
||||
|
||||
|
||||
void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex)
|
||||
{
|
||||
GUINT current_index = m_num_nodes++;
|
||||
|
||||
btAssert((endIndex-startIndex)>0);
|
||||
|
||||
if((endIndex-startIndex) == 1) //we got a leaf
|
||||
{
|
||||
m_node_array[current_index].m_left = 0;
|
||||
m_node_array[current_index].m_right = 0;
|
||||
m_node_array[current_index].m_escapeIndex = 0;
|
||||
|
||||
m_node_array[current_index].m_bound = primitive_boxes[startIndex].m_bound;
|
||||
m_node_array[current_index].m_data = primitive_boxes[startIndex].m_data;
|
||||
return;
|
||||
}
|
||||
|
||||
//configure inner node
|
||||
|
||||
GUINT splitIndex;
|
||||
|
||||
//calc this node bounding box
|
||||
m_node_array[current_index].m_bound.invalidate();
|
||||
for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++)
|
||||
{
|
||||
m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound);
|
||||
}
|
||||
|
||||
//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
|
||||
|
||||
//split axis
|
||||
splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
|
||||
|
||||
splitIndex = _sort_and_calc_splitting_index(
|
||||
primitive_boxes,startIndex,endIndex,splitIndex);
|
||||
|
||||
//configure this inner node : the left node index
|
||||
m_node_array[current_index].m_left = m_num_nodes;
|
||||
//build left child tree
|
||||
_build_sub_tree(primitive_boxes, startIndex, splitIndex );
|
||||
|
||||
//configure this inner node : the right node index
|
||||
m_node_array[current_index].m_right = m_num_nodes;
|
||||
|
||||
//build right child tree
|
||||
_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
|
||||
|
||||
//configure this inner node : the escape index
|
||||
m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index;
|
||||
}
|
||||
|
||||
//! stackless build tree
|
||||
void GIM_BOX_TREE::build_tree(
|
||||
gim_array<GIM_AABB_DATA> & primitive_boxes)
|
||||
{
|
||||
// initialize node count to 0
|
||||
m_num_nodes = 0;
|
||||
// allocate nodes
|
||||
m_node_array.resize(primitive_boxes.size()*2);
|
||||
|
||||
_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
|
||||
}
|
||||
|
||||
142
Extras/GIMPACT/src/core/gim_contact.cpp
Executable file
142
Extras/GIMPACT/src/core/gim_contact.cpp
Executable file
@@ -0,0 +1,142 @@
|
||||
|
||||
/*
|
||||
-----------------------------------------------------------------------------
|
||||
This source file is part of GIMPACT Library.
|
||||
|
||||
For the latest info, see http://gimpact.sourceforge.net/
|
||||
|
||||
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
|
||||
email: projectileman@yahoo.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of EITHER:
|
||||
(1) The GNU Lesser General Public License as published by the Free
|
||||
Software Foundation; either version 2.1 of the License, or (at
|
||||
your option) any later version. The text of the GNU Lesser
|
||||
General Public License is included with this library in the
|
||||
file GIMPACT-LICENSE-LGPL.TXT.
|
||||
(2) The BSD-style license that is included with this library in
|
||||
the file GIMPACT-LICENSE-BSD.TXT.
|
||||
(3) The zlib/libpng license that is included with this library in
|
||||
the file GIMPACT-LICENSE-ZLIB.TXT.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
||||
GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
||||
|
||||
-----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "GIMPACT/core/gim_contact.h"
|
||||
|
||||
#define MAX_COINCIDENT 8
|
||||
|
||||
void gim_contact_array::merge_contacts(
|
||||
const gim_contact_array & contacts, bool normal_contact_average)
|
||||
{
|
||||
clear();
|
||||
|
||||
if(contacts.size()==1)
|
||||
{
|
||||
push_back(contacts.back());
|
||||
return;
|
||||
}
|
||||
|
||||
gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
|
||||
keycontacts.resize(contacts.size(),false);
|
||||
|
||||
//fill key contacts
|
||||
|
||||
for (GUINT i = 0;i<contacts.size() ;i++ )
|
||||
{
|
||||
keycontacts[i].m_key = contacts[i].calc_key_contact();
|
||||
keycontacts[i].m_value = i;
|
||||
}
|
||||
|
||||
//sort keys
|
||||
gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
|
||||
|
||||
// Merge contacts
|
||||
|
||||
GUINT coincident_count=0;
|
||||
btVector3 coincident_normals[MAX_COINCIDENT];
|
||||
|
||||
GUINT last_key = keycontacts[0].m_key;
|
||||
GUINT key = 0;
|
||||
|
||||
push_back(contacts[keycontacts[0].m_value]);
|
||||
GIM_CONTACT * pcontact = &back();
|
||||
|
||||
for(GUINT i=1;i<keycontacts.size();i++)
|
||||
{
|
||||
key = keycontacts[i].m_key;
|
||||
const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
|
||||
|
||||
if(last_key == key)//same points
|
||||
{
|
||||
//merge contact
|
||||
if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
|
||||
{
|
||||
*pcontact = *scontact;
|
||||
coincident_count = 0;
|
||||
}
|
||||
else if(normal_contact_average)
|
||||
{
|
||||
if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
|
||||
{
|
||||
if(coincident_count<MAX_COINCIDENT)
|
||||
{
|
||||
coincident_normals[coincident_count] = scontact->m_normal;
|
||||
coincident_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{//add new contact
|
||||
|
||||
if(normal_contact_average && coincident_count>0)
|
||||
{
|
||||
pcontact->interpolate_normals(coincident_normals,coincident_count);
|
||||
coincident_count = 0;
|
||||
}
|
||||
|
||||
push_back(*scontact);
|
||||
pcontact = &back();
|
||||
}
|
||||
last_key = key;
|
||||
}
|
||||
}
|
||||
|
||||
void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
|
||||
{
|
||||
clear();
|
||||
|
||||
if(contacts.size()==1)
|
||||
{
|
||||
push_back(contacts.back());
|
||||
return;
|
||||
}
|
||||
|
||||
GIM_CONTACT average_contact = contacts.back();
|
||||
|
||||
for (GUINT i=1;i<contacts.size() ;i++ )
|
||||
{
|
||||
average_contact.m_point += contacts[i].m_point;
|
||||
average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
|
||||
}
|
||||
|
||||
//divide
|
||||
GREAL divide_average = 1.0f/((GREAL)contacts.size());
|
||||
|
||||
average_contact.m_point *= divide_average;
|
||||
|
||||
average_contact.m_normal *= divide_average;
|
||||
|
||||
average_contact.m_depth = average_contact.m_normal.length();
|
||||
|
||||
average_contact.m_normal /= average_contact.m_depth;
|
||||
|
||||
}
|
||||
|
||||
135
Extras/GIMPACT/src/core/gim_memory.cpp
Executable file
135
Extras/GIMPACT/src/core/gim_memory.cpp
Executable file
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
-----------------------------------------------------------------------------
|
||||
This source file is part of GIMPACT Library.
|
||||
|
||||
For the latest info, see http://gimpact.sourceforge.net/
|
||||
|
||||
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
|
||||
email: projectileman@yahoo.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of EITHER:
|
||||
(1) The GNU Lesser General Public License as published by the Free
|
||||
Software Foundation; either version 2.1 of the License, or (at
|
||||
your option) any later version. The text of the GNU Lesser
|
||||
General Public License is included with this library in the
|
||||
file GIMPACT-LICENSE-LGPL.TXT.
|
||||
(2) The BSD-style license that is included with this library in
|
||||
the file GIMPACT-LICENSE-BSD.TXT.
|
||||
(3) The zlib/libpng license that is included with this library in
|
||||
the file GIMPACT-LICENSE-ZLIB.TXT.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
||||
GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
||||
|
||||
-----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "GIMPACT/core/gim_memory.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
#ifdef GIM_SIMD_MEMORY
|
||||
#include "LinearMath/btAlignedAllocator.h"
|
||||
#endif
|
||||
|
||||
static gim_alloc_function *g_allocfn = 0;
|
||||
static gim_alloca_function *g_allocafn = 0;
|
||||
static gim_realloc_function *g_reallocfn = 0;
|
||||
static gim_free_function *g_freefn = 0;
|
||||
|
||||
void gim_set_alloc_handler (gim_alloc_function *fn)
|
||||
{
|
||||
g_allocfn = fn;
|
||||
}
|
||||
|
||||
void gim_set_alloca_handler (gim_alloca_function *fn)
|
||||
{
|
||||
g_allocafn = fn;
|
||||
}
|
||||
|
||||
void gim_set_realloc_handler (gim_realloc_function *fn)
|
||||
{
|
||||
g_reallocfn = fn;
|
||||
}
|
||||
|
||||
void gim_set_free_handler (gim_free_function *fn)
|
||||
{
|
||||
g_freefn = fn;
|
||||
}
|
||||
|
||||
gim_alloc_function *gim_get_alloc_handler()
|
||||
{
|
||||
return g_allocfn;
|
||||
}
|
||||
|
||||
gim_alloca_function *gim_get_alloca_handler()
|
||||
{
|
||||
return g_allocafn;
|
||||
}
|
||||
|
||||
|
||||
gim_realloc_function *gim_get_realloc_handler ()
|
||||
{
|
||||
return g_reallocfn;
|
||||
}
|
||||
|
||||
|
||||
gim_free_function *gim_get_free_handler ()
|
||||
{
|
||||
return g_freefn;
|
||||
}
|
||||
|
||||
|
||||
void * gim_alloc(size_t size)
|
||||
{
|
||||
void * ptr;
|
||||
if (g_allocfn)
|
||||
{
|
||||
ptr = g_allocfn(size);
|
||||
}
|
||||
else
|
||||
{
|
||||
#ifdef GIM_SIMD_MEMORY
|
||||
ptr = btAlignedAlloc(size,16);
|
||||
#else
|
||||
ptr = malloc(size);
|
||||
#endif
|
||||
}
|
||||
return ptr;
|
||||
}
|
||||
|
||||
void * gim_alloca(size_t size)
|
||||
{
|
||||
if (g_allocafn) return g_allocafn(size); else return gim_alloc(size);
|
||||
}
|
||||
|
||||
|
||||
void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
|
||||
{
|
||||
void * newptr = gim_alloc(newsize);
|
||||
size_t copysize = oldsize<newsize?oldsize:newsize;
|
||||
gim_simd_memcpy(newptr,ptr,copysize);
|
||||
gim_free(ptr);
|
||||
return newptr;
|
||||
}
|
||||
|
||||
void gim_free(void *ptr)
|
||||
{
|
||||
if (!ptr) return;
|
||||
if (g_freefn)
|
||||
{
|
||||
g_freefn(ptr);
|
||||
}
|
||||
else
|
||||
{
|
||||
#ifdef GIM_SIMD_MEMORY
|
||||
btAlignedFree(ptr);
|
||||
#else
|
||||
free(ptr);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
640
Extras/GIMPACT/src/core/gim_tri_collision.cpp
Executable file
640
Extras/GIMPACT/src/core/gim_tri_collision.cpp
Executable file
@@ -0,0 +1,640 @@
|
||||
|
||||
/*! \file gim_tri_collision.h
|
||||
\author Francisco Le<4C>n N<>jera
|
||||
*/
|
||||
/*
|
||||
-----------------------------------------------------------------------------
|
||||
This source file is part of GIMPACT Library.
|
||||
|
||||
For the latest info, see http://gimpact.sourceforge.net/
|
||||
|
||||
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
|
||||
email: projectileman@yahoo.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of EITHER:
|
||||
(1) The GNU Lesser General Public License as published by the Free
|
||||
Software Foundation; either version 2.1 of the License, or (at
|
||||
your option) any later version. The text of the GNU Lesser
|
||||
General Public License is included with this library in the
|
||||
file GIMPACT-LICENSE-LGPL.TXT.
|
||||
(2) The BSD-style license that is included with this library in
|
||||
the file GIMPACT-LICENSE-BSD.TXT.
|
||||
(3) The zlib/libpng license that is included with this library in
|
||||
the file GIMPACT-LICENSE-ZLIB.TXT.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
||||
GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
||||
|
||||
-----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "GIMPACT/core/gim_tri_collision.h"
|
||||
|
||||
|
||||
#define TRI_LOCAL_EPSILON 0.000001f
|
||||
#define MIN_EDGE_EDGE_DIS 0.00001f
|
||||
|
||||
|
||||
class _GIM_TRIANGLE_CALCULATION_CACHE
|
||||
{
|
||||
public:
|
||||
GREAL margin;
|
||||
btVector3 tu_vertices[3];
|
||||
btVector3 tv_vertices[3];
|
||||
btVector4 tu_plane;
|
||||
btVector4 tv_plane;
|
||||
btVector3 closest_point_u;
|
||||
btVector3 closest_point_v;
|
||||
btVector3 edge_edge_dir;
|
||||
btVector3 distances;
|
||||
GREAL du[4];
|
||||
GREAL du0du1;
|
||||
GREAL du0du2;
|
||||
GREAL dv[4];
|
||||
GREAL dv0dv1;
|
||||
GREAL dv0dv2;
|
||||
btVector3 temp_points[MAX_TRI_CLIPPING];
|
||||
btVector3 temp_points1[MAX_TRI_CLIPPING];
|
||||
btVector3 contact_points[MAX_TRI_CLIPPING];
|
||||
|
||||
|
||||
|
||||
//! if returns false, the faces are paralele
|
||||
SIMD_FORCE_INLINE bool compute_intervals(
|
||||
const GREAL &D0,
|
||||
const GREAL &D1,
|
||||
const GREAL &D2,
|
||||
const GREAL &D0D1,
|
||||
const GREAL &D0D2,
|
||||
GREAL & scale_edge0,
|
||||
GREAL & scale_edge1,
|
||||
GUINT &edge_index0,
|
||||
GUINT &edge_index1)
|
||||
{
|
||||
if(D0D1>0.0f)
|
||||
{
|
||||
/* here we know that D0D2<=0.0 */
|
||||
/* that is D0, D1 are on the same side, D2 on the other or on the plane */
|
||||
scale_edge0 = -D2/(D0-D2);
|
||||
scale_edge1 = -D1/(D2-D1);
|
||||
edge_index0 = 2;edge_index1 = 1;
|
||||
}
|
||||
else if(D0D2>0.0f)
|
||||
{
|
||||
/* here we know that d0d1<=0.0 */
|
||||
scale_edge0 = -D0/(D1-D0);
|
||||
scale_edge1 = -D1/(D2-D1);
|
||||
edge_index0 = 0;edge_index1 = 1;
|
||||
}
|
||||
else if(D1*D2>0.0f || D0!=0.0f)
|
||||
{
|
||||
/* here we know that d0d1<=0.0 or that D0!=0.0 */
|
||||
scale_edge0 = -D0/(D1-D0);
|
||||
scale_edge1 = -D2/(D0-D2);
|
||||
edge_index0 = 0 ;edge_index1 = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
//! clip triangle
|
||||
/*!
|
||||
*/
|
||||
SIMD_FORCE_INLINE GUINT clip_triangle(
|
||||
const btVector4 & tri_plane,
|
||||
const btVector3 * tripoints,
|
||||
const btVector3 * srcpoints,
|
||||
btVector3 * clip_points)
|
||||
{
|
||||
// edge 0
|
||||
|
||||
btVector4 edgeplane;
|
||||
|
||||
EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
|
||||
|
||||
GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
|
||||
edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
|
||||
|
||||
if(clipped_count == 0) return 0;
|
||||
|
||||
// edge 1
|
||||
|
||||
EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
|
||||
|
||||
clipped_count = PLANE_CLIP_POLYGON3D(
|
||||
edgeplane,temp_points,clipped_count,temp_points1);
|
||||
|
||||
if(clipped_count == 0) return 0;
|
||||
|
||||
// edge 2
|
||||
|
||||
EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
|
||||
|
||||
clipped_count = PLANE_CLIP_POLYGON3D(
|
||||
edgeplane,temp_points1,clipped_count,clip_points);
|
||||
|
||||
return clipped_count;
|
||||
|
||||
|
||||
/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
|
||||
GUINT i1 = (i0+1)%3;
|
||||
// edge 0
|
||||
btVector3 temp_points[MAX_TRI_CLIPPING];
|
||||
btVector3 temp_points1[MAX_TRI_CLIPPING];
|
||||
|
||||
GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
|
||||
0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
|
||||
DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));
|
||||
|
||||
|
||||
if(clipped_count == 0) return 0;
|
||||
|
||||
// edge 1
|
||||
clipped_count = PLANE_CLIP_POLYGON_GENERIC(
|
||||
0,temp_points,clipped_count,temp_points1,
|
||||
DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
|
||||
|
||||
if(clipped_count == 0) return 0;
|
||||
|
||||
// edge 2
|
||||
clipped_count = PLANE_CLIP_POLYGON_GENERIC(
|
||||
0,temp_points1,clipped_count,clipped_points,
|
||||
DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
|
||||
|
||||
return clipped_count;*/
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void sort_isect(
|
||||
GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
|
||||
{
|
||||
if(isect1<isect0)
|
||||
{
|
||||
//swap
|
||||
GIM_SWAP_NUMBERS(isect0,isect1);
|
||||
GIM_SWAP_NUMBERS(e0,e1);
|
||||
btVector3 tmp = vec0;
|
||||
vec0 = vec1;
|
||||
vec1 = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
//! Test verifying interval intersection with the direction between planes
|
||||
/*!
|
||||
\pre tv_plane and tu_plane must be set
|
||||
\post
|
||||
distances[2] is set with the distance
|
||||
closest_point_u, closest_point_v, edge_edge_dir are set too
|
||||
\return
|
||||
- 0: faces are paralele
|
||||
- 1: face U casts face V
|
||||
- 2: face V casts face U
|
||||
- 3: nearest edges
|
||||
*/
|
||||
SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
|
||||
{
|
||||
// Compute direction of intersection line
|
||||
edge_edge_dir = tu_plane.cross(tv_plane);
|
||||
GREAL Dlen;
|
||||
VEC_LENGTH(edge_edge_dir,Dlen);
|
||||
|
||||
if(Dlen<0.0001)
|
||||
{
|
||||
return 0; //faces near paralele
|
||||
}
|
||||
|
||||
edge_edge_dir*= 1/Dlen;//normalize
|
||||
|
||||
|
||||
// Compute interval for triangle 1
|
||||
GUINT tu_e0,tu_e1;//edge indices
|
||||
GREAL tu_scale_e0,tu_scale_e1;//edge scale
|
||||
if(!compute_intervals(du[0],du[1],du[2],
|
||||
du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
|
||||
|
||||
// Compute interval for triangle 2
|
||||
GUINT tv_e0,tv_e1;//edge indices
|
||||
GREAL tv_scale_e0,tv_scale_e1;//edge scale
|
||||
|
||||
if(!compute_intervals(dv[0],dv[1],dv[2],
|
||||
dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
|
||||
|
||||
//proyected vertices
|
||||
btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
|
||||
btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
|
||||
|
||||
btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
|
||||
btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
|
||||
|
||||
//proyected intervals
|
||||
GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
|
||||
GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
|
||||
|
||||
sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
|
||||
sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
|
||||
|
||||
const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
|
||||
const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
|
||||
|
||||
if(midpoint_u<midpoint_v)
|
||||
{
|
||||
if(isect_u[1]>=isect_v[1]) // face U casts face V
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
else if(isect_v[0]<=isect_u[0]) // face V casts face U
|
||||
{
|
||||
return 2;
|
||||
}
|
||||
// closest points
|
||||
closest_point_u = up_e1;
|
||||
closest_point_v = vp_e0;
|
||||
// calc edges and separation
|
||||
|
||||
if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
|
||||
{
|
||||
SEGMENT_COLLISION(
|
||||
tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
|
||||
tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
|
||||
closest_point_u,
|
||||
closest_point_v);
|
||||
|
||||
edge_edge_dir = closest_point_u-closest_point_v;
|
||||
VEC_LENGTH(edge_edge_dir,distances[2]);
|
||||
edge_edge_dir *= 1.0f/distances[2];// normalize
|
||||
}
|
||||
else
|
||||
{
|
||||
distances[2] = isect_v[0]-isect_u[1];//distance negative
|
||||
//edge_edge_dir *= -1.0f; //normal pointing from V to U
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
if(isect_v[1]>=isect_u[1]) // face V casts face U
|
||||
{
|
||||
return 2;
|
||||
}
|
||||
else if(isect_u[0]<=isect_v[0]) // face U casts face V
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
// closest points
|
||||
closest_point_u = up_e0;
|
||||
closest_point_v = vp_e1;
|
||||
// calc edges and separation
|
||||
|
||||
if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
|
||||
{
|
||||
SEGMENT_COLLISION(
|
||||
tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
|
||||
tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
|
||||
closest_point_u,
|
||||
closest_point_v);
|
||||
|
||||
edge_edge_dir = closest_point_u-closest_point_v;
|
||||
VEC_LENGTH(edge_edge_dir,distances[2]);
|
||||
edge_edge_dir *= 1.0f/distances[2];// normalize
|
||||
}
|
||||
else
|
||||
{
|
||||
distances[2] = isect_u[0]-isect_v[1];//distance negative
|
||||
//edge_edge_dir *= -1.0f; //normal pointing from V to U
|
||||
}
|
||||
}
|
||||
return 3;
|
||||
}
|
||||
|
||||
|
||||
//! collides by two sides
|
||||
SIMD_FORCE_INLINE bool triangle_collision(
|
||||
const btVector3 & u0,
|
||||
const btVector3 & u1,
|
||||
const btVector3 & u2,
|
||||
GREAL margin_u,
|
||||
const btVector3 & v0,
|
||||
const btVector3 & v1,
|
||||
const btVector3 & v2,
|
||||
GREAL margin_v,
|
||||
GIM_TRIANGLE_CONTACT_DATA & contacts)
|
||||
{
|
||||
|
||||
margin = margin_u + margin_v;
|
||||
|
||||
tu_vertices[0] = u0;
|
||||
tu_vertices[1] = u1;
|
||||
tu_vertices[2] = u2;
|
||||
|
||||
tv_vertices[0] = v0;
|
||||
tv_vertices[1] = v1;
|
||||
tv_vertices[2] = v2;
|
||||
|
||||
//create planes
|
||||
// plane v vs U points
|
||||
|
||||
TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
|
||||
|
||||
du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
|
||||
du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
|
||||
du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
|
||||
|
||||
|
||||
du0du1 = du[0] * du[1];
|
||||
du0du2 = du[0] * du[2];
|
||||
|
||||
|
||||
if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
|
||||
{
|
||||
if(du[0]<0) //we need test behind the triangle plane
|
||||
{
|
||||
distances[0] = GIM_MAX3(du[0],du[1],du[2]);
|
||||
distances[0] = -distances[0];
|
||||
if(distances[0]>margin) return false; //never intersect
|
||||
|
||||
//reorder triangle v
|
||||
VEC_SWAP(tv_vertices[0],tv_vertices[1]);
|
||||
VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
|
||||
}
|
||||
else
|
||||
{
|
||||
distances[0] = GIM_MIN3(du[0],du[1],du[2]);
|
||||
if(distances[0]>margin) return false; //never intersect
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//Look if we need to invert the triangle
|
||||
distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
|
||||
|
||||
if(distances[0]<0.0f)
|
||||
{
|
||||
//reorder triangle v
|
||||
VEC_SWAP(tv_vertices[0],tv_vertices[1]);
|
||||
VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
|
||||
|
||||
distances[0] = GIM_MAX3(du[0],du[1],du[2]);
|
||||
distances[0] = -distances[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
distances[0] = GIM_MIN3(du[0],du[1],du[2]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// plane U vs V points
|
||||
|
||||
TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
|
||||
|
||||
dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
|
||||
dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
|
||||
dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
|
||||
|
||||
dv0dv1 = dv[0] * dv[1];
|
||||
dv0dv2 = dv[0] * dv[2];
|
||||
|
||||
|
||||
if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
|
||||
{
|
||||
if(dv[0]<0) //we need test behind the triangle plane
|
||||
{
|
||||
distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
|
||||
distances[1] = -distances[1];
|
||||
if(distances[1]>margin) return false; //never intersect
|
||||
|
||||
//reorder triangle u
|
||||
VEC_SWAP(tu_vertices[0],tu_vertices[1]);
|
||||
VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
|
||||
}
|
||||
else
|
||||
{
|
||||
distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
|
||||
if(distances[1]>margin) return false; //never intersect
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//Look if we need to invert the triangle
|
||||
distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
|
||||
|
||||
if(distances[1]<0.0f)
|
||||
{
|
||||
//reorder triangle v
|
||||
VEC_SWAP(tu_vertices[0],tu_vertices[1]);
|
||||
VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
|
||||
|
||||
distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
|
||||
distances[1] = -distances[1];
|
||||
}
|
||||
else
|
||||
{
|
||||
distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
|
||||
}
|
||||
}
|
||||
|
||||
GUINT bl;
|
||||
/* bl = cross_line_intersection_test();
|
||||
if(bl==3)
|
||||
{
|
||||
//take edge direction too
|
||||
bl = distances.maxAxis();
|
||||
}
|
||||
else
|
||||
{*/
|
||||
bl = 0;
|
||||
if(distances[0]<distances[1]) bl = 1;
|
||||
//}
|
||||
|
||||
if(bl==2) //edge edge separation
|
||||
{
|
||||
if(distances[2]>margin) return false;
|
||||
|
||||
contacts.m_penetration_depth = -distances[2] + margin;
|
||||
contacts.m_points[0] = closest_point_v;
|
||||
contacts.m_point_count = 1;
|
||||
VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//clip face against other
|
||||
|
||||
|
||||
GUINT point_count;
|
||||
//TODO
|
||||
if(bl == 0) //clip U points against V
|
||||
{
|
||||
point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
|
||||
if(point_count == 0) return false;
|
||||
contacts.merge_points(tv_plane,margin,contact_points,point_count);
|
||||
}
|
||||
else //clip V points against U
|
||||
{
|
||||
point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
|
||||
if(point_count == 0) return false;
|
||||
contacts.merge_points(tu_plane,margin,contact_points,point_count);
|
||||
contacts.m_separating_normal *= -1.f;
|
||||
}
|
||||
if(contacts.m_point_count == 0) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
/*class _GIM_TRIANGLE_CALCULATION_CACHE
|
||||
{
|
||||
public:
|
||||
GREAL margin;
|
||||
GUINT clipped_count;
|
||||
btVector3 tu_vertices[3];
|
||||
btVector3 tv_vertices[3];
|
||||
btVector3 temp_points[MAX_TRI_CLIPPING];
|
||||
btVector3 temp_points1[MAX_TRI_CLIPPING];
|
||||
btVector3 clipped_points[MAX_TRI_CLIPPING];
|
||||
GIM_TRIANGLE_CONTACT_DATA contacts1;
|
||||
GIM_TRIANGLE_CONTACT_DATA contacts2;
|
||||
|
||||
|
||||
//! clip triangle
|
||||
GUINT clip_triangle(
|
||||
const btVector4 & tri_plane,
|
||||
const btVector3 * tripoints,
|
||||
const btVector3 * srcpoints,
|
||||
btVector3 * clipped_points)
|
||||
{
|
||||
// edge 0
|
||||
|
||||
btVector4 edgeplane;
|
||||
|
||||
EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
|
||||
|
||||
GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
|
||||
edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
|
||||
|
||||
if(clipped_count == 0) return 0;
|
||||
|
||||
// edge 1
|
||||
|
||||
EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
|
||||
|
||||
clipped_count = PLANE_CLIP_POLYGON3D(
|
||||
edgeplane,temp_points,clipped_count,temp_points1);
|
||||
|
||||
if(clipped_count == 0) return 0;
|
||||
|
||||
// edge 2
|
||||
|
||||
EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
|
||||
|
||||
clipped_count = PLANE_CLIP_POLYGON3D(
|
||||
edgeplane,temp_points1,clipped_count,clipped_points);
|
||||
|
||||
return clipped_count;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//! collides only on one side
|
||||
bool triangle_collision(
|
||||
const btVector3 & u0,
|
||||
const btVector3 & u1,
|
||||
const btVector3 & u2,
|
||||
GREAL margin_u,
|
||||
const btVector3 & v0,
|
||||
const btVector3 & v1,
|
||||
const btVector3 & v2,
|
||||
GREAL margin_v,
|
||||
GIM_TRIANGLE_CONTACT_DATA & contacts)
|
||||
{
|
||||
|
||||
margin = margin_u + margin_v;
|
||||
|
||||
|
||||
tu_vertices[0] = u0;
|
||||
tu_vertices[1] = u1;
|
||||
tu_vertices[2] = u2;
|
||||
|
||||
tv_vertices[0] = v0;
|
||||
tv_vertices[1] = v1;
|
||||
tv_vertices[2] = v2;
|
||||
|
||||
//create planes
|
||||
// plane v vs U points
|
||||
|
||||
|
||||
TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
|
||||
|
||||
clipped_count = clip_triangle(
|
||||
contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
|
||||
|
||||
if(clipped_count == 0 )
|
||||
{
|
||||
return false;//Reject
|
||||
}
|
||||
|
||||
//find most deep interval face1
|
||||
contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
|
||||
if(contacts1.m_point_count == 0) return false; // too far
|
||||
|
||||
//Normal pointing to triangle1
|
||||
//contacts1.m_separating_normal *= -1.f;
|
||||
|
||||
//Clip tri1 by tri2 edges
|
||||
|
||||
TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
|
||||
|
||||
clipped_count = clip_triangle(
|
||||
contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
|
||||
|
||||
if(clipped_count == 0 )
|
||||
{
|
||||
return false;//Reject
|
||||
}
|
||||
|
||||
//find most deep interval face1
|
||||
contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
|
||||
if(contacts2.m_point_count == 0) return false; // too far
|
||||
|
||||
contacts2.m_separating_normal *= -1.f;
|
||||
|
||||
////check most dir for contacts
|
||||
if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
|
||||
{
|
||||
contacts.copy_from(contacts2);
|
||||
}
|
||||
else
|
||||
{
|
||||
contacts.copy_from(contacts1);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
};*/
|
||||
|
||||
|
||||
|
||||
bool GIM_TRIANGLE::collide_triangle_hard_test(
|
||||
const GIM_TRIANGLE & other,
|
||||
GIM_TRIANGLE_CONTACT_DATA & contact_data) const
|
||||
{
|
||||
_GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
|
||||
return calc_cache.triangle_collision(
|
||||
m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
|
||||
other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
|
||||
contact_data);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user