summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp')
-rw-r--r--thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp1562
1 files changed, 805 insertions, 757 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
index d636f983c6..974b246f03 100644
--- a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
+++ b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
@@ -29,902 +29,951 @@ GJK-EPA collision solver by Nathanael Presson, 2008
namespace gjkepa2_impl2
{
+// Config
- // Config
+/* GJK */
+#define GJK_MAX_ITERATIONS 128
+#define GJK_ACCURACY ((b3Scalar)0.0001)
+#define GJK_MIN_DISTANCE ((b3Scalar)0.0001)
+#define GJK_DUPLICATED_EPS ((b3Scalar)0.0001)
+#define GJK_SIMPLEX2_EPS ((b3Scalar)0.0)
+#define GJK_SIMPLEX3_EPS ((b3Scalar)0.0)
+#define GJK_SIMPLEX4_EPS ((b3Scalar)0.0)
- /* GJK */
-#define GJK_MAX_ITERATIONS 128
-#define GJK_ACCURACY ((b3Scalar)0.0001)
-#define GJK_MIN_DISTANCE ((b3Scalar)0.0001)
-#define GJK_DUPLICATED_EPS ((b3Scalar)0.0001)
-#define GJK_SIMPLEX2_EPS ((b3Scalar)0.0)
-#define GJK_SIMPLEX3_EPS ((b3Scalar)0.0)
-#define GJK_SIMPLEX4_EPS ((b3Scalar)0.0)
+/* EPA */
+#define EPA_MAX_VERTICES 64
+#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2)
+#define EPA_MAX_ITERATIONS 255
+#define EPA_ACCURACY ((b3Scalar)0.0001)
+#define EPA_FALLBACK (10 * EPA_ACCURACY)
+#define EPA_PLANE_EPS ((b3Scalar)0.00001)
+#define EPA_INSIDE_EPS ((b3Scalar)0.01)
- /* EPA */
-#define EPA_MAX_VERTICES 64
-#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
-#define EPA_MAX_ITERATIONS 255
-#define EPA_ACCURACY ((b3Scalar)0.0001)
-#define EPA_FALLBACK (10*EPA_ACCURACY)
-#define EPA_PLANE_EPS ((b3Scalar)0.00001)
-#define EPA_INSIDE_EPS ((b3Scalar)0.01)
+// Shorthands
+// MinkowskiDiff
+struct b3MinkowskiDiff
+{
+ const b3ConvexPolyhedronData* m_shapes[2];
- // Shorthands
-
-
- // MinkowskiDiff
- struct b3MinkowskiDiff
- {
-
-
- const b3ConvexPolyhedronData* m_shapes[2];
-
-
- b3Matrix3x3 m_toshape1;
- b3Transform m_toshape0;
+ b3Matrix3x3 m_toshape1;
+ b3Transform m_toshape0;
- bool m_enableMargin;
-
+ bool m_enableMargin;
- void EnableMargin(bool enable)
- {
- m_enableMargin = enable;
- }
- inline b3Vector3 Support0(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA) const
+ void EnableMargin(bool enable)
+ {
+ m_enableMargin = enable;
+ }
+ inline b3Vector3 Support0(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA) const
+ {
+ if (m_enableMargin)
{
- if (m_enableMargin)
- {
- return localGetSupportVertexWithMargin(d,m_shapes[0],verticesA,0.f);
- } else
- {
- return localGetSupportVertexWithoutMargin(d,m_shapes[0],verticesA);
- }
+ return localGetSupportVertexWithMargin(d, m_shapes[0], verticesA, 0.f);
}
- inline b3Vector3 Support1(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesB) const
+ else
{
- if (m_enableMargin)
- {
- return m_toshape0*(localGetSupportVertexWithMargin(m_toshape1*d,m_shapes[1],verticesB,0.f));
- } else
- {
- return m_toshape0*(localGetSupportVertexWithoutMargin(m_toshape1*d,m_shapes[1],verticesB));
- }
+ return localGetSupportVertexWithoutMargin(d, m_shapes[0], verticesA);
}
-
- inline b3Vector3 Support(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const
+ }
+ inline b3Vector3 Support1(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesB) const
+ {
+ if (m_enableMargin)
{
- return(Support0(d,verticesA)-Support1(-d,verticesB));
+ return m_toshape0 * (localGetSupportVertexWithMargin(m_toshape1 * d, m_shapes[1], verticesB, 0.f));
}
- b3Vector3 Support(const b3Vector3& d,unsigned int index,const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const
+ else
{
- if(index)
- return(Support1(d,verticesA));
- else
- return(Support0(d,verticesB));
+ return m_toshape0 * (localGetSupportVertexWithoutMargin(m_toshape1 * d, m_shapes[1], verticesB));
}
- };
+ }
- typedef b3MinkowskiDiff tShape;
+ inline b3Vector3 Support(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const
+ {
+ return (Support0(d, verticesA) - Support1(-d, verticesB));
+ }
+ b3Vector3 Support(const b3Vector3& d, unsigned int index, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const
+ {
+ if (index)
+ return (Support1(d, verticesA));
+ else
+ return (Support0(d, verticesB));
+ }
+};
+typedef b3MinkowskiDiff tShape;
- // GJK
- struct b3GJK
+// GJK
+struct b3GJK
+{
+ /* Types */
+ struct sSV
{
- /* Types */
- struct sSV
- {
- b3Vector3 d,w;
- };
- struct sSimplex
+ b3Vector3 d, w;
+ };
+ struct sSimplex
+ {
+ sSV* c[4];
+ b3Scalar p[4];
+ unsigned int rank;
+ };
+ struct eStatus
+ {
+ enum _
{
- sSV* c[4];
- b3Scalar p[4];
- unsigned int rank;
- };
- struct eStatus { enum _ {
Valid,
Inside,
- Failed };};
- /* Fields */
- tShape m_shape;
- const b3AlignedObjectArray<b3Vector3>& m_verticesA;
- const b3AlignedObjectArray<b3Vector3>& m_verticesB;
- b3Vector3 m_ray;
- b3Scalar m_distance;
- sSimplex m_simplices[2];
- sSV m_store[4];
- sSV* m_free[4];
- unsigned int m_nfree;
- unsigned int m_current;
- sSimplex* m_simplex;
- eStatus::_ m_status;
- /* Methods */
- b3GJK(const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB)
- :m_verticesA(verticesA),m_verticesB(verticesB)
- {
- Initialize();
+ Failed
+ };
+ };
+ /* Fields */
+ tShape m_shape;
+ const b3AlignedObjectArray<b3Vector3>& m_verticesA;
+ const b3AlignedObjectArray<b3Vector3>& m_verticesB;
+ b3Vector3 m_ray;
+ b3Scalar m_distance;
+ sSimplex m_simplices[2];
+ sSV m_store[4];
+ sSV* m_free[4];
+ unsigned int m_nfree;
+ unsigned int m_current;
+ sSimplex* m_simplex;
+ eStatus::_ m_status;
+ /* Methods */
+ b3GJK(const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB)
+ : m_verticesA(verticesA), m_verticesB(verticesB)
+ {
+ Initialize();
+ }
+ void Initialize()
+ {
+ m_ray = b3MakeVector3(0, 0, 0);
+ m_nfree = 0;
+ m_status = eStatus::Failed;
+ m_current = 0;
+ m_distance = 0;
+ }
+ eStatus::_ Evaluate(const tShape& shapearg, const b3Vector3& guess)
+ {
+ unsigned int iterations = 0;
+ b3Scalar sqdist = 0;
+ b3Scalar alpha = 0;
+ b3Vector3 lastw[4];
+ unsigned int clastw = 0;
+ /* Initialize solver */
+ m_free[0] = &m_store[0];
+ m_free[1] = &m_store[1];
+ m_free[2] = &m_store[2];
+ m_free[3] = &m_store[3];
+ m_nfree = 4;
+ m_current = 0;
+ m_status = eStatus::Valid;
+ m_shape = shapearg;
+ m_distance = 0;
+ /* Initialize simplex */
+ m_simplices[0].rank = 0;
+ m_ray = guess;
+ const b3Scalar sqrl = m_ray.length2();
+ appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : b3MakeVector3(1, 0, 0));
+ m_simplices[0].p[0] = 1;
+ m_ray = m_simplices[0].c[0]->w;
+ sqdist = sqrl;
+ lastw[0] =
+ lastw[1] =
+ lastw[2] =
+ lastw[3] = m_ray;
+ /* Loop */
+ do
+ {
+ const unsigned int next = 1 - m_current;
+ sSimplex& cs = m_simplices[m_current];
+ sSimplex& ns = m_simplices[next];
+ /* Check zero */
+ const b3Scalar rl = m_ray.length();
+ if (rl < GJK_MIN_DISTANCE)
+ { /* Touching or inside */
+ m_status = eStatus::Inside;
+ break;
}
- void Initialize()
+ /* Append new vertice in -'v' direction */
+ appendvertice(cs, -m_ray);
+ const b3Vector3& w = cs.c[cs.rank - 1]->w;
+ bool found = false;
+ for (unsigned int i = 0; i < 4; ++i)
{
- m_ray = b3MakeVector3(0,0,0);
- m_nfree = 0;
- m_status = eStatus::Failed;
- m_current = 0;
- m_distance = 0;
+ if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS)
+ {
+ found = true;
+ break;
+ }
+ }
+ if (found)
+ { /* Return old simplex */
+ removevertice(m_simplices[m_current]);
+ break;
}
- eStatus::_ Evaluate(const tShape& shapearg,const b3Vector3& guess)
+ else
+ { /* Update lastw */
+ lastw[clastw = (clastw + 1) & 3] = w;
+ }
+ /* Check for termination */
+ const b3Scalar omega = b3Dot(m_ray, w) / rl;
+ alpha = b3Max(omega, alpha);
+ if (((rl - alpha) - (GJK_ACCURACY * rl)) <= 0)
+ { /* Return old simplex */
+ removevertice(m_simplices[m_current]);
+ break;
+ }
+ /* Reduce simplex */
+ b3Scalar weights[4];
+ unsigned int mask = 0;
+ switch (cs.rank)
{
- unsigned int iterations=0;
- b3Scalar sqdist=0;
- b3Scalar alpha=0;
- b3Vector3 lastw[4];
- unsigned int clastw=0;
- /* Initialize solver */
- m_free[0] = &m_store[0];
- m_free[1] = &m_store[1];
- m_free[2] = &m_store[2];
- m_free[3] = &m_store[3];
- m_nfree = 4;
- m_current = 0;
- m_status = eStatus::Valid;
- m_shape = shapearg;
- m_distance = 0;
- /* Initialize simplex */
- m_simplices[0].rank = 0;
- m_ray = guess;
- const b3Scalar sqrl= m_ray.length2();
- appendvertice(m_simplices[0],sqrl>0?-m_ray:b3MakeVector3(1,0,0));
- m_simplices[0].p[0] = 1;
- m_ray = m_simplices[0].c[0]->w;
- sqdist = sqrl;
- lastw[0] =
- lastw[1] =
- lastw[2] =
- lastw[3] = m_ray;
- /* Loop */
- do {
- const unsigned int next=1-m_current;
- sSimplex& cs=m_simplices[m_current];
- sSimplex& ns=m_simplices[next];
- /* Check zero */
- const b3Scalar rl=m_ray.length();
- if(rl<GJK_MIN_DISTANCE)
- {/* Touching or inside */
- m_status=eStatus::Inside;
- break;
- }
- /* Append new vertice in -'v' direction */
- appendvertice(cs,-m_ray);
- const b3Vector3& w=cs.c[cs.rank-1]->w;
- bool found=false;
- for(unsigned int i=0;i<4;++i)
+ case 2:
+ sqdist = projectorigin(cs.c[0]->w,
+ cs.c[1]->w,
+ weights, mask);
+ break;
+ case 3:
+ sqdist = projectorigin(cs.c[0]->w,
+ cs.c[1]->w,
+ cs.c[2]->w,
+ weights, mask);
+ break;
+ case 4:
+ sqdist = projectorigin(cs.c[0]->w,
+ cs.c[1]->w,
+ cs.c[2]->w,
+ cs.c[3]->w,
+ weights, mask);
+ break;
+ }
+ if (sqdist >= 0)
+ { /* Valid */
+ ns.rank = 0;
+ m_ray = b3MakeVector3(0, 0, 0);
+ m_current = next;
+ for (unsigned int i = 0, ni = cs.rank; i < ni; ++i)
+ {
+ if (mask & (1 << i))
{
- if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
- { found=true;break; }
- }
- if(found)
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
+ ns.c[ns.rank] = cs.c[i];
+ ns.p[ns.rank++] = weights[i];
+ m_ray += cs.c[i]->w * weights[i];
}
else
- {/* Update lastw */
- lastw[clastw=(clastw+1)&3]=w;
- }
- /* Check for termination */
- const b3Scalar omega=b3Dot(m_ray,w)/rl;
- alpha=b3Max(omega,alpha);
- if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- /* Reduce simplex */
- b3Scalar weights[4];
- unsigned int mask=0;
- switch(cs.rank)
{
- case 2: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- weights,mask);break;
- case 3: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- cs.c[2]->w,
- weights,mask);break;
- case 4: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- cs.c[2]->w,
- cs.c[3]->w,
- weights,mask);break;
- }
- if(sqdist>=0)
- {/* Valid */
- ns.rank = 0;
- m_ray = b3MakeVector3(0,0,0);
- m_current = next;
- for(unsigned int i=0,ni=cs.rank;i<ni;++i)
- {
- if(mask&(1<<i))
- {
- ns.c[ns.rank] = cs.c[i];
- ns.p[ns.rank++] = weights[i];
- m_ray += cs.c[i]->w*weights[i];
- }
- else
- {
- m_free[m_nfree++] = cs.c[i];
- }
- }
- if(mask==15) m_status=eStatus::Inside;
+ m_free[m_nfree++] = cs.c[i];
}
- else
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
- } while(m_status==eStatus::Valid);
- m_simplex=&m_simplices[m_current];
- switch(m_status)
+ }
+ if (mask == 15) m_status = eStatus::Inside;
+ }
+ else
+ { /* Return old simplex */
+ removevertice(m_simplices[m_current]);
+ break;
+ }
+ m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed;
+ } while (m_status == eStatus::Valid);
+ m_simplex = &m_simplices[m_current];
+ switch (m_status)
+ {
+ case eStatus::Valid:
+ m_distance = m_ray.length();
+ break;
+ case eStatus::Inside:
+ m_distance = 0;
+ break;
+ default:
+ {
+ }
+ }
+ return (m_status);
+ }
+ bool EncloseOrigin()
+ {
+ switch (m_simplex->rank)
+ {
+ case 1:
+ {
+ for (unsigned int i = 0; i < 3; ++i)
{
- case eStatus::Valid: m_distance=m_ray.length();break;
- case eStatus::Inside: m_distance=0;break;
- default:
- {
- }
- }
- return(m_status);
+ b3Vector3 axis = b3MakeVector3(0, 0, 0);
+ axis[i] = 1;
+ appendvertice(*m_simplex, axis);
+ if (EncloseOrigin()) return (true);
+ removevertice(*m_simplex);
+ appendvertice(*m_simplex, -axis);
+ if (EncloseOrigin()) return (true);
+ removevertice(*m_simplex);
+ }
}
- bool EncloseOrigin()
+ break;
+ case 2:
{
- switch(m_simplex->rank)
+ const b3Vector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w;
+ for (unsigned int i = 0; i < 3; ++i)
{
- case 1:
+ b3Vector3 axis = b3MakeVector3(0, 0, 0);
+ axis[i] = 1;
+ const b3Vector3 p = b3Cross(d, axis);
+ if (p.length2() > 0)
{
- for(unsigned int i=0;i<3;++i)
- {
- b3Vector3 axis=b3MakeVector3(0,0,0);
- axis[i]=1;
- appendvertice(*m_simplex, axis);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-axis);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
+ appendvertice(*m_simplex, p);
+ if (EncloseOrigin()) return (true);
+ removevertice(*m_simplex);
+ appendvertice(*m_simplex, -p);
+ if (EncloseOrigin()) return (true);
+ removevertice(*m_simplex);
}
- break;
- case 2:
- {
- const b3Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
- for(unsigned int i=0;i<3;++i)
- {
- b3Vector3 axis=b3MakeVector3(0,0,0);
- axis[i]=1;
- const b3Vector3 p=b3Cross(d,axis);
- if(p.length2()>0)
- {
- appendvertice(*m_simplex, p);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-p);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- }
- break;
- case 3:
- {
- const b3Vector3 n=b3Cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
- m_simplex->c[2]->w-m_simplex->c[0]->w);
- if(n.length2()>0)
- {
- appendvertice(*m_simplex,n);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-n);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- break;
- case 4:
- {
- if(b3Fabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
- m_simplex->c[1]->w-m_simplex->c[3]->w,
- m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
- return(true);
- }
- break;
}
- return(false);
}
- /* Internals */
- void getsupport(const b3Vector3& d,sSV& sv) const
+ break;
+ case 3:
{
- sv.d = d/d.length();
- sv.w = m_shape.Support(sv.d,m_verticesA,m_verticesB);
+ const b3Vector3 n = b3Cross(m_simplex->c[1]->w - m_simplex->c[0]->w,
+ m_simplex->c[2]->w - m_simplex->c[0]->w);
+ if (n.length2() > 0)
+ {
+ appendvertice(*m_simplex, n);
+ if (EncloseOrigin()) return (true);
+ removevertice(*m_simplex);
+ appendvertice(*m_simplex, -n);
+ if (EncloseOrigin()) return (true);
+ removevertice(*m_simplex);
+ }
}
- void removevertice(sSimplex& simplex)
+ break;
+ case 4:
{
- m_free[m_nfree++]=simplex.c[--simplex.rank];
+ if (b3Fabs(det(m_simplex->c[0]->w - m_simplex->c[3]->w,
+ m_simplex->c[1]->w - m_simplex->c[3]->w,
+ m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0)
+ return (true);
}
- void appendvertice(sSimplex& simplex,const b3Vector3& v)
+ break;
+ }
+ return (false);
+ }
+ /* Internals */
+ void getsupport(const b3Vector3& d, sSV& sv) const
+ {
+ sv.d = d / d.length();
+ sv.w = m_shape.Support(sv.d, m_verticesA, m_verticesB);
+ }
+ void removevertice(sSimplex& simplex)
+ {
+ m_free[m_nfree++] = simplex.c[--simplex.rank];
+ }
+ void appendvertice(sSimplex& simplex, const b3Vector3& v)
+ {
+ simplex.p[simplex.rank] = 0;
+ simplex.c[simplex.rank] = m_free[--m_nfree];
+ getsupport(v, *simplex.c[simplex.rank++]);
+ }
+ static b3Scalar det(const b3Vector3& a, const b3Vector3& b, const b3Vector3& c)
+ {
+ return (a.y * b.z * c.x + a.z * b.x * c.y -
+ a.x * b.z * c.y - a.y * b.x * c.z +
+ a.x * b.y * c.z - a.z * b.y * c.x);
+ }
+ static b3Scalar projectorigin(const b3Vector3& a,
+ const b3Vector3& b,
+ b3Scalar* w, unsigned int& m)
+ {
+ const b3Vector3 d = b - a;
+ const b3Scalar l = d.length2();
+ if (l > GJK_SIMPLEX2_EPS)
+ {
+ const b3Scalar t(l > 0 ? -b3Dot(a, d) / l : 0);
+ if (t >= 1)
{
- simplex.p[simplex.rank]=0;
- simplex.c[simplex.rank]=m_free[--m_nfree];
- getsupport(v,*simplex.c[simplex.rank++]);
+ w[0] = 0;
+ w[1] = 1;
+ m = 2;
+ return (b.length2());
}
- static b3Scalar det(const b3Vector3& a,const b3Vector3& b,const b3Vector3& c)
+ else if (t <= 0)
{
- return( a.y*b.z*c.x+a.z*b.x*c.y-
- a.x*b.z*c.y-a.y*b.x*c.z+
- a.x*b.y*c.z-a.z*b.y*c.x);
+ w[0] = 1;
+ w[1] = 0;
+ m = 1;
+ return (a.length2());
}
- static b3Scalar projectorigin( const b3Vector3& a,
- const b3Vector3& b,
- b3Scalar* w,unsigned int& m)
+ else
{
- const b3Vector3 d=b-a;
- const b3Scalar l=d.length2();
- if(l>GJK_SIMPLEX2_EPS)
- {
- const b3Scalar t(l>0?-b3Dot(a,d)/l:0);
- if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
- else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
- else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
- }
- return(-1);
+ w[0] = 1 - (w[1] = t);
+ m = 3;
+ return ((a + d * t).length2());
}
- static b3Scalar projectorigin( const b3Vector3& a,
- const b3Vector3& b,
- const b3Vector3& c,
- b3Scalar* w,unsigned int& m)
+ }
+ return (-1);
+ }
+ static b3Scalar projectorigin(const b3Vector3& a,
+ const b3Vector3& b,
+ const b3Vector3& c,
+ b3Scalar* w, unsigned int& m)
+ {
+ static const unsigned int imd3[] = {1, 2, 0};
+ const b3Vector3* vt[] = {&a, &b, &c};
+ const b3Vector3 dl[] = {a - b, b - c, c - a};
+ const b3Vector3 n = b3Cross(dl[0], dl[1]);
+ const b3Scalar l = n.length2();
+ if (l > GJK_SIMPLEX3_EPS)
+ {
+ b3Scalar mindist = -1;
+ b3Scalar subw[2] = {0.f, 0.f};
+ unsigned int subm(0);
+ for (unsigned int i = 0; i < 3; ++i)
{
- static const unsigned int imd3[]={1,2,0};
- const b3Vector3* vt[]={&a,&b,&c};
- const b3Vector3 dl[]={a-b,b-c,c-a};
- const b3Vector3 n=b3Cross(dl[0],dl[1]);
- const b3Scalar l=n.length2();
- if(l>GJK_SIMPLEX3_EPS)
+ if (b3Dot(*vt[i], b3Cross(dl[i], n)) > 0)
{
- b3Scalar mindist=-1;
- b3Scalar subw[2]={0.f,0.f};
- unsigned int subm(0);
- for(unsigned int i=0;i<3;++i)
- {
- if(b3Dot(*vt[i],b3Cross(dl[i],n))>0)
- {
- const unsigned int j=imd3[i];
- const b3Scalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
- if((mindist<0)||(subd<mindist))
- {
- mindist = subd;
- m = static_cast<unsigned int>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
- w[i] = subw[0];
- w[j] = subw[1];
- w[imd3[j]] = 0;
- }
- }
- }
- if(mindist<0)
+ const unsigned int j = imd3[i];
+ const b3Scalar subd(projectorigin(*vt[i], *vt[j], subw, subm));
+ if ((mindist < 0) || (subd < mindist))
{
- const b3Scalar d=b3Dot(a,n);
- const b3Scalar s=b3Sqrt(l);
- const b3Vector3 p=n*(d/l);
- mindist = p.length2();
- m = 7;
- w[0] = (b3Cross(dl[1],b-p)).length()/s;
- w[1] = (b3Cross(dl[2],c-p)).length()/s;
- w[2] = 1-(w[0]+w[1]);
+ mindist = subd;
+ m = static_cast<unsigned int>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0));
+ w[i] = subw[0];
+ w[j] = subw[1];
+ w[imd3[j]] = 0;
}
- return(mindist);
}
- return(-1);
}
- static b3Scalar projectorigin( const b3Vector3& a,
- const b3Vector3& b,
- const b3Vector3& c,
- const b3Vector3& d,
- b3Scalar* w,unsigned int& m)
+ if (mindist < 0)
+ {
+ const b3Scalar d = b3Dot(a, n);
+ const b3Scalar s = b3Sqrt(l);
+ const b3Vector3 p = n * (d / l);
+ mindist = p.length2();
+ m = 7;
+ w[0] = (b3Cross(dl[1], b - p)).length() / s;
+ w[1] = (b3Cross(dl[2], c - p)).length() / s;
+ w[2] = 1 - (w[0] + w[1]);
+ }
+ return (mindist);
+ }
+ return (-1);
+ }
+ static b3Scalar projectorigin(const b3Vector3& a,
+ const b3Vector3& b,
+ const b3Vector3& c,
+ const b3Vector3& d,
+ b3Scalar* w, unsigned int& m)
+ {
+ static const unsigned int imd3[] = {1, 2, 0};
+ const b3Vector3* vt[] = {&a, &b, &c, &d};
+ const b3Vector3 dl[] = {a - d, b - d, c - d};
+ const b3Scalar vl = det(dl[0], dl[1], dl[2]);
+ const bool ng = (vl * b3Dot(a, b3Cross(b - c, a - b))) <= 0;
+ if (ng && (b3Fabs(vl) > GJK_SIMPLEX4_EPS))
+ {
+ b3Scalar mindist = -1;
+ b3Scalar subw[3] = {0.f, 0.f, 0.f};
+ unsigned int subm(0);
+ for (unsigned int i = 0; i < 3; ++i)
{
- static const unsigned int imd3[]={1,2,0};
- const b3Vector3* vt[]={&a,&b,&c,&d};
- const b3Vector3 dl[]={a-d,b-d,c-d};
- const b3Scalar vl=det(dl[0],dl[1],dl[2]);
- const bool ng=(vl*b3Dot(a,b3Cross(b-c,a-b)))<=0;
- if(ng&&(b3Fabs(vl)>GJK_SIMPLEX4_EPS))
+ const unsigned int j = imd3[i];
+ const b3Scalar s = vl * b3Dot(d, b3Cross(dl[i], dl[j]));
+ if (s > 0)
{
- b3Scalar mindist=-1;
- b3Scalar subw[3]={0.f,0.f,0.f};
- unsigned int subm(0);
- for(unsigned int i=0;i<3;++i)
+ const b3Scalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm);
+ if ((mindist < 0) || (subd < mindist))
{
- const unsigned int j=imd3[i];
- const b3Scalar s=vl*b3Dot(d,b3Cross(dl[i],dl[j]));
- if(s>0)
- {
- const b3Scalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
- if((mindist<0)||(subd<mindist))
- {
- mindist = subd;
- m = static_cast<unsigned int>((subm&1?1<<i:0)+
- (subm&2?1<<j:0)+
- (subm&4?8:0));
- w[i] = subw[0];
- w[j] = subw[1];
- w[imd3[j]] = 0;
- w[3] = subw[2];
- }
- }
+ mindist = subd;
+ m = static_cast<unsigned int>((subm & 1 ? 1 << i : 0) +
+ (subm & 2 ? 1 << j : 0) +
+ (subm & 4 ? 8 : 0));
+ w[i] = subw[0];
+ w[j] = subw[1];
+ w[imd3[j]] = 0;
+ w[3] = subw[2];
}
- if(mindist<0)
- {
- mindist = 0;
- m = 15;
- w[0] = det(c,b,d)/vl;
- w[1] = det(a,c,d)/vl;
- w[2] = det(b,a,d)/vl;
- w[3] = 1-(w[0]+w[1]+w[2]);
- }
- return(mindist);
}
- return(-1);
}
- };
+ if (mindist < 0)
+ {
+ mindist = 0;
+ m = 15;
+ w[0] = det(c, b, d) / vl;
+ w[1] = det(a, c, d) / vl;
+ w[2] = det(b, a, d) / vl;
+ w[3] = 1 - (w[0] + w[1] + w[2]);
+ }
+ return (mindist);
+ }
+ return (-1);
+ }
+};
- // EPA
- struct b3EPA
+// EPA
+struct b3EPA
+{
+ /* Types */
+ typedef b3GJK::sSV sSV;
+ struct sFace
{
- /* Types */
- typedef b3GJK::sSV sSV;
- struct sFace
- {
- b3Vector3 n;
- b3Scalar d;
- sSV* c[3];
- sFace* f[3];
- sFace* l[2];
- unsigned char e[3];
- unsigned char pass;
- };
- struct sList
- {
- sFace* root;
- unsigned int count;
- sList() : root(0),count(0) {}
- };
- struct sHorizon
+ b3Vector3 n;
+ b3Scalar d;
+ sSV* c[3];
+ sFace* f[3];
+ sFace* l[2];
+ unsigned char e[3];
+ unsigned char pass;
+ };
+ struct sList
+ {
+ sFace* root;
+ unsigned int count;
+ sList() : root(0), count(0) {}
+ };
+ struct sHorizon
+ {
+ sFace* cf;
+ sFace* ff;
+ unsigned int nf;
+ sHorizon() : cf(0), ff(0), nf(0) {}
+ };
+ struct eStatus
+ {
+ enum _
{
- sFace* cf;
- sFace* ff;
- unsigned int nf;
- sHorizon() : cf(0),ff(0),nf(0) {}
- };
- struct eStatus { enum _ {
Valid,
Touching,
Degenerated,
NonConvex,
- InvalidHull,
+ InvalidHull,
OutOfFaces,
OutOfVertices,
AccuraryReached,
FallBack,
- Failed };};
- /* Fields */
- eStatus::_ m_status;
- b3GJK::sSimplex m_result;
- b3Vector3 m_normal;
- b3Scalar m_depth;
- sSV m_sv_store[EPA_MAX_VERTICES];
- sFace m_fc_store[EPA_MAX_FACES];
- unsigned int m_nextsv;
- sList m_hull;
- sList m_stock;
- /* Methods */
- b3EPA()
- {
- Initialize();
- }
+ Failed
+ };
+ };
+ /* Fields */
+ eStatus::_ m_status;
+ b3GJK::sSimplex m_result;
+ b3Vector3 m_normal;
+ b3Scalar m_depth;
+ sSV m_sv_store[EPA_MAX_VERTICES];
+ sFace m_fc_store[EPA_MAX_FACES];
+ unsigned int m_nextsv;
+ sList m_hull;
+ sList m_stock;
+ /* Methods */
+ b3EPA()
+ {
+ Initialize();
+ }
+ static inline void bind(sFace* fa, unsigned int ea, sFace* fb, unsigned int eb)
+ {
+ fa->e[ea] = (unsigned char)eb;
+ fa->f[ea] = fb;
+ fb->e[eb] = (unsigned char)ea;
+ fb->f[eb] = fa;
+ }
+ static inline void append(sList& list, sFace* face)
+ {
+ face->l[0] = 0;
+ face->l[1] = list.root;
+ if (list.root) list.root->l[0] = face;
+ list.root = face;
+ ++list.count;
+ }
+ static inline void remove(sList& list, sFace* face)
+ {
+ if (face->l[1]) face->l[1]->l[0] = face->l[0];
+ if (face->l[0]) face->l[0]->l[1] = face->l[1];
+ if (face == list.root) list.root = face->l[1];
+ --list.count;
+ }
- static inline void bind(sFace* fa,unsigned int ea,sFace* fb,unsigned int eb)
- {
- fa->e[ea]=(unsigned char)eb;fa->f[ea]=fb;
- fb->e[eb]=(unsigned char)ea;fb->f[eb]=fa;
- }
- static inline void append(sList& list,sFace* face)
+ void Initialize()
+ {
+ m_status = eStatus::Failed;
+ m_normal = b3MakeVector3(0, 0, 0);
+ m_depth = 0;
+ m_nextsv = 0;
+ for (unsigned int i = 0; i < EPA_MAX_FACES; ++i)
+ {
+ append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]);
+ }
+ }
+ eStatus::_ Evaluate(b3GJK& gjk, const b3Vector3& guess)
+ {
+ b3GJK::sSimplex& simplex = *gjk.m_simplex;
+ if ((simplex.rank > 1) && gjk.EncloseOrigin())
+ {
+ /* Clean up */
+ while (m_hull.root)
{
- face->l[0] = 0;
- face->l[1] = list.root;
- if(list.root) list.root->l[0]=face;
- list.root = face;
- ++list.count;
+ sFace* f = m_hull.root;
+ remove(m_hull, f);
+ append(m_stock, f);
}
- static inline void remove(sList& list,sFace* face)
+ m_status = eStatus::Valid;
+ m_nextsv = 0;
+ /* Orient simplex */
+ if (gjk.det(simplex.c[0]->w - simplex.c[3]->w,
+ simplex.c[1]->w - simplex.c[3]->w,
+ simplex.c[2]->w - simplex.c[3]->w) < 0)
{
- if(face->l[1]) face->l[1]->l[0]=face->l[0];
- if(face->l[0]) face->l[0]->l[1]=face->l[1];
- if(face==list.root) list.root=face->l[1];
- --list.count;
+ b3Swap(simplex.c[0], simplex.c[1]);
+ b3Swap(simplex.p[0], simplex.p[1]);
}
-
-
- void Initialize()
+ /* Build initial hull */
+ sFace* tetra[] = {newface(simplex.c[0], simplex.c[1], simplex.c[2], true),
+ newface(simplex.c[1], simplex.c[0], simplex.c[3], true),
+ newface(simplex.c[2], simplex.c[1], simplex.c[3], true),
+ newface(simplex.c[0], simplex.c[2], simplex.c[3], true)};
+ if (m_hull.count == 4)
{
- m_status = eStatus::Failed;
- m_normal = b3MakeVector3(0,0,0);
- m_depth = 0;
- m_nextsv = 0;
- for(unsigned int i=0;i<EPA_MAX_FACES;++i)
+ sFace* best = findbest();
+ sFace outer = *best;
+ unsigned int pass = 0;
+ unsigned int iterations = 0;
+ bind(tetra[0], 0, tetra[1], 0);
+ bind(tetra[0], 1, tetra[2], 0);
+ bind(tetra[0], 2, tetra[3], 0);
+ bind(tetra[1], 1, tetra[3], 2);
+ bind(tetra[1], 2, tetra[2], 1);
+ bind(tetra[2], 2, tetra[3], 1);
+ m_status = eStatus::Valid;
+ for (; iterations < EPA_MAX_ITERATIONS; ++iterations)
{
- append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
- }
- }
- eStatus::_ Evaluate(b3GJK& gjk,const b3Vector3& guess)
- {
- b3GJK::sSimplex& simplex=*gjk.m_simplex;
- if((simplex.rank>1)&&gjk.EncloseOrigin())
- {
-
- /* Clean up */
- while(m_hull.root)
+ if (m_nextsv < EPA_MAX_VERTICES)
{
- sFace* f = m_hull.root;
- remove(m_hull,f);
- append(m_stock,f);
- }
- m_status = eStatus::Valid;
- m_nextsv = 0;
- /* Orient simplex */
- if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
- simplex.c[1]->w-simplex.c[3]->w,
- simplex.c[2]->w-simplex.c[3]->w)<0)
- {
- b3Swap(simplex.c[0],simplex.c[1]);
- b3Swap(simplex.p[0],simplex.p[1]);
- }
- /* Build initial hull */
- sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
- newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
- newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
- newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
- if(m_hull.count==4)
- {
- sFace* best=findbest();
- sFace outer=*best;
- unsigned int pass=0;
- unsigned int iterations=0;
- bind(tetra[0],0,tetra[1],0);
- bind(tetra[0],1,tetra[2],0);
- bind(tetra[0],2,tetra[3],0);
- bind(tetra[1],1,tetra[3],2);
- bind(tetra[1],2,tetra[2],1);
- bind(tetra[2],2,tetra[3],1);
- m_status=eStatus::Valid;
- for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+ sHorizon horizon;
+ sSV* w = &m_sv_store[m_nextsv++];
+ bool valid = true;
+ best->pass = (unsigned char)(++pass);
+ gjk.getsupport(best->n, *w);
+ const b3Scalar wdist = b3Dot(best->n, w->w) - best->d;
+ if (wdist > EPA_ACCURACY)
{
- if(m_nextsv<EPA_MAX_VERTICES)
- {
- sHorizon horizon;
- sSV* w=&m_sv_store[m_nextsv++];
- bool valid=true;
- best->pass = (unsigned char)(++pass);
- gjk.getsupport(best->n,*w);
- const b3Scalar wdist=b3Dot(best->n,w->w)-best->d;
- if(wdist>EPA_ACCURACY)
- {
- for(unsigned int j=0;(j<3)&&valid;++j)
- {
- valid&=expand( pass,w,
- best->f[j],best->e[j],
- horizon);
- }
- if(valid&&(horizon.nf>=3))
- {
- bind(horizon.cf,1,horizon.ff,2);
- remove(m_hull,best);
- append(m_stock,best);
- best=findbest();
- outer=*best;
- } else {
- m_status=eStatus::Failed;
- //m_status=eStatus::InvalidHull;
- break; }
- } else { m_status=eStatus::AccuraryReached;break; }
- } else { m_status=eStatus::OutOfVertices;break; }
+ for (unsigned int j = 0; (j < 3) && valid; ++j)
+ {
+ valid &= expand(pass, w,
+ best->f[j], best->e[j],
+ horizon);
+ }
+ if (valid && (horizon.nf >= 3))
+ {
+ bind(horizon.cf, 1, horizon.ff, 2);
+ remove(m_hull, best);
+ append(m_stock, best);
+ best = findbest();
+ outer = *best;
+ }
+ else
+ {
+ m_status = eStatus::Failed;
+ //m_status=eStatus::InvalidHull;
+ break;
+ }
+ }
+ else
+ {
+ m_status = eStatus::AccuraryReached;
+ break;
}
- const b3Vector3 projection=outer.n*outer.d;
- m_normal = outer.n;
- m_depth = outer.d;
- m_result.rank = 3;
- m_result.c[0] = outer.c[0];
- m_result.c[1] = outer.c[1];
- m_result.c[2] = outer.c[2];
- m_result.p[0] = b3Cross( outer.c[1]->w-projection,
- outer.c[2]->w-projection).length();
- m_result.p[1] = b3Cross( outer.c[2]->w-projection,
- outer.c[0]->w-projection).length();
- m_result.p[2] = b3Cross( outer.c[0]->w-projection,
- outer.c[1]->w-projection).length();
- const b3Scalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
- m_result.p[0] /= sum;
- m_result.p[1] /= sum;
- m_result.p[2] /= sum;
- return(m_status);
- }
- }
- /* Fallback */
- m_status = eStatus::FallBack;
- m_normal = -guess;
- const b3Scalar nl=m_normal.length();
- if(nl>0)
- m_normal = m_normal/nl;
- else
- m_normal = b3MakeVector3(1,0,0);
- m_depth = 0;
- m_result.rank=1;
- m_result.c[0]=simplex.c[0];
- m_result.p[0]=1;
- return(m_status);
- }
- bool getedgedist(sFace* face, sSV* a, sSV* b, b3Scalar& dist)
- {
- const b3Vector3 ba = b->w - a->w;
- const b3Vector3 n_ab = b3Cross(ba, face->n); // Outward facing edge normal direction, on triangle plane
- const b3Scalar a_dot_nab = b3Dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
-
- if(a_dot_nab < 0)
- {
- // Outside of edge a->b
-
- const b3Scalar ba_l2 = ba.length2();
- const b3Scalar a_dot_ba = b3Dot(a->w, ba);
- const b3Scalar b_dot_ba = b3Dot(b->w, ba);
-
- if(a_dot_ba > 0)
- {
- // Pick distance vertex a
- dist = a->w.length();
- }
- else if(b_dot_ba < 0)
- {
- // Pick distance vertex b
- dist = b->w.length();
}
else
{
- // Pick distance to edge a->b
- const b3Scalar a_dot_b = b3Dot(a->w, b->w);
- dist = b3Sqrt(b3Max((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (b3Scalar)0));
+ m_status = eStatus::OutOfVertices;
+ break;
}
-
- return true;
}
+ const b3Vector3 projection = outer.n * outer.d;
+ m_normal = outer.n;
+ m_depth = outer.d;
+ m_result.rank = 3;
+ m_result.c[0] = outer.c[0];
+ m_result.c[1] = outer.c[1];
+ m_result.c[2] = outer.c[2];
+ m_result.p[0] = b3Cross(outer.c[1]->w - projection,
+ outer.c[2]->w - projection)
+ .length();
+ m_result.p[1] = b3Cross(outer.c[2]->w - projection,
+ outer.c[0]->w - projection)
+ .length();
+ m_result.p[2] = b3Cross(outer.c[0]->w - projection,
+ outer.c[1]->w - projection)
+ .length();
+ const b3Scalar sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
+ m_result.p[0] /= sum;
+ m_result.p[1] /= sum;
+ m_result.p[2] /= sum;
+ return (m_status);
+ }
+ }
+ /* Fallback */
+ m_status = eStatus::FallBack;
+ m_normal = -guess;
+ const b3Scalar nl = m_normal.length();
+ if (nl > 0)
+ m_normal = m_normal / nl;
+ else
+ m_normal = b3MakeVector3(1, 0, 0);
+ m_depth = 0;
+ m_result.rank = 1;
+ m_result.c[0] = simplex.c[0];
+ m_result.p[0] = 1;
+ return (m_status);
+ }
+ bool getedgedist(sFace* face, sSV* a, sSV* b, b3Scalar& dist)
+ {
+ const b3Vector3 ba = b->w - a->w;
+ const b3Vector3 n_ab = b3Cross(ba, face->n); // Outward facing edge normal direction, on triangle plane
+ const b3Scalar a_dot_nab = b3Dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
+
+ if (a_dot_nab < 0)
+ {
+ // Outside of edge a->b
+
+ const b3Scalar ba_l2 = ba.length2();
+ const b3Scalar a_dot_ba = b3Dot(a->w, ba);
+ const b3Scalar b_dot_ba = b3Dot(b->w, ba);
- return false;
+ if (a_dot_ba > 0)
+ {
+ // Pick distance vertex a
+ dist = a->w.length();
}
- sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
+ else if (b_dot_ba < 0)
{
- if(m_stock.root)
- {
- sFace* face=m_stock.root;
- remove(m_stock,face);
- append(m_hull,face);
- face->pass = 0;
- face->c[0] = a;
- face->c[1] = b;
- face->c[2] = c;
- face->n = b3Cross(b->w-a->w,c->w-a->w);
- const b3Scalar l=face->n.length();
- const bool v=l>EPA_ACCURACY;
-
- if(v)
- {
- if(!(getedgedist(face, a, b, face->d) ||
- getedgedist(face, b, c, face->d) ||
- getedgedist(face, c, a, face->d)))
- {
- // Origin projects to the interior of the triangle
- // Use distance to triangle plane
- face->d = b3Dot(a->w, face->n) / l;
- }
+ // Pick distance vertex b
+ dist = b->w.length();
+ }
+ else
+ {
+ // Pick distance to edge a->b
+ const b3Scalar a_dot_b = b3Dot(a->w, b->w);
+ dist = b3Sqrt(b3Max((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (b3Scalar)0));
+ }
- face->n /= l;
- if(forced || (face->d >= -EPA_PLANE_EPS))
- {
- return face;
- }
- else
- m_status=eStatus::NonConvex;
- }
- else
- m_status=eStatus::Degenerated;
+ return true;
+ }
- remove(m_hull, face);
- append(m_stock, face);
- return 0;
+ return false;
+ }
+ sFace* newface(sSV* a, sSV* b, sSV* c, bool forced)
+ {
+ if (m_stock.root)
+ {
+ sFace* face = m_stock.root;
+ remove(m_stock, face);
+ append(m_hull, face);
+ face->pass = 0;
+ face->c[0] = a;
+ face->c[1] = b;
+ face->c[2] = c;
+ face->n = b3Cross(b->w - a->w, c->w - a->w);
+ const b3Scalar l = face->n.length();
+ const bool v = l > EPA_ACCURACY;
+ if (v)
+ {
+ if (!(getedgedist(face, a, b, face->d) ||
+ getedgedist(face, b, c, face->d) ||
+ getedgedist(face, c, a, face->d)))
+ {
+ // Origin projects to the interior of the triangle
+ // Use distance to triangle plane
+ face->d = b3Dot(a->w, face->n) / l;
}
- m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
- return 0;
+
+ face->n /= l;
+ if (forced || (face->d >= -EPA_PLANE_EPS))
+ {
+ return face;
+ }
+ else
+ m_status = eStatus::NonConvex;
}
- sFace* findbest()
+ else
+ m_status = eStatus::Degenerated;
+
+ remove(m_hull, face);
+ append(m_stock, face);
+ return 0;
+ }
+ m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
+ return 0;
+ }
+ sFace* findbest()
+ {
+ sFace* minf = m_hull.root;
+ b3Scalar mind = minf->d * minf->d;
+ for (sFace* f = minf->l[1]; f; f = f->l[1])
+ {
+ const b3Scalar sqd = f->d * f->d;
+ if (sqd < mind)
{
- sFace* minf=m_hull.root;
- b3Scalar mind=minf->d*minf->d;
- for(sFace* f=minf->l[1];f;f=f->l[1])
+ minf = f;
+ mind = sqd;
+ }
+ }
+ return (minf);
+ }
+ bool expand(unsigned int pass, sSV* w, sFace* f, unsigned int e, sHorizon& horizon)
+ {
+ static const unsigned int i1m3[] = {1, 2, 0};
+ static const unsigned int i2m3[] = {2, 0, 1};
+ if (f->pass != pass)
+ {
+ const unsigned int e1 = i1m3[e];
+ if ((b3Dot(f->n, w->w) - f->d) < -EPA_PLANE_EPS)
+ {
+ sFace* nf = newface(f->c[e1], f->c[e], w, false);
+ if (nf)
{
- const b3Scalar sqd=f->d*f->d;
- if(sqd<mind)
- {
- minf=f;
- mind=sqd;
- }
+ bind(nf, 0, f, e);
+ if (horizon.cf)
+ bind(horizon.cf, 1, nf, 2);
+ else
+ horizon.ff = nf;
+ horizon.cf = nf;
+ ++horizon.nf;
+ return (true);
}
- return(minf);
}
- bool expand(unsigned int pass,sSV* w,sFace* f,unsigned int e,sHorizon& horizon)
+ else
{
- static const unsigned int i1m3[]={1,2,0};
- static const unsigned int i2m3[]={2,0,1};
- if(f->pass!=pass)
+ const unsigned int e2 = i2m3[e];
+ f->pass = (unsigned char)pass;
+ if (expand(pass, w, f->f[e1], f->e[e1], horizon) &&
+ expand(pass, w, f->f[e2], f->e[e2], horizon))
{
- const unsigned int e1=i1m3[e];
- if((b3Dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
- {
- sFace* nf=newface(f->c[e1],f->c[e],w,false);
- if(nf)
- {
- bind(nf,0,f,e);
- if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
- horizon.cf=nf;
- ++horizon.nf;
- return(true);
- }
- }
- else
- {
- const unsigned int e2=i2m3[e];
- f->pass = (unsigned char)pass;
- if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
- expand(pass,w,f->f[e2],f->e[e2],horizon))
- {
- remove(m_hull,f);
- append(m_stock,f);
- return(true);
- }
- }
+ remove(m_hull, f);
+ append(m_stock, f);
+ return (true);
}
- return(false);
}
-
- };
-
- //
- static void Initialize(const b3Transform& transA, const b3Transform& transB,
- const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
- const b3AlignedObjectArray<b3Vector3>& verticesA,
- const b3AlignedObjectArray<b3Vector3>& verticesB,
- b3GjkEpaSolver2::sResults& results,
- tShape& shape,
- bool withmargins)
- {
- /* Results */
- results.witnesses[0] =
- results.witnesses[1] = b3MakeVector3(0,0,0);
- results.status = b3GjkEpaSolver2::sResults::Separated;
- /* Shape */
- shape.m_shapes[0] = hullA;
- shape.m_shapes[1] = hullB;
- shape.m_toshape1 = transB.getBasis().transposeTimes(transA.getBasis());
- shape.m_toshape0 = transA.inverseTimes(transB);
- shape.EnableMargin(withmargins);
+ }
+ return (false);
}
+};
+//
+static void Initialize(const b3Transform& transA, const b3Transform& transB,
+ const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
+ const b3AlignedObjectArray<b3Vector3>& verticesA,
+ const b3AlignedObjectArray<b3Vector3>& verticesB,
+ b3GjkEpaSolver2::sResults& results,
+ tShape& shape,
+ bool withmargins)
+{
+ /* Results */
+ results.witnesses[0] =
+ results.witnesses[1] = b3MakeVector3(0, 0, 0);
+ results.status = b3GjkEpaSolver2::sResults::Separated;
+ /* Shape */
+ shape.m_shapes[0] = hullA;
+ shape.m_shapes[1] = hullB;
+ shape.m_toshape1 = transB.getBasis().transposeTimes(transA.getBasis());
+ shape.m_toshape0 = transA.inverseTimes(transB);
+ shape.EnableMargin(withmargins);
}
+} // namespace gjkepa2_impl2
+
//
// Api
//
-using namespace gjkepa2_impl2;
+using namespace gjkepa2_impl2;
//
-int b3GjkEpaSolver2::StackSizeRequirement()
+int b3GjkEpaSolver2::StackSizeRequirement()
{
- return(sizeof(b3GJK)+sizeof(b3EPA));
+ return (sizeof(b3GJK) + sizeof(b3EPA));
}
//
-bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& transB,
- const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
- const b3AlignedObjectArray<b3Vector3>& verticesA,
- const b3AlignedObjectArray<b3Vector3>& verticesB,
- const b3Vector3& guess,
- sResults& results)
+bool b3GjkEpaSolver2::Distance(const b3Transform& transA, const b3Transform& transB,
+ const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
+ const b3AlignedObjectArray<b3Vector3>& verticesA,
+ const b3AlignedObjectArray<b3Vector3>& verticesB,
+ const b3Vector3& guess,
+ sResults& results)
{
- tShape shape;
- Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,false);
- b3GJK gjk(verticesA,verticesB);
- b3GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
- if(gjk_status==b3GJK::eStatus::Valid)
+ tShape shape;
+ Initialize(transA, transB, hullA, hullB, verticesA, verticesB, results, shape, false);
+ b3GJK gjk(verticesA, verticesB);
+ b3GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess);
+ if (gjk_status == b3GJK::eStatus::Valid)
{
- b3Vector3 w0=b3MakeVector3(0,0,0);
- b3Vector3 w1=b3MakeVector3(0,0,0);
- for(unsigned int i=0;i<gjk.m_simplex->rank;++i)
+ b3Vector3 w0 = b3MakeVector3(0, 0, 0);
+ b3Vector3 w1 = b3MakeVector3(0, 0, 0);
+ for (unsigned int i = 0; i < gjk.m_simplex->rank; ++i)
{
- const b3Scalar p=gjk.m_simplex->p[i];
- w0+=shape.Support( gjk.m_simplex->c[i]->d,0,verticesA,verticesB)*p;
- w1+=shape.Support(-gjk.m_simplex->c[i]->d,1,verticesA,verticesB)*p;
+ const b3Scalar p = gjk.m_simplex->p[i];
+ w0 += shape.Support(gjk.m_simplex->c[i]->d, 0, verticesA, verticesB) * p;
+ w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1, verticesA, verticesB) * p;
}
- results.witnesses[0] = transA*w0;
- results.witnesses[1] = transA*w1;
- results.normal = w0-w1;
- results.distance = results.normal.length();
- results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
- return(true);
+ results.witnesses[0] = transA * w0;
+ results.witnesses[1] = transA * w1;
+ results.normal = w0 - w1;
+ results.distance = results.normal.length();
+ results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
+ return (true);
}
else
{
- results.status = gjk_status==b3GJK::eStatus::Inside?
- sResults::Penetrating :
- sResults::GJK_Failed ;
- return(false);
+ results.status = gjk_status == b3GJK::eStatus::Inside ? sResults::Penetrating : sResults::GJK_Failed;
+ return (false);
}
}
//
-bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform& transB,
- const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
- const b3AlignedObjectArray<b3Vector3>& verticesA,
- const b3AlignedObjectArray<b3Vector3>& verticesB,
- const b3Vector3& guess,
- sResults& results,
- bool usemargins)
+bool b3GjkEpaSolver2::Penetration(const b3Transform& transA, const b3Transform& transB,
+ const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
+ const b3AlignedObjectArray<b3Vector3>& verticesA,
+ const b3AlignedObjectArray<b3Vector3>& verticesB,
+ const b3Vector3& guess,
+ sResults& results,
+ bool usemargins)
{
-
- tShape shape;
- Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,usemargins);
- b3GJK gjk(verticesA,verticesB);
- b3GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
- switch(gjk_status)
+ tShape shape;
+ Initialize(transA, transB, hullA, hullB, verticesA, verticesB, results, shape, usemargins);
+ b3GJK gjk(verticesA, verticesB);
+ b3GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess);
+ switch (gjk_status)
{
- case b3GJK::eStatus::Inside:
+ case b3GJK::eStatus::Inside:
{
- b3EPA epa;
- b3EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
- if(epa_status!=b3EPA::eStatus::Failed)
+ b3EPA epa;
+ b3EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess);
+ if (epa_status != b3EPA::eStatus::Failed)
{
- b3Vector3 w0=b3MakeVector3(0,0,0);
- for(unsigned int i=0;i<epa.m_result.rank;++i)
+ b3Vector3 w0 = b3MakeVector3(0, 0, 0);
+ for (unsigned int i = 0; i < epa.m_result.rank; ++i)
{
- w0+=shape.Support(epa.m_result.c[i]->d,0,verticesA,verticesB)*epa.m_result.p[i];
+ w0 += shape.Support(epa.m_result.c[i]->d, 0, verticesA, verticesB) * epa.m_result.p[i];
}
- results.status = sResults::Penetrating;
- results.witnesses[0] = transA*w0;
- results.witnesses[1] = transA*(w0-epa.m_normal*epa.m_depth);
- results.normal = -epa.m_normal;
- results.distance = -epa.m_depth;
- return(true);
- } else results.status=sResults::EPA_Failed;
+ results.status = sResults::Penetrating;
+ results.witnesses[0] = transA * w0;
+ results.witnesses[1] = transA * (w0 - epa.m_normal * epa.m_depth);
+ results.normal = -epa.m_normal;
+ results.distance = -epa.m_depth;
+ return (true);
+ }
+ else
+ results.status = sResults::EPA_Failed;
}
break;
- case b3GJK::eStatus::Failed:
- results.status=sResults::GJK_Failed;
- break;
+ case b3GJK::eStatus::Failed:
+ results.status = sResults::GJK_Failed;
+ break;
default:
- {
- }
+ {
+ }
}
- return(false);
+ return (false);
}
-
#if 0
//
b3Scalar b3GjkEpaSolver2::SignedDistance(const b3Vector3& position,
@@ -994,8 +1043,7 @@ bool b3GjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
}
#endif
-
-/* Symbols cleanup */
+/* Symbols cleanup */
#undef GJK_MAX_ITERATIONS
#undef GJK_ACCURACY
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-17 00:58:55 +0000