forked from bartvdbraak/blender
574 lines
14 KiB
C++
574 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_EdgeEdge.h"
|
|
#include "BU_Screwing.h"
|
|
#include <SimdPoint3.h>
|
|
#include <SimdPoint3.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 SimdPoint3& a,//edge in object A
|
|
const SimdVector3& u,
|
|
const SimdPoint3& c,//edge in object B
|
|
const SimdVector3& v,
|
|
SimdScalar &minTime,
|
|
SimdScalar &lambda1,
|
|
SimdScalar& mu1
|
|
|
|
)
|
|
{
|
|
bool hit=false;
|
|
|
|
SimdScalar lambda;
|
|
SimdScalar mu;
|
|
|
|
const SimdScalar w=screwAB.GetW();
|
|
const SimdScalar s=screwAB.GetS();
|
|
|
|
if (SimdFuzzyZero(s) &&
|
|
SimdFuzzyZero(w))
|
|
{
|
|
//no motion, no collision
|
|
return false;
|
|
}
|
|
|
|
if (SimdFuzzyZero(w) )
|
|
{
|
|
//pure translation W=0, S <> 0
|
|
//no trig, f(t)=t
|
|
SimdScalar det = u.y()*v.x()-u.x()*v.y();
|
|
if (!SimdFuzzyZero(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
|
|
SimdScalar 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 (SimdFuzzyZero(s) )
|
|
{
|
|
if (SimdFuzzyZero(u.z()) )
|
|
{
|
|
if (SimdFuzzyZero(v.z()) )
|
|
{
|
|
//u.z()=0,v.z()=0
|
|
if (SimdFuzzyZero(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
|
|
{
|
|
SimdScalar 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 (SimdFuzzyZero(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");
|
|
SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
|
|
SimdScalar 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
|
|
SimdScalar aa;
|
|
SimdScalar bb;
|
|
|
|
if (SimdFuzzyZero(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();
|
|
}
|
|
|
|
SimdScalar disc = aa*aa*r2 + r2 - bb*bb;
|
|
if (disc <0)
|
|
{
|
|
//edge doesn't intersect the circle (motion of the vertex)
|
|
return false;
|
|
}
|
|
SimdScalar rad = sqrtf(r2);
|
|
|
|
if (SimdFuzzyZero(disc))
|
|
{
|
|
SimdPoint3 intersectPt;
|
|
|
|
SimdScalar mu;
|
|
//intersectionPoint edge with circle;
|
|
if (SimdFuzzyZero(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;
|
|
SimdPoint3 intersectPt;
|
|
//intersectionPoint edge with circle;
|
|
if (SimdFuzzyZero(v.x()))
|
|
{
|
|
SimdScalar mu;
|
|
|
|
intersectPt.setY((-2.f*aa*bb+2.f*sqrtf(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*sqrtf(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
|
|
{
|
|
SimdScalar mu;
|
|
|
|
intersectPt.setX((-2.f*aa*bb+2.f*sqrtf(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*sqrtf(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 = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
|
|
if (hit)
|
|
{
|
|
lambda1 = lambda;
|
|
mu1 = mu;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
} else
|
|
{
|
|
//printf("general case, W<>0,S<>0\n");
|
|
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
|
|
if (hit)
|
|
{
|
|
lambda1 = lambda;
|
|
mu1 = mu;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
//W <> 0,pure rotation
|
|
}
|
|
|
|
return hit;
|
|
}
|
|
|
|
|
|
bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
|
|
const BU_Screwing& screwAB,
|
|
const SimdPoint3& a,//edge in object A
|
|
const SimdVector3& u,
|
|
const SimdPoint3& c,//edge in object B
|
|
const SimdVector3& v,
|
|
SimdScalar &minTime,
|
|
SimdScalar &lambda,
|
|
SimdScalar& mu
|
|
|
|
)
|
|
{
|
|
bool hit = false;
|
|
|
|
SimdScalar coefs[4];
|
|
BU_Polynomial polynomialSolver;
|
|
int numroots = 0;
|
|
|
|
SimdScalar eps=1e-15f;
|
|
SimdScalar eps2=1e-20f;
|
|
SimdScalar s=screwAB.GetS();
|
|
SimdScalar w = screwAB.GetW();
|
|
|
|
SimdScalar ax = a.x();
|
|
SimdScalar ay = a.y();
|
|
SimdScalar az = a.z();
|
|
SimdScalar cx = c.x();
|
|
SimdScalar cy = c.y();
|
|
SimdScalar cz = c.z();
|
|
SimdScalar vx = v.x();
|
|
SimdScalar vy = v.y();
|
|
SimdScalar vz = v.z();
|
|
SimdScalar ux = u.x();
|
|
SimdScalar uy = u.y();
|
|
SimdScalar uz = u.z();
|
|
|
|
|
|
if (!SimdFuzzyZero(v.z()))
|
|
{
|
|
|
|
//Maple Autogenerated C code
|
|
SimdScalar t1,t2,t3,t4,t7,t8,t10;
|
|
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
|
|
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
|
|
SimdScalar t31,t32,t33,t34,t35,t36,t39,t40;
|
|
SimdScalar t41,t43,t48;
|
|
SimdScalar t63;
|
|
|
|
SimdScalar aa,bb,cc,dd;//the coefficients
|
|
|
|
t1 = v.y()*s; t2 = t1*u.x();
|
|
t3 = v.x()*s;
|
|
t4 = t3*u.y();
|
|
t7 = tanf(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
|
|
{
|
|
|
|
SimdScalar t1,t2,t3,t4,t7,t8,t10;
|
|
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
|
|
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
|
|
SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
|
|
SimdScalar p1,p2,p3,p4;
|
|
|
|
t1 = uy*s;
|
|
t2 = t1*vx;
|
|
t3 = ux*s;
|
|
t4 = t3*vy;
|
|
t7 = tanf(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++)
|
|
{
|
|
//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i);
|
|
SimdScalar tau = polynomialSolver.GetRoot(i);
|
|
|
|
//check whether mu and lambda are in range [0..1]
|
|
|
|
if (!SimdFuzzyZero(v.z()))
|
|
{
|
|
SimdScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
|
|
SimdScalar B1=((1.f+tau*tau)*(cx*tanf(1.f/2.f*w)*vz+
|
|
vx*az*tanf(1.f/2.f*w)-vx*cz*tanf(1.f/2.f*w)+
|
|
vx*s*tau)/tanf(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)/(tanf(w/2.f)))/v.z();
|
|
|
|
|
|
//double check in original equation
|
|
|
|
SimdScalar 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));
|
|
|
|
SimdScalar rhs = c.x()+mu*v.x();
|
|
|
|
rhs = ((1.f+tau*tau)*(cx*tanf(1.f/2.f*w)*vz+vx*az*tanf(1.f/2.f*w)-
|
|
vx*cz*tanf(1.f/2.f*w)+vx*s*tau)/(tanf(1.f/2.f*w)*vz))-
|
|
|
|
(ax-ax*tau*tau-2.f*tau*ay);
|
|
|
|
SimdScalar 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;
|
|
}
|
|
|
|
}
|
|
|
|
SimdScalar t = 2.f*atanf(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 SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime)
|
|
{
|
|
bool hit = false;
|
|
|
|
// now calculate the planeEquation for the vertex motion,
|
|
// and check if the intersectionpoint is at the positive side
|
|
SimdPoint3 rotPt1(cosf(rotW)*rotPt.x()-sinf(rotW)*rotPt.y(),
|
|
sinf(rotW)*rotPt.x()+cosf(rotW)*rotPt.y(),
|
|
0.f);
|
|
|
|
SimdVector3 rotVec = rotPt1-rotPt;
|
|
|
|
SimdVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
|
|
|
|
//SimdPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
|
|
SimdScalar planeD = planeNormal.dot(rotPt1);
|
|
|
|
SimdScalar 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);
|
|
SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
|
|
SimdScalar hypo = rotRadius;
|
|
SimdScalar alpha = acosf(adjacent/hypo);
|
|
SimdScalar 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 SimdPoint3& a,//edge in object A
|
|
const SimdVector3& u,
|
|
const SimdPoint3& c,//edge in object B
|
|
const SimdVector3& v,
|
|
SimdScalar &minTime,
|
|
SimdScalar &lamda,
|
|
SimdScalar& mu
|
|
|
|
)
|
|
{
|
|
return false;
|
|
}
|