Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/bullet/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp')
-rw-r--r--extern/bullet/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp573
1 files changed, 573 insertions, 0 deletions
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp
new file mode 100644
index 00000000000..26a4fe74fbf
--- /dev/null
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp
@@ -0,0 +1,573 @@
+/*
+ * 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;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-08-06 23:15:26 +0300