[ODE] GeomTransform
Erwin de Vries
erwin at vo.com
Fri Jan 4 12:20:02 2002
This is a multi-part message in MIME format.
------=_NextPart_000_006D_01C1953B.B9C466F0
Content-Type: text/plain;
charset="iso-8859-1"
Content-Transfer-Encoding: 7bit
My suggested changes. I use these changes myself now, and it solved my
problem.
How to find the actual changes?
Find all the occurrences of //@@Erwin and everywhere where the GEOMGETBODY
macro is used.
I hope you are aware of the problem now. In case you're not; Try writing an
app which in the nearcallback gets you the body pointer of a box transformed
by a GeomTransform, in a GeomGroup connected to a body like described in the
docs.
Erwin
------=_NextPart_000_006D_01C1953B.B9C466F0
Content-Type: application/octet-stream;
name="geom.cpp"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="geom.cpp"
/************************************************************************=
*
* =
*
* Open Dynamics Engine, Copyright (C) 2001 Russell L. Smith. =
*
* 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 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. =
*
* =
*
* 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 GNU =
*
* Lesser General Public License for more details. =
*
* =
*
* You should have received a copy of the GNU Lesser General Public =
*
* License along with this library (see the file LICENSE.TXT); if not, =
*
* write to the Free Software Foundation, Inc., 59 Temple Place, =
*
* Suite 330, Boston, MA 02111-1307 USA. =
*
* =
*
=
*************************************************************************=
/
/*
the rule is that only the low level primitive collision functions should =
set
dContactGeom::g1 and dContactGeom::g2.
*/
#include <ode/common.h>
#include <ode/geom.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include <ode/memory.h>
#include <ode/misc.h>
#include <ode/objects.h>
#include <ode/matrix.h>
#include "objects.h"
#include "array.h"
#include "geom_internal.h"
//***********************************************************************=
*****
// collision utilities.
// given a pointer `p' to a dContactGeom, return the dContactGeom at
// p + skip bytes.
#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
#define GEOMGETBODY(g) (g->body) //@@Erwin
// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
// return 1, else return 0.
static int dCollideSpheres (dVector3 p1, dReal r1,
dVector3 p2, dReal r2, dContactGeom *c)
{
// printf ("d=3D%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=3D%.2f =
r2=3D%.2f\n",
// d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2);
dReal d =3D dDISTANCE (p1,p2);
if (d > (r1 + r2)) return 0;
if (d <=3D 0) {
c->pos[0] =3D p1[0];
c->pos[1] =3D p1[1];
c->pos[2] =3D p1[2];
c->normal[0] =3D 1;
c->normal[1] =3D 0;
c->normal[2] =3D 0;
c->depth =3D r1 + r2;
}
else {
dReal d1 =3D dRecip (d);
c->normal[0] =3D (p1[0]-p2[0])*d1;
c->normal[1] =3D (p1[1]-p2[1])*d1;
c->normal[2] =3D (p1[2]-p2[2])*d1;
dReal k =3D REAL(0.5) * (r2 - r1 - d);
c->pos[0] =3D p1[0] + c->normal[0]*k;
c->pos[1] =3D p1[1] + c->normal[1]*k;
c->pos[2] =3D p1[2] + c->normal[2]*k;
c->depth =3D r1 + r2 - d;
}
return 1;
}
// given two lines
// qa =3D pa + alpha* ua
// qb =3D pb + beta * ub
// where pa,pb are two points, ua,ub are two unit length vectors, and =
alpha,
// beta go from [-inf,inf], return alpha and beta such that qa and qb =
are
// as close as possible
static void lineClosestApproach (const dVector3 pa, const dVector3 ua,
const dVector3 pb, const dVector3 ub,
dReal *alpha, dReal *beta)
{
dVector3 p;
p[0] =3D pb[0] - pa[0];
p[1] =3D pb[1] - pa[1];
p[2] =3D pb[2] - pa[2];
dReal uaub =3D dDOT(ua,ub);
dReal q1 =3D dDOT(ua,p);
dReal q2 =3D -dDOT(ub,p);
dReal d =3D 1-uaub*uaub;
if (d <=3D 0) {
// @@@ this needs to be made more robust
*alpha =3D 0;
*beta =3D 0;
}
else {
d =3D dRecip(d);
*alpha =3D (q1 + uaub*q2)*d;
*beta =3D (uaub*q1 + q2)*d;
}
}
// given a box (R,side), `R' is the rotation matrix for the box, and =
`side'
// is a vector of x/y/z side lengths, return the size of the interval of =
the
// box projected along the given axis. if the axis has unit length then =
the
// return value will be the actual diameter, otherwise the result will =
be
// scaled by the axis length.
static inline dReal boxDiameter (const dMatrix3 R, const dVector3 side,
const dVector3 axis)
{
dVector3 q;
dMULTIPLY1_331 (q,R,axis); // transform axis to body-relative
return dFabs(q[0])*side[0] + dFabs(q[1])*side[1] + =
dFabs(q[2])*side[2];
}
// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they =
intersect
// or 0 if not.
int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1,
const dVector3 side1, const dVector3 p2,
const dMatrix3 R2, const dVector3 side2)
{
// two boxes are disjoint if (and only if) there is a separating axis
// perpendicular to a face from one box or perpendicular to an edge =
from
// either box. the following tests are derived from:
// "OBB Tree: A Hierarchical Structure for Rapid Interference =
Detection",
// S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996.
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2.
// Qij is abs(Rij)
dVector3 p,pp;
dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] =3D p2[0] - p1[0];
p[1] =3D p2[1] - p1[1];
p[2] =3D p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp =3D p relative to body 1
// get side lengths / 2
A1 =3D side1[0]*REAL(0.5); A2 =3D side1[1]*REAL(0.5); A3 =3D =
side1[2]*REAL(0.5);
B1 =3D side2[0]*REAL(0.5); B2 =3D side2[1]*REAL(0.5); B3 =3D =
side2[2]*REAL(0.5);
// for the following tests, excluding computation of Rij, in the worst =
case,
// 15 compares, 60 adds, 81 multiplies, and 24 absolutes.
// notation: R1=3D[u1 u2 u3], R2=3D[v1 v2 v3]
// separating axis =3D u1,u2,u3
R11 =3D dDOT44(R1+0,R2+0); R12 =3D dDOT44(R1+0,R2+1); R13 =3D =
dDOT44(R1+0,R2+2);
Q11 =3D dFabs(R11); Q12 =3D dFabs(R12); Q13 =3D dFabs(R13);
if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0;
R21 =3D dDOT44(R1+1,R2+0); R22 =3D dDOT44(R1+1,R2+1); R23 =3D =
dDOT44(R1+1,R2+2);
Q21 =3D dFabs(R21); Q22 =3D dFabs(R22); Q23 =3D dFabs(R23);
if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0;
R31 =3D dDOT44(R1+2,R2+0); R32 =3D dDOT44(R1+2,R2+1); R33 =3D =
dDOT44(R1+2,R2+2);
Q31 =3D dFabs(R31); Q32 =3D dFabs(R32); Q33 =3D dFabs(R33);
if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0;
// separating axis =3D v1,v2,v3
if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0;
if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0;
if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0;
// separating axis =3D u1 x (v1,v2,v3)
if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) =
return 0;
if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) =
return 0;
if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) =
return 0;
// separating axis =3D u2 x (v1,v2,v3)
if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) =
return 0;
if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) =
return 0;
if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) =
return 0;
// separating axis =3D u3 x (v1,v2,v3)
if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) =
return 0;
if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) =
return 0;
if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) =
return 0;
return 1;
}
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them =
together and
// generate contact points. this returns 0 if there is no contact =
otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `code' returns a number indicating the type of contact that was =
detected:
// 1,2,3 =3D box 2 intersects with a face of box 1
// 4,5,6 =3D box 1 intersects with a face of box 2
// 7..15 =3D edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, =
i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to =
the
// collision functions. this function only fills in the position and =
depth
// fields.
//
// @@@ some stuff to optimize here, reuse code in contact point =
calculations.
extern "C" int dBoxBox (const dVector3 p1, const dMatrix3 R1,
const dVector3 side1, const dVector3 p2,
const dMatrix3 R2, const dVector3 side2,
dVector3 normal, dReal *depth, int *code,
int maxc, dContactGeom *contact, int skip)
{
dVector3 p,pp,normalC;
const dReal *normalR =3D 0;
dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,invert_normal;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] =3D p2[0] - p1[0];
p[1] =3D p2[1] - p1[1];
p[2] =3D p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp =3D p relative to body 1
// get side lengths / 2
A1 =3D side1[0]*REAL(0.5); A2 =3D side1[1]*REAL(0.5); A3 =3D =
side1[2]*REAL(0.5);
B1 =3D side2[0]*REAL(0.5); B2 =3D side2[1]*REAL(0.5); B3 =3D =
side2[2]*REAL(0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 =3D dDOT44(R1+0,R2+0); R12 =3D dDOT44(R1+0,R2+1); R13 =3D =
dDOT44(R1+0,R2+2);
R21 =3D dDOT44(R1+1,R2+0); R22 =3D dDOT44(R1+1,R2+1); R23 =3D =
dDOT44(R1+1,R2+2);
R31 =3D dDOT44(R1+2,R2+0); R32 =3D dDOT44(R1+2,R2+1); R33 =3D =
dDOT44(R1+2,R2+2);
Q11 =3D dFabs(R11); Q12 =3D dFabs(R12); Q13 =3D dFabs(R13);
Q21 =3D dFabs(R21); Q22 =3D dFabs(R22); Q23 =3D dFabs(R23);
Q31 =3D dFabs(R31); Q32 =3D dFabs(R32); Q33 =3D dFabs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis =
(s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the =
smallest
// depth. note: normalR is set to point to a column of R1 or R2 if =
that is
// the smallest depth normal so far. otherwise normalR is 0 and =
normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign =
of
// the normal should be flipped.
#define TEST(expr1,expr2,norm,cc) \
s2 =3D dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s =3D s2; \
normalR =3D norm; \
invert_normal =3D ((expr1) < 0); \
*code =3D (cc); \
}
s =3D -dInfinity;
invert_normal =3D 0;
*code =3D 0;
// separating axis =3D u1,u2,u3
TEST (pp[0],(A1 + B1*Q11 + B2*Q12 + B3*Q13),R1+0,1);
TEST (pp[1],(A2 + B1*Q21 + B2*Q22 + B3*Q23),R1+1,2);
TEST (pp[2],(A3 + B1*Q31 + B2*Q32 + B3*Q33),R1+2,3);
// separating axis =3D v1,v2,v3
TEST (dDOT41(R2+0,p),(A1*Q11 + A2*Q21 + A3*Q31 + B1),R2+0,4);
TEST (dDOT41(R2+1,p),(A1*Q12 + A2*Q22 + A3*Q32 + B2),R2+1,5);
TEST (dDOT41(R2+2,p),(A1*Q13 + A2*Q23 + A3*Q33 + B3),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TEST
#define TEST(expr1,expr2,n1,n2,n3,cc) \
s2 =3D dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
l =3D dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /=3D l; \
if (s2 > s) { \
s =3D s2; \
normalR =3D 0; \
normalC[0] =3D (n1)/l; normalC[1] =3D (n2)/l; normalC[2] =3D =
(n3)/l; \
invert_normal =3D ((expr1) < 0); \
*code =3D (cc); \
} \
}
// separating axis =3D u1 x (v1,v2,v3)
TEST(pp[2]*R21-pp[1]*R31,(A2*Q31+A3*Q21+B2*Q13+B3*Q12),0,-R31,R21,7);
TEST(pp[2]*R22-pp[1]*R32,(A2*Q32+A3*Q22+B1*Q13+B3*Q11),0,-R32,R22,8);
TEST(pp[2]*R23-pp[1]*R33,(A2*Q33+A3*Q23+B1*Q12+B2*Q11),0,-R33,R23,9);
// separating axis =3D u2 x (v1,v2,v3)
TEST(pp[0]*R31-pp[2]*R11,(A1*Q31+A3*Q11+B2*Q23+B3*Q22),R31,0,-R11,10);
TEST(pp[0]*R32-pp[2]*R12,(A1*Q32+A3*Q12+B1*Q23+B3*Q21),R32,0,-R12,11);
TEST(pp[0]*R33-pp[2]*R13,(A1*Q33+A3*Q13+B1*Q22+B2*Q21),R33,0,-R13,12);
// separating axis =3D u3 x (v1,v2,v3)
TEST(pp[1]*R11-pp[0]*R21,(A1*Q21+A2*Q11+B2*Q33+B3*Q32),-R21,R11,0,13);
TEST(pp[1]*R12-pp[0]*R22,(A1*Q22+A2*Q12+B1*Q33+B3*Q31),-R22,R12,0,14);
TEST(pp[1]*R13-pp[0]*R23,(A1*Q23+A2*Q13+B1*Q32+B2*Q31),-R23,R13,0,15);
#undef TEST
// if we get to this point, the boxes interpenetrate. compute the =
normal
// in global coordinates.
if (normalR) {
normal[0] =3D normalR[0];
normal[1] =3D normalR[4];
normal[2] =3D normalR[8];
}
else {
dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal[0] =3D -normal[0];
normal[1] =3D -normal[1];
normal[2] =3D -normal[2];
}
*depth =3D -s;
// compute contact point(s)
if (*code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
dVector3 pa;
dReal sign;
for (i=3D0; i<3; i++) pa[i] =3D p1[i];
sign =3D (dDOT14(normal,R1+0) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=3D0; i<3; i++) pa[i] +=3D sign * A1 * R1[i*4];
sign =3D (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=3D0; i<3; i++) pa[i] +=3D sign * A2 * R1[i*4+1];
sign =3D (dDOT14(normal,R1+2) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=3D0; i<3; i++) pa[i] +=3D sign * A3 * R1[i*4+2];
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=3D0; i<3; i++) pb[i] =3D p2[i];
sign =3D (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=3D0; i<3; i++) pb[i] +=3D sign * B1 * R2[i*4];
sign =3D (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=3D0; i<3; i++) pb[i] +=3D sign * B2 * R2[i*4+1];
sign =3D (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=3D0; i<3; i++) pb[i] +=3D sign * B3 * R2[i*4+2];
dReal alpha,beta;
dVector3 ua,ub;
for (i=3D0; i<3; i++) ua[i] =3D R1[((*code)-7)/3 + i*4];
for (i=3D0; i<3; i++) ub[i] =3D R2[((*code)-7)%3 + i*4];
lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=3D0; i<3; i++) pa[i] +=3D ua[i]*alpha;
for (i=3D0; i<3; i++) pb[i] +=3D ub[i]*beta;
for (i=3D0; i<3; i++) contact[0].pos[i] =3D REAL(0.5)*(pa[i]+pb[i]);
contact[0].depth =3D *depth;
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face).
// @@@ temporary: make deepest vertex on the "other" box the contact =
point.
// @@@ this kind of works, but we need multiple contact points for =
stability,
// @@@ especially for face-face contact.
dVector3 vertex;
if (*code <=3D 3) {
// face from box 1 touches a vertex/edge/face from box 2.
dReal sign;
for (i=3D0; i<3; i++) vertex[i] =3D p2[i];
sign =3D (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=3D0; i<3; i++) vertex[i] +=3D sign * B1 * R2[i*4];
sign =3D (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=3D0; i<3; i++) vertex[i] +=3D sign * B2 * R2[i*4+1];
sign =3D (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=3D0; i<3; i++) vertex[i] +=3D sign * B3 * R2[i*4+2];
}
else {
// face from box 2 touches a vertex/edge/face from box 1.
dReal sign;
for (i=3D0; i<3; i++) vertex[i] =3D p1[i];
sign =3D (dDOT14(normal,R1+0) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=3D0; i<3; i++) vertex[i] +=3D sign * A1 * R1[i*4];
sign =3D (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=3D0; i<3; i++) vertex[i] +=3D sign * A2 * R1[i*4+1];
sign =3D (dDOT14(normal,R1+2) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=3D0; i<3; i++) vertex[i] +=3D sign * A3 * R1[i*4+2];
}
for (i=3D0; i<3; i++) contact[0].pos[i] =3D vertex[i];
contact[0].depth =3D *depth;
return 1;
}
//***********************************************************************=
*****
// general support for geometry objects and classes
struct dColliderEntry {
dColliderFn *fn; // collider function
int mode; // 1 =3D reverse o1 and o2, 2 =3D no function available
};
static dArray<dxGeomClass*> *classes=3D0;
// function pointers and modes for n^2 class collider functions. this is =
an
// n*n matrix stored by row. the functions pointers are extracted from =
the
// class get-collider-function function.
static dArray<dColliderEntry> *colliders=3D0;
static inline void initCollisionArrays()
{
if (classes=3D=3D0) {
// old way:
// classes =3D (dArray<dxGeomClass*> *) dAllocNoFree =
(sizeof(dArrayBase));
// classes->constructor();
classes =3D new dArray<dxGeomClass*>;
classes->setSize (1); // force allocation of array data memory
dAllocDontReport (classes);
dAllocDontReport (classes->data());
classes->setSize (0);
}
if (colliders=3D=3D0) {
// old way:
// colliders=3D(dArray<dColliderEntry> *)dAllocNoFree =
(sizeof(dArrayBase));
// colliders->constructor();
colliders =3D new dArray<dColliderEntry>;
colliders->setSize (1); // force allocation of array data memory
dAllocDontReport (colliders);
dAllocDontReport (colliders->data());
colliders->setSize (0);
}
}
int dCreateGeomClass (const dGeomClass *c)
{
dUASSERT(c && c->bytes >=3D 0 && c->collider && c->aabb,"bad geom =
class");
initCollisionArrays();
int n =3D classes->size();
dxGeomClass *gc =3D (dxGeomClass*) dAlloc (sizeof(dxGeomClass));
dAllocDontReport (gc);
gc->collider =3D c->collider;
gc->aabb =3D c->aabb;
gc->aabb_test =3D c->aabb_test;
gc->dtor =3D c->dtor;
gc->num =3D n;
gc->size =3D SIZEOF_DXGEOM + c->bytes;
classes->push (gc);
// make room for n^2 class collider function pointers - these entries =
will
// be filled as dCollide() is called.
colliders->setSize ((n+1)*(n+1));
memset (colliders->data(),0,(n+1)*(n+1)*sizeof(dColliderEntry));
return n;
}
int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,
int skip)
{
int i,c1,c2,a1,a2,count,swap;
dColliderFn *fn;
dAASSERT(o1 && o2 && contact);
dUASSERT(classes && colliders,"no registered geometry classes");
// no contacts if both geoms on the same body, and the body is not 0
if (o1->body =3D=3D o2->body && o1->body) return 0;
dColliderEntry *colliders2 =3D colliders->data();
c1 =3D o1->_class->num;
c2 =3D o2->_class->num;
a1 =3D c1 * classes->size() + c2; // address 1 in collider array
a2 =3D c2 * classes->size() + c1; // address 2 in collider array
swap =3D 0; // set to 1 to swap normals before returning
// return if there are no collider functions available
if ((colliders2[a1].mode=3D=3D2) || (colliders2[a2].mode=3D=3D2)) =
return 0;
if ((fn =3D colliders2[a1].fn)) {
swap =3D colliders2[a1].mode;
if (swap) count =3D (*fn) (o2,o1,flags,contact,skip);
else count =3D (*fn) (o1,o2,flags,contact,skip);
}
else if ((fn =3D (*classes)[c1]->collider (c2))) {
colliders2 [a2].fn =3D fn;
colliders2 [a2].mode =3D 1;
colliders2 [a1].fn =3D fn; // do mode=3D0 assignment second so that
colliders2 [a1].mode =3D 0; // diagonal entries will have mode 0
count =3D (*fn) (o1,o2,flags,contact,skip);
swap =3D 0;
}
else if ((fn =3D (*classes)[c2]->collider (c1))) {
colliders2 [a1].fn =3D fn;
colliders2 [a1].mode =3D 1;
colliders2 [a2].fn =3D fn; // do mode=3D0 assignment second so that
colliders2 [a2].mode =3D 0; // diagonal entries will have mode 0
count =3D (*fn) (o2,o1,flags,contact,skip);
swap =3D 1;
}
else {
colliders2[a1].mode =3D 2;
colliders2[a2].mode =3D 2;
return 0;
}
if (swap) {
for (i=3D0; i<count; i++) {
dContactGeom *c =3D CONTACT(contact,skip*i);
c->normal[0] =3D -c->normal[0];
c->normal[1] =3D -c->normal[1];
c->normal[2] =3D -c->normal[2];
dxGeom *tmpg =3D c->g1; //@@ErwinSTART
c->g1 =3D c->g2;
c->g2 =3D tmpg;
dxBody* tmpb =3D c->b1;
c->b1 =3D c->b2;
c->b2 =3D tmpb; //@@ErwinEND
}
}
return count;
}
int dGeomGetClass (dxGeom *g)
{
dAASSERT (g);
return g->_class->num;
}
void dGeomSetData (dxGeom *g, void *data)
{
dAASSERT (g);
g->data =3D data;
}
void *dGeomGetData (dxGeom *g)
{
dAASSERT (g);
return g->data;
}
void dGeomSetBody (dxGeom *g, dBodyID b)
{
dAASSERT (g);
if (b) {
if (!g->body) dFree (g->pos,sizeof(dxPosR));
g->body =3D b;
g->pos =3D b->pos;
g->R =3D b->R;
}
else {
if (g->body) {
g->body =3D 0;
dxPosR *pr =3D (dxPosR*) dAlloc (sizeof(dxPosR));
g->pos =3D pr->pos;
g->R =3D pr->R;
}
}
}
dBodyID dGeomGetBody (dxGeom *g)
{
dAASSERT (g);
return g->body;
}
void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
dAASSERT (g);
if (g->body) dBodySetPosition (g->body,x,y,z);
else {
g->pos[0] =3D x;
g->pos[1] =3D y;
g->pos[2] =3D z;
}
}
void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
{
dAASSERT (g);
if (g->body) dBodySetRotation (g->body,R);
else memcpy (g->R,R,sizeof(dMatrix3));
}
const dReal * dGeomGetPosition (dxGeom *g)
{
dAASSERT (g);
return g->pos;
}
const dReal * dGeomGetRotation (dxGeom *g)
{
dAASSERT (g);
return g->R;
}
// for external use only. use the CLASSDATA macro inside ODE.
void * dGeomGetClassData (dxGeom *g)
{
dAASSERT (g);
return (void*) CLASSDATA(g);
}
dxGeom * dCreateGeom (int classnum)
{
dUASSERT (classes && colliders && classnum >=3D 0 &&
classnum < classes->size(),"bad class number");
int size =3D (*classes)[classnum]->size;
dxGeom *geom =3D (dxGeom*) dAlloc (size);
memset (geom,0,size); // everything is initially zeroed
geom->_class =3D (*classes)[classnum];
geom->data =3D 0;
geom->body =3D 0;
dxPosR *pr =3D (dxPosR*) dAlloc (sizeof(dxPosR));
geom->pos =3D pr->pos;
geom->R =3D pr->R;
dSetZero (geom->pos,4);
dRSetIdentity (geom->R);
return geom;
}
void dGeomDestroy (dxGeom *g)
{
dAASSERT (g);
if (g->spaceid) dSpaceRemove (g->spaceid,g);
if (g->_class->dtor) g->_class->dtor (g);
if (!g->body) dFree (g->pos,sizeof(dxPosR));
dFree (g,g->_class->size);
}
void dGeomGetAABB (dxGeom *g, dReal aabb[6])
{
dAASSERT (g);
g->_class->aabb (g,aabb);
}
dReal *dGeomGetSpaceAABB (dxGeom *g)
{
dAASSERT (g);
return g->space_aabb;
}
//***********************************************************************=
*****
// data for the standard classes
struct dxSphere {
dReal radius; // sphere radius
};
struct dxBox {
dVector3 side; // side lengths (x,y,z)
};
struct dxCCylinder { // capped cylinder
dReal radius,lz; // radius, length along z axis */
};
struct dxPlane {
dReal p[4];
};
struct dxGeomGroup {
dArray<dxGeom*> parts; // all the geoms that make up the group
};
//***********************************************************************=
*****
// primitive collision functions
// same interface as dCollide().
// S=3Dsphere, B=3Dbox, C=3Dcapped cylinder, P=3Dplane, G=3Dgroup, =
T=3Dtransform
int dCollideSS (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >=3D (int)sizeof(dContactGeom));
dIASSERT (o1->_class->num =3D=3D dSphereClass);
dIASSERT (o2->_class->num =3D=3D dSphereClass);
dxSphere *s1 =3D (dxSphere*) CLASSDATA(o1);
dxSphere *s2 =3D (dxSphere*) CLASSDATA(o2);
contact->g1 =3D const_cast<dxGeom*> (o1);
contact->g2 =3D const_cast<dxGeom*> (o2);
contact->b1 =3D GEOMGETBODY(o1);
contact->b2 =3D GEOMGETBODY(o2);
return dCollideSpheres (o1->pos,s1->radius,
o2->pos,s2->radius,contact);
}
int dCollideSB (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
// this is easy. get the sphere center `p' relative to the box, and =
then clip
// that to the boundary of the box (call that point `q'). if q is on =
the
// boundary of the box and |p-q| is <=3D sphere radius, they touch.
// if q is inside the box, the sphere is inside the box, so set a =
contact
// normal to push the sphere to the closest box edge.
dVector3 l,t,p,q,r;
dReal depth;
int onborder =3D 0;
dIASSERT (skip >=3D (int)sizeof(dContactGeom));
dIASSERT (o1->_class->num =3D=3D dSphereClass);
dIASSERT (o2->_class->num =3D=3D dBoxClass);
dxSphere *sphere =3D (dxSphere*) CLASSDATA(o1);
dxBox *box =3D (dxBox*) CLASSDATA(o2);
contact->g1 =3D const_cast<dxGeom*> (o1);
contact->g2 =3D const_cast<dxGeom*> (o2);
contact->b1 =3D GEOMGETBODY(o1);
contact->b2 =3D GEOMGETBODY(o2);
p[0] =3D o1->pos[0] - o2->pos[0];
p[1] =3D o1->pos[1] - o2->pos[1];
p[2] =3D o1->pos[2] - o2->pos[2];
l[0] =3D box->side[0]*REAL(0.5);
t[0] =3D dDOT14(p,o2->R);
if (t[0] < -l[0]) { t[0] =3D -l[0]; onborder =3D 1; }
if (t[0] > l[0]) { t[0] =3D l[0]; onborder =3D 1; }
l[1] =3D box->side[1]*REAL(0.5);
t[1] =3D dDOT14(p,o2->R+1);
if (t[1] < -l[1]) { t[1] =3D -l[1]; onborder =3D 1; }
if (t[1] > l[1]) { t[1] =3D l[1]; onborder =3D 1; }
t[2] =3D dDOT14(p,o2->R+2);
l[2] =3D box->side[2]*REAL(0.5);
if (t[2] < -l[2]) { t[2] =3D -l[2]; onborder =3D 1; }
if (t[2] > l[2]) { t[2] =3D l[2]; onborder =3D 1; }
if (!onborder) {
// sphere center inside box. find largest `t' value
dReal max =3D dFabs(t[0]);
int maxi =3D 0;
for (int i=3D1; i<3; i++) {
dReal tt =3D dFabs(t[i]);
if (tt > max) {
max =3D tt;
maxi =3D i;
}
}
// contact position =3D sphere center
contact->pos[0] =3D o1->pos[0];
contact->pos[1] =3D o1->pos[1];
contact->pos[2] =3D o1->pos[2];
// contact normal aligned with box edge along largest `t' value
dVector3 tmp;
tmp[0] =3D 0;
tmp[1] =3D 0;
tmp[2] =3D 0;
tmp[maxi] =3D (t[maxi] > 0) ? REAL(1.0) : REAL(-1.0);
dMULTIPLY0_331 (contact->normal,o2->R,tmp);
// contact depth =3D distance to wall along normal plus radius
contact->depth =3D l[maxi] - max + sphere->radius;
return 1;
}
t[3] =3D 0; //@@@ hmmm
dMULTIPLY0_331 (q,o2->R,t);
r[0] =3D p[0] - q[0];
r[1] =3D p[1] - q[1];
r[2] =3D p[2] - q[2];
depth =3D sphere->radius - dSqrt(dDOT(r,r));
if (depth < 0) return 0;
contact->pos[0] =3D q[0] + o2->pos[0];
contact->pos[1] =3D q[1] + o2->pos[1];
contact->pos[2] =3D q[2] + o2->pos[2];
contact->normal[0] =3D r[0];
contact->normal[1] =3D r[1];
contact->normal[2] =3D r[2];
dNormalize3 (contact->normal);
contact->depth =3D depth;
return 1;
}
int dCollideSP (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >=3D (int)sizeof(dContactGeom));
dIASSERT (o1->_class->num =3D=3D dSphereClass);
dIASSERT (o2->_class->num =3D=3D dPlaneClass);
contact->g1 =3D const_cast<dxGeom*> (o1);
contact->g2 =3D const_cast<dxGeom*> (o2);
contact->b1 =3D GEOMGETBODY(o1);
contact->b2 =3D GEOMGETBODY(o2);
dxSphere *sphere =3D (dxSphere*) CLASSDATA(o1);
dxPlane *plane =3D (dxPlane*) CLASSDATA(o2);
dReal k =3D dDOT (o1->pos,plane->p);
dReal depth =3D plane->p[3] - k + sphere->radius;
if (depth >=3D 0) {
contact->normal[0] =3D plane->p[0];
contact->normal[1] =3D plane->p[1];
contact->normal[2] =3D plane->p[2];
contact->pos[0] =3D o1->pos[0] - plane->p[0] * sphere->radius;
contact->pos[1] =3D o1->pos[1] - plane->p[1] * sphere->radius;
contact->pos[2] =3D o1->pos[2] - plane->p[2] * sphere->radius;
contact->depth =3D depth;
return 1;
}
else return 0;
}
int dCollideBB (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dVector3 normal;
dReal depth;
int code;
dxBox *b1 =3D (dxBox*) CLASSDATA(o1);
dxBox *b2 =3D (dxBox*) CLASSDATA(o2);
int num =3D dBoxBox (o1->pos,o1->R,b1->side, o2->pos,o2->R,b2->side,
normal,&depth,&code,flags & NUMC_MASK,contact,skip);
for (int i=3D0; i<num; i++) {
CONTACT(contact,i*skip)->normal[0] =3D -normal[0];
CONTACT(contact,i*skip)->normal[1] =3D -normal[1];
CONTACT(contact,i*skip)->normal[2] =3D -normal[2];
CONTACT(contact,i*skip)->g1 =3D const_cast<dxGeom*> (o1);
CONTACT(contact,i*skip)->g2 =3D const_cast<dxGeom*> (o2);
CONTACT(contact,i*skip)->b1 =3D GEOMGETBODY(o1);
CONTACT(contact,i*skip)->b2 =3D GEOMGETBODY(o2);
}
return num;
}
int dCollideBP (const dxGeom *o1, const dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >=3D (int)sizeof(dContactGeom));
dIASSERT (o1->_class->num =3D=3D dBoxClass);
//dIASSERT (o2->_class->num =3D=3D dPlaneClass);
contact->g1 =3D const_cast<dxGeom*> (o1);
contact->g2 =3D const_cast<dxGeom*> (o2);
contact->b1 =3D GEOMGETBODY(o1);
contact->b2 =3D GEOMGETBODY(o2);
dxBox *box =3D (dxBox*) CLASSDATA(o1);
dxPlane *plane =3D (dxPlane*) CLASSDATA(o2);
int ret =3D 0;
//@@@ problem: using 4-vector (plane->p) as 3-vector (normal).
const dReal *R =3D o1->R; // rotation of box
const dReal *n =3D plane->p; // normal vector
// project sides lengths along normal vector, get absolute values
dReal Q1 =3D dDOT14(n,R+0);
dReal Q2 =3D dDOT14(n,R+1);
dReal Q3 =3D dDOT14(n,R+2);
dReal A1 =3D box->side[0] * Q1;
dReal A2 =3D box->side[1] * Q2;
dReal A3 =3D box->side[2] * Q3;
dReal B1 =3D dFabs(A1);
dReal B2 =3D dFabs(A2);
dReal B3 =3D dFabs(A3);
// early exit test
dReal depth =3D plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->pos);
if (depth < 0) return 0;
// find number of contacts requested
int maxc =3D flags & NUMC_MASK;
if (maxc < 1) maxc =3D 1;
if (maxc > 3) maxc =3D 3; // no more than 3 contacts per box allowed
// find deepest point
dVector3 p;
p[0] =3D o1->pos[0];
p[1] =3D o1->pos[1];
p[2] =3D o1->pos[2];
#define FOO(i,op) \
p[0] op REAL(0.5)*box->side[i] * R[0+i]; \
p[1] op REAL(0.5)*box->side[i] * R[4+i]; \
p[2] op REAL(0.5)*box->side[i] * R[8+i];
#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=3D) } else { =
FOO(i,+=3D) }
BAR(0,1);
BAR(1,2);
BAR(2,3);
#undef FOO
#undef BAR
// the deepest point is the first contact point
contact->pos[0] =3D p[0];
contact->pos[1] =3D p[1];
contact->pos[2] =3D p[2];
contact->normal[0] =3D n[0];
contact->normal[1] =3D n[1];
contact->normal[2] =3D n[2];
contact->depth =3D depth;
ret =3D 1; // ret is number of contact points found so far
if (maxc =3D=3D 1) goto done;
// get the second and third contact points by starting from `p' and =
going
// along the two sides with the smallest projected length.
#define FOO(i,j,op) \
CONTACT(contact,i*skip)->pos[0] =3D p[0] op box->side[j] * R[0+j]; \
CONTACT(contact,i*skip)->pos[1] =3D p[1] op box->side[j] * R[4+j]; \
CONTACT(contact,i*skip)->pos[2] =3D p[2] op box->side[j] * R[8+j];
#define BAR(ctact,side,sideinc) \
depth -=3D B ## sideinc; \
if (depth < 0) goto done; \
if (A ## sideinc > 0) { FOO(ctact,side,+) } else { FOO(ctact,side,-) } =
\
CONTACT(contact,ctact*skip)->depth =3D depth; \
ret++;
CONTACT(contact,skip)->normal[0] =3D n[0];
CONTACT(contact,skip)->normal[1] =3D n[1];
CONTACT(contact,skip)->normal[2] =3D n[2];
if (maxc =3D=3D 3) {
CONTACT(contact,2*skip)->normal[0] =3D n[0];
CONTACT(contact,2*skip)->normal[1] =3D n[1];
CONTACT(contact,2*skip)->normal[2] =3D n[2];
}
if (B1 < B2) {
if (B3 < B1) goto use_side_3; else {
BAR(1,0,1); // use side 1
if (maxc =3D=3D 2) goto done;
if (B2 < B3) goto contact2_2; else goto contact2_3;
}
}
else {
if (B3 < B2) {
use_side_3: // use side 3
BAR(1,2,3);
if (maxc =3D=3D 2) goto done;
if (B1 < B2) goto contact2_1; else goto contact2_2;
}
else {
BAR(1,1,2); // use side 2
if (maxc =3D=3D 2) goto done;
if (B1 < B3) goto contact2_1; else goto contact2_3;
}
}
contact2_1: BAR(2,0,1); goto done;
contact2_2: BAR(2,1,2); goto done;
contact2_3: BAR(2,2,3); goto done;
#undef FOO
#undef BAR
done:
for (int i=3D0; i<ret; i++) {
CONTACT(contact,i*skip)->g1 =3D const_cast<dxGeom*> (o1);
CONTACT(contact,i*skip)->g2 =3D const_cast<dxGeom*> (o2);
CONTACT(contact,i*skip)->b1 =3D GEOMGETBODY(o1);
CONTACT(contact,i*skip)->b2 =3D GEOMGETBODY(o2);
}
return ret;
}
int dCollideCS (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >=3D (int)sizeof(dContactGeom));
dIASSERT (o1->_class->num =3D=3D dCCylinderClass);
dIASSERT (o2->_class->num =3D=3D dSphereClass);
contact->g1 =3D const_cast<dxGeom*> (o1);
contact->g2 =3D const_cast<dxGeom*> (o2);
contact->b1 =3D GEOMGETBODY(o1);
contact->b2 =3D GEOMGETBODY(o2);
dxCCylinder *ccyl =3D (dxCCylinder*) CLASSDATA(o1);
dxSphere *sphere =3D (dxSphere*) CLASSDATA(o2);
// find the point on the cylinder axis that is closest to the sphere
dReal alpha =3D=20
o1->R[2] * (o2->pos[0] - o1->pos[0]) +
o1->R[6] * (o2->pos[1] - o1->pos[1]) +
o1->R[10] * (o2->pos[2] - o1->pos[2]);
dReal lz2 =3D ccyl->lz * REAL(0.5);
if (alpha > lz2) alpha =3D lz2;
if (alpha < -lz2) alpha =3D -lz2;
// collide the spheres
dVector3 p;
p[0] =3D o1->pos[0] + alpha * o1->R[2];
p[1] =3D o1->pos[1] + alpha * o1->R[6];
p[2] =3D o1->pos[2] + alpha * o1->R[10];
return dCollideSpheres =
(p,ccyl->radius,o2->pos,sphere->radius,contact);
}
int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dDebug (0,"unimplemented");
return 0;
}
// this returns at most one contact point when the two cylinder's axes =
are not
// aligned, and at most two (for stability) when they are aligned.
// the algorithm minimizes the distance between two "sample spheres" =
that are
// positioned along the cylinder axes according to:
// sphere1 =3D pos1 + alpha1 * axis1
// sphere2 =3D pos2 + alpha2 * axis2
// alpha1 and alpha2 are limited to +/- half the length of the =
cylinders.
// the algorithm works by finding a solution that has both alphas free, =
or
// a solution that has one or both alphas fixed to the ends of the =
cylinder.
int dCollideCC (const dxGeom *o1, const dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
int i;
const dReal tolerance =3D REAL(1e-5);
dIASSERT (skip >=3D (int)sizeof(dContactGeom));
dIASSERT (o1->_class->num =3D=3D dCCylinderClass);
dIASSERT (o2->_class->num =3D=3D dCCylinderClass);
contact->g1 =3D const_cast<dxGeom*> (o1);
contact->g2 =3D const_cast<dxGeom*> (o2);
contact->b1 =3D GEOMGETBODY(o1);
contact->b2 =3D GEOMGETBODY(o2);
dxCCylinder *cyl1 =3D (dxCCylinder*) CLASSDATA(o1);
dxCCylinder *cyl2 =3D (dxCCylinder*) CLASSDATA(o2);
// copy out some variables, for convenience
dReal lz1 =3D cyl1->lz * REAL(0.5);
dReal lz2 =3D cyl2->lz * REAL(0.5);
dReal *pos1 =3D o1->pos;
dReal *pos2 =3D o2->pos;
dReal axis1[3],axis2[3];
axis1[0] =3D o1->R[2];
axis1[1] =3D o1->R[6];
axis1[2] =3D o1->R[10];
axis2[0] =3D o2->R[2];
axis2[1] =3D o2->R[6];
axis2[2] =3D o2->R[10];
dReal alpha1,alpha2,sphere1[3],sphere2[3];
int fix1 =3D 0; // 0 if alpha1 is free, +/-1 to fix at +/- lz1
int fix2 =3D 0; // 0 if alpha2 is free, +/-1 to fix at +/- lz2
for (int count=3D0; count<9; count++) {
// find a trial solution by fixing or not fixing the alphas
if (fix1) {
if (fix2) {
// alpha1 and alpha2 are fixed, so the solution is easy
if (fix1 > 0) alpha1 =3D lz1; else alpha1 =3D -lz1;
if (fix2 > 0) alpha2 =3D lz2; else alpha2 =3D -lz2;
for (i=3D0; i<3; i++) sphere1[i] =3D pos1[i] + alpha1*axis1[i];
for (i=3D0; i<3; i++) sphere2[i] =3D pos2[i] + alpha2*axis2[i];
}
else {
// fix alpha1 but let alpha2 be free
if (fix1 > 0) alpha1 =3D lz1; else alpha1 =3D -lz1;
for (i=3D0; i<3; i++) sphere1[i] =3D pos1[i] + alpha1*axis1[i];
alpha2 =3D (axis2[0]*(sphere1[0]-pos2[0]) +
axis2[1]*(sphere1[1]-pos2[1]) +
axis2[2]*(sphere1[2]-pos2[2]));
for (i=3D0; i<3; i++) sphere2[i] =3D pos2[i] + alpha2*axis2[i];
}
}
else {
if (fix2) {
// fix alpha2 but let alpha1 be free
if (fix2 > 0) alpha2 =3D lz2; else alpha2 =3D -lz2;
for (i=3D0; i<3; i++) sphere2[i] =3D pos2[i] + alpha2*axis2[i];
alpha1 =3D (axis1[0]*(sphere2[0]-pos1[0]) +
axis1[1]*(sphere2[1]-pos1[1]) +
axis1[2]*(sphere2[2]-pos1[2]));
for (i=3D0; i<3; i++) sphere1[i] =3D pos1[i] + alpha1*axis1[i];
}
else {
// let alpha1 and alpha2 be free
// compute determinant of d(d^2)\d(alpha) jacobian
dReal a1a2 =3D dDOT (axis1,axis2);
dReal det =3D REAL(1.0)-a1a2*a1a2;
if (det < tolerance) {
// the cylinder axes (almost) parallel, so we will generate up to two
// contacts. the solution matrix is rank deficient so alpha1 and
// alpha2 are related by:
// alpha2 =3D alpha1 + (pos1-pos2)'*axis1 (if =
axis1=3D=3Daxis2)
// or alpha2 =3D -(alpha1 + (pos1-pos2)'*axis1) (if =
axis1=3D=3D-axis2)
// first compute where the two cylinders overlap in alpha1 space:
if (a1a2 < 0) {
axis2[0] =3D -axis2[0];
axis2[1] =3D -axis2[1];
axis2[2] =3D -axis2[2];
}
dReal q[3];
for (i=3D0; i<3; i++) q[i] =3D pos1[i]-pos2[i];
dReal k =3D dDOT (axis1,q);
dReal a1lo =3D -lz1;
dReal a1hi =3D lz1;
dReal a2lo =3D -lz2 - k;
dReal a2hi =3D lz2 - k;
dReal lo =3D (a1lo > a2lo) ? a1lo : a2lo;
dReal hi =3D (a1hi < a2hi) ? a1hi : a2hi;
if (lo <=3D hi) {
int num_contacts =3D flags & NUMC_MASK;
if (num_contacts >=3D 2 && lo < hi) {
// generate up to two contacts. if one of those contacts is
// not made, fall back on the one-contact strategy.
for (i=3D0; i<3; i++) sphere1[i] =3D pos1[i] + lo*axis1[i];
for (i=3D0; i<3; i++) sphere2[i] =3D pos2[i] + (lo+k)*axis2[i];
int n1 =3D dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
if (n1) {
for (i=3D0; i<3; i++) sphere1[i] =3D pos1[i] + hi*axis1[i];
for (i=3D0; i<3; i++) sphere2[i] =3D pos2[i] + (hi+k)*axis2[i];
dContactGeom *c2 =3D CONTACT(contact,skip);
int n2 =3D dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius, c2);
if (n2) {
c2->g1 =3D const_cast<dxGeom*> (o1);
c2->g2 =3D const_cast<dxGeom*> (o2);
c2->b1 =3D GEOMGETBODY(o1);
c2->b2 =3D GEOMGETBODY(o2);
return 2;
}
}
}
// just one contact to generate, so put it in the middle of
// the range
alpha1 =3D (lo + hi) * REAL(0.5);
alpha2 =3D alpha1 + k;
for (i=3D0; i<3; i++) sphere1[i] =3D pos1[i] + alpha1*axis1[i];
for (i=3D0; i<3; i++) sphere2[i] =3D pos2[i] + alpha2*axis2[i];
return dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
}
else return 0;
}
det =3D REAL(1.0)/det;
dReal delta[3];
for (i=3D0; i<3; i++) delta[i] =3D pos1[i] - pos2[i];
dReal q1 =3D dDOT (delta,axis1);
dReal q2 =3D dDOT (delta,axis2);
alpha1 =3D det*(a1a2*q2-q1);
alpha2 =3D det*(q2-a1a2*q1);
for (i=3D0; i<3; i++) sphere1[i] =3D pos1[i] + alpha1*axis1[i];
for (i=3D0; i<3; i++) sphere2[i] =3D pos2[i] + alpha2*axis2[i];
}
}
// if the alphas are outside their allowed ranges then fix them and
// try again
if (fix1=3D=3D0) {
if (alpha1 < -lz1) {
fix1 =3D -1;
continue;
}
if (alpha1 > lz1) {
fix1 =3D 1;
continue;
}
}
if (fix2=3D=3D0) {
if (alpha2 < -lz2) {
fix2 =3D -1;
continue;
}
if (alpha2 > lz2) {
fix2 =3D 1;
continue;
}
}
// unfix the alpha variables if the local distance gradient =
indicates
// that we are not yet at the minimum
dReal tmp[3];
for (i=3D0; i<3; i++) tmp[i] =3D sphere1[i] - sphere2[i];
if (fix1) {
dReal gradient =3D dDOT (tmp,axis1);
if ((fix1 > 0 && gradient > 0) || (fix1 < 0 && gradient < 0)) {
fix1 =3D 0;
continue;
}
}
if (fix2) {
dReal gradient =3D -dDOT (tmp,axis2);
if ((fix2 > 0 && gradient > 0) || (fix2 < 0 && gradient < 0)) {
fix2 =3D 0;
continue;
}
}
return dCollideSpheres =
(sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
}
// if we go through the loop too much, then give up. we should NEVER =
get to
// this point (i hope).
dMessage (0,"dCollideCC(): too many iterations");
return 0;
}
int dCollideCP (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >=3D (int)sizeof(dContactGeom));
dIASSERT (o1->_class->num =3D=3D dCCylinderClass);
dIASSERT (o2->_class->num =3D=3D dPlaneClass);
dxCCylinder *ccyl =3D (dxCCylinder*) CLASSDATA(o1);
dxPlane *plane =3D (dxPlane*) CLASSDATA(o2);
// collide the deepest capping sphere with the plane
dReal sign =3D (dDOT14 (plane->p,o1->R+2) > 0) ? REAL(-1.0) : =
REAL(1.0);
dVector3 p;
p[0] =3D o1->pos[0] + o1->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] =3D o1->pos[1] + o1->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] =3D o1->pos[2] + o1->R[10] * ccyl->lz * REAL(0.5) * sign;
dReal k =3D dDOT (p,plane->p);
dReal depth =3D plane->p[3] - k + ccyl->radius;
if (depth < 0) return 0;
contact->normal[0] =3D plane->p[0];
contact->normal[1] =3D plane->p[1];
contact->normal[2] =3D plane->p[2];
contact->pos[0] =3D p[0] - plane->p[0] * ccyl->radius;
contact->pos[1] =3D p[1] - plane->p[1] * ccyl->radius;
contact->pos[2] =3D p[2] - plane->p[2] * ccyl->radius;
contact->depth =3D depth;
int ncontacts =3D 1;
if ((flags & NUMC_MASK) >=3D 2) {
// collide the other capping sphere with the plane
p[0] =3D o1->pos[0] - o1->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] =3D o1->pos[1] - o1->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] =3D o1->pos[2] - o1->R[10] * ccyl->lz * REAL(0.5) * sign;
k =3D dDOT (p,plane->p);
depth =3D plane->p[3] - k + ccyl->radius;
if (depth >=3D 0) {
dContactGeom *c2 =3D CONTACT(contact,skip);
c2->normal[0] =3D plane->p[0];
c2->normal[1] =3D plane->p[1];
c2->normal[2] =3D plane->p[2];
c2->pos[0] =3D p[0] - plane->p[0] * ccyl->radius;
c2->pos[1] =3D p[1] - plane->p[1] * ccyl->radius;
c2->pos[2] =3D p[2] - plane->p[2] * ccyl->radius;
c2->depth =3D depth;
ncontacts =3D 2;
}
}
for (int i=3D0; i < ncontacts; i++) {
CONTACT(contact,i*skip)->g1 =3D const_cast<dxGeom*> (o1);
CONTACT(contact,i*skip)->g2 =3D const_cast<dxGeom*> (o2);
CONTACT(contact,i*skip)->b1 =3D GEOMGETBODY(o1);
CONTACT(contact,i*skip)->b2 =3D GEOMGETBODY(o2);
}
return ncontacts;
}
// this collides a group with another geom. the other geom can also be a
// group, but this case is not handled specially.
int dCollideG (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(o1);
int numleft =3D flags & NUMC_MASK;
if (numleft =3D=3D 0) numleft =3D 1;
flags &=3D ~NUMC_MASK;
int num=3D0,i=3D0;
while (i < gr->parts.size() && numleft > 0) {
int n =3D dCollide (gr->parts[i],const_cast<dxGeom*>(o2),
flags | numleft,contact,skip);
contact =3D CONTACT (contact,skip*n);
numleft -=3D n;
num +=3D n;
i++;
}
return num;
}
//***********************************************************************=
*****
// standard classes
int dSphereClass =3D -1;
int dBoxClass =3D -1;
int dCCylinderClass =3D -1;
int dPlaneClass =3D -1;
static dColliderFn * dSphereColliderFn (int num)
{
if (num =3D=3D dSphereClass) return (dColliderFn *) &dCollideSS;
if (num =3D=3D dBoxClass) return (dColliderFn *) &dCollideSB;
if (num =3D=3D dPlaneClass) return (dColliderFn *) &dCollideSP;
return 0;
}
static void dSphereAABB (dxGeom *geom, dReal aabb[6])
{
dxSphere *s =3D (dxSphere*) CLASSDATA(geom);
aabb[0] =3D geom->pos[0] - s->radius;
aabb[1] =3D geom->pos[0] + s->radius;
aabb[2] =3D geom->pos[1] - s->radius;
aabb[3] =3D geom->pos[1] + s->radius;
aabb[4] =3D geom->pos[2] - s->radius;
aabb[5] =3D geom->pos[2] + s->radius;
}
static dColliderFn * dBoxColliderFn (int num)
{
if (num =3D=3D dBoxClass) return (dColliderFn *) &dCollideBB;
if (num =3D=3D dPlaneClass) return (dColliderFn *) &dCollideBP;
return 0;
}
static void dBoxAABB (dxGeom *geom, dReal aabb[6])
{
dxBox *b =3D (dxBox*) CLASSDATA(geom);
dReal xrange =3D REAL(0.5) * (dFabs (geom->R[0] * b->side[0]) +
dFabs (geom->R[1] * b->side[1]) + dFabs (geom->R[2] * b->side[2]));
dReal yrange =3D REAL(0.5) * (dFabs (geom->R[4] * b->side[0]) +
dFabs (geom->R[5] * b->side[1]) + dFabs (geom->R[6] * b->side[2]));
dReal zrange =3D REAL(0.5) * (dFabs (geom->R[8] * b->side[0]) +
dFabs (geom->R[9] * b->side[1]) + dFabs (geom->R[10] * b->side[2]));
aabb[0] =3D geom->pos[0] - xrange;
aabb[1] =3D geom->pos[0] + xrange;
aabb[2] =3D geom->pos[1] - yrange;
aabb[3] =3D geom->pos[1] + yrange;
aabb[4] =3D geom->pos[2] - zrange;
aabb[5] =3D geom->pos[2] + zrange;
}
static dColliderFn * dCCylinderColliderFn (int num)
{
if (num =3D=3D dSphereClass) return (dColliderFn *) &dCollideCS;
if (num =3D=3D dPlaneClass) return (dColliderFn *) &dCollideCP;
if (num =3D=3D dCCylinderClass) return (dColliderFn *) &dCollideCC;
return 0;
}
static void dCCylinderAABB (dxGeom *geom, dReal aabb[6])
{
dxCCylinder *c =3D (dxCCylinder*) CLASSDATA(geom);
dReal xrange =3D dFabs(geom->R[2] * c->lz) * REAL(0.5) + c->radius;
dReal yrange =3D dFabs(geom->R[6] * c->lz) * REAL(0.5) + c->radius;
dReal zrange =3D dFabs(geom->R[10] * c->lz) * REAL(0.5) + c->radius;
aabb[0] =3D geom->pos[0] - xrange;
aabb[1] =3D geom->pos[0] + xrange;
aabb[2] =3D geom->pos[1] - yrange;
aabb[3] =3D geom->pos[1] + yrange;
aabb[4] =3D geom->pos[2] - zrange;
aabb[5] =3D geom->pos[2] + zrange;
}
dColliderFn * dPlaneColliderFn (int num)
{
return 0;
}
static void dPlaneAABB (dxGeom *geom, dReal aabb[6])
{
// @@@ planes that have normal vectors aligned along an axis can use a
// @@@ less comprehensive bounding box.
aabb[0] =3D -dInfinity;
aabb[1] =3D dInfinity;
aabb[2] =3D -dInfinity;
aabb[3] =3D dInfinity;
aabb[4] =3D -dInfinity;
aabb[5] =3D dInfinity;
}
dxGeom *dCreateSphere (dSpaceID space, dReal radius)
{
dAASSERT (radius > 0);
if (dSphereClass =3D=3D -1) {
dGeomClass c;
c.bytes =3D sizeof (dxSphere);
c.collider =3D &dSphereColliderFn;
c.aabb =3D &dSphereAABB;
c.aabb_test =3D 0;
c.dtor =3D 0;
dSphereClass =3D dCreateGeomClass (&c);
}
dxGeom *g =3D dCreateGeom (dSphereClass);
if (space) dSpaceAdd (space,g);
dxSphere *s =3D (dxSphere*) CLASSDATA(g);
s->radius =3D radius;
return g;
}
dxGeom *dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
{
dAASSERT (lx > 0 && ly > 0 && lz > 0);
if (dBoxClass =3D=3D -1) {
dGeomClass c;
c.bytes =3D sizeof (dxBox);
c.collider =3D &dBoxColliderFn;
c.aabb =3D &dBoxAABB;
c.aabb_test =3D 0;
c.dtor =3D 0;
dBoxClass =3D dCreateGeomClass (&c);
}
dxGeom *g =3D dCreateGeom (dBoxClass);
if (space) dSpaceAdd (space,g);
dxBox *b =3D (dxBox*) CLASSDATA(g);
b->side[0] =3D lx;
b->side[1] =3D ly;
b->side[2] =3D lz;
return g;
}
dxGeom * dCreateCCylinder (dSpaceID space, dReal radius, dReal length)
{
dAASSERT (radius > 0 && length > 0);
if (dCCylinderClass =3D=3D -1) {
dGeomClass c;
c.bytes =3D sizeof (dxCCylinder);
c.collider =3D &dCCylinderColliderFn;
c.aabb =3D &dCCylinderAABB;
c.aabb_test =3D 0;
c.dtor =3D 0;
dCCylinderClass =3D dCreateGeomClass (&c);
}
dxGeom *g =3D dCreateGeom (dCCylinderClass);
if (space) dSpaceAdd (space,g);
dxCCylinder *c =3D (dxCCylinder*) CLASSDATA(g);
c->radius =3D radius;
c->lz =3D length;
return g;
}
dxGeom *dCreatePlane (dSpaceID space,
dReal a, dReal b, dReal c, dReal d)
{
if (dPlaneClass =3D=3D -1) {
dGeomClass c;
c.bytes =3D sizeof (dxPlane);
c.collider =3D &dPlaneColliderFn;
c.aabb =3D &dPlaneAABB;
c.aabb_test =3D 0;
c.dtor =3D 0;
dPlaneClass =3D dCreateGeomClass (&c);
}
dxGeom *g =3D dCreateGeom (dPlaneClass);
if (space) dSpaceAdd (space,g);
dxPlane *p =3D (dxPlane*) CLASSDATA(g);
// make sure plane normal has unit length
dReal l =3D a*a + b*b + c*c;
if (l > 0) {
l =3D dRecipSqrt(l);
p->p[0] =3D a*l;
p->p[1] =3D b*l;
p->p[2] =3D c*l;
p->p[3] =3D d*l;
}
else {
p->p[0] =3D 1;
p->p[1] =3D 0;
p->p[2] =3D 0;
p->p[3] =3D 0;
}
return g;
}
void dGeomSphereSetRadius (dGeomID g, dReal radius)
{
dUASSERT (g && g->_class->num =3D=3D dSphereClass,"argument not a =
sphere");
dAASSERT (radius > 0);
dxSphere *s =3D (dxSphere*) CLASSDATA(g);
s->radius =3D radius;
}
void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
{
dUASSERT (g && g->_class->num =3D=3D dBoxClass,"argument not a box");
dAASSERT (lx > 0 && ly > 0 && lz > 0);
dxBox *b =3D (dxBox*) CLASSDATA(g);
b->side[0] =3D lx;
b->side[1] =3D ly;
b->side[2] =3D lz;
}
void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d)
{
dUASSERT (g && g->_class->num =3D=3D dPlaneClass,"argument not a =
plane");
dxPlane *p =3D (dxPlane*) CLASSDATA(g);
p->p[0] =3D a;
p->p[1] =3D b;
p->p[2] =3D c;
p->p[3] =3D d;
}
void dGeomCCylinderSetParams (dGeomID g, dReal radius, dReal length)
{
dUASSERT (g && g->_class->num =3D=3D dCCylinderClass,"argument not a =
ccylinder");
dAASSERT (radius > 0 && length > 0);
dxCCylinder *c =3D (dxCCylinder*) CLASSDATA(g);
c->radius =3D radius;
c->lz =3D length;
}
dReal dGeomSphereGetRadius (dGeomID g)
{
dUASSERT (g && g->_class->num =3D=3D dSphereClass,"argument not a =
sphere");
dxSphere *s =3D (dxSphere*) CLASSDATA(g);
return s->radius;
}
void dGeomBoxGetLengths (dGeomID g, dVector3 result)
{
dUASSERT (g && g->_class->num =3D=3D dBoxClass,"argument not a box");
dxBox *b =3D (dxBox*) CLASSDATA(g);
result[0] =3D b->side[0];
result[1] =3D b->side[1];
result[2] =3D b->side[2];
}
void dGeomPlaneGetParams (dGeomID g, dVector4 result)
{
dUASSERT (g && g->_class->num =3D=3D dPlaneClass,"argument not a =
plane");
dxPlane *p =3D (dxPlane*) CLASSDATA(g);
result[0] =3D p->p[0];
result[1] =3D p->p[1];
result[2] =3D p->p[2];
result[3] =3D p->p[3];
}
void dGeomCCylinderGetParams (dGeomID g, dReal *radius, dReal *length)
{
dUASSERT (g && g->_class->num =3D=3D dCCylinderClass,"argument not a =
ccylinder");
dxCCylinder *c =3D (dxCCylinder*) CLASSDATA(g);
*radius =3D c->radius;
*length =3D c->lz;
}
//***********************************************************************=
*****
// geom group
int dGeomGroupClass =3D -1;
static dColliderFn * dGeomGroupColliderFn (int num)
{
return (dColliderFn *) &dCollideG;
}
static void dGeomGroupAABB (dxGeom *geom, dReal aabb[6])
{
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(geom);
aabb[0] =3D dInfinity;
aabb[1] =3D -dInfinity;
aabb[2] =3D dInfinity;
aabb[3] =3D -dInfinity;
aabb[4] =3D dInfinity;
aabb[5] =3D -dInfinity;
int i,j;
for (i=3D0; i < gr->parts.size(); i++) {
dReal aabb2[6];
gr->parts[i]->_class->aabb (gr->parts[i],aabb2);
for (j=3D0; j<6; j +=3D 2) if (aabb2[j] < aabb[j]) aabb[j] =3D =
aabb2[j];
for (j=3D1; j<6; j +=3D 2) if (aabb2[j] > aabb[j]) aabb[j] =3D =
aabb2[j];
}
}
static void dGeomGroupDtor (dxGeom *geom)
{
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(geom);
gr->parts.~dArray();
}
dxGeom *dCreateGeomGroup (dSpaceID space)
{
if (dGeomGroupClass =3D=3D -1) {
dGeomClass c;
c.bytes =3D sizeof (dxGeomGroup);
c.collider =3D &dGeomGroupColliderFn;
c.aabb =3D &dGeomGroupAABB;
c.aabb_test =3D 0;
c.dtor =3D &dGeomGroupDtor;
dGeomGroupClass =3D dCreateGeomClass (&c);
}
dxGeom *g =3D dCreateGeom (dGeomGroupClass);
if (space) dSpaceAdd (space,g);
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(g);
gr->parts.constructor();
return g;
}
void dGeomGroupAdd (dxGeom *g, dxGeom *x)
{
dUASSERT (g && g->_class->num =3D=3D dGeomGroupClass,"argument not a =
geomgroup");
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(g);
gr->parts.push (x);
}
void dGeomGroupRemove (dxGeom *g, dxGeom *x)
{
dUASSERT (g && g->_class->num =3D=3D dGeomGroupClass,"argument not a =
geomgroup");
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(g);
for (int i=3D0; i < gr->parts.size(); i++) {
if (gr->parts[i] =3D=3D x) {
gr->parts.remove (i);
return;
}
}
}
int dGeomGroupGetNumGeoms (dxGeom *g)
{
dUASSERT (g && g->_class->num =3D=3D dGeomGroupClass,"argument not a =
geomgroup");
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(g);
return gr->parts.size();
}
dxGeom * dGeomGroupGetGeom (dxGeom *g, int i)
{
dUASSERT (g && g->_class->num =3D=3D dGeomGroupClass,"argument not a =
geomgroup");
dxGeomGroup *gr =3D (dxGeomGroup*) CLASSDATA(g);
dAASSERT (i >=3D 0 && i < gr->parts.size());
return gr->parts[i];
}
//***********************************************************************=
*****
// transformed geom
int dGeomTransformClass =3D -1;
struct dxGeomTransform {
dxGeom *obj; // object that is being transformed
int cleanup; // 1 to destroy obj when destroyed
dVector3 final_pos; // final tx (body tx + relative tx) of the object.
dMatrix3 final_R; // this is only set if the AABB function is called
}; // by space collision before the collide fn is called
// compute final pos and R for the encapsulated geom object
static void compute_final_tx (const dxGeom *g)
{
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(g);
dMULTIPLY0_331 (tr->final_pos,g->R,tr->obj->pos);
tr->final_pos[0] +=3D g->pos[0];
tr->final_pos[1] +=3D g->pos[1];
tr->final_pos[2] +=3D g->pos[2];
dMULTIPLY0_333 (tr->final_R,g->R,tr->obj->R);
}
// this collides a transformed geom with another geom. the other geom =
can
// also be a transformed geom, but this case is not handled specially.
int dCollideT (const dxGeom *o1, const dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(o1);
if (!tr->obj) return 0;
dUASSERT (tr->obj->spaceid=3D=3D0,
"GeomTransform encapsulated object must not be in a space");
dUASSERT (tr->obj->body=3D=3D0,
"GeomTransform encapsulated object must not be attach to a body");
// backup the relative pos and R pointers of the encapsulated geom =
object,
// and the body pointer
dReal *posbak =3D tr->obj->pos;
dReal *Rbak =3D tr->obj->R;
dxBody *bodybak =3D tr->obj->body;
// compute temporary pos and R for the encapsulated geom object
if (!o1->space_aabb) compute_final_tx (o1);
tr->obj->pos =3D tr->final_pos;
tr->obj->R =3D tr->final_R;
tr->obj->body =3D o1->body;
// do the collision
int n =3D dCollide =
(tr->obj,const_cast<dxGeom*>(o2),flags,contact,skip);
for (int i =3D 0; i < n; i++){ //@@ErwinSTART
CONTACT(contact, skip)->b1 =3D GEOMGETBODY(o1);
} //@@ErwinEND
// restore the pos, R and body
tr->obj->pos =3D posbak;
tr->obj->R =3D Rbak;
tr->obj->body =3D bodybak;
return n;
}
static dColliderFn * dGeomTransformColliderFn (int num)
{
return (dColliderFn *) &dCollideT;
}
static void dGeomTransformAABB (dxGeom *geom, dReal aabb[6])
{
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(geom);
if (!tr->obj) {
dSetZero (aabb,6);
return;
}
// backup the relative pos and R pointers of the encapsulated geom =
object
dReal *posbak =3D tr->obj->pos;
dReal *Rbak =3D tr->obj->R;
// compute temporary pos and R for the encapsulated geom object
compute_final_tx (geom);
tr->obj->pos =3D tr->final_pos;
tr->obj->R =3D tr->final_R;
// compute the AABB
tr->obj->_class->aabb (tr->obj,aabb);
// restore the pos and R
tr->obj->pos =3D posbak;
tr->obj->R =3D Rbak;
}
static void dGeomTransformDtor (dxGeom *geom)
{
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(geom);
if (tr->obj && tr->cleanup) {
dGeomDestroy (tr->obj);
}
}
dxGeom *dCreateGeomTransform (dSpaceID space)
{
if (dGeomTransformClass =3D=3D -1) {
dGeomClass c;
c.bytes =3D sizeof (dxGeomTransform);
c.collider =3D &dGeomTransformColliderFn;
c.aabb =3D &dGeomTransformAABB;
c.aabb_test =3D 0;
c.dtor =3D dGeomTransformDtor;
dGeomTransformClass =3D dCreateGeomClass (&c);
}
dxGeom *g =3D dCreateGeom (dGeomTransformClass);
if (space) dSpaceAdd (space,g);
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(g);
tr->obj =3D 0;
tr->cleanup =3D 0;
dSetZero (tr->final_pos,4);
dRSetIdentity (tr->final_R);
return g;
}
void dGeomTransformSetGeom (dxGeom *g, dxGeom *obj)
{
dUASSERT (g && g->_class->num =3D=3D dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(g);
if (tr->obj && tr->cleanup) {
dGeomDestroy (tr->obj);
}
tr->obj =3D obj;
}
dxGeom * dGeomTransformGetGeom (dxGeom *g)
{
dUASSERT (g && g->_class->num =3D=3D dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(g);
return tr->obj;
}
void dGeomTransformSetCleanup (dGeomID g, int mode)
{
dUASSERT (g && g->_class->num =3D=3D dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(g);
tr->cleanup =3D mode;
}
int dGeomTransformGetCleanup (dGeomID g)
{
dUASSERT (g && g->_class->num =3D=3D dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr =3D (dxGeomTransform*) CLASSDATA(g);
return tr->cleanup;
}
//***********************************************************************=
*****
// other utility functions
void dInfiniteAABB (dxGeom *geom, dReal aabb[6])
{
aabb[0] =3D -dInfinity;
aabb[1] =3D dInfinity;
aabb[2] =3D -dInfinity;
aabb[3] =3D dInfinity;
aabb[4] =3D -dInfinity;
aabb[5] =3D dInfinity;
}
------=_NextPart_000_006D_01C1953B.B9C466F0
Content-Type: application/octet-stream;
name="contact.h"
Content-Transfer-Encoding: 7bit
Content-Disposition: attachment;
filename="contact.h"
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001 Russell L. Smith. *
* 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 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. *
* *
* 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 GNU *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library (see the file LICENSE.TXT); if not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA. *
* *
*************************************************************************/
#ifndef _ODE_CONTACT_H_
#define _ODE_CONTACT_H_
#include <ode/common.h>
#ifdef __cplusplus
extern "C" {
#endif
enum {
dContactMu2 = 0x001,
dContactFDir1 = 0x002,
dContactBounce = 0x004,
dContactSoftERP = 0x008,
dContactSoftCFM = 0x010,
dContactMotion1 = 0x020,
dContactMotion2 = 0x040,
dContactSlip1 = 0x080,
dContactSlip2 = 0x100,
dContactApprox0 = 0x0000,
dContactApprox1_1 = 0x1000,
dContactApprox1_2 = 0x2000,
dContactApprox1 = 0x3000
};
typedef struct dSurfaceParameters {
/* must always be defined */
int mode;
dReal mu;
/* only defined if the corresponding flag is set in mode */
dReal mu2;
dReal bounce;
dReal bounce_vel;
dReal soft_erp;
dReal soft_cfm;
dReal motion1,motion2;
dReal slip1,slip2;
} dSurfaceParameters;
/* contact info set by collision functions */
typedef struct dContactGeom {
dVector3 pos;
dVector3 normal;
dReal depth;
dGeomID g1,g2;
dBodyID b1, b2; //@@Erwin
} dContactGeom;
/* contact info used by contact joint */
typedef struct dContact {
dSurfaceParameters surface;
dContactGeom geom;
dVector3 fdir1;
} dContact;
#ifdef __cplusplus
}
#endif
#endif
------=_NextPart_000_006D_01C1953B.B9C466F0--