summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/src/convex.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/src/convex.cpp')
-rw-r--r--libs/ode-0.16.1/ode/src/convex.cpp1621
1 files changed, 1621 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/convex.cpp b/libs/ode-0.16.1/ode/src/convex.cpp
new file mode 100644
index 0000000..7a17941
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/convex.cpp
@@ -0,0 +1,1621 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+/*
+Code for Convex Collision Detection
+By Rodrigo Hernandez
+*/
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_util.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+#if 1
+#define dMIN(A,B) ((A)>(B) ? (B) : (A))
+#define dMAX(A,B) ((A)>(B) ? (A) : (B))
+#else
+#define dMIN(A,B) std::min(A,B)
+#define dMAX(A,B) std::max(A,B)
+#endif
+
+//****************************************************************************
+// Convex public API
+dxConvex::dxConvex (dSpaceID space,
+ const dReal *_planes,
+ unsigned int _planecount,
+ const dReal *_points,
+ unsigned int _pointcount,
+ const unsigned int *_polygons) :
+dxGeom (space,1)
+{
+ dAASSERT (_planes != NULL);
+ dAASSERT (_points != NULL);
+ dAASSERT (_polygons != NULL);
+ //fprintf(stdout,"dxConvex Constructor planes %X\n",_planes);
+ type = dConvexClass;
+ planes = _planes;
+ planecount = _planecount;
+ // we need points as well
+ points = _points;
+ pointcount = _pointcount;
+ polygons=_polygons;
+ edges = NULL;
+ FillEdges();
+#ifndef dNODEBUG
+ // Check for properly build polygons by calculating the determinant
+ // of the 3x3 matrix composed of the first 3 points in the polygon.
+ const unsigned int *points_in_poly=polygons;
+ const unsigned int *index=polygons+1;
+
+ for(unsigned int i=0;i<planecount;++i)
+ {
+ dAASSERT (*points_in_poly > 2 );
+ if((
+ points[(index[0]*3)+0]*points[(index[1]*3)+1]*points[(index[2]*3)+2] +
+ points[(index[0]*3)+1]*points[(index[1]*3)+2]*points[(index[2]*3)+0] +
+ points[(index[0]*3)+2]*points[(index[1]*3)+0]*points[(index[2]*3)+1] -
+ points[(index[0]*3)+2]*points[(index[1]*3)+1]*points[(index[2]*3)+0] -
+ points[(index[0]*3)+1]*points[(index[1]*3)+0]*points[(index[2]*3)+2] -
+ points[(index[0]*3)+0]*points[(index[1]*3)+2]*points[(index[2]*3)+1])<0)
+ {
+ fprintf(stdout,"WARNING: Polygon %d is not defined counterclockwise\n",i);
+ }
+ points_in_poly+=(*points_in_poly+1);
+ index=points_in_poly+1;
+ if(planes[(i*4)+3]<0) fprintf(stdout,"WARNING: Plane %d does not contain the origin\n",i);
+ }
+#endif
+
+ //CreateTree();
+}
+
+
+void dxConvex::computeAABB()
+{
+ // this can, and should be optimized
+ dVector3 point;
+ dMultiply0_331 (point,final_posr->R,points);
+ aabb[0] = point[0]+final_posr->pos[0];
+ aabb[1] = point[0]+final_posr->pos[0];
+ aabb[2] = point[1]+final_posr->pos[1];
+ aabb[3] = point[1]+final_posr->pos[1];
+ aabb[4] = point[2]+final_posr->pos[2];
+ aabb[5] = point[2]+final_posr->pos[2];
+ for(unsigned int i=3;i<(pointcount*3);i+=3)
+ {
+ dMultiply0_331 (point,final_posr->R,&points[i]);
+ aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]);
+ aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]);
+ aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]);
+ aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]);
+ aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]);
+ aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]);
+ }
+}
+
+/*! \brief Populates the edges set, should be called only once whenever the polygon array gets updated */
+void dxConvex::FillEdges()
+{
+ const unsigned int *points_in_poly=polygons;
+ const unsigned int *index=polygons+1;
+ if (edges!=NULL) delete[] edges;
+ edgecount = 0;
+ edge e;
+ bool isinset;
+ for(unsigned int i=0;i<planecount;++i)
+ {
+ for(unsigned int j=0;j<*points_in_poly;++j)
+ {
+ e.first = dMIN(index[j],index[(j+1)%*points_in_poly]);
+ e.second = dMAX(index[j],index[(j+1)%*points_in_poly]);
+ isinset=false;
+ for(unsigned int k=0;k<edgecount;++k)
+ {
+ if((edges[k].first==e.first)&&(edges[k].second==e.second))
+ {
+ isinset=true;
+ break;
+ }
+ }
+ if(!isinset)
+ {
+ edge* tmp = new edge[edgecount+1];
+ if(edgecount!=0)
+ {
+ memcpy(tmp,edges,(edgecount)*sizeof(edge));
+ delete[] edges;
+ }
+ tmp[edgecount].first=e.first;
+ tmp[edgecount].second=e.second;
+ edges = tmp;
+ ++edgecount;
+ }
+ }
+ points_in_poly+=(*points_in_poly+1);
+ index=points_in_poly+1;
+ }
+}
+#if 0
+dxConvex::BSPNode* dxConvex::CreateNode(std::vector<Arc> Arcs,std::vector<Polygon> Polygons)
+{
+#if 0
+ dVector3 ea,eb,e;
+ dVector3Copy(points+((edges.begin()+Arcs[0].edge)first*3),ea);
+ dMultiply0_331(e1b,cvx1.final_posr->R,cvx1.points+(i->second*3));
+
+ dVector3Copy(points[edges[Arcs[0].edge]
+#endif
+ return NULL;
+}
+
+void dxConvex::CreateTree()
+{
+ std::vector<Arc> A;
+ A.reserve(edgecount);
+ for(unsigned int i=0;i<edgecount;++i)
+ {
+ this->GetFacesSharedByEdge(i,A[i].normals);
+ A[i].edge = i;
+ }
+ std::vector<Polygon> S;
+ S.reserve(pointcount);
+ for(unsigned int i=0;i<pointcount;++i)
+ {
+ this->GetFacesSharedByVertex(i,S[i].normals);
+ S[i].vertex=i;
+ }
+ this->tree = CreateNode(A,S);
+}
+
+void dxConvex::GetFacesSharedByVertex(int i, std::vector<int> f)
+{
+}
+void dxConvex::GetFacesSharedByEdge(int i, int* f)
+{
+}
+void dxConvex::GetFaceNormal(int i, dVector3 normal)
+{
+}
+#endif
+
+dGeomID dCreateConvex (dSpaceID space,const dReal *_planes,unsigned int _planecount,
+ const dReal *_points,
+ unsigned int _pointcount,
+ const unsigned int *_polygons)
+{
+ //fprintf(stdout,"dxConvex dCreateConvex\n");
+ return new dxConvex(space,_planes, _planecount,
+ _points,
+ _pointcount,
+ _polygons);
+}
+
+void dGeomSetConvex (dGeomID g,const dReal *_planes,unsigned int _planecount,
+ const dReal *_points,
+ unsigned int _pointcount,
+ const unsigned int *_polygons)
+{
+ //fprintf(stdout,"dxConvex dGeomSetConvex\n");
+ dUASSERT (g && g->type == dConvexClass,"argument not a convex shape");
+ dxConvex *s = (dxConvex*) g;
+ s->planes = _planes;
+ s->planecount = _planecount;
+ s->points = _points;
+ s->pointcount = _pointcount;
+ s->polygons=_polygons;
+}
+
+//****************************************************************************
+// Helper Inlines
+//
+
+/*! \brief Returns Whether or not the segment ab intersects plane p
+ \param a origin of the segment
+ \param b segment destination
+ \param p plane to test for intersection
+ \param t returns the time "t" in the segment ray that gives us the intersecting
+ point
+ \param q returns the intersection point
+ \return true if there is an intersection, otherwise false.
+*/
+bool IntersectSegmentPlane(dVector3 a,
+ dVector3 b,
+ dVector4 p,
+ dReal &t,
+ dVector3 q)
+{
+ // Compute the t value for the directed line ab intersecting the plane
+ dVector3 ab;
+ ab[0]= b[0] - a[0];
+ ab[1]= b[1] - a[1];
+ ab[2]= b[2] - a[2];
+
+ t = (p[3] - dCalcVectorDot3(p,a)) / dCalcVectorDot3(p,ab);
+
+ // If t in [0..1] compute and return intersection point
+ if (t >= 0.0 && t <= 1.0)
+ {
+ q[0] = a[0] + t * ab[0];
+ q[1] = a[1] + t * ab[1];
+ q[2] = a[2] + t * ab[2];
+ return true;
+ }
+ // Else no intersection
+ return false;
+}
+
+/*! \brief Returns the Closest Point in Ray 1 to Ray 2
+ \param Origin1 The origin of Ray 1
+ \param Direction1 The direction of Ray 1
+ \param Origin1 The origin of Ray 2
+ \param Direction1 The direction of Ray 3
+ \param t the time "t" in Ray 1 that gives us the closest point
+ (closest_point=Origin1+(Direction1*t).
+ \return true if there is a closest point, false if the rays are paralell.
+*/
+inline bool ClosestPointInRay(const dVector3 Origin1,
+ const dVector3 Direction1,
+ const dVector3 Origin2,
+ const dVector3 Direction2,
+ dReal& t)
+{
+ dVector3 w = {Origin1[0]-Origin2[0],
+ Origin1[1]-Origin2[1],
+ Origin1[2]-Origin2[2]};
+ dReal a = dCalcVectorDot3(Direction1 , Direction1);
+ dReal b = dCalcVectorDot3(Direction1 , Direction2);
+ dReal c = dCalcVectorDot3(Direction2 , Direction2);
+ dReal d = dCalcVectorDot3(Direction1 , w);
+ dReal e = dCalcVectorDot3(Direction2 , w);
+ dReal denominator = (a*c)-(b*b);
+ if(denominator==0.0f)
+ {
+ return false;
+ }
+ t = ((a*e)-(b*d))/denominator;
+ return true;
+}
+
+/*! \brief Returns the Closest Points from Segment 1 to Segment 2
+ \param p1 start of segment 1
+ \param q1 end of segment 1
+ \param p2 start of segment 2
+ \param q2 end of segment 2
+ \param t the time "t" in Ray 1 that gives us the closest point
+ (closest_point=Origin1+(Direction1*t).
+ \return true if there is a closest point, false if the rays are paralell.
+ \note Adapted from Christer Ericson's Real Time Collision Detection Book.
+*/
+inline void ClosestPointBetweenSegments(dVector3& p1,
+ dVector3& q1,
+ dVector3& p2,
+ dVector3& q2,
+ dVector3& c1,
+ dVector3& c2)
+{
+ // s & t were originaly part of the output args, but since
+ // we don't really need them, we'll just declare them in here
+ dReal s;
+ dReal t;
+ dVector3 d1 = {q1[0] - p1[0],
+ q1[1] - p1[1],
+ q1[2] - p1[2]};
+ dVector3 d2 = {q2[0] - p2[0],
+ q2[1] - p2[1],
+ q2[2] - p2[2]};
+ dVector3 r = {p1[0] - p2[0],
+ p1[1] - p2[1],
+ p1[2] - p2[2]};
+ dReal a = dCalcVectorDot3(d1, d1);
+ dReal e = dCalcVectorDot3(d2, d2);
+ dReal f = dCalcVectorDot3(d2, r);
+ // Check if either or both segments degenerate into points
+ if (a <= dEpsilon && e <= dEpsilon)
+ {
+ // Both segments degenerate into points
+ s = t = 0.0f;
+ dVector3Copy(p1,c1);
+ dVector3Copy(p2,c2);
+ return;
+ }
+ if (a <= dEpsilon)
+ {
+ // First segment degenerates into a point
+ s = 0.0f;
+ t = f / e; // s = 0 => t = (b*s + f) / e = f / e
+ t = dxClamp(t, 0.0f, 1.0f);
+ }
+ else
+ {
+ dReal c = dCalcVectorDot3(d1, r);
+ if (e <= dEpsilon)
+ {
+ // Second segment degenerates into a point
+ t = 0.0f;
+ s = dxClamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
+ }
+ else
+ {
+ // The general non degenerate case starts here
+ dReal b = dCalcVectorDot3(d1, d2);
+ dReal denom = a*e-b*b; // Always nonnegative
+
+ // If segments not parallel, compute closest point on L1 to L2, and
+ // clamp to segment S1. Else pick arbitrary s (here 0)
+ if (denom != 0.0f)
+ {
+ s = dxClamp((b*f - c*e) / denom, 0.0f, 1.0f);
+ }
+ else s = 0.0f;
+#if 0
+ // Compute point on L2 closest to S1(s) using
+ // t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e
+ t = (b*s + f) / e;
+
+ // If t in [0,1] done. Else clamp t, recompute s for the new value
+ // of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a
+ // and clamp s to [0, 1]
+ if (t < 0.0f) {
+ t = 0.0f;
+ s = dxClamp(-c / a, 0.0f, 1.0f);
+ } else if (t > 1.0f) {
+ t = 1.0f;
+ s = dxClamp((b - c) / a, 0.0f, 1.0f);
+ }
+#else
+ dReal tnom = b*s + f;
+ if (tnom < 0.0f)
+ {
+ t = 0.0f;
+ s = dxClamp(-c / a, 0.0f, 1.0f);
+ }
+ else if (tnom > e)
+ {
+ t = 1.0f;
+ s = dxClamp((b - c) / a, 0.0f, 1.0f);
+ }
+ else
+ {
+ t = tnom / e;
+ }
+#endif
+ }
+ }
+
+ c1[0] = p1[0] + d1[0] * s;
+ c1[1] = p1[1] + d1[1] * s;
+ c1[2] = p1[2] + d1[2] * s;
+ c2[0] = p2[0] + d2[0] * t;
+ c2[1] = p2[1] + d2[1] * t;
+ c2[2] = p2[2] + d2[2] * t;
+}
+
+#if 0
+dReal tnom = b*s + f;
+if (tnom < 0.0f) {
+ t = 0.0f;
+ s = dxClamp(-c / a, 0.0f, 1.0f);
+} else if (tnom > e) {
+ t = 1.0f;
+ s = dxClamp((b - c) / a, 0.0f, 1.0f);
+} else {
+ t = tnom / e;
+}
+#endif
+
+/*! \brief Returns the Ray on which 2 planes intersect if they do.
+ \param p1 Plane 1
+ \param p2 Plane 2
+ \param p Contains the origin of the ray upon returning if planes intersect
+ \param d Contains the direction of the ray upon returning if planes intersect
+ \return true if the planes intersect, false if paralell.
+*/
+inline bool IntersectPlanes(const dVector4 p1, const dVector4 p2, dVector3 p, dVector3 d)
+{
+ // Compute direction of intersection line
+ dCalcVectorCross3(d,p1,p2);
+ // If d is (near) zero, the planes are parallel (and separated)
+ // or coincident, so they're not considered intersecting
+ dReal denom = dCalcVectorDot3(d, d);
+ if (denom < dEpsilon) return false;
+ dVector3 n;
+ n[0]=p1[3]*p2[0] - p2[3]*p1[0];
+ n[1]=p1[3]*p2[1] - p2[3]*p1[1];
+ n[2]=p1[3]*p2[2] - p2[3]*p1[2];
+ // Compute point on intersection line
+ dCalcVectorCross3(p,n,d);
+ p[0]/=denom;
+ p[1]/=denom;
+ p[2]/=denom;
+ return true;
+}
+
+
+#if 0
+/*! \brief Finds out if a point lies inside a convex
+ \param p Point to test
+ \param convex a pointer to convex to test against
+ \return true if the point lies inside the convex, false if not.
+*/
+inline bool IsPointInConvex(dVector3 p,
+ dxConvex *convex)
+{
+ dVector3 lp,tmp;
+ // move point into convex space to avoid plane local to world calculations
+ tmp[0] = p[0] - convex->final_posr->pos[0];
+ tmp[1] = p[1] - convex->final_posr->pos[1];
+ tmp[2] = p[2] - convex->final_posr->pos[2];
+ dMultiply1_331 (lp,convex->final_posr->R,tmp);
+ for(unsigned int i=0;i<convex->planecount;++i)
+ {
+ if((
+ ((convex->planes+(i*4))[0]*lp[0])+
+ ((convex->planes+(i*4))[1]*lp[1])+
+ ((convex->planes+(i*4))[2]*lp[2])+
+ -(convex->planes+(i*4))[3]
+ )>0)
+ {
+ return false;
+ }
+ }
+ return true;
+}
+#endif
+
+/*! \brief Finds out if a point lies inside a 2D polygon
+ \param p Point to test
+ \param polygon a pointer to the start of the convex polygon index buffer
+ \param out the closest point in the polygon if the point is not inside
+ \return true if the point lies inside of the polygon, false if not.
+*/
+inline bool IsPointInPolygon(dVector3 p,
+ const unsigned int *polygon,
+ dReal *plane,
+ dxConvex *convex,
+ dVector3 out)
+{
+ // p is the point we want to check,
+ // polygon is a pointer to the polygon we
+ // are checking against, remember it goes
+ // number of vertices then that many indexes
+ // out returns the closest point on the border of the
+ // polygon if the point is not inside it.
+ dVector3 a;
+ dVector3 b;
+ dVector3 ab;
+ dVector3 ap;
+ dVector3 v;
+
+ unsigned pointcount=polygon[0];
+ dIASSERT(pointcount != 0);
+ polygon++; // skip past pointcount
+
+ dMultiply0_331 (b,convex->final_posr->R,
+ &convex->points[(polygon[pointcount-1]*3)]);
+ b[0]=convex->final_posr->pos[0]+b[0];
+ b[1]=convex->final_posr->pos[1]+b[1];
+ b[2]=convex->final_posr->pos[2]+b[2];
+
+ for(unsigned i=0; i != pointcount; ++i)
+ {
+ a[0] = b[0];
+ a[1] = b[1];
+ a[2] = b[2];
+
+ dMultiply0_331 (b,convex->final_posr->R,&convex->points[(polygon[i]*3)]);
+ b[0]=convex->final_posr->pos[0]+b[0];
+ b[1]=convex->final_posr->pos[1]+b[1];
+ b[2]=convex->final_posr->pos[2]+b[2];
+
+ ab[0] = b[0] - a[0];
+ ab[1] = b[1] - a[1];
+ ab[2] = b[2] - a[2];
+ ap[0] = p[0] - a[0];
+ ap[1] = p[1] - a[1];
+ ap[2] = p[2] - a[2];
+
+ dCalcVectorCross3(v, ab, plane);
+
+ if (dCalcVectorDot3(ap, v) > REAL(0.0))
+ {
+ dReal ab_m2 = dCalcVectorDot3(ab, ab);
+ dReal s = ab_m2 != REAL(0.0) ? dCalcVectorDot3(ab, ap) / ab_m2 : REAL(0.0);
+
+ if (s <= REAL(0.0))
+ {
+ out[0] = a[0];
+ out[1] = a[1];
+ out[2] = a[2];
+ }
+ else if (s >= REAL(1.0))
+ {
+ out[0] = b[0];
+ out[1] = b[1];
+ out[2] = b[2];
+ }
+ else
+ {
+ out[0] = a[0] + ab[0] * s;
+ out[1] = a[1] + ab[1] * s;
+ out[2] = a[2] + ab[2] * s;
+ }
+
+ return false;
+ }
+ }
+
+ return true;
+}
+
+int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
+ dContactGeom *contact, int skip)
+{
+ dIASSERT (skip >= (int)sizeof(dContactGeom));
+ dIASSERT (o1->type == dConvexClass);
+ dIASSERT (o2->type == dPlaneClass);
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+
+ dxConvex *Convex = (dxConvex*) o1;
+ dxPlane *Plane = (dxPlane*) o2;
+ unsigned int contacts=0;
+ unsigned int maxc = flags & NUMC_MASK;
+ dVector3 v2;
+
+#define LTEQ_ZERO 0x10000000
+#define GTEQ_ZERO 0x20000000
+#define BOTH_SIGNS (LTEQ_ZERO | GTEQ_ZERO)
+ dIASSERT((BOTH_SIGNS & NUMC_MASK) == 0); // used in conditional operator later
+
+ unsigned int totalsign = 0;
+ for(unsigned int i=0;i<Convex->pointcount;++i)
+ {
+ dMultiply0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]);
+ dVector3Add(Convex->final_posr->pos, v2, v2);
+
+ unsigned int distance2sign = GTEQ_ZERO;
+ dReal distance2 = dVector3Dot(Plane->p, v2) - Plane->p[3]; // Ax + By + Cz - D
+ if((distance2 <= REAL(0.0)))
+ {
+ distance2sign = distance2 != REAL(0.0) ? LTEQ_ZERO : BOTH_SIGNS;
+
+ if (contacts != maxc)
+ {
+ dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
+ dVector3Copy(Plane->p, target->normal);
+ dVector3Copy(v2, target->pos);
+ target->depth = -distance2;
+ target->g1 = Convex;
+ target->g2 = Plane;
+ target->side1 = -1; // TODO: set plane index?
+ target->side2 = -1;
+ contacts++;
+ }
+ }
+
+ // Take new sign into account
+ totalsign |= distance2sign;
+ // Check if contacts are full and both signs have been already found
+ if (((contacts ^ maxc) | totalsign) == BOTH_SIGNS) // harder to comprehend but requires one register less
+ {
+ break; // Nothing can be changed any more
+ }
+ }
+ if (totalsign == BOTH_SIGNS) return contacts;
+ return 0;
+#undef BOTH_SIGNS
+#undef GTEQ_ZERO
+#undef LTEQ_ZERO
+}
+
+int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
+ dContactGeom *contact, int skip)
+{
+ dIASSERT (skip >= (int)sizeof(dContactGeom));
+ dIASSERT (o1->type == dSphereClass);
+ dIASSERT (o2->type == dConvexClass);
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+
+ dxSphere *Sphere = (dxSphere*) o1;
+ dxConvex *Convex = (dxConvex*) o2;
+ dReal dist,closestdist=dInfinity;
+ dVector4 plane;
+ // dVector3 contactpoint;
+ dVector3 offsetpos,out,temp;
+ const unsigned int *pPoly=Convex->polygons;
+ int closestplane=-1;
+ bool sphereinside=true;
+ /*
+ Do a good old sphere vs plane check first,
+ if a collision is found then check if the contact point
+ is within the polygon
+ */
+ // offset the sphere final_posr->position into the convex space
+ offsetpos[0]=Sphere->final_posr->pos[0]-Convex->final_posr->pos[0];
+ offsetpos[1]=Sphere->final_posr->pos[1]-Convex->final_posr->pos[1];
+ offsetpos[2]=Sphere->final_posr->pos[2]-Convex->final_posr->pos[2];
+ for(unsigned int i=0;i<Convex->planecount;++i)
+ {
+ // apply rotation to the plane
+ dMultiply0_331(plane,Convex->final_posr->R,&Convex->planes[(i*4)]);
+ plane[3]=(&Convex->planes[(i*4)])[3];
+ // Get the distance from the sphere origin to the plane
+ dist = dVector3Dot(plane, offsetpos) - plane[3]; // Ax + By + Cz - D
+ if(dist>0)
+ {
+ // if we get here, we know the center of the sphere is
+ // outside of the convex hull.
+ if(dist<Sphere->radius)
+ {
+ // if we get here we know the sphere surface penetrates
+ // the plane
+ if(IsPointInPolygon(Sphere->final_posr->pos,pPoly,plane,Convex,out))
+ {
+ // finally if we get here we know that the
+ // sphere is directly touching the inside of the polyhedron
+ contact->normal[0] = plane[0];
+ contact->normal[1] = plane[1];
+ contact->normal[2] = plane[2];
+ contact->pos[0] = Sphere->final_posr->pos[0]+
+ (-contact->normal[0]*Sphere->radius);
+ contact->pos[1] = Sphere->final_posr->pos[1]+
+ (-contact->normal[1]*Sphere->radius);
+ contact->pos[2] = Sphere->final_posr->pos[2]+
+ (-contact->normal[2]*Sphere->radius);
+ contact->depth = Sphere->radius-dist;
+ contact->g1 = Sphere;
+ contact->g2 = Convex;
+ contact->side1 = -1;
+ contact->side2 = -1; // TODO: set plane index?
+ return 1;
+ }
+ else
+ {
+ // the sphere may not be directly touching
+ // the polyhedron, but it may be touching
+ // a point or an edge, if the distance between
+ // the closest point on the poly (out) and the
+ // center of the sphere is less than the sphere
+ // radius we have a hit.
+ temp[0] = (Sphere->final_posr->pos[0]-out[0]);
+ temp[1] = (Sphere->final_posr->pos[1]-out[1]);
+ temp[2] = (Sphere->final_posr->pos[2]-out[2]);
+ dist=(temp[0]*temp[0])+(temp[1]*temp[1])+(temp[2]*temp[2]);
+ // avoid the sqrt unless really necesary
+ if(dist<(Sphere->radius*Sphere->radius))
+ {
+ // We got an indirect hit
+ dist=dSqrt(dist);
+ contact->normal[0] = temp[0]/dist;
+ contact->normal[1] = temp[1]/dist;
+ contact->normal[2] = temp[2]/dist;
+ contact->pos[0] = Sphere->final_posr->pos[0]+
+ (-contact->normal[0]*Sphere->radius);
+ contact->pos[1] = Sphere->final_posr->pos[1]+
+ (-contact->normal[1]*Sphere->radius);
+ contact->pos[2] = Sphere->final_posr->pos[2]+
+ (-contact->normal[2]*Sphere->radius);
+ contact->depth = Sphere->radius-dist;
+ contact->g1 = Sphere;
+ contact->g2 = Convex;
+ contact->side1 = -1;
+ contact->side2 = -1; // TODO: set plane index?
+ return 1;
+ }
+ }
+ }
+ sphereinside=false;
+ }
+ if(sphereinside)
+ {
+ if(closestdist>dFabs(dist))
+ {
+ closestdist=dFabs(dist);
+ closestplane=i;
+ }
+ }
+ pPoly+=pPoly[0]+1;
+ }
+ if(sphereinside)
+ {
+ // if the center of the sphere is inside
+ // the Convex, we need to pop it out
+ dMultiply0_331(contact->normal,
+ Convex->final_posr->R,
+ &Convex->planes[(closestplane*4)]);
+ contact->pos[0] = Sphere->final_posr->pos[0];
+ contact->pos[1] = Sphere->final_posr->pos[1];
+ contact->pos[2] = Sphere->final_posr->pos[2];
+ contact->depth = closestdist+Sphere->radius;
+ contact->g1 = Sphere;
+ contact->g2 = Convex;
+ contact->side1 = -1;
+ contact->side2 = -1; // TODO: set plane index?
+ return 1;
+ }
+ return 0;
+}
+
+int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
+ dContactGeom * /*contact*/, int skip)
+{
+ dIASSERT (skip >= (int)sizeof(dContactGeom));
+ dIASSERT (o1->type == dConvexClass);
+ dIASSERT (o2->type == dBoxClass);
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+
+ //dxConvex *Convex = (dxConvex*) o1;
+ //dxBox *Box = (dxBox*) o2;
+
+ return 0;
+}
+
+int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
+ int flags, dContactGeom * /*contact*/, int skip)
+{
+ dIASSERT (skip >= (int)sizeof(dContactGeom));
+ dIASSERT (o1->type == dConvexClass);
+ dIASSERT (o2->type == dCapsuleClass);
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+
+ //dxConvex *Convex = (dxConvex*) o1;
+ //dxCapsule *Capsule = (dxCapsule*) o2;
+
+ return 0;
+}
+
+inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max)
+{
+ /* TODO: Use Support points here */
+ dVector3 point;
+ dReal value;
+ //fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]);
+ dMultiply0_331(point,cvx.final_posr->R,cvx.points);
+ //fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]);
+ point[0]+=cvx.final_posr->pos[0];
+ point[1]+=cvx.final_posr->pos[1];
+ point[2]+=cvx.final_posr->pos[2];
+ max = min = dCalcVectorDot3(point,axis)-axis[3];//(*)
+ for (unsigned int i = 1; i < cvx.pointcount; ++i)
+ {
+ dMultiply0_331(point,cvx.final_posr->R,cvx.points+(i*3));
+ point[0]+=cvx.final_posr->pos[0];
+ point[1]+=cvx.final_posr->pos[1];
+ point[2]+=cvx.final_posr->pos[2];
+ value=dCalcVectorDot3(point,axis)-axis[3];//(*)
+ if(value<min)
+ {
+ min=value;
+ }
+ else if(value>max)
+ {
+ max=value;
+ }
+ }
+ // *: usually using the distance part of the plane (axis) is
+ // not necesary, however, here we need it here in order to know
+ // which face to pick when there are 2 parallel sides.
+}
+
+bool CheckEdgeIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,int& curc,
+ dContactGeom *contact, int skip)
+{
+ int maxc = flags & NUMC_MASK;
+ dIASSERT(maxc != 0);
+ dVector3 e1,e2,q;
+ dVector4 plane,depthplane;
+ dReal t;
+ for(unsigned int i = 0;i<cvx1.edgecount;++i)
+ {
+ // Rotate
+ dMultiply0_331(e1,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].first*3));
+ // translate
+ e1[0]+=cvx1.final_posr->pos[0];
+ e1[1]+=cvx1.final_posr->pos[1];
+ e1[2]+=cvx1.final_posr->pos[2];
+ // Rotate
+ dMultiply0_331(e2,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].second*3));
+ // translate
+ e2[0]+=cvx1.final_posr->pos[0];
+ e2[1]+=cvx1.final_posr->pos[1];
+ e2[2]+=cvx1.final_posr->pos[2];
+ const unsigned int* pPoly=cvx2.polygons;
+ for(sizeint j=0;j<cvx2.planecount;++j)
+ {
+ // Rotate
+ dMultiply0_331(plane,cvx2.final_posr->R,cvx2.planes+(j*4));
+ dNormalize3(plane);
+ // Translate
+ plane[3]=
+ (cvx2.planes[(j*4)+3])+
+ ((plane[0] * cvx2.final_posr->pos[0]) +
+ (plane[1] * cvx2.final_posr->pos[1]) +
+ (plane[2] * cvx2.final_posr->pos[2]));
+ dContactGeom *target = SAFECONTACT(flags, contact, curc, skip);
+ target->g1=&cvx1; // g1 is the one pushed
+ target->g2=&cvx2;
+ if(IntersectSegmentPlane(e1,e2,plane,t,target->pos))
+ {
+ if(IsPointInPolygon(target->pos,pPoly,plane,&cvx2,q))
+ {
+ target->depth = dInfinity;
+ for(sizeint k=0;k<cvx2.planecount;++k)
+ {
+ if(k==j) continue; // we're already at 0 depth on this plane
+ // Rotate
+ dMultiply0_331(depthplane,cvx2.final_posr->R,cvx2.planes+(k*4));
+ dNormalize3(depthplane);
+ // Translate
+ depthplane[3]=
+ (cvx2.planes[(k*4)+3])+
+ ((plane[0] * cvx2.final_posr->pos[0]) +
+ (plane[1] * cvx2.final_posr->pos[1]) +
+ (plane[2] * cvx2.final_posr->pos[2]));
+ dReal depth = (dVector3Dot(depthplane, target->pos) - depthplane[3]); // Ax + By + Cz - D
+ if((fabs(depth)<fabs(target->depth))&&((depth<-dEpsilon)||(depth>dEpsilon)))
+ {
+ target->depth=depth;
+ dVector3Copy(depthplane,target->normal);
+ }
+ }
+ ++curc;
+ if(curc==maxc)
+ return true;
+ }
+ }
+ pPoly+=pPoly[0]+1;
+ }
+ }
+ return false;
+}
+
+/*
+Helper struct
+*/
+struct ConvexConvexSATOutput
+{
+ dReal min_depth;
+ int depth_type;
+ dVector3 dist; // distance from center to center, from cvx1 to cvx2
+ dVector3 e1a,e1b,e2a,e2b; // e1a to e1b = edge in cvx1,e2a to e2b = edge in cvx2.
+};
+
+/*! \brief Does an axis separation test using cvx1 planes on cvx1 and cvx2, returns true for a collision false for no collision
+ \param cvx1 [IN] First Convex object, its planes are used to do the tests
+ \param cvx2 [IN] Second Convex object
+ \param min_depth [IN/OUT] Used to input as well as output the minimum depth so far, must be set to a huge value such as dInfinity for initialization.
+ \param g1 [OUT] Pointer to the convex which should be used in the returned contact as g1
+ \param g2 [OUT] Pointer to the convex which should be used in the returned contact as g2
+*/
+inline bool CheckSATConvexFaces(dxConvex& cvx1,
+ dxConvex& cvx2,
+ ConvexConvexSATOutput& ccso)
+{
+ dReal min,max,min1,max1,min2,max2,depth;
+ dVector4 plane;
+ for(unsigned int i=0;i<cvx1.planecount;++i)
+ {
+ // -- Apply Transforms --
+ // Rotate
+ dMultiply0_331(plane,cvx1.final_posr->R,cvx1.planes+(i*4));
+ dNormalize3(plane);
+ // Translate
+ plane[3]=
+ (cvx1.planes[(i*4)+3])+
+ ((plane[0] * cvx1.final_posr->pos[0]) +
+ (plane[1] * cvx1.final_posr->pos[1]) +
+ (plane[2] * cvx1.final_posr->pos[2]));
+ ComputeInterval(cvx1,plane,min1,max1);
+ ComputeInterval(cvx2,plane,min2,max2);
+ if(max2<min1 || max1<min2) return false;
+ min = dMAX(min1, min2);
+ max = dMIN(max1, max2);
+ depth = max-min;
+ /*
+ Take only into account the faces that penetrate cvx1 to determine
+ minimum depth
+ ((max2*min2)<=0) = different sign, or one is zero and thus
+ cvx2 barelly touches cvx1
+ */
+ if (((max2*min2)<=0) && (dFabs(depth)<dFabs(ccso.min_depth)))
+ {
+ // Flip plane because the contact normal must point INTO g1,
+ // plus the integrator seems to like positive depths better than negative ones
+ ccso.min_depth=-depth;
+ ccso.depth_type = 1; // 1 = face-something
+ }
+ }
+ return true;
+}
+/*! \brief Does an axis separation test using cvx1 and cvx2 edges, returns true for a collision false for no collision
+ \param cvx1 [IN] First Convex object
+ \param cvx2 [IN] Second Convex object
+ \param min_depth [IN/OUT] Used to input as well as output the minimum depth so far, must be set to a huge value such as dInfinity for initialization.
+ \param g1 [OUT] Pointer to the convex which should be used in the returned contact as g1
+ \param g2 [OUT] Pointer to the convex which should be used in the returned contact as g2
+*/
+inline bool CheckSATConvexEdges(dxConvex& cvx1,
+ dxConvex& cvx2,
+ ConvexConvexSATOutput& ccso)
+{
+ // Test cross products of pairs of edges
+ dReal depth,min,max,min1,max1,min2,max2;
+ dVector4 plane;
+ dVector3 e1,e2,e1a,e1b,e2a,e2b;
+ dVector3 dist;
+ dVector3Copy(ccso.dist,dist);
+ unsigned int s1 = cvx1.SupportIndex(dist);
+ // invert direction
+ dVector3Inv(dist);
+ unsigned int s2 = cvx2.SupportIndex(dist);
+ for(unsigned int i = 0;i<cvx1.edgecount;++i)
+ {
+ // Skip edge if it doesn't contain the extremal vertex
+ if((cvx1.edges[i].first!=s1)&&(cvx1.edges[i].second!=s1)) continue;
+ // we only need to apply rotation here
+ dMultiply0_331(e1a,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].first*3));
+ dMultiply0_331(e1b,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].second*3));
+ e1[0]=e1b[0]-e1a[0];
+ e1[1]=e1b[1]-e1a[1];
+ e1[2]=e1b[2]-e1a[2];
+ for(unsigned int j = 0;j<cvx2.edgecount;++j)
+ {
+ // Skip edge if it doesn't contain the extremal vertex
+ if((cvx2.edges[j].first!=s2)&&(cvx2.edges[j].second!=s2)) continue;
+ // we only need to apply rotation here
+ dMultiply0_331 (e2a,cvx2.final_posr->R,cvx2.points+(cvx2.edges[j].first*3));
+ dMultiply0_331 (e2b,cvx2.final_posr->R,cvx2.points+(cvx2.edges[j].second*3));
+ e2[0]=e2b[0]-e2a[0];
+ e2[1]=e2b[1]-e2a[1];
+ e2[2]=e2b[2]-e2a[2];
+ dCalcVectorCross3(plane,e1,e2);
+ if(dCalcVectorDot3(plane,plane)<dEpsilon) /* edges are parallel */ continue;
+ dNormalize3(plane);
+ plane[3]=0;
+ ComputeInterval(cvx1,plane,min1,max1);
+ ComputeInterval(cvx2,plane,min2,max2);
+ if(max2 < min1 || max1 < min2) return false;
+ min = dMAX(min1, min2);
+ max = dMIN(max1, max2);
+ depth = max-min;
+ if (((dFabs(depth)+dEpsilon)<dFabs(ccso.min_depth)))
+ {
+ ccso.min_depth=depth;
+ ccso.depth_type = 2; // 2 means edge-edge
+ // use cached values, add position
+ dVector3Copy(e1a,ccso.e1a);
+ dVector3Copy(e1b,ccso.e1b);
+ ccso.e1a[0]+=cvx1.final_posr->pos[0];
+ ccso.e1a[1]+=cvx1.final_posr->pos[1];
+ ccso.e1a[2]+=cvx1.final_posr->pos[2];
+ ccso.e1b[0]+=cvx1.final_posr->pos[0];
+ ccso.e1b[1]+=cvx1.final_posr->pos[1];
+ ccso.e1b[2]+=cvx1.final_posr->pos[2];
+ dVector3Copy(e2a,ccso.e2a);
+ dVector3Copy(e2b,ccso.e2b);
+ ccso.e2a[0]+=cvx2.final_posr->pos[0];
+ ccso.e2a[1]+=cvx2.final_posr->pos[1];
+ ccso.e2a[2]+=cvx2.final_posr->pos[2];
+ ccso.e2b[0]+=cvx2.final_posr->pos[0];
+ ccso.e2b[1]+=cvx2.final_posr->pos[1];
+ ccso.e2b[2]+=cvx2.final_posr->pos[2];
+ }
+ }
+ }
+ return true;
+}
+
+#if 0
+/*! \brief Returns the index of the plane/side of the incident convex (ccso.g2)
+ * which is closer to the reference convex (ccso.g1) side
+ *
+ * This function just looks for the incident face that is facing the reference face
+ * and is the closest to being parallel to it, which sometimes is.
+ */
+inline unsigned int GetIncidentSide(ConvexConvexSATOutput& ccso)
+{
+ dVector3 nis; // (N)ormal in (I)ncident convex (S)pace
+ dReal SavedDot;
+ dReal Dot;
+ unsigned int incident_side=0;
+ // Rotate the plane normal into incident convex space
+ // (things like this should be done all over this file,
+ // will look into that)
+ dMultiply1_331(nis,ccso.g2->final_posr->R,ccso.plane);
+ SavedDot = dCalcVectorDot3(nis,ccso.g2->planes);
+ for(unsigned int i=1;i<ccso.g2->planecount;++i)
+ {
+ Dot = dCalcVectorDot3(nis,ccso.g2->planes+(i*4));
+ if(Dot>SavedDot)
+ {
+ SavedDot=Dot;
+ incident_side=i;
+ }
+ }
+ return incident_side;
+}
+#endif
+
+inline unsigned int GetSupportSide(dVector3& dir,dxConvex& cvx)
+{
+ dVector3 dics,tmp; // Direction in convex space
+ dReal SavedDot;
+ dReal Dot;
+ unsigned int side=0;
+ dVector3Copy(dir,tmp);
+ dNormalize3(tmp);
+ dMultiply1_331(dics,cvx.final_posr->R,tmp);
+ SavedDot = dCalcVectorDot3(dics,cvx.planes);
+ for(unsigned int i=1;i<cvx.planecount;++i)
+ {
+ Dot = dCalcVectorDot3(dics,cvx.planes+(i*4));
+ if(Dot>SavedDot)
+ {
+ SavedDot=Dot;
+ side=i;
+ }
+ }
+ return side;
+}
+
+/*! \brief Does an axis separation test between the 2 convex shapes
+using faces and edges */
+int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,
+ dContactGeom *contact, int skip)
+{
+ ConvexConvexSATOutput ccso;
+#ifndef dNDEBUG
+ memset(&ccso, 0, sizeof(ccso)); // get rid of 'uninitialized values' warning
+#endif
+ ccso.min_depth=dInfinity; // Min not min at all
+ ccso.depth_type=0; // no type
+ // precompute distance vector
+ dSubtractVectors3(ccso.dist, cvx2.final_posr->pos, cvx1.final_posr->pos);
+ int maxc = flags & NUMC_MASK;
+ dIASSERT(maxc != 0);
+ dVector3 i1,i2,r1,r2; // edges of incident and reference faces respectively
+ int contacts=0;
+ if(!CheckSATConvexFaces(cvx1,cvx2,ccso))
+ {
+ return 0;
+ }
+ else
+ if(!CheckSATConvexFaces(cvx2,cvx1,ccso))
+ {
+ return 0;
+ }
+ else if(!CheckSATConvexEdges(cvx1,cvx2,ccso))
+ {
+ return 0;
+ }
+ // If we get here, there was a collision
+ if(ccso.depth_type==1) // face-face
+ {
+ // cvx1 MUST always be in contact->g1 and cvx2 in contact->g2
+ // This was learned the hard way :(
+ unsigned int incident_side;
+ const unsigned int* pIncidentPoly;
+ const unsigned int* pIncidentPoints;
+ unsigned int reference_side;
+ const unsigned int* pReferencePoly;
+ const unsigned int* pReferencePoints;
+ dVector4 plane,rplane,iplane;
+ dVector3 tmp;
+ dVector3 dist,p;
+ dReal t,d,d1,d2;
+ bool outside,out;
+ dVector3Copy(ccso.dist,dist);
+ reference_side = GetSupportSide(dist,cvx1);
+ dNegateVector3(dist);
+ incident_side = GetSupportSide(dist,cvx2);
+
+ pReferencePoly = cvx1.polygons;
+ pIncidentPoly = cvx2.polygons;
+ // Get Reference plane (We may not have to apply transforms Optimization Oportunity)
+ // Rotate
+ dMultiply0_331(rplane,cvx1.final_posr->R,cvx1.planes+(reference_side*4));
+ dNormalize3(rplane);
+ // Translate
+ rplane[3]=
+ (cvx1.planes[(reference_side*4)+3])+
+ ((rplane[0] * cvx1.final_posr->pos[0]) +
+ (rplane[1] * cvx1.final_posr->pos[1]) +
+ (rplane[2] * cvx1.final_posr->pos[2]));
+ // flip
+ rplane[0]=-rplane[0];
+ rplane[1]=-rplane[1];
+ rplane[2]=-rplane[2];
+ rplane[3]=-rplane[3];
+ for(unsigned int i=0;i<incident_side;++i)
+ {
+ pIncidentPoly+=pIncidentPoly[0]+1;
+ }
+ pIncidentPoints = pIncidentPoly+1;
+ // Get the first point of the incident face
+ dMultiply0_331(i2,cvx2.final_posr->R,&cvx2.points[(pIncidentPoints[0]*3)]);
+ dVector3Add(i2,cvx2.final_posr->pos,i2);
+ // Get the same point in the reference convex space
+ dVector3Copy(i2,r2);
+ dVector3Subtract(r2,cvx1.final_posr->pos,r2);
+ dVector3Copy(r2,tmp);
+ dMultiply1_331(r2,cvx1.final_posr->R,tmp);
+ for(unsigned int i=0;i<pIncidentPoly[0];++i)
+ {
+ // Move i2 to i1, r2 to r1
+ dVector3Copy(i2,i1);
+ dVector3Copy(r2,r1);
+ dMultiply0_331(i2,cvx2.final_posr->R,&cvx2.points[(pIncidentPoints[(i+1)%pIncidentPoly[0]]*3)]);
+ dVector3Add(i2,cvx2.final_posr->pos,i2);
+ // Get the same point in the reference convex space
+ dVector3Copy(i2,r2);
+ dVector3Subtract(r2,cvx1.final_posr->pos,r2);
+ dVector3Copy(r2,tmp);
+ dMultiply1_331(r2,cvx1.final_posr->R,tmp);
+ outside=false;
+ for(unsigned int j=0;j<cvx1.planecount;++j)
+ {
+ plane[0]=cvx1.planes[(j*4)+0];
+ plane[1]=cvx1.planes[(j*4)+1];
+ plane[2]=cvx1.planes[(j*4)+2];
+ plane[3]=cvx1.planes[(j*4)+3];
+ // Get the distance from the points to the plane
+ d1 = r1[0]*plane[0]+
+ r1[1]*plane[1]+
+ r1[2]*plane[2]-
+ plane[3];
+ d2 = r2[0]*plane[0]+
+ r2[1]*plane[1]+
+ r2[2]*plane[2]-
+ plane[3];
+ if(d1*d2<0)
+ {
+ out = false;
+
+ // Edge intersects plane
+ if (!IntersectSegmentPlane(r1,r2,plane,t,p))
+ {
+ out = true;
+ }
+
+ if (!out)
+ {
+ // Check the resulting point again to make sure it is inside the reference convex
+ for (unsigned int k = 0; k < cvx1.planecount; ++k)
+ {
+ d = p[0]*cvx1.planes[(k*4)+0]+
+ p[1]*cvx1.planes[(k*4)+1]+
+ p[2]*cvx1.planes[(k*4)+2]-
+ cvx1.planes[(k*4)+3];
+ if(d>0)
+ {
+ out = true;
+ break;
+ }
+ }
+ }
+
+ if(!out)
+ {
+#if 0
+ // Use t to move p into global space
+ p[0] = i1[0]+((i2[0]-i1[0])*t);
+ p[1] = i1[1]+((i2[1]-i1[1])*t);
+ p[2] = i1[2]+((i2[2]-i1[2])*t);
+#else
+ // Apply reference convex transformations to p
+ // The commented out piece of code is likelly to
+ // produce less operations than this one, but
+ // this way we know we are getting the right data
+ dMultiply0_331(tmp,cvx1.final_posr->R,p);
+ dVector3Add(tmp,cvx1.final_posr->pos,p);
+#endif
+ // get p's distance to reference plane
+ d = p[0]*rplane[0]+
+ p[1]*rplane[1]+
+ p[2]*rplane[2]-
+ rplane[3];
+ if(d>0)
+ {
+ dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
+ dVector3Copy(p, target->pos);
+ dVector3Copy(rplane, target->normal);
+ target->g1 = &cvx1;
+ target->g2 = &cvx2;
+ target->depth = d;
+ ++contacts;
+ if (contacts==maxc) return contacts;
+ }
+ }
+ }
+ if(d1>0)
+ {
+ outside=true;
+ }
+ }
+ if(outside) continue;
+ d = i1[0]*rplane[0]+
+ i1[1]*rplane[1]+
+ i1[2]*rplane[2]-
+ rplane[3];
+ if(d>0)
+ {
+ dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
+ dVector3Copy(i1, target->pos);
+ dVector3Copy(rplane, target->normal);
+ target->g1 = &cvx1;
+ target->g2 = &cvx2;
+ target->depth = d;
+ ++contacts;
+ if (contacts==maxc) return contacts;
+ }
+ }
+ // IF we get here, we got the easiest contacts to calculate,
+ // but there is still space in the contacts array for more.
+ // So, project the Reference's face points onto the Incident face
+ // plane and test them for inclusion in the reference plane as well.
+ // We already have computed intersections so, skip those.
+
+ /* Get Incident plane, we need it for projection */
+ /* Rotate */
+ dMultiply0_331(iplane,cvx2.final_posr->R,cvx2.planes+(incident_side*4));
+ dNormalize3(iplane);
+ /* Translate */
+ iplane[3]=
+ (cvx2.planes[(incident_side*4)+3]) +
+ ((iplane[0] * cvx2.final_posr->pos[0]) +
+ (iplane[1] * cvx2.final_posr->pos[1]) +
+ (iplane[2] * cvx2.final_posr->pos[2]));
+ // get reference face
+ for(unsigned int i=0;i<reference_side;++i)
+ {
+ pReferencePoly+=pReferencePoly[0]+1;
+ }
+ pReferencePoints = pReferencePoly+1;
+ for(unsigned int i=0;i<pReferencePoly[0];++i)
+ {
+ dMultiply0_331(i1,cvx1.final_posr->R,&cvx1.points[(pReferencePoints[i]*3)]);
+ dVector3Add(cvx1.final_posr->pos,i1,i1);
+ // Project onto Incident face plane
+ t = -(i1[0]*iplane[0]+
+ i1[1]*iplane[1]+
+ i1[2]*iplane[2]-
+ iplane[3]);
+ i1[0]+=iplane[0]*t;
+ i1[1]+=iplane[1]*t;
+ i1[2]+=iplane[2]*t;
+ // Get the same point in the incident convex space
+ dVector3Copy(i1,r1);
+ dVector3Subtract(r1,cvx2.final_posr->pos,r1);
+ dVector3Copy(r1,tmp);
+ dMultiply1_331(r1,cvx2.final_posr->R,tmp);
+ // Check if it is outside the incident convex
+ out = false;
+ for(unsigned int j=0;j<cvx2.planecount;++j)
+ {
+ d = r1[0]*cvx2.planes[(j*4)+0]+
+ r1[1]*cvx2.planes[(j*4)+1]+
+ r1[2]*cvx2.planes[(j*4)+2]-
+ cvx2.planes[(j*4)+3];
+ if(d>=0){out = true;break;};
+ }
+ if(!out)
+ {
+ // check that the point is not a duplicate
+ outside = false;
+ for(int j=0;j<contacts;++j)
+ {
+ dContactGeom *cur_contact = SAFECONTACT(flags, contact, j, skip);
+ if((cur_contact->pos[0] == i1[0]) &&
+ (cur_contact->pos[1] == i1[1]) &&
+ (cur_contact->pos[2] == i1[2]))
+ {
+ outside=true;
+ }
+ }
+ if(!outside)
+ {
+ d = i1[0]*rplane[0]+
+ i1[1]*rplane[1]+
+ i1[2]*rplane[2]-
+ rplane[3];
+ if(d>0)
+ {
+ dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
+ dVector3Copy(i1, target->pos);
+ dVector3Copy(rplane, target->normal);
+ target->g1 = &cvx1;
+ target->g2 = &cvx2;
+ target->depth = d;
+ ++contacts;
+ if (contacts==maxc) return contacts;
+ }
+ }
+ }
+ }
+ }
+ else if (ccso.depth_type == 2) // edge-edge
+ {
+ dVector3 c1, c2;
+ ClosestPointBetweenSegments(ccso.e1a, ccso.e1b, ccso.e2a, ccso.e2b, c1, c2);
+
+ dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
+ dSubtractVectors3(target->normal, c2, c1);
+ dReal depth_square = dCalcVectorLengthSquare3(target->normal);
+
+ if (dxSafeNormalize3(target->normal))
+ {
+ target->depth = dSqrt(depth_square);
+ }
+ else
+ {
+ // If edges coincide return direction from one center to the other as the contact normal
+ dVector3Copy(ccso.dist, target->normal);
+
+ if (!dxSafeNormalize3(target->normal))
+ {
+ // If the both centers coincide as well return an arbitrary vector. The depth is going to be zero anyway.
+ dAssignVector3(target->normal, 1, 0, 0);
+ }
+
+ target->depth = 0; // Since the edges coincide, return a contact of zero depth
+ }
+
+ target->g1 = &cvx1;
+ target->g2 = &cvx2;
+ dVector3Copy(c1, target->pos);
+ contacts++;
+ }
+ return contacts;
+}
+
+int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
+ dContactGeom *contact, int skip)
+{
+ dIASSERT (skip >= (int)sizeof(dContactGeom));
+ dIASSERT (o1->type == dConvexClass);
+ dIASSERT (o2->type == dConvexClass);
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+ dxConvex *Convex1 = (dxConvex*) o1;
+ dxConvex *Convex2 = (dxConvex*) o2;
+ return TestConvexIntersection(*Convex1,*Convex2,flags,
+ contact,skip);
+}
+
+#if 0
+int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags,
+ dContactGeom *contact, int skip)
+{
+ dIASSERT (skip >= (int)sizeof(dContactGeom));
+ dIASSERT( o1->type == dRayClass );
+ dIASSERT( o2->type == dConvexClass );
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+ dxRay* ray = (dxRay*) o1;
+ dxConvex* convex = (dxConvex*) o2;
+ dVector3 origin,destination,contactpoint,out;
+ dReal depth;
+ dVector4 plane;
+ unsigned int *pPoly=convex->polygons;
+ // Calculate ray origin and destination
+ destination[0]=0;
+ destination[1]=0;
+ destination[2]= ray->length;
+ // -- Rotate --
+ dMultiply0_331(destination,ray->final_posr->R,destination);
+ origin[0]=ray->final_posr->pos[0];
+ origin[1]=ray->final_posr->pos[1];
+ origin[2]=ray->final_posr->pos[2];
+ destination[0]+=origin[0];
+ destination[1]+=origin[1];
+ destination[2]+=origin[2];
+ for(int i=0;i<convex->planecount;++i)
+ {
+ // Rotate
+ dMultiply0_331(plane,convex->final_posr->R,convex->planes+(i*4));
+ // Translate
+ plane[3]=
+ (convex->planes[(i*4)+3])+
+ ((plane[0] * convex->final_posr->pos[0]) +
+ (plane[1] * convex->final_posr->pos[1]) +
+ (plane[2] * convex->final_posr->pos[2]));
+ if(IntersectSegmentPlane(origin,
+ destination,
+ plane,
+ depth,
+ contactpoint))
+ {
+ if(IsPointInPolygon(contactpoint,pPoly,plane,convex,out))
+ {
+ contact->pos[0]=contactpoint[0];
+ contact->pos[1]=contactpoint[1];
+ contact->pos[2]=contactpoint[2];
+ contact->normal[0]=plane[0];
+ contact->normal[1]=plane[1];
+ contact->normal[2]=plane[2];
+ contact->depth=depth;
+ contact->g1 = ray;
+ contact->g2 = convex;
+ contact->side1 = -1;
+ contact->side2 = -1; // TODO: set plane index?
+ return 1;
+ }
+ }
+ pPoly+=pPoly[0]+1;
+ }
+ return 0;
+}
+#else
+// Ray - Convex collider by David Walters, June 2006
+int dCollideRayConvex(dxGeom *o1, dxGeom *o2,
+ int flags, dContactGeom *contact, int skip)
+{
+ dIASSERT(skip >= (int)sizeof(dContactGeom));
+ dIASSERT(o1->type == dRayClass);
+ dIASSERT(o2->type == dConvexClass);
+ dIASSERT((flags & NUMC_MASK) >= 1);
+
+ dxRay* ray = (dxRay*)o1;
+ dxConvex* convex = (dxConvex*)o2;
+
+ contact->g1 = ray;
+ contact->g2 = convex;
+ contact->side1 = -1;
+ contact->side2 = -1; // TODO: set plane index?
+
+ dReal alpha, beta, nsign;
+ int flag = 0;
+
+ //
+ // Compute some useful info
+ //
+
+ dVector3 ray_pos = {
+ ray->final_posr->pos[0] - convex->final_posr->pos[0],
+ ray->final_posr->pos[1] - convex->final_posr->pos[1],
+ ray->final_posr->pos[2] - convex->final_posr->pos[2]
+ };
+
+ dVector3 ray_dir = {
+ ray->final_posr->R[0 * 4 + 2],
+ ray->final_posr->R[1 * 4 + 2],
+ ray->final_posr->R[2 * 4 + 2]
+ };
+
+ dMultiply1_331(ray_pos, convex->final_posr->R, ray_pos);
+ dMultiply1_331(ray_dir, convex->final_posr->R, ray_dir);
+
+ for (unsigned int i = 0; i < convex->planecount; ++i)
+ {
+ // Alias this plane.
+ const dReal* plane = convex->planes + (i * 4);
+
+ // If alpha >= 0 then start point is outside of plane.
+ alpha = dCalcVectorDot3(plane, ray_pos) - plane[3];
+
+ // If any alpha is positive, then
+ // the ray start is _outside_ of the hull
+ if (alpha >= 0)
+ {
+ flag = 1;
+ break;
+ }
+ }
+
+ // If the ray starts inside the convex hull, then everything is flipped.
+ nsign = (flag) ? REAL(1.0) : REAL(-1.0);
+
+
+ //
+ // Find closest contact point
+ //
+
+ // Assume no contacts.
+ contact->depth = dInfinity;
+
+ for (unsigned int i = 0; i < convex->planecount; ++i)
+ {
+ // Alias this plane.
+ const dReal* plane = convex->planes + (i * 4);
+
+ // If alpha >= 0 then point is outside of plane.
+ alpha = nsign * (dCalcVectorDot3(plane, ray_pos) - plane[3]);
+
+ // Compute [ plane-normal DOT ray-normal ], (/flip)
+ beta = dCalcVectorDot3(plane, ray_dir) * nsign;
+
+ // Ray is pointing at the plane? ( beta < 0 )
+ // Ray start to plane is within maximum ray length?
+ // Ray start to plane is closer than the current best distance?
+ if (beta < -dEpsilon &&
+ alpha >= 0 && alpha <= ray->length &&
+ alpha < contact->depth)
+ {
+ // Compute contact point on convex hull surface.
+ contact->pos[0] = ray_pos[0] + alpha * ray_dir[0];
+ contact->pos[1] = ray_pos[1] + alpha * ray_dir[1];
+ contact->pos[2] = ray_pos[2] + alpha * ray_dir[2];
+
+ flag = 0;
+
+ // For all _other_ planes.
+ for (unsigned int j = 0; j < convex->planecount; ++j)
+ {
+ if (i == j)
+ continue; // Skip self.
+
+ // Alias this plane.
+ const dReal* planej = convex->planes + (j * 4);
+
+ // If beta >= 0 then start is outside of plane.
+ beta = dCalcVectorDot3(planej, contact->pos) - planej[3];
+
+ // If any beta is positive, then the contact point
+ // is not on the surface of the convex hull - it's just
+ // intersecting some part of its infinite extent.
+ if (beta > dEpsilon)
+ {
+ flag = 1;
+ break;
+ }
+ }
+
+ // Contact point isn't outside hull's surface? then it's a good contact!
+ if (flag == 0)
+ {
+ // Store the contact normal, possibly flipped.
+ contact->normal[0] = nsign * plane[0];
+ contact->normal[1] = nsign * plane[1];
+ contact->normal[2] = nsign * plane[2];
+
+ // Store depth
+ contact->depth = alpha;
+
+ if ((flags & CONTACTS_UNIMPORTANT) && contact->depth <= ray->length)
+ {
+ // Break on any contact if contacts are not important
+ break;
+ }
+ }
+ }
+ }
+ // Contact?
+ if (contact->depth <= ray->length)
+ {
+ // Adjust contact position and normal back to global space
+ dMultiply0_331(contact->pos, convex->final_posr->R, contact->pos);
+ dMultiply0_331(contact->normal, convex->final_posr->R, contact->normal);
+ contact->pos[0] += convex->final_posr->pos[0];
+ contact->pos[1] += convex->final_posr->pos[1];
+ contact->pos[2] += convex->final_posr->pos[2];
+ return true;
+ }
+ return false;
+}
+
+#endif
+//<-- Convex Collision
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 02:28:21 +0000