forked from bartvdbraak/blender
577 lines
14 KiB
C++
577 lines
14 KiB
C++
|
|
/*
|
|
* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
|
|
*
|
|
* Permission to use, copy, modify, distribute and sell this software
|
|
* and its documentation for any purpose is hereby granted without fee,
|
|
* provided that the above copyright notice appear in all copies.
|
|
* Erwin Coumans makes no representations about the suitability
|
|
* of this software for any purpose.
|
|
* It is provided "as is" without express or implied warranty.
|
|
*/
|
|
|
|
#include "BU_CollisionPair.h"
|
|
#include "NarrowPhaseCollision/BU_VertexPoly.h"
|
|
#include "NarrowPhaseCollision/BU_EdgeEdge.h"
|
|
#include "BU_Collidable.h"
|
|
|
|
|
|
#include "BU_MotionStateInterface.h"
|
|
#include "CollisionShapes/PolyhedralConvexShape.h"
|
|
#include <SimdMinMax.h>
|
|
#include "SimdTransformUtil.h"
|
|
|
|
|
|
|
|
BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
|
|
: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
|
|
m_tolerance(tolerance)
|
|
{
|
|
|
|
}
|
|
|
|
// if there exists a time-of-impact between any feature_pair (edgeA,edgeB),
|
|
// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time
|
|
|
|
|
|
/*
|
|
bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)
|
|
|
|
*/
|
|
|
|
bool BU_CollisionPair::calcTimeOfImpact(
|
|
const SimdTransform& fromA,
|
|
const SimdTransform& toA,
|
|
const SimdTransform& fromB,
|
|
const SimdTransform& toB,
|
|
CastResult& result)
|
|
{
|
|
|
|
|
|
|
|
|
|
SimdVector3 linvelA,angvelA;
|
|
SimdVector3 linvelB,angvelB;
|
|
|
|
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
|
|
SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
|
|
|
|
|
|
SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
|
|
SimdQuaternion angularMotionA(0,0,0,1.f);
|
|
SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
|
|
SimdQuaternion angularMotionB(0,0,0,1);
|
|
|
|
|
|
|
|
result.m_fraction = 1.f;
|
|
|
|
SimdTransform impactTransA;
|
|
SimdTransform impactTransB;
|
|
|
|
int index=0;
|
|
|
|
SimdScalar toiUnscaled=result.m_fraction;
|
|
const SimdScalar toiUnscaledLimit = result.m_fraction;
|
|
|
|
SimdTransform a2w;
|
|
a2w = fromA;
|
|
SimdTransform b2w = fromB;
|
|
|
|
/* debugging code
|
|
{
|
|
const int numvertsB = m_convexB->GetNumVertices();
|
|
for (int v=0;v<numvertsB;v++)
|
|
{
|
|
SimdPoint3 pt;
|
|
m_convexB->GetVertex(v,pt);
|
|
pt = b2w * pt;
|
|
char buf[1000];
|
|
|
|
if (pt.y() < 0.)
|
|
{
|
|
sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
} else
|
|
{
|
|
sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
|
|
|
|
SimdTransform b2wp = b2w;
|
|
|
|
b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
|
|
b2wp.setRotation( b2w.getRotation() + angularMotionB);
|
|
|
|
impactTransB = b2wp;
|
|
|
|
SimdTransform a2wp;
|
|
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
|
|
a2wp.setRotation(a2w.getRotation()+angularMotionA);
|
|
|
|
impactTransA = a2wp;
|
|
|
|
SimdTransform a2winv;
|
|
a2winv = a2w.inverse();
|
|
|
|
SimdTransform b2wpinv;
|
|
b2wpinv = b2wp.inverse();
|
|
|
|
SimdTransform b2winv;
|
|
b2winv = b2w.inverse();
|
|
|
|
SimdTransform a2wpinv;
|
|
a2wpinv = a2wp.inverse();
|
|
|
|
//Redon's version with concatenated transforms
|
|
|
|
SimdTransform relative;
|
|
|
|
relative = b2w * b2wpinv * a2wp * a2winv;
|
|
|
|
//relative = a2winv * a2wp * b2wpinv * b2w;
|
|
|
|
SimdQuaternion qrel;
|
|
relative.getBasis().getRotation(qrel);
|
|
|
|
SimdVector3 linvel = relative.getOrigin();
|
|
|
|
if (linvel.length() < SCREWEPSILON)
|
|
{
|
|
linvel.setValue(0.,0.,0.);
|
|
}
|
|
SimdVector3 angvel;
|
|
angvel[0] = 2.f * asinf (qrel[0]);
|
|
angvel[1] = 2.f * asinf (qrel[1]);
|
|
angvel[2] = 2.f * asinf (qrel[2]);
|
|
|
|
if (angvel.length() < SCREWEPSILON)
|
|
{
|
|
angvel.setValue(0.f,0.f,0.f);
|
|
}
|
|
|
|
//Redon's version with concatenated transforms
|
|
m_screwing = BU_Screwing(linvel,angvel);
|
|
|
|
SimdTransform w2s;
|
|
m_screwing.LocalMatrix(w2s);
|
|
|
|
SimdTransform s2w;
|
|
s2w = w2s.inverse();
|
|
|
|
//impactTransA = a2w;
|
|
//impactTransB = b2w;
|
|
|
|
bool hit = false;
|
|
|
|
if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
|
|
{
|
|
//W = 0 , S = 0 , no collision
|
|
//toi = 0;
|
|
/*
|
|
{
|
|
const int numvertsB = m_convexB->GetNumVertices();
|
|
for (int v=0;v<numvertsB;v++)
|
|
{
|
|
SimdPoint3 pt;
|
|
m_convexB->GetVertex(v,pt);
|
|
pt = impactTransB * pt;
|
|
char buf[1000];
|
|
|
|
if (pt.y() < 0.)
|
|
{
|
|
sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
}
|
|
else
|
|
{
|
|
sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
|
|
return false;//don't continue moving within epsilon
|
|
}
|
|
|
|
#define EDGEEDGE
|
|
#ifdef EDGEEDGE
|
|
|
|
BU_EdgeEdge edgeEdge;
|
|
|
|
//for all edged in A check agains all edges in B
|
|
for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
|
|
{
|
|
SimdPoint3 pA0,pA1;
|
|
|
|
m_convexA->GetEdge(ea,pA0,pA1);
|
|
|
|
pA0= a2w * pA0;//in world space
|
|
pA0 = w2s * pA0;//in screwing space
|
|
|
|
pA1= a2w * pA1;//in world space
|
|
pA1 = w2s * pA1;//in screwing space
|
|
|
|
int numedgesB = m_convexB->GetNumEdges();
|
|
for (int eb = 0; eb < numedgesB;eb++)
|
|
{
|
|
{
|
|
SimdPoint3 pB0,pB1;
|
|
m_convexB->GetEdge(eb,pB0,pB1);
|
|
|
|
pB0= b2w * pB0;//in world space
|
|
pB0 = w2s * pB0;//in screwing space
|
|
|
|
pB1= b2w * pB1;//in world space
|
|
pB1 = w2s * pB1;//in screwing space
|
|
|
|
|
|
SimdScalar lambda,mu;
|
|
|
|
toiUnscaled = 1.;
|
|
|
|
SimdVector3 edgeDirA(pA1-pA0);
|
|
SimdVector3 edgeDirB(pB1-pB0);
|
|
|
|
if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
|
|
{
|
|
//printf("edgeedge potential hit\n");
|
|
if (toiUnscaled>=0)
|
|
{
|
|
if (toiUnscaled < toiUnscaledLimit)
|
|
{
|
|
|
|
//inside check is already done by checking the mu and gamma !
|
|
|
|
SimdPoint3 vtx = pA0+lambda * (pA1-pA0);
|
|
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
|
|
|
|
SimdPoint3 hitptWorld = s2w * hitpt;
|
|
{
|
|
|
|
if (toiUnscaled < result.m_fraction)
|
|
result.m_fraction = toiUnscaled;
|
|
|
|
hit = true;
|
|
|
|
SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
|
|
|
|
hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
|
|
|
|
|
|
hitNormal.normalize();
|
|
|
|
//an approximated normal can be calculated by taking the cross product of both edges
|
|
//take care of the sign !
|
|
|
|
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
|
|
|
|
SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
|
|
if (dist > 0)
|
|
hitNormalWorld *= -1;
|
|
|
|
//todo: this is the wrong point, because b2winv is still at begin of motion
|
|
// not at time-of-impact location!
|
|
//bhitpt = b2winv * hitptWorld;
|
|
|
|
// m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
index++;
|
|
}
|
|
};
|
|
#endif //EDGEEDGE
|
|
|
|
#define VERTEXFACE
|
|
#ifdef VERTEXFACE
|
|
|
|
// for all vertices in A, for each face in B,do vertex-face
|
|
{
|
|
const int numvertsA = m_convexA->GetNumVertices();
|
|
for (int v=0;v<numvertsA;v++)
|
|
//int v=3;
|
|
|
|
{
|
|
SimdPoint3 vtx;
|
|
m_convexA->GetVertex(v,vtx);
|
|
|
|
vtx = a2w * vtx;//in world space
|
|
vtx = w2s * vtx;//in screwing space
|
|
|
|
const int numplanesB = m_convexB->GetNumPlanes();
|
|
|
|
for (int p = 0 ; p < numplanesB; p++)
|
|
//int p=2;
|
|
{
|
|
|
|
{
|
|
|
|
SimdVector3 planeNorm;
|
|
SimdPoint3 planeSupport;
|
|
|
|
m_convexB->GetPlane(planeNorm,planeSupport,p);
|
|
|
|
|
|
planeSupport = b2w * planeSupport;//transform to world space
|
|
SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm;
|
|
|
|
planeSupport = w2s * planeSupport ; //transform to screwing space
|
|
planeNorm = w2s.getBasis() * planeNormWorld;
|
|
|
|
planeNorm.normalize();
|
|
|
|
SimdScalar d = planeSupport.dot(planeNorm);
|
|
|
|
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
|
|
|
|
BU_VertexPoly vtxApolyB;
|
|
|
|
toiUnscaled = 1.;
|
|
|
|
if ((p==2) && (v==6))
|
|
{
|
|
// printf("%f toiUnscaled\n",toiUnscaled);
|
|
|
|
}
|
|
if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
|
|
{
|
|
|
|
|
|
|
|
|
|
if (toiUnscaled >= 0. )
|
|
{
|
|
//not only collect the first point, get every contactpoint, later we have to check the
|
|
//manifold properly!
|
|
|
|
if (toiUnscaled <= toiUnscaledLimit)
|
|
{
|
|
// printf("toiUnscaled %f\n",toiUnscaled );
|
|
|
|
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
|
|
SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
|
|
|
|
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
|
|
SimdPoint3 hitptWorld = s2w * hitpt;
|
|
|
|
|
|
hitpt = b2winv * hitptWorld;
|
|
//vertex has to be 'within' the facet's boundary
|
|
if (m_convexB->IsInside(hitpt,m_tolerance))
|
|
{
|
|
// m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
|
|
|
|
if (toiUnscaled < result.m_fraction)
|
|
result.m_fraction= toiUnscaled;
|
|
hit = true;
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
index++;
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
// for all vertices in B, for each face in A,do vertex-face
|
|
//copy and pasted from all verts A -> all planes B so potential typos!
|
|
//todo: make this into one method with a kind of 'swapped' logic
|
|
//
|
|
{
|
|
const int numvertsB = m_convexB->GetNumVertices();
|
|
for (int v=0;v<numvertsB;v++)
|
|
//int v=0;
|
|
|
|
{
|
|
SimdPoint3 vtx;
|
|
m_convexB->GetVertex(v,vtx);
|
|
|
|
vtx = b2w * vtx;//in world space
|
|
/*
|
|
|
|
char buf[1000];
|
|
|
|
if (vtx.y() < 0.)
|
|
{
|
|
sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
}
|
|
sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
|
|
*/
|
|
vtx = w2s * vtx;//in screwing space
|
|
|
|
const int numplanesA = m_convexA->GetNumPlanes();
|
|
|
|
for (int p = 0 ; p < numplanesA; p++)
|
|
//int p=2;
|
|
{
|
|
|
|
{
|
|
SimdVector3 planeNorm;
|
|
SimdPoint3 planeSupport;
|
|
|
|
m_convexA->GetPlane(planeNorm,planeSupport,p);
|
|
|
|
|
|
planeSupport = a2w * planeSupport;//transform to world space
|
|
SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm;
|
|
|
|
planeSupport = w2s * planeSupport ; //transform to screwing space
|
|
planeNorm = w2s.getBasis() * planeNormWorld;
|
|
|
|
planeNorm.normalize();
|
|
|
|
SimdScalar d = planeSupport.dot(planeNorm);
|
|
|
|
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
|
|
|
|
BU_VertexPoly vtxBpolyA;
|
|
|
|
toiUnscaled = 1.;
|
|
|
|
if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
|
|
{
|
|
if (toiUnscaled>=0.)
|
|
{
|
|
if (toiUnscaled < toiUnscaledLimit)
|
|
{
|
|
SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
|
|
SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
|
|
SimdScalar len = hitNormal.length()-1;
|
|
|
|
//assert( SimdFuzzyZero(len) );
|
|
|
|
|
|
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
|
|
SimdPoint3 hitptWorld = s2w * hitpt;
|
|
hitpt = a2winv * hitptWorld;
|
|
|
|
|
|
//vertex has to be 'within' the facet's boundary
|
|
if (m_convexA->IsInside(hitpt,m_tolerance))
|
|
{
|
|
|
|
// m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
|
|
if (toiUnscaled <result.m_fraction)
|
|
result.m_fraction = toiUnscaled;
|
|
hit = true;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
index++;
|
|
}
|
|
}
|
|
|
|
|
|
#endif// VERTEXFACE
|
|
|
|
//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
|
|
//in addition there are contact points from previous frames
|
|
//we have to cleanup the manifold, using an additional epsilon/tolerance
|
|
//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
|
|
//else throw it away
|
|
|
|
|
|
if (hit)
|
|
{
|
|
|
|
//try to avoid numerical drift on close contact
|
|
|
|
if (result.m_fraction < 0.00001)
|
|
{
|
|
// printf("toiUnscaledMin< 0.00001\n");
|
|
impactTransA = a2w;
|
|
impactTransB = b2w;
|
|
|
|
} else
|
|
{
|
|
|
|
//SimdScalar vel = linearMotionB.length();
|
|
|
|
//todo: check this margin
|
|
result.m_fraction *= 0.99f;
|
|
|
|
//move B to new position
|
|
impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
|
|
SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
|
|
ornB.normalize();
|
|
impactTransB.setRotation(ornB);
|
|
|
|
//now transform A
|
|
SimdTransform a2s,a2b;
|
|
a2s.mult( w2s , a2w);
|
|
a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
|
|
a2s.multInverseLeft(w2s,a2s);
|
|
a2b.multInverseLeft(b2w, a2s);
|
|
|
|
//transform by motion B
|
|
impactTransA.mult(impactTransB, a2b);
|
|
//normalize rotation
|
|
SimdQuaternion orn;
|
|
impactTransA.getBasis().getRotation(orn);
|
|
orn.normalize();
|
|
impactTransA.setBasis(SimdMatrix3x3(orn));
|
|
}
|
|
}
|
|
|
|
/*
|
|
{
|
|
const int numvertsB = m_convexB->GetNumVertices();
|
|
for (int v=0;v<numvertsB;v++)
|
|
{
|
|
SimdPoint3 pt;
|
|
m_convexB->GetVertex(v,pt);
|
|
pt = impactTransB * pt;
|
|
char buf[1000];
|
|
|
|
if (pt.y() < 0.)
|
|
{
|
|
sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
}
|
|
else
|
|
{
|
|
sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
|
|
if (debugFile)
|
|
fwrite(buf,1,strlen(buf),debugFile);
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
return hit;
|
|
}
|
|
|
|
|