1288 lines
38 KiB
C++
1288 lines
38 KiB
C++
/*************************************************************************
|
|
* *
|
|
* 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 <algorithm>
|
|
#include <ode/common.h>
|
|
#include <ode/collision.h>
|
|
#include <ode/matrix.h>
|
|
#include <ode/rotation.h>
|
|
#include <ode/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 _MSC_VER <= 1200
|
|
#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,
|
|
dReal *_planes,
|
|
unsigned int _planecount,
|
|
dReal *_points,
|
|
unsigned int _pointcount,
|
|
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;
|
|
FillEdges();
|
|
}
|
|
|
|
|
|
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()
|
|
{
|
|
unsigned int *points_in_poly=polygons;
|
|
unsigned int *index=polygons+1;
|
|
for(unsigned int i=0;i<planecount;++i)
|
|
{
|
|
//fprintf(stdout,"Points in Poly: %d\n",*points_in_poly);
|
|
for(unsigned int j=0;j<*points_in_poly;++j)
|
|
{
|
|
edges.insert(edge(dMIN(index[j],index[(j+1)%*points_in_poly]),
|
|
dMAX(index[j],index[(j+1)%*points_in_poly])));
|
|
//fprintf(stdout,"Insert %d-%d\n",index[j],index[(j+1)%*points_in_poly]);
|
|
}
|
|
points_in_poly+=(*points_in_poly+1);
|
|
index=points_in_poly+1;
|
|
}
|
|
/*
|
|
fprintf(stdout,"Edge Count: %d\n",edges.size());
|
|
for(std::set<edge>::iterator it=edges.begin();
|
|
it!=edges.end();
|
|
++it)
|
|
{
|
|
fprintf(stdout,"Edge: %d-%d\n",it->first,it->second);
|
|
}
|
|
*/
|
|
}
|
|
|
|
dGeomID dCreateConvex (dSpaceID space,dReal *_planes,unsigned int _planecount,
|
|
dReal *_points,
|
|
unsigned int _pointcount,
|
|
unsigned int *_polygons)
|
|
{
|
|
//fprintf(stdout,"dxConvex dCreateConvex\n");
|
|
return new dxConvex(space,_planes, _planecount,
|
|
_points,
|
|
_pointcount,
|
|
_polygons);
|
|
}
|
|
|
|
void dGeomSetConvex (dGeomID g,dReal *_planes,unsigned int _planecount,
|
|
dReal *_points,
|
|
unsigned int _pointcount,
|
|
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] - dDOT(p,a)) / dDOT(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+(Direction*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 = dDOT(Direction1 , Direction1);
|
|
dReal b = dDOT(Direction1 , Direction2);
|
|
dReal c = dDOT(Direction2 , Direction2);
|
|
dReal d = dDOT(Direction1 , w);
|
|
dReal e = dDOT(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 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
|
|
//Cross(p1, p2,d);
|
|
dCROSS(d,=,p1,p2);
|
|
|
|
// If d is (near) zero, the planes are parallel (and separated)
|
|
// or coincident, so they're not considered intersecting
|
|
dReal denom = dDOT(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
|
|
//Cross(n, d,p);
|
|
dCROSS(p,=,n,d);
|
|
p[0]/=denom;
|
|
p[1]/=denom;
|
|
p[2]/=denom;
|
|
return true;
|
|
}
|
|
|
|
/*! \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,
|
|
unsigned int *polygon,
|
|
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.
|
|
size_t pointcount=polygon[0];
|
|
dVector3 a;
|
|
dVector3 b;
|
|
dVector3 c;
|
|
dVector3 ab;
|
|
dVector3 ac;
|
|
dVector3 ap;
|
|
dVector3 bp;
|
|
dReal d1;
|
|
dReal d2;
|
|
dReal d3;
|
|
dReal d4;
|
|
dReal vc;
|
|
polygon++; // skip past pointcount
|
|
for(size_t i=0;i<pointcount;++i)
|
|
{
|
|
dMULTIPLY0_331 (a,convex->final_posr->R,&convex->points[(polygon[i]*3)]);
|
|
a[0]=convex->final_posr->pos[0]+a[0];
|
|
a[1]=convex->final_posr->pos[1]+a[1];
|
|
a[2]=convex->final_posr->pos[2]+a[2];
|
|
|
|
dMULTIPLY0_331 (b,convex->final_posr->R,
|
|
&convex->points[(polygon[(i+1)%pointcount]*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];
|
|
|
|
dMULTIPLY0_331 (c,convex->final_posr->R,
|
|
&convex->points[(polygon[(i+2)%pointcount]*3)]);
|
|
c[0]=convex->final_posr->pos[0]+c[0];
|
|
c[1]=convex->final_posr->pos[1]+c[1];
|
|
c[2]=convex->final_posr->pos[2]+c[2];
|
|
|
|
ab[0] = b[0] - a[0];
|
|
ab[1] = b[1] - a[1];
|
|
ab[2] = b[2] - a[2];
|
|
ac[0] = c[0] - a[0];
|
|
ac[1] = c[1] - a[1];
|
|
ac[2] = c[2] - a[2];
|
|
ap[0] = p[0] - a[0];
|
|
ap[1] = p[1] - a[1];
|
|
ap[2] = p[2] - a[2];
|
|
d1 = dDOT(ab,ap);
|
|
d2 = dDOT(ac,ap);
|
|
if (d1 <= 0.0f && d2 <= 0.0f)
|
|
{
|
|
out[0]=a[0];
|
|
out[1]=a[1];
|
|
out[2]=a[2];
|
|
return false;
|
|
}
|
|
bp[0] = p[0] - b[0];
|
|
bp[1] = p[1] - b[1];
|
|
bp[2] = p[2] - b[2];
|
|
d3 = dDOT(ab,bp);
|
|
d4 = dDOT(ac,bp);
|
|
if (d3 >= 0.0f && d4 <= d3)
|
|
{
|
|
out[0]=b[0];
|
|
out[1]=b[1];
|
|
out[2]=b[2];
|
|
return false;
|
|
}
|
|
vc = d1*d4 - d3*d2;
|
|
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
|
|
{
|
|
dReal v = d1 / (d1 - d3);
|
|
out[0] = a[0] + (ab[0]*v);
|
|
out[1] = a[1] + (ab[1]*v);
|
|
out[2] = a[2] + (ab[2]*v);
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
|
|
dContactGeom *contact, int skip)
|
|
{
|
|
dIASSERT (o1->type == dConvexClass);
|
|
dIASSERT (o2->type == dPlaneClass);
|
|
dxConvex *Convex = (dxConvex*) o1;
|
|
dxPlane *Plane = (dxPlane*) o2;
|
|
unsigned int contacts=0;
|
|
unsigned int maxc = flags & NUMC_MASK;
|
|
dVector3 v1;
|
|
dVector3 v2;
|
|
bool Hit=false;
|
|
|
|
dMULTIPLY0_331 (v1,Convex->final_posr->R,Convex->points);
|
|
v1[0]=Convex->final_posr->pos[0]+v1[0];
|
|
v1[1]=Convex->final_posr->pos[1]+v1[1];
|
|
v1[2]=Convex->final_posr->pos[2]+v1[2];
|
|
|
|
dReal distance1 = ((Plane->p[0] * v1[0]) + // Ax +
|
|
(Plane->p[1] * v1[1]) + // Bx +
|
|
(Plane->p[2] * v1[2])) - Plane->p[3]; // Cz - D
|
|
if(distance1<=0)
|
|
{
|
|
CONTACT(contact,skip*contacts)->normal[0] = Plane->p[0];
|
|
CONTACT(contact,skip*contacts)->normal[1] = Plane->p[1];
|
|
CONTACT(contact,skip*contacts)->normal[2] = Plane->p[2];
|
|
CONTACT(contact,skip*contacts)->pos[0] = v1[0];
|
|
CONTACT(contact,skip*contacts)->pos[1] = v1[1];
|
|
CONTACT(contact,skip*contacts)->pos[2] = v1[2];
|
|
CONTACT(contact,skip*contacts)->depth = -distance1;
|
|
CONTACT(contact,skip*contacts)->g1 = Convex;
|
|
CONTACT(contact,skip*contacts)->g2 = Plane;
|
|
contacts++;
|
|
}
|
|
for(unsigned int i=1;i<Convex->pointcount;++i)
|
|
{
|
|
dMULTIPLY0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]);
|
|
v2[0]=Convex->final_posr->pos[0]+v2[0];
|
|
v2[1]=Convex->final_posr->pos[1]+v2[1];
|
|
v2[2]=Convex->final_posr->pos[2]+v2[2];
|
|
dReal distance2 = ((Plane->p[0] * v2[0]) + // Ax +
|
|
(Plane->p[1] * v2[1]) + // Bx +
|
|
(Plane->p[2] * v2[2])) - Plane->p[3]; // Cz + D
|
|
if(!Hit)
|
|
/*
|
|
Avoid multiplication
|
|
if we have already determined there is a hit
|
|
*/
|
|
{
|
|
if(distance1 * distance2 <= 0)
|
|
{
|
|
// there is a hit.
|
|
Hit=true;
|
|
}
|
|
}
|
|
if((distance2<=0)&&(contacts<maxc))
|
|
{
|
|
CONTACT(contact,skip*contacts)->normal[0] = Plane->p[0];
|
|
CONTACT(contact,skip*contacts)->normal[1] = Plane->p[1];
|
|
CONTACT(contact,skip*contacts)->normal[2] = Plane->p[2];
|
|
CONTACT(contact,skip*contacts)->pos[0] = v2[0];
|
|
CONTACT(contact,skip*contacts)->pos[1] = v2[1];
|
|
CONTACT(contact,skip*contacts)->pos[2] = v2[2];
|
|
CONTACT(contact,skip*contacts)->depth = -distance2;
|
|
CONTACT(contact,skip*contacts)->g1 = Convex;
|
|
CONTACT(contact,skip*contacts)->g2 = Plane;
|
|
contacts++;
|
|
}
|
|
}
|
|
if(Hit) return contacts;
|
|
return 0;
|
|
}
|
|
|
|
int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
|
|
dContactGeom *contact, int skip)
|
|
{
|
|
dIASSERT (o1->type == dSphereClass);
|
|
dIASSERT (o2->type == dConvexClass);
|
|
dxSphere *Sphere = (dxSphere*) o1;
|
|
dxConvex *Convex = (dxConvex*) o2;
|
|
dReal dist,closestdist=dInfinity;
|
|
dVector4 plane;
|
|
// dVector3 contactpoint;
|
|
dVector3 offsetpos,out,temp;
|
|
unsigned int *pPoly=Convex->polygons;
|
|
int closestplane;
|
|
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];
|
|
//fprintf(stdout,"Begin Check\n");
|
|
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 = ((plane[0] * offsetpos[0]) + // Ax +
|
|
(plane[1] * offsetpos[1]) + // Bx +
|
|
(plane[2] * offsetpos[2])) - plane[3]; // 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,Convex,out))
|
|
{
|
|
// finally if we get here we know that the
|
|
// sphere is directly touching the inside of the polyhedron
|
|
//fprintf(stdout,"Contact! distance=%f\n",dist);
|
|
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;
|
|
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;
|
|
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;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
|
|
dContactGeom *contact, int skip)
|
|
{
|
|
dIASSERT (o1->type == dConvexClass);
|
|
dIASSERT (o2->type == dBoxClass);
|
|
dxConvex *Convex = (dxConvex*) o1;
|
|
dxBox *Box = (dxBox*) o2;
|
|
return 0;
|
|
}
|
|
|
|
int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
|
|
int flags, dContactGeom *contact, int skip)
|
|
{
|
|
dIASSERT (o1->type == dConvexClass);
|
|
dIASSERT (o2->type == dCapsuleClass);
|
|
dxConvex *Convex = (dxConvex*) o1;
|
|
dxCapsule *Capsule = (dxCapsule*) o2;
|
|
return 0;
|
|
}
|
|
|
|
/*!
|
|
\brief Retrieves the proper convex and plane index between 2 convex objects.
|
|
|
|
Seidel's Algorithm does not discriminate between 2 different Convex Hulls,
|
|
it only cares about planes, so we feed it the planes of Convex 1 followed
|
|
by the planes of Convex 2 as a single collection of planes,
|
|
given an index into the single collection,
|
|
this function determines the correct convex object and index to retrieve
|
|
the current plane.
|
|
|
|
\param c1 Convex 1
|
|
\param c2 Convex 2
|
|
\param i Plane Index to retrieve
|
|
\param index contains the translated index uppon return
|
|
\return a pointer to the convex object containing plane index "i"
|
|
*/
|
|
inline dxConvex* GetPlaneIndex(dxConvex& c1,
|
|
dxConvex& c2,
|
|
unsigned int i,unsigned int& index)
|
|
{
|
|
if(i>=c1.planecount)
|
|
{
|
|
index = i-c1.planecount;
|
|
return &c2;
|
|
}
|
|
index=i;
|
|
return &c1;
|
|
}
|
|
|
|
inline void dumpplanes(dxConvex& cvx)
|
|
{
|
|
// This is just a dummy debug function
|
|
dVector4 plane;
|
|
fprintf(stdout,"DUMP PLANES\n");
|
|
for (unsigned int i=0;i<cvx.planecount;++i)
|
|
{
|
|
dMULTIPLY0_331(plane,cvx.final_posr->R,&cvx.planes[(i*4)]);
|
|
// Translate
|
|
plane[3]=
|
|
(cvx.planes[(i*4)+3])+
|
|
((plane[0] * cvx.final_posr->pos[0]) +
|
|
(plane[1] * cvx.final_posr->pos[1]) +
|
|
(plane[2] * cvx.final_posr->pos[2]));
|
|
fprintf(stdout,"%f %f %f %f\n",plane[0],plane[1],plane[2],plane[3]);
|
|
}
|
|
}
|
|
|
|
// this variable is for debuggin purpuses only, should go once everything works
|
|
static bool hit=false;
|
|
|
|
/*
|
|
\brief Tests whether 2 convex objects collide
|
|
|
|
Seidel's algorithm is a method to solve linear programs,
|
|
it finds the optimum vertex "v" of a set of functions,
|
|
in our case, the set of functions are the plane functions
|
|
for the 2 convex objects being tested.
|
|
We don't really care about the value optimum vertex though,
|
|
but the 2 convex objects only collide if this vertex exists,
|
|
otherwise, the set of functions is said to be "empty" or "void".
|
|
|
|
Seidel's Original algorithm is recursive and not bound to any number
|
|
of dimensions, the one I present here is Iterative rather than recursive
|
|
and bound to 3 dimensions, which is what we care about.
|
|
|
|
If you're interested (and you should be!) on the algorithm, the paper
|
|
by Raimund Seidel himself should explain a lot better than I did, you can
|
|
find it here: http://www.cs.berkeley.edu/~jrs/meshpapers/SeidelLP.pdf
|
|
|
|
If posible, read the section about Linear Programming in
|
|
Christer Ericson's RealTime Collision Detection book.
|
|
|
|
\note currently there seem to be some issues with this function since
|
|
it doesn't detect collisions except for the most simple tests :(.
|
|
*/
|
|
bool SeidelLP(dxConvex& cvx1,dxConvex& cvx2)
|
|
{
|
|
dVector3 c={1,0,0}; // The Objective vector can be anything
|
|
dVector3 solution; // We dont really need the solution so its local
|
|
dxConvex *cvx;
|
|
unsigned int index;
|
|
unsigned int planecount=cvx1.planecount+cvx2.planecount;
|
|
dReal sum,min,max,med;
|
|
dVector3 c1; // ,c2;
|
|
dVector4 aoveral,aoveram; // these will contain cached computations
|
|
unsigned int l,m,n; // l and m are the axes to the zerod dimensions, n is the axe for the last dimension
|
|
unsigned int i,j,k;
|
|
dVector4 eq1,eq2,eq3; // cached equations for 3d,2d and 1d respectivelly
|
|
// Get the support mapping for a HUGE bounding box in direction c
|
|
solution[0]= (c[0]>0) ? dInfinity : -dInfinity;
|
|
solution[1]= (c[1]>0) ? dInfinity : -dInfinity;
|
|
solution[2]= (c[2]>0) ? dInfinity : -dInfinity;
|
|
for( i=0;i<planecount;++i)
|
|
{
|
|
// Isolate plane equation
|
|
cvx=GetPlaneIndex(cvx1,cvx2,i,index);
|
|
// Rotate
|
|
dMULTIPLY0_331(eq1,cvx->final_posr->R,&cvx->planes[(index*4)]);
|
|
|
|
// Translate
|
|
eq1[3]=(cvx->planes[(index*4)+3])+
|
|
((eq1[0] * cvx->final_posr->pos[0]) +
|
|
(eq1[1] * cvx->final_posr->pos[1]) +
|
|
(eq1[2] * cvx->final_posr->pos[2]));
|
|
// if(!hit)
|
|
// {
|
|
// fprintf(stdout,"Plane I %d: %f %f %f %f\n",i,
|
|
// cvx->planes[(index*4)+0],
|
|
// cvx->planes[(index*4)+1],
|
|
// cvx->planes[(index*4)+2],
|
|
// cvx->planes[(index*4)+3]);
|
|
// fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",i,
|
|
// eq1[0],
|
|
// eq1[1],eq1[2],eq1[3]);
|
|
// fprintf(stdout,"POS %f,%f,%f\n",
|
|
// cvx->final_posr->pos[0],
|
|
// cvx->final_posr->pos[1],
|
|
// cvx->final_posr->pos[2]);
|
|
// }
|
|
// find if the solution is behind the current plane
|
|
sum=
|
|
((eq1[0]*solution[0])+
|
|
(eq1[1]*solution[1])+
|
|
(eq1[2]*solution[2]))-eq1[3];
|
|
// if not we need to discard the current solution
|
|
if(sum>0)
|
|
{
|
|
// go down a dimension by eliminating a variable
|
|
// first find l
|
|
l=0;
|
|
for( j=0;j<3;++j)
|
|
{
|
|
if(fabs(eq1[j])>fabs(eq1[l]))
|
|
{
|
|
l=j;
|
|
}
|
|
}
|
|
if(eq1[l]==0)
|
|
{
|
|
if(!hit)
|
|
{
|
|
fprintf(stdout,"Plane %d: %f %f %f %f is invalid\n",i,
|
|
eq1[0],eq1[1],eq1[2],eq1[3]);
|
|
}
|
|
return false;
|
|
}
|
|
// then compute a/a[l] c1 and solution
|
|
aoveral[0]=eq1[0]/eq1[l];
|
|
aoveral[1]=eq1[1]/eq1[l];
|
|
aoveral[2]=eq1[2]/eq1[l];
|
|
aoveral[3]=eq1[3]/eq1[l];
|
|
c1[0]=c[0]-((c[l]/eq1[l])*eq1[0]);
|
|
c1[1]=c[1]-((c[l]/eq1[l])*eq1[1]);
|
|
c1[2]=c[2]-((c[l]/eq1[l])*eq1[2]);
|
|
solution[0]=solution[0]-((solution[l]/eq1[l])*eq1[0]);
|
|
solution[1]=solution[1]-((solution[l]/eq1[l])*eq1[1]);
|
|
solution[2]=solution[2]-((solution[l]/eq1[l])*eq1[2]);
|
|
// iterate a to get the new equations with the help of a/a[l]
|
|
for( j=0;j<planecount;++j)
|
|
{
|
|
if(i!=j)
|
|
{
|
|
cvx=GetPlaneIndex(cvx1,cvx2,j,index);
|
|
// Rotate
|
|
dMULTIPLY0_331(eq2,cvx->final_posr->R,&cvx->planes[(index*4)]);
|
|
// Translate
|
|
eq2[3]=(cvx->planes[(index*4)+3])+
|
|
((eq2[0] * cvx->final_posr->pos[0]) +
|
|
(eq2[1] * cvx->final_posr->pos[1]) +
|
|
(eq2[2] * cvx->final_posr->pos[2]));
|
|
|
|
// if(!hit)
|
|
// {
|
|
// fprintf(stdout,"Plane J %d: %f %f %f %f\n",j,
|
|
// cvx->planes[(index*4)+0],
|
|
// cvx->planes[(index*4)+1],
|
|
// cvx->planes[(index*4)+2],
|
|
// cvx->planes[(index*4)+3]);
|
|
// fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",j,
|
|
// eq2[0],
|
|
// eq2[1],
|
|
// eq2[2],
|
|
// eq2[3]);
|
|
// fprintf(stdout,"POS %f,%f,%f\n",
|
|
// cvx->final_posr->pos[0],
|
|
// cvx->final_posr->pos[1],
|
|
// cvx->final_posr->pos[2]);
|
|
// }
|
|
|
|
// Take The equation down a dimension
|
|
eq2[0]-=(cvx->planes[(index*4)+l]*aoveral[0]);
|
|
eq2[1]-=(cvx->planes[(index*4)+l]*aoveral[1]);
|
|
eq2[2]-=(cvx->planes[(index*4)+l]*aoveral[2]);
|
|
eq2[3]-=(cvx->planes[(index*4)+l]*aoveral[3]);
|
|
sum=
|
|
((eq2[0]*solution[0])+
|
|
(eq2[1]*solution[1])+
|
|
(eq2[2]*solution[2]))-eq2[3];
|
|
if(sum>0)
|
|
{
|
|
m=0;
|
|
for( k=0;k<3;++k)
|
|
{
|
|
if(fabs(eq2[k])>fabs(eq2[m]))
|
|
{
|
|
m=k;
|
|
}
|
|
}
|
|
if(eq2[m]==0)
|
|
{
|
|
/*
|
|
if(!hit) fprintf(stdout,
|
|
"Plane %d: %f %f %f %f is invalid\n",j,
|
|
eq2[0],eq2[1],eq2[2],eq2[3]);
|
|
*/
|
|
return false;
|
|
}
|
|
// then compute a/a[m] c1 and solution
|
|
aoveram[0]=eq2[0]/eq2[m];
|
|
aoveram[1]=eq2[1]/eq2[m];
|
|
aoveram[2]=eq2[2]/eq2[m];
|
|
aoveram[3]=eq2[3]/eq2[m];
|
|
c1[0]=c[0]-((c[m]/eq2[m])*eq2[0]);
|
|
c1[1]=c[1]-((c[m]/eq2[m])*eq2[1]);
|
|
c1[2]=c[2]-((c[m]/eq2[m])*eq2[2]);
|
|
solution[0]=solution[0]-((solution[m]/eq2[m])*eq2[0]);
|
|
solution[1]=solution[1]-((solution[m]/eq2[m])*eq2[1]);
|
|
solution[2]=solution[2]-((solution[m]/eq2[m])*eq2[2]);
|
|
// figure out the value for n by elimination
|
|
n = (l==0) ? ((m==1)? 2:1):((m==0)?((l==1)?2:1):0);
|
|
// iterate a to get the new equations with the help of a/a[l]
|
|
min=-dInfinity;
|
|
max=med=dInfinity;
|
|
for(k=0;k<planecount;++k)
|
|
{
|
|
if((i!=k)&&(j!=k))
|
|
{
|
|
cvx=GetPlaneIndex(cvx1,cvx2,k,index);
|
|
// Rotate
|
|
dMULTIPLY0_331(eq3,cvx->final_posr->R,&cvx->planes[(index*4)]);
|
|
// Translate
|
|
eq3[3]=(cvx->planes[(index*4)+3])+
|
|
((eq3[0] * cvx->final_posr->pos[0]) +
|
|
(eq3[1] * cvx->final_posr->pos[1]) +
|
|
(eq3[2] * cvx->final_posr->pos[2]));
|
|
// if(!hit)
|
|
// {
|
|
// fprintf(stdout,"Plane K %d: %f %f %f %f\n",k,
|
|
// cvx->planes[(index*4)+0],
|
|
// cvx->planes[(index*4)+1],
|
|
// cvx->planes[(index*4)+2],
|
|
// cvx->planes[(index*4)+3]);
|
|
// fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",k,
|
|
// eq3[0],
|
|
// eq3[1],
|
|
// eq3[2],
|
|
// eq3[3]);
|
|
// fprintf(stdout,"POS %f,%f,%f\n",
|
|
// cvx->final_posr->pos[0],
|
|
// cvx->final_posr->pos[1],
|
|
// cvx->final_posr->pos[2]);
|
|
// }
|
|
|
|
// Take the equation Down to 1D
|
|
eq3[0]-=(cvx->planes[(index*4)+m]*aoveram[0]);
|
|
eq3[1]-=(cvx->planes[(index*4)+m]*aoveram[1]);
|
|
eq3[2]-=(cvx->planes[(index*4)+m]*aoveram[2]);
|
|
eq3[3]-=(cvx->planes[(index*4)+m]*aoveram[3]);
|
|
if(eq3[n]>0)
|
|
{
|
|
max=dMIN(max,eq3[3]/eq3[n]);
|
|
}
|
|
else if(eq3[n]<0)
|
|
{
|
|
min=dMAX(min,eq3[3]/eq3[n]);
|
|
}
|
|
else
|
|
{
|
|
med=dMIN(med,eq3[3]);
|
|
}
|
|
}
|
|
}
|
|
if ((max<min)||(med<0))
|
|
{
|
|
// if(!hit) fprintf(stdout,"((max<min)||(med<0)) ((%f<%f)||(%f<0))",max,min,med);
|
|
if(!hit)
|
|
{
|
|
dumpplanes(cvx1);
|
|
dumpplanes(cvx2);
|
|
}
|
|
//fprintf(stdout,"Planes %d %d\n",i,j);
|
|
return false;
|
|
|
|
}
|
|
// find the value for the solution in one dimension
|
|
solution[n] = (c1[n]>=0) ? max:min;
|
|
// lift to 2D
|
|
solution[m] = (eq2[3]-(eq2[n]*solution[n]))/eq2[m];
|
|
// lift to 3D
|
|
solution[l] = (eq1[3]-(eq1[m]*solution[m]+
|
|
eq1[n]*solution[n]))/eq1[l];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*! \brief A Support mapping function for convex shapes
|
|
\param dir direction to find the Support Point for
|
|
\param cvx convex object to find the support point for
|
|
\param out the support mapping in dir.
|
|
*/
|
|
inline void Support(dVector3 dir,dxConvex& cvx,dVector3 out)
|
|
{
|
|
unsigned int index = 0;
|
|
dVector3 point;
|
|
dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);
|
|
point[0]+=cvx.final_posr->pos[0];
|
|
point[1]+=cvx.final_posr->pos[1];
|
|
point[2]+=cvx.final_posr->pos[2];
|
|
|
|
dReal max = dDOT(point,dir);
|
|
dReal tmp;
|
|
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];
|
|
tmp = dDOT(point, dir);
|
|
if (tmp > max)
|
|
{
|
|
out[0]=point[0];
|
|
out[1]=point[1];
|
|
out[2]=point[2];
|
|
max = tmp;
|
|
}
|
|
}
|
|
}
|
|
|
|
inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max)
|
|
{
|
|
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 = dDOT(axis,cvx.points);
|
|
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=dDOT(axis,point);
|
|
if(value<min)
|
|
{
|
|
min=value;
|
|
}
|
|
else if(value>max)
|
|
{
|
|
max=value;
|
|
}
|
|
}
|
|
//fprintf(stdout,"Compute Interval Min Max %f,%f\n",min,max);
|
|
}
|
|
|
|
/*! \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)
|
|
{
|
|
dVector4 plane,savedplane;
|
|
dReal min1,max1,min2,max2,min_depth=-dInfinity;
|
|
dVector3 e1,e2,t;
|
|
int maxc = flags & NUMC_MASK; // this is causing a segfault
|
|
//int maxc = 3;
|
|
int contacts=0;
|
|
dxConvex *g1,*g2;
|
|
unsigned int *pPoly;
|
|
dVector3 v;
|
|
// Test faces of cvx1 for separation
|
|
pPoly=cvx1.polygons;
|
|
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);
|
|
//fprintf(stdout,"width %f\n",max1-min1);
|
|
if(max2<min1 || max1<min2) return 0;
|
|
#if 1
|
|
// this one ON works
|
|
else if ((max1>min2)&&(max1<max2))
|
|
{
|
|
min_depth=max1-min2;
|
|
savedplane[0]=plane[0];
|
|
savedplane[1]=plane[1];
|
|
savedplane[2]=plane[2];
|
|
savedplane[3]=plane[3];
|
|
g1=&cvx2; // cvx2 moves
|
|
g2=&cvx1;
|
|
}
|
|
#endif
|
|
pPoly+=pPoly[0]+1;
|
|
}
|
|
// Test faces of cvx2 for separation
|
|
pPoly=cvx2.polygons;
|
|
for(unsigned int i=0;i<cvx2.planecount;++i)
|
|
{
|
|
// fprintf(stdout,"Poly verts %d\n",pPoly[0]);
|
|
// -- Apply Transforms --
|
|
// Rotate
|
|
dMULTIPLY0_331(plane,
|
|
cvx2.final_posr->R,
|
|
cvx2.planes+(i*4));
|
|
dNormalize3(plane);
|
|
// Translate
|
|
plane[3]=
|
|
(cvx2.planes[(i*4)+3])+
|
|
((plane[0] * cvx2.final_posr->pos[0]) +
|
|
(plane[1] * cvx2.final_posr->pos[1]) +
|
|
(plane[2] * cvx2.final_posr->pos[2]));
|
|
ComputeInterval(cvx2,plane,min1,max1);
|
|
ComputeInterval(cvx1,plane,min2,max2);
|
|
//fprintf(stdout,"width %f\n",max1-min1);
|
|
if(max2<min1 || max1<min2) return 0;
|
|
#if 1
|
|
// this one ON does not work
|
|
else if ((max1>min2)&&(max1<max2))
|
|
{
|
|
min_depth=max1-min2;
|
|
savedplane[0]=plane[0];
|
|
savedplane[1]=plane[1];
|
|
savedplane[2]=plane[2];
|
|
savedplane[3]=plane[3];
|
|
g1=&cvx1; // cvx1 moves
|
|
g2=&cvx2;
|
|
}
|
|
#endif
|
|
pPoly+=pPoly[0]+1;
|
|
}
|
|
// Test cross products of pairs of edges
|
|
for(std::set<edge>::iterator i = cvx1.edges.begin();
|
|
i!= cvx1.edges.end();
|
|
++i)
|
|
{
|
|
// we only need to apply rotation here
|
|
dMULTIPLY0_331 (t,cvx1.final_posr->R,cvx1.points+(i->first*3));
|
|
dMULTIPLY0_331 (e1,cvx1.final_posr->R,cvx1.points+(i->second*3));
|
|
e1[0]-=t[0];
|
|
e1[1]-=t[1];
|
|
e1[2]-=t[2];
|
|
for(std::set<edge>::iterator j = cvx2.edges.begin();
|
|
j!= cvx2.edges.end();
|
|
++j)
|
|
{
|
|
// we only need to apply rotation here
|
|
dMULTIPLY0_331 (t,cvx2.final_posr->R,cvx2.points+(j->first*3));
|
|
dMULTIPLY0_331 (e2,cvx2.final_posr->R,cvx2.points+(j->second*3));
|
|
e2[0]-=t[0];
|
|
e2[1]-=t[1];
|
|
e2[2]-=t[2];
|
|
dCROSS(plane,=,e1,e2);
|
|
plane[3]=0;
|
|
ComputeInterval(cvx1,plane,min1,max1);
|
|
ComputeInterval(cvx2,plane,min2,max2);
|
|
if(max2<min1 || max1 < min2) return 0;
|
|
}
|
|
}
|
|
// If we get here, there was a collision
|
|
static int cvxhit=0;
|
|
contacts=0;
|
|
if(cvxhit<2)
|
|
fprintf(stdout,"Plane: %f,%f,%f,%f\n",
|
|
savedplane[0],
|
|
savedplane[1],
|
|
savedplane[2],
|
|
savedplane[3]);
|
|
for(unsigned int i=0;i<g1->pointcount;++i)
|
|
{
|
|
if(contacts==maxc) break;
|
|
dMULTIPLY0_331 (v,g1->final_posr->R,&g1->points[(i*3)]);
|
|
v[0]=g1->final_posr->pos[0]+v[0];
|
|
v[1]=g1->final_posr->pos[1]+v[1];
|
|
v[2]=g1->final_posr->pos[2]+v[2];
|
|
dReal distance = ((savedplane[0] * v[0]) + // Ax +
|
|
(savedplane[1] * v[1]) + // Bx +
|
|
(savedplane[2] * v[2])) - savedplane[3]; // Cz + D
|
|
|
|
if((contacts<maxc)&&(distance<0))
|
|
{
|
|
CONTACT(contact,skip*contacts)->normal[0] = savedplane[0];
|
|
CONTACT(contact,skip*contacts)->normal[1] = savedplane[1];
|
|
CONTACT(contact,skip*contacts)->normal[2] = savedplane[2];
|
|
CONTACT(contact,skip*contacts)->pos[0]=v[0];
|
|
CONTACT(contact,skip*contacts)->pos[1]=v[1];
|
|
CONTACT(contact,skip*contacts)->pos[2]=v[2];
|
|
CONTACT(contact,skip*contacts)->depth = -distance;
|
|
CONTACT(contact,skip*contacts)->g1 = g1;
|
|
CONTACT(contact,skip*contacts)->g2 = g2;
|
|
if(cvxhit<2)
|
|
fprintf(stdout,"Contact: %f,%f,%f depth %f\n",
|
|
CONTACT(contact,skip*contacts)->pos[0],
|
|
CONTACT(contact,skip*contacts)->pos[1],
|
|
CONTACT(contact,skip*contacts)->pos[2],
|
|
CONTACT(contact,skip*contacts)->depth);
|
|
contacts++;
|
|
}
|
|
}
|
|
cvxhit++;
|
|
return contacts;
|
|
}
|
|
|
|
int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
|
|
dContactGeom *contact, int skip)
|
|
{
|
|
dIASSERT (o1->type == dConvexClass);
|
|
dIASSERT (o2->type == dConvexClass);
|
|
// if(!hit) fprintf(stdout,"dCollideConvexConvex\n");
|
|
dxConvex *Convex1 = (dxConvex*) o1;
|
|
dxConvex *Convex2 = (dxConvex*) o2;
|
|
int contacts;
|
|
if(contacts=TestConvexIntersection(*Convex1,*Convex2,flags,
|
|
contact,skip))
|
|
{
|
|
//fprintf(stdout,"We have a Hit!\n");
|
|
}
|
|
return contacts;
|
|
}
|
|
|
|
#if 0
|
|
|
|
int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags,
|
|
dContactGeom *contact, int skip)
|
|
{
|
|
dIASSERT( o1->type == dRayClass );
|
|
dIASSERT( o2->type == dConvexClass );
|
|
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,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;
|
|
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 );
|
|
dxRay* ray = (dxRay*) o1;
|
|
dxConvex* convex = (dxConvex*) o2;
|
|
|
|
contact->g1 = ray;
|
|
contact->g2 = convex;
|
|
|
|
dReal alpha, beta, nsign;
|
|
int flag;
|
|
|
|
//
|
|
// Compute some useful info
|
|
//
|
|
|
|
flag = 0; // Assume start point is behind all planes.
|
|
|
|
for ( unsigned int i = 0; i < convex->planecount; ++i )
|
|
{
|
|
// Alias this plane.
|
|
dReal* plane = convex->planes + ( i * 4 );
|
|
|
|
// If alpha >= 0 then start point is outside of plane.
|
|
alpha = dDOT( plane, ray->final_posr->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.
|
|
dReal* plane = convex->planes + ( i * 4 );
|
|
|
|
// If alpha >= 0 then point is outside of plane.
|
|
alpha = nsign * ( dDOT( plane, ray->final_posr->pos ) - plane[3] );
|
|
|
|
// Compute [ plane-normal DOT ray-normal ], (/flip)
|
|
beta = dDOT13( plane, ray->final_posr->R+2 ) * 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->final_posr->pos[0] + alpha * ray->final_posr->R[0*4+2];
|
|
contact->pos[1] = ray->final_posr->pos[1] + alpha * ray->final_posr->R[1*4+2];
|
|
contact->pos[2] = ray->final_posr->pos[2] + alpha * ray->final_posr->R[2*4+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.
|
|
dReal* planej = convex->planes + ( j * 4 );
|
|
|
|
// If beta >= 0 then start is outside of plane.
|
|
beta = dDOT( planej, contact->pos ) - plane[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;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Contact?
|
|
return ( contact->depth <= ray->length );
|
|
}
|
|
|
|
#endif
|
|
|
|
//<-- Convex Collision
|