int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxRay *ray = (dxRay*) o1;
dxPlane *plane = (dxPlane*) o2;
dReal alpha = plane->p[3] - dCalcVectorDot3 (plane->p,ray->final_posr->pos);
// note: if alpha > 0 the starting point is below the plane
dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0);
dReal k = dCalcVectorDot3_14(plane->p,ray->final_posr->R+2);
if (k==0) return 0; // ray parallel to plane
alpha /= k;
if (alpha < 0 || alpha > ray->length) return 0;
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];
contact->normal[0] = nsign*plane->p[0];
contact->normal[1] = nsign*plane->p[1];
contact->normal[2] = nsign*plane->p[2];
contact->depth = alpha;
contact->g1 = ray;
contact->g2 = plane;
contact->side1 = -1;
contact->side2 = -1;
return 1;
}
int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxCapsule *ccyl = (dxCapsule*) o1;
dxPlane *plane = (dxPlane*) o2;
// collide the deepest capping sphere with the plane
dReal sign = (dCalcVectorDot3_14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
dVector3 p;
p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
dReal k = dCalcVectorDot3 (p,plane->p);
dReal depth = plane->p[3] - k + ccyl->radius;
if (depth < 0) return 0;
contact->normal[0] = plane->p[0];
contact->normal[1] = plane->p[1];
contact->normal[2] = plane->p[2];
contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
contact->depth = depth;
int ncontacts = 1;
if ((flags & NUMC_MASK) >= 2) {
// collide the other capping sphere with the plane
p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
k = dCalcVectorDot3 (p,plane->p);
depth = plane->p[3] - k + ccyl->radius;
if (depth >= 0) {
dContactGeom *c2 = CONTACT(contact,skip);
c2->normal[0] = plane->p[0];
c2->normal[1] = plane->p[1];
c2->normal[2] = plane->p[2];
c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
c2->depth = depth;
ncontacts = 2;
}
}
for (int i=0; i < ncontacts; i++) {
dContactGeom *currContact = CONTACT(contact,i*skip);
currContact->g1 = o1;
currContact->g2 = o2;
currContact->side1 = -1;
currContact->side2 = -1;
}
return ncontacts;
}
dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
g->recomputePosr();
dxCapsule *c = (dxCapsule*) g;
const dReal* R = g->final_posr->R;
const dReal* pos = g->final_posr->pos;
dVector3 a;
a[0] = x - pos[0];
a[1] = y - pos[1];
a[2] = z - pos[2];
dReal beta = dCalcVectorDot3_14(a,R+2);
dReal lz2 = c->lz*REAL(0.5);
if (beta < -lz2) beta = -lz2;
else if (beta > lz2) beta = lz2;
a[0] = c->final_posr->pos[0] + beta*R[0*4+2];
a[1] = c->final_posr->pos[1] + beta*R[1*4+2];
a[2] = c->final_posr->pos[2] + beta*R[2*4+2];
return c->radius -
dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2]));
}
static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius,
dContactGeom *contact, int mode)
{
dVector3 q;
q[0] = ray->final_posr->pos[0] - sphere_pos[0];
q[1] = ray->final_posr->pos[1] - sphere_pos[1];
q[2] = ray->final_posr->pos[2] - sphere_pos[2];
dReal B = dCalcVectorDot3_14(q,ray->final_posr->R+2);
dReal C = dCalcVectorDot3(q,q) - radius*radius;
// note: if C <= 0 then the start of the ray is inside the sphere
dReal k = B*B - C;
if (k < 0) return 0;
k = dSqrt(k);
dReal alpha;
if (mode && C >= 0) {
alpha = -B + k;
if (alpha < 0) return 0;
}
else {
alpha = -B - k;
if (alpha < 0) {
alpha = -B + k;
if (alpha < 0) return 0;
}
}
if (alpha > ray->length) return 0;
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];
dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0);
contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]);
contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]);
contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]);
dNormalize3 (contact->normal);
contact->depth = alpha;
return 1;
}
static int box1inside2 (const dVector3 p1, const dMatrix3 R1,
const dVector3 side1, const dVector3 p2,
const dMatrix3 R2, const dVector3 side2)
{
for (int i=-1; i<=1; i+=2) {
for (int j=-1; j<=1; j+=2) {
for (int k=-1; k<=1; k+=2) {
dVector3 v,vv;
v[0] = i*0.5*side1[0];
v[1] = j*0.5*side1[1];
v[2] = k*0.5*side1[2];
dMultiply0_331 (vv,R1,v);
vv[0] += p1[0] - p2[0];
vv[1] += p1[1] - p2[1];
vv[2] += p1[2] - p2[2];
for (int axis=0; axis < 3; axis++) {
dReal z = dCalcVectorDot3_14(vv,R2+axis);
if (z < (-side2[axis]*0.5) || z > (side2[axis]*0.5)) return 0;
}
}
}
}
return 1;
}
int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dSphereClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
// 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 <= 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 face.
dVector3 l,t,p,q,r;
dReal depth;
int onborder = 0;
dxSphere *sphere = (dxSphere*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0];
p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1];
p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2];
l[0] = box->side[0]*REAL(0.5);
t[0] = dCalcVectorDot3_14(p,o2->final_posr->R);
if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; }
l[1] = box->side[1]*REAL(0.5);
t[1] = dCalcVectorDot3_14(p,o2->final_posr->R+1);
if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; }
t[2] = dCalcVectorDot3_14(p,o2->final_posr->R+2);
l[2] = box->side[2]*REAL(0.5);
if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; }
if (!onborder) {
// sphere center inside box. find closest face to `t'
dReal min_distance = l[0] - dFabs(t[0]);
int mini = 0;
for (int i=1; i<3; i++) {
dReal face_distance = l[i] - dFabs(t[i]);
if (face_distance < min_distance) {
min_distance = face_distance;
mini = i;
}
}
// contact position = sphere center
contact->pos[0] = o1->final_posr->pos[0];
contact->pos[1] = o1->final_posr->pos[1];
contact->pos[2] = o1->final_posr->pos[2];
// contact normal points to closest face
dVector3 tmp;
tmp[0] = 0;
tmp[1] = 0;
tmp[2] = 0;
tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0);
dMultiply0_331 (contact->normal,o2->final_posr->R,tmp);
// contact depth = distance to wall along normal plus radius
contact->depth = min_distance + sphere->radius;
return 1;
}
t[3] = 0; //@@@ hmmm
dMultiply0_331 (q,o2->final_posr->R,t);
r[0] = p[0] - q[0];
r[1] = p[1] - q[1];
r[2] = p[2] - q[2];
depth = sphere->radius - dSqrt(dCalcVectorDot3(r,r));
if (depth < 0) return 0;
contact->pos[0] = q[0] + o2->final_posr->pos[0];
contact->pos[1] = q[1] + o2->final_posr->pos[1];
contact->pos[2] = q[2] + o2->final_posr->pos[2];
contact->normal[0] = r[0];
contact->normal[1] = r[1];
contact->normal[2] = r[2];
dNormalize3 (contact->normal);
contact->depth = depth;
return 1;
}
int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dBoxClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxBox *box = (dxBox*) o1;
dxPlane *plane = (dxPlane*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
int ret = 0;
//@@@ problem: using 4-vector (plane->p) as 3-vector (normal).
const dReal *R = o1->final_posr->R; // rotation of box
const dReal *n = plane->p; // normal vector
// project sides lengths along normal vector, get absolute values
dReal Q1 = dCalcVectorDot3_14(n,R+0);
dReal Q2 = dCalcVectorDot3_14(n,R+1);
dReal Q3 = dCalcVectorDot3_14(n,R+2);
dReal A1 = box->side[0] * Q1;
dReal A2 = box->side[1] * Q2;
dReal A3 = box->side[2] * Q3;
dReal B1 = dFabs(A1);
dReal B2 = dFabs(A2);
dReal B3 = dFabs(A3);
// early exit test
dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dCalcVectorDot3(n,o1->final_posr->pos);
if (depth < 0) return 0;
// find number of contacts requested
int maxc = flags & NUMC_MASK;
// if (maxc < 1) maxc = 1; // an assertion is made on entry
if (maxc > 4) maxc = 4; // not more than 4 contacts per box allowed
// find deepest point
dVector3 p;
p[0] = o1->final_posr->pos[0];
p[1] = o1->final_posr->pos[1];
p[2] = o1->final_posr->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,-=) } else { FOO(i,+=) }
BAR(0,1);
BAR(1,2);
BAR(2,3);
#undef FOO
#undef BAR
// the deepest point is the first contact point
contact->pos[0] = p[0];
contact->pos[1] = p[1];
contact->pos[2] = p[2];
contact->depth = depth;
ret = 1; // ret is number of contact points found so far
if (maxc == 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] = p[0] op box->side[j] * R[0+j]; \
CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \
CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j];
#define BAR(ctact,side,sideinc) \
if (depth - B ## sideinc < 0) goto done; \
if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO(ctact,side,-); } \
CONTACT(contact,ctact*skip)->depth = depth - B ## sideinc; \
ret++;
if (B1 < B2) {
if (B3 < B1) goto use_side_3; else {
BAR(1,0,1); // use side 1
if (maxc == 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 == 2) goto done;
if (B1 < B2) goto contact2_1; else goto contact2_2;
}
else {
BAR(1,1,2); // use side 2
if (maxc == 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:
if (maxc == 4 && ret == 3) { // If user requested 4 contacts, and the first 3 were created...
// Combine contacts 2 and 3 (vectorial sum) and get the fourth one
// Result: if a box face is completely inside a plane, contacts are created for all the 4 vertices
dReal d4 = CONTACT(contact,1*skip)->depth + CONTACT(contact,2*skip)->depth - depth; // depth is the depth for first contact
if (d4 > 0) {
CONTACT(contact,3*skip)->pos[0] = CONTACT(contact,1*skip)->pos[0] + CONTACT(contact,2*skip)->pos[0] - p[0]; // p is the position of first contact
CONTACT(contact,3*skip)->pos[1] = CONTACT(contact,1*skip)->pos[1] + CONTACT(contact,2*skip)->pos[1] - p[1];
CONTACT(contact,3*skip)->pos[2] = CONTACT(contact,1*skip)->pos[2] + CONTACT(contact,2*skip)->pos[2] - p[2];
CONTACT(contact,3*skip)->depth = d4;
ret++;
}
}
for (int i=0; i<ret; i++) {
dContactGeom *currContact = CONTACT(contact,i*skip);
currContact->g1 = o1;
currContact->g2 = o2;
currContact->side1 = -1;
currContact->side2 = -1;
currContact->normal[0] = n[0];
currContact->normal[1] = n[1];
currContact->normal[2] = n[2];
}
return ret;
}
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 *return_code,
int flags, dContactGeom *contact, int skip)
{
const dReal fudge_factor = REAL(1.05);
dVector3 p,pp,normalC={0,0,0};
const dReal *normalR = 0;
dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMultiply1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0]*REAL(0.5);
A[1] = side1[1]*REAL(0.5);
A[2] = side1[2]*REAL(0.5);
B[0] = side2[0]*REAL(0.5);
B[1] = side2[1]*REAL(0.5);
B[2] = side2[2]*REAL(0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2);
Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = 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.
do {
#define TST(expr1,expr2,norm,cc) \
expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
s2 = dFabs(expr1_val) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1_val) < 0); \
code = (cc); \
if (flags & CONTACTS_UNIMPORTANT) break; \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (dCalcVectorDot3_41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),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 TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
s2 = dFabs(expr1_val) - (expr2); \
if (s2 > 0) return 0; \
l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1_val) < 0); \
code = (cc); \
if (flags & CONTACTS_UNIMPORTANT) break; \
} \
}
// We only need to check 3 edges per box
// since parallel edges are equivalent.
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
} while (0);
if (!code) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR) {
normal[0] = normalR[0];
normal[1] = normalR[4];
normal[2] = normalR[8];
}
else {
dMultiply0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
*depth = -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;
// Copy p1 into pa
for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy?
// Get world position of p2 into pa
for (j=0; j<3; j++) {
sign = (dCalcVectorDot3_14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
// Copy p2 into pb
for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy?
// Get world position of p2 into pb
for (j=0; j<3; j++) {
sign = (dCalcVectorDot3_14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
dReal alpha,beta;
dVector3 ua,ub;
// Get direction of first edge
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
// Get direction of second edge
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
// Get closest points between edges (one at each)
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
// Set the contact point as halfway between the 2 closest points
for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
contact[0].depth = *depth;
*return_code = code;
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
// Note: Unmodified parameter values are being used here
const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) { // One of the faces of box 1 is the reference face
Ra = R1; // Rotation of 'a'
Rb = R2; // Rotation of 'b'
pa = p1; // Center (location) of 'a'
pb = p2; // Center (location) of 'b'
Sa = A; // Side Lenght of 'a'
Sb = B; // Side Lenght of 'b'
}
else { // One of the faces of box 2 is the reference face
Ra = R2; // Rotation of 'a'
Rb = R1; // Rotation of 'b'
pa = p2; // Center (location) of 'a'
pb = p1; // Center (location) of 'b'
Sa = B; // Side Lenght of 'a'
Sb = A; // Side Lenght of 'b'
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
/*
The normal is flipped if necessary so it always points outward from box 'a',
box 'b' is thus always the incident box
*/
dVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
// Rotate normal2 in incident box opposite direction
dMultiply1_331 (nr,Rb,normal2);
anr[0] = dFabs (nr[0]);
anr[1] = dFabs (nr[1]);
anr[2] = dFabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the incident face. the other axis numbers of the incident face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
dVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
dReal quad[8]; // 2D coordinate of incident face (x,y pairs)
dReal c1,c2,m11,m12,m21,m22;
c1 = dCalcVectorDot3_14 (center,Ra+code1);
c2 = dCalcVectorDot3_14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dCalcVectorDot3_44 (Ra+code1,Rb+a1);
m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2);
m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1);
m22 = dCalcVectorDot3_44 (Ra+code2,Rb+a2);
{
dReal k1 = m11*Sb[a1];
dReal k2 = m21*Sb[a1];
dReal k3 = m12*Sb[a2];
dReal k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
dReal rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
dReal ret[16];
int n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
dReal point[3*8]; // penetrating contact points
dReal dep[8]; // depths for those points
dReal det1 = dRecip(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
dep[cnum] = Sa[codeN] - dCalcVectorDot3(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
break;
}
}
}
if (cnum < 1) {
return 0; // this should not happen, yet does at times (demo_plane2d single precision).
}
// we can't generate more contacts than we actually have
int maxc = flags & NUMC_MASK;
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1; // Even though max count must not be zero this check is kept for backward compatibility as this is a public function
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
con->depth = dep[j];
}
}
else {
dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
dReal maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints (cnum,ret,maxc,i1,iret);
for (j=0; j < maxc; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
con->depth = dep[iret[j]];
}
cnum = maxc;
}
*return_code = code;
return cnum;
}
// Ray - Cylinder collider by David Walters (June 2006)
int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dRayClass );
dIASSERT( o2->type == dCylinderClass );
dIASSERT( (flags & NUMC_MASK) >= 1 );
dxRay* ray = (dxRay*)( o1 );
dxCylinder* cyl = (dxCylinder*)( o2 );
// Fill in contact information.
contact->g1 = ray;
contact->g2 = cyl;
contact->side1 = -1;
contact->side2 = -1;
const dReal half_length = cyl->lz * REAL( 0.5 );
//
// Compute some useful info
//
dVector3 q, r;
dReal d, C, k;
// Vector 'r', line segment from C to R (ray start) ( r = R - C )
r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0];
r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1];
r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2];
// Distance that ray start is along cyl axis ( Z-axis direction )
d = dCalcVectorDot3_41( cyl->final_posr->R + 2, r );
//
// Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz).
//
// Point on axis ( in world space ): cp = ( d * Cz ) + C
//
// Line 'q' from R to cp: q = cp - R
// q = ( d * Cz ) + C - R
// q = ( d * Cz ) - ( R - C )
q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ];
q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ];
q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ];
// Compute square length of 'q'. Subtract from radius squared to
// get square distance 'C' between the line q and the radius.
// if C < 0 then ray start position is within infinite extension of cylinder
C = dCalcVectorDot3( q, q ) - ( cyl->radius * cyl->radius );
// Compute the projection of ray direction normal onto cylinder direction normal.
dReal uv = dCalcVectorDot3_44( cyl->final_posr->R+2, ray->final_posr->R+2 );
//
// Find ray collision with infinite cylinder
//
// Compute vector from end of ray direction normal to projection on cylinder direction normal.
r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2];
r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2];
r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2];
// Quadratic Formula Magic
// Compute discriminant 'k':
// k < 0 : No intersection
// k = 0 : Tangent
// k > 0 : Intersection
dReal A = dCalcVectorDot3( r, r );
dReal B = 2 * dCalcVectorDot3( q, r );
k = B*B - 4*A*C;
//
// Collision with Flat Caps ?
//
// No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases )
if ( k < dEpsilon && C <= 0 )
{
// The ray does not intersect the edge of the infinite cylinder,
// but the ray start is inside and so must run parallel to the axis.
// It may yet intersect an end cap. The following cases are valid:
// -ve-cap , -half centre +half , +ve-cap
// <<================|-------------------|------------->>>---|================>>
// | |
// | d-------------------> 1.
// 2. d------------------> |
// 3. <------------------d |
// | <-------------------d 4.
// | |
// <<================|-------------------|------------->>>---|===============>>
// Negative if the ray and cylinder axes point in opposite directions.
const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
// Negative if the ray start is inside the cylinder
const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 );
// Ray and Cylinder axes run in the same direction ( cases 1, 2 )
// Ray and Cylinder axes run in opposite directions ( cases 3, 4 )
if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) ||
( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) )
{
return 0; // No intersection with caps or curved surface.
}
// Compute depth (distance from ray to cylinder)
contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) );
// Compute contact point.
contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] );
contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] );
contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] );
// Compute reflected contact normal.
contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] );
contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] );
contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] );
// Contact!
return 1;
}
//
// Collision with Curved Edge ?
//
if ( k > 0 )
{
// Finish off quadratic formula to get intersection co-efficient
k = dSqrt( k );
A = dRecip( 2 * A );
// Compute distance along line to contact point.
dReal alpha = ( -B - k ) * A;
if ( alpha < 0 )
{
// Flip in the other direction.
alpha = ( -B + k ) * A;
}
// Intersection point is within ray length?
if ( alpha >= 0 && alpha <= ray->length )
{
// The ray intersects the infinite cylinder!
// Compute contact point.
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] );
// q is the vector from the cylinder centre to the contact point.
q[0] = contact->pos[0] - cyl->final_posr->pos[0];
q[1] = contact->pos[1] - cyl->final_posr->pos[1];
q[2] = contact->pos[2] - cyl->final_posr->pos[2];
// Compute the distance along the cylinder axis of this contact point.
d = dCalcVectorDot3_14( q, cyl->final_posr->R+2 );
// Check to see if the intersection point is between the flat end caps
if ( d >= -half_length && d <= +half_length )
{
// Flip the normal if the start point is inside the cylinder.
const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
// Compute contact normal.
contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2]));
contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2]));
contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2]));
dNormalize3( contact->normal );
// Store depth.
contact->depth = alpha;
// Contact!
return 1;
}
}
}
// No contact with anything.
return 0;
}
int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxRay *ray = (dxRay*) o1;
dxCapsule *ccyl = (dxCapsule*) o2;
contact->g1 = ray;
contact->g2 = ccyl;
contact->side1 = -1;
contact->side2 = -1;
dReal lz2 = ccyl->lz * REAL(0.5);
// compute some useful info
dVector3 cs,q,r;
dReal C,k;
cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0];
cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1];
cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2];
k = dCalcVectorDot3_41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis
q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0];
q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1];
q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2];
C = dCalcVectorDot3(q,q) - ccyl->radius*ccyl->radius;
// if C < 0 then ray start position within infinite extension of cylinder
// see if ray start position is inside the capped cylinder
int inside_ccyl = 0;
if (C < 0) {
if (k < -lz2) k = -lz2;
else if (k > lz2) k = lz2;
r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) +
(ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) +
(ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) {
inside_ccyl = 1;
}
}
// compute ray collision with infinite cylinder, except for the case where
// the ray is outside the capped cylinder but within the infinite cylinder
// (it that case the ray can only hit endcaps)
if (!inside_ccyl && C < 0) {
// set k to cap position to check
if (k < 0) k = -lz2; else k = lz2;
}
else {
dReal uv = dCalcVectorDot3_44(ccyl->final_posr->R+2,ray->final_posr->R+2);
r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2];
r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2];
r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2];
dReal A = dCalcVectorDot3(r,r);
dReal B = 2*dCalcVectorDot3(q,r);
k = B*B-4*A*C;
if (k < 0) {
// the ray does not intersect the infinite cylinder, but if the ray is
// inside and parallel to the cylinder axis it may intersect the end
// caps. set k to cap position to check.
if (!inside_ccyl) return 0;
if (uv < 0) k = -lz2; else k = lz2;
}
else {
k = dSqrt(k);
A = dRecip (2*A);
dReal alpha = (-B-k)*A;
if (alpha < 0) {
alpha = (-B+k)*A;
if (alpha < 0) return 0;
}
if (alpha > ray->length) return 0;
// the ray intersects the infinite cylinder. check to see if the
// intersection point is between the caps
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];
q[0] = contact->pos[0] - ccyl->final_posr->pos[0];
q[1] = contact->pos[1] - ccyl->final_posr->pos[1];
q[2] = contact->pos[2] - ccyl->final_posr->pos[2];
k = dCalcVectorDot3_14(q,ccyl->final_posr->R+2);
dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0);
if (k >= -lz2 && k <= lz2) {
contact->normal[0] = nsign * (contact->pos[0] -
(ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]));
contact->normal[1] = nsign * (contact->pos[1] -
(ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]));
contact->normal[2] = nsign * (contact->pos[2] -
(ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]));
dNormalize3 (contact->normal);
contact->depth = alpha;
return 1;
}
// the infinite cylinder intersection point is not between the caps.
// set k to cap position to check.
if (k < 0) k = -lz2; else k = lz2;
}
}
// check for ray intersection with the caps. k must indicate the cap
// position to check
q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl);
}
int test_ray_and_ccylinder()
{
int j;
dContactGeom contact;
dVector3 p,a,b,n;
dMatrix3 R;
dReal r,l,k,x,y;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,ccyl);
// ********** make a random capped cylinder
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test ray completely within ccyl
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
dNormalize3 (b);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
dGeomRaySetLength (ray,dCalcPointsDistance3(a,b));
for (j=0; j<3; j++) b[j] -= a[j];
dNormalize3 (b);
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just misses (between caps)
k = dRandReal()*2*M_PI;
x = sin(k);
y = cos(k);
for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (between caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test ray outside ccyl that just misses (caps)
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) < 0) {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
}
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test random rays
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
dNormalize3 (n);
dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_ccylinder_point_depth()
{
int j;
dVector3 p,a;
dMatrix3 R;
dReal r,l,beta,x,y,d;
dSimpleSpace space(0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ccyl);
// ********** make a random ccyl
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test point on axis has depth of 'radius'
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol)
FAILED();
// ********** test point on surface (excluding caps) has depth 0
beta = dRandReal()*2*M_PI;
x = r*sin(beta);
y = r*cos(beta);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
// ********** test point on surface of caps has depth 0
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) > 0) {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2];
}
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
// ********** test point inside ccyl has positive depth
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2];
if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED();
// ********** test point depth (1)
d = (dRandReal()*2-1) * r;
beta = dRandReal()*2*M_PI;
x = (r-d)*sin(beta);
y = (r-d)*cos(beta);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
FAILED();
// ********** test point depth (2)
d = (dRandReal()*2-1) * r;
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) > 0) {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2];
}
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
FAILED();
PASSED();
}