579 lines
15 KiB
C++
579 lines
15 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
|
|
#include "BU_EdgeEdge.h"
|
|
#include "BU_Screwing.h"
|
|
#include <LinearMath/btPoint3.h>
|
|
#include <LinearMath/btPoint3.h>
|
|
|
|
//#include "BU_IntervalArithmeticPolynomialSolver.h"
|
|
#include "BU_AlgebraicPolynomialSolver.h"
|
|
|
|
#define USE_ALGEBRAIC
|
|
#ifdef USE_ALGEBRAIC
|
|
#define BU_Polynomial BU_AlgebraicPolynomialSolver
|
|
#else
|
|
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
|
|
#endif
|
|
|
|
BU_EdgeEdge::BU_EdgeEdge()
|
|
{
|
|
}
|
|
|
|
|
|
bool BU_EdgeEdge::GetTimeOfImpact(
|
|
const BU_Screwing& screwAB,
|
|
const btPoint3& a,//edge in object A
|
|
const btVector3& u,
|
|
const btPoint3& c,//edge in object B
|
|
const btVector3& v,
|
|
btScalar &minTime,
|
|
btScalar &lambda1,
|
|
btScalar& mu1
|
|
|
|
)
|
|
{
|
|
bool hit=false;
|
|
|
|
btScalar lambda;
|
|
btScalar mu;
|
|
|
|
const btScalar w=screwAB.GetW();
|
|
const btScalar s=screwAB.GetS();
|
|
|
|
if (btFuzzyZero(s) &&
|
|
btFuzzyZero(w))
|
|
{
|
|
//no motion, no collision
|
|
return false;
|
|
}
|
|
|
|
if (btFuzzyZero(w) )
|
|
{
|
|
//pure translation W=0, S <> 0
|
|
//no trig, f(t)=t
|
|
btScalar det = u.y()*v.x()-u.x()*v.y();
|
|
if (!btFuzzyZero(det))
|
|
{
|
|
lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det;
|
|
mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det;
|
|
|
|
if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
|
|
{
|
|
// single potential collision is
|
|
btScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
|
|
//if this is on the edge, and time t within [0..1] report hit
|
|
if (t>=0 && t <= minTime)
|
|
{
|
|
hit = true;
|
|
lambda1 = lambda;
|
|
mu1 = mu;
|
|
minTime=t;
|
|
}
|
|
}
|
|
|
|
} else
|
|
{
|
|
//parallel case, not yet
|
|
}
|
|
} else
|
|
{
|
|
if (btFuzzyZero(s) )
|
|
{
|
|
if (btFuzzyZero(u.z()) )
|
|
{
|
|
if (btFuzzyZero(v.z()) )
|
|
{
|
|
//u.z()=0,v.z()=0
|
|
if (btFuzzyZero(a.z()-c.z()))
|
|
{
|
|
//printf("NOT YET planar problem, 4 vertex=edge cases\n");
|
|
|
|
} else
|
|
{
|
|
//printf("parallel but distinct planes, no collision\n");
|
|
return false;
|
|
}
|
|
|
|
} else
|
|
{
|
|
btScalar mu = (a.z() - c.z())/v.z();
|
|
if (0<=mu && mu <= 1)
|
|
{
|
|
// printf("NOT YET//u.z()=0,v.z()<>0\n");
|
|
} else
|
|
{
|
|
return false;
|
|
}
|
|
|
|
}
|
|
} else
|
|
{
|
|
//u.z()<>0
|
|
|
|
if (btFuzzyZero(v.z()) )
|
|
{
|
|
//printf("u.z()<>0,v.z()=0\n");
|
|
lambda = (c.z() - a.z())/u.z();
|
|
if (0<=lambda && lambda <= 1)
|
|
{
|
|
//printf("u.z()<>0,v.z()=0\n");
|
|
btPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
|
|
btScalar r2 = rotPt.length2();//px*px + py*py;
|
|
|
|
//either y=a*x+b, or x = a*x+b...
|
|
//depends on whether value v.x() is zero or not
|
|
btScalar aa;
|
|
btScalar bb;
|
|
|
|
if (btFuzzyZero(v.x()))
|
|
{
|
|
aa = v.x()/v.y();
|
|
bb= c.x()+ (-c.y() /v.y()) *v.x();
|
|
} else
|
|
{
|
|
//line is c+mu*v;
|
|
//x = c.x()+mu*v.x();
|
|
//mu = ((x-c.x())/v.x());
|
|
//y = c.y()+((x-c.x())/v.x())*v.y();
|
|
//y = c.y()+ (-c.x() /v.x()) *v.y() + (x /v.x()) *v.y();
|
|
//y = a*x+b,where a = v.y()/v.x(), b= c.y()+ (-c.x() /v.x()) *v.y();
|
|
aa = v.y()/v.x();
|
|
bb= c.y()+ (-c.x() /v.x()) *v.y();
|
|
}
|
|
|
|
btScalar disc = aa*aa*r2 + r2 - bb*bb;
|
|
if (disc <0)
|
|
{
|
|
//edge doesn't intersect the circle (motion of the vertex)
|
|
return false;
|
|
}
|
|
btScalar rad = btSqrt(r2);
|
|
|
|
if (btFuzzyZero(disc))
|
|
{
|
|
btPoint3 intersectPt;
|
|
|
|
btScalar mu;
|
|
//intersectionPoint edge with circle;
|
|
if (btFuzzyZero(v.x()))
|
|
{
|
|
intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
|
|
intersectPt.setX( aa*intersectPt.y()+bb );
|
|
mu = ((intersectPt.y()-c.y())/v.y());
|
|
} else
|
|
{
|
|
intersectPt.setX((-2*aa*bb)/(2*(aa*aa+1)));
|
|
intersectPt.setY(aa*intersectPt.x()+bb);
|
|
mu = ((intersectPt.getX()-c.getX())/v.getX());
|
|
|
|
}
|
|
|
|
if (0 <= mu && mu <= 1)
|
|
{
|
|
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
|
}
|
|
//only one solution
|
|
} else
|
|
{
|
|
//two points...
|
|
//intersectionPoint edge with circle;
|
|
btPoint3 intersectPt;
|
|
//intersectionPoint edge with circle;
|
|
if (btFuzzyZero(v.x()))
|
|
{
|
|
btScalar mu;
|
|
|
|
intersectPt.setY((-2.f*aa*bb+2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
|
|
intersectPt.setX(aa*intersectPt.y()+bb);
|
|
mu = ((intersectPt.getY()-c.getY())/v.getY());
|
|
if (0.f <= mu && mu <= 1.f)
|
|
{
|
|
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
|
}
|
|
intersectPt.setY((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
|
|
intersectPt.setX(aa*intersectPt.y()+bb);
|
|
mu = ((intersectPt.getY()-c.getY())/v.getY());
|
|
if (0 <= mu && mu <= 1)
|
|
{
|
|
hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
|
}
|
|
|
|
} else
|
|
{
|
|
btScalar mu;
|
|
|
|
intersectPt.setX((-2.f*aa*bb+2.f*btSqrt(disc))/(2*(aa*aa+1.f)));
|
|
intersectPt.setY(aa*intersectPt.x()+bb);
|
|
mu = ((intersectPt.getX()-c.getX())/v.getX());
|
|
if (0 <= mu && mu <= 1)
|
|
{
|
|
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
|
}
|
|
intersectPt.setX((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
|
|
intersectPt.setY(aa*intersectPt.x()+bb);
|
|
mu = ((intersectPt.getX()-c.getX())/v.getX());
|
|
if (0.f <= mu && mu <= 1.f)
|
|
{
|
|
hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//int k=0;
|
|
|
|
} else
|
|
{
|
|
return false;
|
|
}
|
|
|
|
|
|
} else
|
|
{
|
|
//u.z()<>0,v.z()<>0
|
|
//printf("general case with s=0\n");
|
|
hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
|
|
if (hit)
|
|
{
|
|
lambda1 = lambda;
|
|
mu1 = mu;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
} else
|
|
{
|
|
//printf("general case, W<>0,S<>0\n");
|
|
hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
|
|
if (hit)
|
|
{
|
|
lambda1 = lambda;
|
|
mu1 = mu;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
//W <> 0,pure rotation
|
|
}
|
|
|
|
return hit;
|
|
}
|
|
|
|
|
|
bool BU_EdgeEdge::GetTimeOfImpactbteralCase(
|
|
const BU_Screwing& screwAB,
|
|
const btPoint3& a,//edge in object A
|
|
const btVector3& u,
|
|
const btPoint3& c,//edge in object B
|
|
const btVector3& v,
|
|
btScalar &minTime,
|
|
btScalar &lambda,
|
|
btScalar& mu
|
|
|
|
)
|
|
{
|
|
bool hit = false;
|
|
|
|
btScalar coefs[4]={0.f,0.f,0.f,0.f};
|
|
BU_Polynomial polynomialSolver;
|
|
int numroots = 0;
|
|
|
|
//btScalar eps=1e-15f;
|
|
//btScalar eps2=1e-20f;
|
|
btScalar s=screwAB.GetS();
|
|
btScalar w = screwAB.GetW();
|
|
|
|
btScalar ax = a.x();
|
|
btScalar ay = a.y();
|
|
btScalar az = a.z();
|
|
btScalar cx = c.x();
|
|
btScalar cy = c.y();
|
|
btScalar cz = c.z();
|
|
btScalar vx = v.x();
|
|
btScalar vy = v.y();
|
|
btScalar vz = v.z();
|
|
btScalar ux = u.x();
|
|
btScalar uy = u.y();
|
|
btScalar uz = u.z();
|
|
|
|
|
|
if (!btFuzzyZero(v.z()))
|
|
{
|
|
|
|
//Maple Autogenerated C code
|
|
btScalar t1,t2,t3,t4,t7,t8,t10;
|
|
btScalar t13,t14,t15,t16,t17,t18,t19,t20;
|
|
btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
|
|
btScalar t31,t32,t33,t34,t35,t36,t39,t40;
|
|
btScalar t41,t43,t48;
|
|
btScalar t63;
|
|
|
|
btScalar aa,bb,cc,dd;//the coefficients
|
|
|
|
t1 = v.y()*s; t2 = t1*u.x();
|
|
t3 = v.x()*s;
|
|
t4 = t3*u.y();
|
|
t7 = btTan(w/2.0f);
|
|
t8 = 1.0f/t7;
|
|
t10 = 1.0f/v.z();
|
|
aa = (t2-t4)*t8*t10;
|
|
t13 = a.x()*t7;
|
|
t14 = u.z()*v.y();
|
|
t15 = t13*t14;
|
|
t16 = u.x()*v.z();
|
|
t17 = a.y()*t7;
|
|
t18 = t16*t17;
|
|
t19 = u.y()*v.z();
|
|
t20 = t13*t19;
|
|
t21 = v.y()*u.x();
|
|
t22 = c.z()*t7;
|
|
t23 = t21*t22;
|
|
t24 = v.x()*a.z();
|
|
t25 = t7*u.y();
|
|
t26 = t24*t25;
|
|
t27 = c.y()*t7;
|
|
t28 = t16*t27;
|
|
t29 = a.z()*t7;
|
|
t30 = t21*t29;
|
|
t31 = u.z()*v.x();
|
|
t32 = t31*t27;
|
|
t33 = t31*t17;
|
|
t34 = c.x()*t7;
|
|
t35 = t34*t19;
|
|
t36 = t34*t14;
|
|
t39 = v.x()*c.z();
|
|
t40 = t39*t25;
|
|
t41 = 2.0f*t1*u.y()-t15+t18-t20-t23-t26+t28+t30+t32+t33-t35-t36+2.0f*t3*u.x()+t40;
|
|
bb = t41*t8*t10;
|
|
t43 = t7*u.x();
|
|
t48 = u.y()*v.y();
|
|
cc = (-2.0f*t39*t43+2.0f*t24*t43+t4-2.0f*t48*t22+2.0f*t34*t16-2.0f*t31*t13-t2
|
|
-2.0f*t17*t14+2.0f*t19*t27+2.0f*t48*t29)*t8*t10;
|
|
t63 = -t36+t26+t32-t40+t23+t35-t20+t18-t28-t33+t15-t30;
|
|
dd = t63*t8*t10;
|
|
|
|
coefs[0]=aa;
|
|
coefs[1]=bb;
|
|
coefs[2]=cc;
|
|
coefs[3]=dd;
|
|
|
|
} else
|
|
{
|
|
|
|
btScalar t1,t2,t3,t4,t7,t8,t10;
|
|
btScalar t13,t14,t15,t16,t17,t18,t19,t20;
|
|
btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
|
|
btScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
|
|
btScalar p1,p2,p3,p4;
|
|
|
|
t1 = uy*s;
|
|
t2 = t1*vx;
|
|
t3 = ux*s;
|
|
t4 = t3*vy;
|
|
t7 = btTan(w/2.0f);
|
|
t8 = 1/t7;
|
|
t10 = 1/uz;
|
|
t13 = ux*az;
|
|
t14 = t7*vy;
|
|
t15 = t13*t14;
|
|
t16 = ax*t7;
|
|
t17 = uy*vz;
|
|
t18 = t16*t17;
|
|
t19 = cx*t7;
|
|
t20 = t19*t17;
|
|
t21 = vy*uz;
|
|
t22 = t19*t21;
|
|
t23 = ay*t7;
|
|
t24 = vx*uz;
|
|
t25 = t23*t24;
|
|
t26 = uy*cz;
|
|
t27 = t7*vx;
|
|
t28 = t26*t27;
|
|
t29 = t16*t21;
|
|
t30 = cy*t7;
|
|
t31 = ux*vz;
|
|
t32 = t30*t31;
|
|
t33 = ux*cz;
|
|
t34 = t33*t14;
|
|
t35 = t23*t31;
|
|
t36 = t30*t24;
|
|
t37 = uy*az;
|
|
t38 = t37*t27;
|
|
|
|
p4 = (-t2+t4)*t8*t10;
|
|
p3 = 2.0f*t1*vy+t15-t18-t20-t22+t25+t28-t29+t32-t34+t35+t36-t38+2.0f*t3*vx;
|
|
p2 = -2.0f*t33*t27-2.0f*t26*t14-2.0f*t23*t21+2.0f*t37*t14+2.0f*t30*t17+2.0f*t13
|
|
*t27+t2-t4+2.0f*t19*t31-2.0f*t16*t24;
|
|
t57 = -t22+t29+t36-t25-t32+t34+t35-t28-t15+t20-t18+t38;
|
|
p1 = t57*t8*t10;
|
|
|
|
coefs[0] = p4;
|
|
coefs[1] = p3;
|
|
coefs[2] = p2;
|
|
coefs[1] = p1;
|
|
|
|
}
|
|
|
|
numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
|
|
|
|
for (int i=0;i<numroots;i++)
|
|
{
|
|
//btScalar tau = roots[i];//polynomialSolver.GetRoot(i);
|
|
btScalar tau = polynomialSolver.GetRoot(i);
|
|
|
|
//check whether mu and lambda are in range [0..1]
|
|
|
|
if (!btFuzzyZero(v.z()))
|
|
{
|
|
btScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
|
|
btScalar B1=((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+
|
|
vx*az*btTan(1.f/2.f*w)-vx*cz*btTan(1.f/2.f*w)+
|
|
vx*s*tau)/btTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
|
|
lambda = B1/A1;
|
|
|
|
mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(btTan(w/2.f)))/v.z();
|
|
|
|
|
|
//double check in original equation
|
|
|
|
btScalar lhs = (a.x()+lambda*u.x())
|
|
*((1.f-tau*tau)/(1.f+tau*tau))-
|
|
(a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau));
|
|
|
|
lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz));
|
|
|
|
btScalar rhs = c.x()+mu*v.x();
|
|
|
|
rhs = ((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+vx*az*btTan(1.f/2.f*w)-
|
|
vx*cz*btTan(1.f/2.f*w)+vx*s*tau)/(btTan(1.f/2.f*w)*vz))-
|
|
|
|
(ax-ax*tau*tau-2.f*tau*ay);
|
|
|
|
/*btScalar res = coefs[0]*tau*tau*tau+
|
|
coefs[1]*tau*tau+
|
|
coefs[2]*tau+
|
|
coefs[3];*/
|
|
|
|
//lhs should be rhs !
|
|
|
|
if (0.<= mu && mu <=1 && 0.<=lambda && lambda <= 1)
|
|
{
|
|
|
|
} else
|
|
{
|
|
//skip this solution, not really touching
|
|
continue;
|
|
}
|
|
|
|
}
|
|
|
|
btScalar t = 2.f*btAtan(tau)/screwAB.GetW();
|
|
//tau = tan (wt/2) so 2*atan (tau)/w
|
|
if (t>=0.f && t<minTime)
|
|
{
|
|
#ifdef STATS_EDGE_EDGE
|
|
printf(" ax = %12.12f\n ay = %12.12f\n az = %12.12f\n",a.x(),a.y(),a.z());
|
|
printf(" ux = %12.12f\n uy = %12.12f\n uz = %12.12f\n",u.x(),u.y(),u.z());
|
|
printf(" cx = %12.12f\n cy = %12.12f\n cz = %12.12f\n",c.x(),c.y(),c.z());
|
|
printf(" vx = %12.12f\n vy = %12.12f\n vz = %12.12f\n",v.x(),v.y(),v.z());
|
|
printf(" s = %12.12f\n w = %12.12f\n", s, w);
|
|
|
|
printf(" tau = %12.12f \n lambda = %12.12f \n mu = %f\n",tau,lambda,mu);
|
|
printf(" ---------------------------------------------\n");
|
|
|
|
#endif
|
|
|
|
// v,u,a,c,s,w
|
|
|
|
// BU_IntervalArithmeticPolynomialSolver iaSolver;
|
|
// int numroots2 = iaSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
|
|
|
|
minTime = t;
|
|
hit = true;
|
|
}
|
|
}
|
|
|
|
return hit;
|
|
}
|
|
|
|
|
|
//C -S
|
|
//S C
|
|
|
|
bool BU_EdgeEdge::Calc2DRotationPointPoint(const btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime)
|
|
{
|
|
bool hit = false;
|
|
|
|
// now calculate the planeEquation for the vertex motion,
|
|
// and check if the intersectionpoint is at the positive side
|
|
btPoint3 rotPt1(btCos(rotW)*rotPt.x()-btSin(rotW)*rotPt.y(),
|
|
btSin(rotW)*rotPt.x()+btCos(rotW)*rotPt.y(),
|
|
0.f);
|
|
|
|
btVector3 rotVec = rotPt1-rotPt;
|
|
|
|
btVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
|
|
|
|
//btPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
|
|
btScalar planeD = planeNormal.dot(rotPt1);
|
|
|
|
btScalar dist = (planeNormal.dot(intersectPt)-planeD);
|
|
hit = (dist >= -0.001);
|
|
|
|
//if (hit)
|
|
{
|
|
// minTime = 0;
|
|
//calculate the time of impact, using the fact of
|
|
//toi = alpha / screwAB.getW();
|
|
// cos (alpha) = adjacent/hypothenuse;
|
|
//adjacent = dotproduct(ipedge,point);
|
|
//hypothenuse = sqrt(r2);
|
|
btScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
|
|
btScalar hypo = rotRadius;
|
|
btScalar alpha = btAcos(adjacent/hypo);
|
|
btScalar t = alpha / rotW;
|
|
if (t >= 0 && t < minTime)
|
|
{
|
|
hit = true;
|
|
minTime = t;
|
|
} else
|
|
{
|
|
hit = false;
|
|
}
|
|
|
|
}
|
|
return hit;
|
|
}
|
|
|
|
bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
|
|
const BU_Screwing& screwAB,
|
|
const btPoint3& a,//edge in object A
|
|
const btVector3& u,
|
|
const btPoint3& c,//edge in object B
|
|
const btVector3& v,
|
|
btScalar &minTime,
|
|
btScalar &lamda,
|
|
btScalar& mu
|
|
|
|
)
|
|
{
|
|
return false;
|
|
}
|