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 = (dDOT14 (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 = dDOT (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 = dDOT (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;
}
void
contact_init (void)
{
/* Pull-up. */
#define CONTACT(io, pullup) if (pullup) IO_SET_ (io);
CONTACT (CONTACT_COLOR, 1);
CONTACT (CONTACT_JACK, 1);
CONTACT_LIST
#undef CONTACT
/* Default to jack out. */
ctx.jack_state = 1;
}
int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dxCapsule *ccyl = (dxCapsule*) o1;
dxPlane *plane = (dxPlane*) o2;
// collide the deepest capping sphere with the plane
dReal sign = (dDOT14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
dVector3 p;
p[0] = o1->final_posr->pos[0] + dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
p[1] = o1->final_posr->pos[1] + dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
p[2] = o1->final_posr->pos[2] + dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
dReal k = dDOT (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] - dMUL(plane->p[0],ccyl->radius);
contact->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius);
contact->pos[2] = p[2] - dMUL(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] - dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
p[1] = o1->final_posr->pos[1] - dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
p[2] = o1->final_posr->pos[2] - dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
k = dDOT (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] - dMUL(plane->p[0],ccyl->radius);
c2->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius);
c2->pos[2] = p[2] - dMUL(plane->p[2],ccyl->radius);
c2->depth = depth;
ncontacts = 2;
}
}
for (int i=0; i < ncontacts; i++) {
CONTACT(contact,i*skip)->g1 = o1;
CONTACT(contact,i*skip)->g2 = o2;
}
return ncontacts;
}
uint32_t
contact_all (void)
{
uint32_t contacts = 0, bit = 1;
#define CONTACT(io, pullup) do { \
contacts |= (IO_GET_ (io) ? bit : 0); \
bit <<= 1; \
} while (0);
CONTACT (CONTACT_COLOR, 1);
CONTACT (CONTACT_JACK, 1);
CONTACT_LIST
#undef CONTACT
return contacts;
}
int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,
int skip)
{
dAASSERT(o1 && o2 && contact);
dUASSERT(colliders_initialized,"colliders array not initialized");
dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number");
dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number");
// no contacts if both geoms are the same
if (o1 == o2) return 0;
// no contacts if both geoms on the same body, and the body is not 0
if (o1->body == o2->body && o1->body) return 0;
dColliderEntry *ce = &colliders[o1->type][o2->type];
int count = 0;
if (ce->fn) {
if (ce->reverse) {
count = (*ce->fn) (o2,o1,flags,contact,skip);
for (int i=0; i<count; i++) {
dContactGeom *c = CONTACT(contact,skip*i);
c->normal[0] = -c->normal[0];
c->normal[1] = -c->normal[1];
c->normal[2] = -c->normal[2];
dxGeom *tmp = c->g1;
c->g1 = c->g2;
c->g2 = tmp;
}
}
else {
count = (*ce->fn) (o1,o2,flags,contact,skip);
}
}
return count;
}
int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dBoxClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dVector3 normal;
dReal depth;
int code;
dxBox *b1 = (dxBox*) o1;
dxBox *b2 = (dxBox*) o2;
int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
normal,&depth,&code,flags,contact,skip);
for (int i=0; i<num; i++) {
dContactGeom *currContact = CONTACT(contact,i*skip);
currContact->normal[0] = -normal[0];
currContact->normal[1] = -normal[1];
currContact->normal[2] = -normal[2];
currContact->g1 = o1;
currContact->g2 = o2;
currContact->side1 = -1;
currContact->side2 = -1;
}
return num;
}
static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2)
{
SpaceGeomColliderData *d = (SpaceGeomColliderData*) data;
if (d->flags & NUMC_MASK) {
int n = dCollide (o1,o2,d->flags,d->contact,d->skip);
d->contact = CONTACT (d->contact,d->skip*n);
d->flags -= n;
}
}
int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dVector3 normal;
dReal depth;
int code;
dxBox *b1 = (dxBox*) o1;
dxBox *b2 = (dxBox*) o2;
int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
normal,&depth,&code,flags & NUMC_MASK,contact,skip);
for (int i=0; i<num; i++) {
CONTACT(contact,i*skip)->normal[0] = -normal[0];
CONTACT(contact,i*skip)->normal[1] = -normal[1];
CONTACT(contact,i*skip)->normal[2] = -normal[2];
CONTACT(contact,i*skip)->g1 = o1;
CONTACT(contact,i*skip)->g2 = o2;
}
return num;
}
int dCollideRMCyl (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dxRayMotions *rm = (dxRayMotions*) dGeomGetClassData(o1);
int ret= dCollideCylRay (o2,rm->ray,flags,contact,skip);
for (int i=0; i<ret; i++) {
dContactGeom *c = CONTACT(contact,skip*i);
revert_contact( c );
c->g1 = rm->ray_ownwer;
}
return ret;
}
int dCollideRMS(dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dxRayMotions *rm = (dxRayMotions*) dGeomGetClassData(o1);
int ret= dCollideRaySphere (rm->ray, o2,flags, contact, skip);
for (int i=0; i<ret; i++) {
dContactGeom *c = CONTACT(contact,skip*i);
c->g1 = rm->ray_ownwer;
//c->depth*=60.f;
}
return ret;
}
int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dGeomTransformClass);
dxGeomTransform *tr = (dxGeomTransform*) o1;
if (!tr->obj) return 0;
dUASSERT (tr->obj->parent_space==0,
"GeomTransform encapsulated object must not be in a space");
dUASSERT (tr->obj->body==0,
"GeomTransform encapsulated object must not be attached "
"to a body");
// backup the relative pos and R pointers of the encapsulated geom object,
// and the body pointer
dReal *posbak = tr->obj->pos;
dReal *Rbak = tr->obj->R;
dxBody *bodybak = tr->obj->body;
// compute temporary pos and R for the encapsulated geom object.
// note that final_pos and final_R are valid if no GEOM_AABB_BAD flag,
// because computeFinalTx() will have already been called in
// dxGeomTransform::computeAABB()
if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx();
tr->obj->pos = tr->final_pos;
tr->obj->R = tr->final_R;
tr->obj->body = o1->body;
// do the collision
int n = dCollide (tr->obj,o2,flags,contact,skip);
// if required, adjust the 'g1' values in the generated contacts so that
// thay indicated the GeomTransform object instead of the encapsulated
// object.
if (tr->infomode) {
for (int i=0; i<n; i++) {
dContactGeom *c = CONTACT(contact,skip*i);
c->g1 = o1;
}
}
// restore the pos, R and body
tr->obj->pos = posbak;
tr->obj->R = Rbak;
tr->obj->body = bodybak;
return n;
}
/*
* NOTE!
* If it is necessary to add special processing mode without contact generation
* use NULL contact parameter value as indicator, not zero in flags.
*/
int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,
int skip)
{
dAASSERT(o1 && o2 && contact);
dUASSERT(colliders_initialized,"Please call ODE initialization (dInitODE() or similar) before using the library");
dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number");
dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number");
// Even though comparison for greater or equal to one is used in all the
// other places, here it is more logical to check for greater than zero
// because function does not require any specific number of contact slots -
// it must be just a positive.
dUASSERT((flags & NUMC_MASK) > 0, "no contacts requested");
// Extra precaution for zero contact count in parameters
if ((flags & NUMC_MASK) == 0) return 0;
// no contacts if both geoms are the same
if (o1 == o2) return 0;
// no contacts if both geoms on the same body, and the body is not 0
if (o1->body == o2->body && o1->body) return 0;
o1->recomputePosr();
o2->recomputePosr();
dColliderEntry *ce = &colliders[o1->type][o2->type];
int count = 0;
if (ce->fn) {
if (ce->reverse) {
count = (*ce->fn) (o2,o1,flags,contact,skip);
for (int i=0; i<count; i++) {
dContactGeom *c = CONTACT(contact,skip*i);
c->normal[0] = -c->normal[0];
c->normal[1] = -c->normal[1];
c->normal[2] = -c->normal[2];
dxGeom *tmp = c->g1;
c->g1 = c->g2;
c->g2 = tmp;
int tmpint = c->side1;
c->side1 = c->side2;
c->side2 = tmpint;
}
}
else {
count = (*ce->fn) (o1,o2,flags,contact,skip);
}
}
return count;
}
static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2)
{
SpaceGeomColliderData *d = (SpaceGeomColliderData*) data;
if (d->flags & NUMC_MASK) {
int n = dCollide (o1,o2,d->flags,d->contact,d->skip);
d->contact = CONTACT (d->contact,d->skip*n);
d->flags -= n;
#ifndef dNODEBUG
if(d->flags<=0)
{
//char s[64];
//_snprintf (s,sizeof(s),"tmp %f,%f,%f \n avel %f,%f,%f",tmp[0],tmp[1],tmp[2],b->avel[0],b->avel[1],b->avel[2]);
//dUASSERT(0,"tmp %f,%f,%f \n avel %f,%f,%f",tmp[0],tmp[1],tmp[2],b->avel[0],b->avel[1],b->avel[2]);
dMessage(0, "Collider exceed contact buffer");
}
#endif
}
}
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;
}
int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxCapsule *cyl = (dxCapsule*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
// get p1,p2 = cylinder axis endpoints, get radius
dVector3 p1,p2;
dReal clen = cyl->lz * REAL(0.5);
p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2];
p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6];
p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10];
p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2];
p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6];
p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10];
dReal radius = cyl->radius;
// copy out box center, rotation matrix, and side array
dReal *c = o2->final_posr->pos;
dReal *R = o2->final_posr->R;
const dReal *side = box->side;
// get the closest point between the cylinder axis and the box
dVector3 pl,pb;
dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
// if the capsule is penetrated further than radius
// then pl and pb are equal (up to eps) -> unknown normal
// we simply consider the capsule as box and use the box-box algorithm
#ifdef dSINGLE
dReal mindist = REAL(1e-6);
#else
dReal mindist = REAL(1e-15);
#endif
// if (dCalcPointsDistance3(pl, pb) < mindist) {
if (dDISTANCE(pl, pb) < mindist) {
dVector3 normal;
dReal depth;
int code;
// consider capsule as box
dReal rad2 = radius*REAL(2.0);
const dVector3 capboxside = {rad2, rad2, cyl->lz + rad2};
int num = dBoxBox (c, R, side,
o1->final_posr->pos, o1->final_posr->R, capboxside,
normal, &depth, &code, flags, contact, skip);
for (int i=0; i<num; i++) {
dContactGeom *currContact = CONTACT(contact,i*skip);
currContact->normal[0] = normal[0];
currContact->normal[1] = normal[1];
currContact->normal[2] = normal[2];
currContact->g1 = o1;
currContact->g2 = o2;
currContact->side1 = -1;
currContact->side2 = -1;
}
return num;
}else{
// generate contact point
return dCollideSpheres (pl,radius,pb,0,contact);
}
}
int dcTriListCollider::dTriCyl (
const dReal* v0,const dReal* v1,const dReal* v2,
Triangle* T,
dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip
)
{
// VERIFY (skip >= (int)sizeof(dContactGeom));
VERIFY (dGeomGetClass(o1)== dCylinderClassUser);
const dReal *R = dGeomGetRotation(o1);
const dReal* p=dGeomGetPosition(o1);
dReal radius;
dReal hlz;
dGeomCylinderGetParams(o1,&radius,&hlz);
hlz/=2.f;
// find number of contacts requested
int maxc = flags & NUMC_MASK;
if (maxc < 1) maxc = 1;
if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed
const dVector3 &triAx=T->norm;
dVector3 triSideAx0={T->side0[0],T->side0[1],T->side0[2]}; //{v1[0]-v0[0],v1[1]-v0[1],v1[2]-v0[2]};
dVector3 triSideAx1={T->side1[0],T->side1[1],T->side1[2]}; //{v2[0]-v1[0],v2[1]-v1[1],v2[2]-v1[2]};
dVector3 triSideAx2={v0[0]-v2[0],v0[1]-v2[1],v0[2]-v2[2]};
//dCROSS(triAx,=,triSideAx0,triSideAx1);
int code=0;
dReal signum, outDepth,cos0,cos1,cos2,sin1;
////////////////////////////////////////////////////////////////////////////
//sepparation along tri plane normal;///////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
//accurate_normalize(triAx);
//cos0=dDOT14(triAx,R+0);
cos1=dFabs(dDOT14(triAx,R+1));
//cos2=dDOT14(triAx,R+2);
//sin1=_sqrt(cos0*cos0+cos2*cos2);
////////////////////////
//another way //////////
cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
sin1=_sqrt(REAL(1.)-cos1*cos1);
//////////////////////////////
dReal sidePr=cos1*hlz+sin1*radius;
dReal dist=-T->dist; //dDOT(triAx,v0)-dDOT(triAx,p);
if(dist>0.f) RETURN0;
dReal depth=sidePr-dFabs(dist);
outDepth=depth;
signum=dist>0.f ? 1.f : -1.f;
code=0;
if(depth<0.f) RETURN0;
dReal depth0,depth1,depth2,dist0,dist1,dist2;
bool isPdist0,isPdist1,isPdist2;
bool testV0,testV1,testV2;
bool sideTestV00,sideTestV01,sideTestV02;
bool sideTestV10,sideTestV11,sideTestV12;
bool sideTestV20,sideTestV21,sideTestV22;
//////////////////////////////////////////////////////////////////////////////
//cylinder axis - one of the triangle vertexes touches cylinder's flat surface
//////////////////////////////////////////////////////////////////////////////
dist0=dDOT14(v0,R+1)-dDOT14(p,R+1);
dist1=dDOT14(v1,R+1)-dDOT14(p,R+1);
dist2=dDOT14(v2,R+1)-dDOT14(p,R+1);
isPdist0=dist0>0.f;
isPdist1=dist1>0.f;
isPdist2=dist2>0.f;
depth0=hlz-dFabs(dist0);
depth1=hlz-dFabs(dist1);
depth2=hlz-dFabs(dist2);
testV0=depth0>0.f;
testV1=depth1>0.f;
testV2=depth2>0.f;
if(isPdist0==isPdist1 && isPdist1== isPdist2) //(here and lower) check the tryangle is on one side of the cylinder
{
if(depth0>depth1)
if(depth0>depth2)
if(testV0){
if(depth0<outDepth)
{
signum= isPdist0 ? 1.f : -1.f;
outDepth=depth0;
code=1;
}
}
else
RETURN0;
else
if(testV2){
if(depth2<outDepth)
{
outDepth=depth2;
signum= isPdist2 ? 1.f : -1.f;
code=3;
}
}
else
RETURN0;
else
if(depth1>depth2)
if(testV1){
if(depth1<outDepth)
{
outDepth=depth1;
signum= isPdist1 ? 1.f : -1.f;
code=2;
}
}
else
RETURN0;
else
if(testV2){
if(depth2<outDepth)
{
outDepth=depth2;
signum= isPdist2 ? 1.f : -1.f;
code=2;
}
}
else RETURN0;
}
dVector3 axis,outAx;
dReal posProj;
dReal pointDepth=0.f;
#define TEST(vx,ox1,ox2,c) \
{\
posProj=dDOT14(v##vx,R+1)-dDOT14(p,R+1);\
\
axis[0]=v##vx[0]-p[0]-R[1]*posProj;\
axis[1]=v##vx[1]-p[1]-R[5]*posProj;\
axis[2]=v##vx[2]-p[2]-R[9]*posProj;\
\
accurate_normalize(axis);\
\
\
dist0=dDOT(v0,axis)-dDOT(p,axis);\
dist1=dDOT(v1,axis)-dDOT(p,axis);\
dist2=dDOT(v2,axis)-dDOT(p,axis);\
\
isPdist0=dist0>0.f;\
isPdist1=dist1>0.f;\
isPdist2=dist2>0.f;\
\
depth0=radius-dFabs(dist0);\
depth1=radius-dFabs(dist1);\
depth2=radius-dFabs(dist2);\
\
sideTestV##vx##0=depth0>0.f;\
sideTestV##vx##1=depth1>0.f;\
sideTestV##vx##2=depth2>0.f;\
\
if(isPdist0==isPdist1 && isPdist1== isPdist2)\
\
{\
if(sideTestV##vx##0||sideTestV##vx##1||sideTestV##vx##2){\
if(!(depth##vx<depth##ox1 || depth##vx<depth##ox2))\
{\
if(depth##vx<outDepth && depth##vx > pointDepth)\
{\
pointDepth=depth##vx;\
signum= isPdist##vx ? 1.f : -1.f;\
outAx[0]=axis[0];\
outAx[1]=axis[1];\
outAx[2]=axis[2];\
code=c;\
}\
}\
}\
else RETURN0;\
\
\
\
}\
}
if(testV0) TEST(0,1,2,4)
if(testV1 ) TEST(1,2,0,5)
//&& sideTestV01
if(testV2 ) TEST(2,0,1,6)
//&& sideTestV02 && sideTestV12
#undef TEST
dVector3 tpos,pos;
if(code>3) outDepth=pointDepth; //deepest vertex axis used if its depth less than outDepth
//else{
//bool outV0=!(testV0&&sideTestV00&&sideTestV10&&sideTestV20);
//bool outV1=!(testV1&&sideTestV01&&sideTestV11&&sideTestV21);
//bool outV2=!(testV2&&sideTestV02&&sideTestV12&&sideTestV22);
bool outV0=true;
bool outV1=true;
bool outV2=true;
/////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
///crosses between triangle sides and cylinder axis//////////////////////////
/////////////////////////////////////////////////////////////////////////////
#define TEST(ax,nx,ox,c) if(cylinderCrossesLine(p,R+1,hlz,v##ax,v##nx,triSideAx##ax,tpos)) {\
dCROSS114(axis,=,triSideAx##ax,R+1);\
accurate_normalize(axis);\
dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\
dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\
\
isPdist##ax=dist##ax>0.f;\
isPdist##ox=dist##ox>0.f;\
\
if(isPdist##ax == isPdist##ox)\
{\
depth##ax=radius-dFabs(dist##ax);\
depth##ox=radius-dFabs(dist##ox);\
\
if(depth##ax>0.f){\
if(depth##ax<=outDepth && depth##ax>=depth##ox) \
{\
outDepth=depth##ax;\
signum= isPdist##ax ? 1.f : -1.f;\
outAx[0]=axis[0];\
outAx[1]=axis[1];\
outAx[2]=axis[2];\
pos[0]=tpos[0];\
pos[1]=tpos[1];\
pos[2]=tpos[2];\
code=c;\
}\
}\
else if(depth##ox<0.f) RETURN0;\
\
}\
}
accurate_normalize(triSideAx0);
if(outV0&&outV1)
TEST(0,1,2,7)
accurate_normalize(triSideAx1);
if(outV1&&outV2)
TEST(1,2,0,8)
accurate_normalize(triSideAx2);
if(outV2&&outV0)
TEST(2,0,1,9)
#undef TEST
////////////////////////////////////
//test cylinder rings on triangle sides////
////////////////////////////////////
dVector3 tAx,cen;
dReal sign;
bool cs;
#define TEST(ax,nx,ox,c) \
{\
posProj=dDOT(p,triSideAx##ax)-dDOT(v##ax,triSideAx##ax);\
axis[0]=p[0]-v0[0]-triSideAx##ax[0]*posProj;\
axis[1]=p[1]-v0[1]-triSideAx##ax[1]*posProj;\
axis[2]=p[2]-v0[2]-triSideAx##ax[2]*posProj;\
\
sign=dDOT14(axis,R+1)>0.f ? 1.f :-1.f;\
cen[0]=p[0]-sign*R[1]*hlz;\
cen[1]=p[1]-sign*R[5]*hlz;\
cen[2]=p[2]-sign*R[9]*hlz;\
\
cs=circleLineIntersection(R+1,cen,radius,triSideAx##ax,v##ax,-sign,tpos);\
\
axis[0]=tpos[0]-cen[0];\
axis[1]=tpos[1]-cen[1];\
axis[2]=tpos[2]-cen[2];\
\
if(cs){ \
\
cos0=dDOT14(axis,R+0);\
cos2=dDOT14(axis,R+2);\
tAx[0]=R[2]*cos0-R[0]*cos2;\
tAx[1]=R[6]*cos0-R[4]*cos2;\
tAx[2]=R[10]*cos0-R[8]*cos2;\
\
dCROSS(axis,=,triSideAx##ax,tAx);\
\
}\
accurate_normalize(axis);\
dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\
if(dist##ax*dDOT(axis,triSideAx##nx)>0.f){\
\
cos0=dDOT14(axis,R+0);\
cos1=dFabs(dDOT14(axis,R+1));\
cos2=dDOT14(axis,R+2);\
\
\
sin1=_sqrt(cos0*cos0+cos2*cos2);\
\
sidePr=cos1*hlz+sin1*radius;\
\
\
dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\
\
isPdist##ax=dist##ax>0.f;\
isPdist##ox=dist##ox>0.f;\
\
if(isPdist##ax == isPdist##ox) \
\
{\
depth##ax=sidePr-dFabs(dist##ax);\
depth##ox=sidePr-dFabs(dist##ox);\
\
if(depth##ax>0.f){\
if(depth##ax<outDepth) \
{\
outDepth=depth##ax;\
signum= isPdist##ax ? 1.f : -1.f;\
outAx[0]=axis[0];\
outAx[1]=axis[1];\
outAx[2]=axis[2];\
pos[0]=tpos[0];\
pos[1]=tpos[1];\
pos[2]=tpos[2];\
code=c;\
}\
}\
else if(depth##ox<0.f) RETURN0;\
\
\
}\
}\
}
if(7!=code)
TEST(0,1,2,10)
if(8!=code)
TEST(1,2,0,11)
if(9!=code)
TEST(2,0,1,12)
#undef TEST
//}
//////////////////////////////////////////////////////////////////////
///if we get to this poit tri touches cylinder///////////////////////
/////////////////////////////////////////////////////////////////////
//VERIFY( g_pGameLevel );
CDB::TRI* T_array = inl_ph_world().ObjectSpace().GetStaticTris();
dVector3 norm;
unsigned int ret;
flags8& gl_state=gl_cl_tries_state[I-B];
if(code==0){
norm[0]=triAx[0]*signum;
norm[1]=triAx[1]*signum;
norm[2]=triAx[2]*signum;
dReal Q1 = dDOT14(norm,R+0);
dReal Q2 = dDOT14(norm,R+1);
dReal Q3 = dDOT14(norm,R+2);
dReal factor =_sqrt(Q1*Q1+Q3*Q3);
dReal C1,C3;
dReal centerDepth;//depth in the cirle centre
if(factor>0.f)
{
C1=Q1/factor;
C3=Q3/factor;
}
else
{
C1=1.f;
C3=0.f;
}
dReal A1 = radius * C1;//cosinus
dReal A2 = hlz;//Q2
dReal A3 = radius * C3;//sinus
if(factor>0.f) centerDepth=outDepth-A1*Q1-A3*Q3; else centerDepth=outDepth;
pos[0]=p[0];
pos[1]=p[1];
pos[2]=p[2];
pos[0]+= Q2>0 ? hlz*R[1]:-hlz*R[1];
pos[1]+= Q2>0 ? hlz*R[5]:-hlz*R[5];
pos[2]+= Q2>0 ? hlz*R[9]:-hlz*R[9];
ret=0;
dVector3 cross0, cross1, cross2;
dReal ds0,ds1,ds2;
dCROSS(cross0,=,triAx,triSideAx0);
ds0=dDOT(cross0,v0);
dCROSS(cross1,=,triAx,triSideAx1);
ds1=dDOT(cross1,v1);
dCROSS(cross2,=,triAx,triSideAx2);
ds2=dDOT(cross2,v2);
contact->pos[0] = pos[0]+A1*R[0]+A3*R[2];
contact->pos[1] = pos[1]+A1*R[4]+A3*R[6];
contact->pos[2] = pos[2]+A1*R[8]+A3*R[10];
if(dDOT(cross0,contact->pos)-ds0>0.f &&
dDOT(cross1,contact->pos)-ds1>0.f &&
dDOT(cross2,contact->pos)-ds2>0.f){
contact->depth = outDepth;
ret=1;
}
if(dFabs(Q2)>M_SQRT1_2){
A1=(-C1*M_COS_PI_3-C3*M_SIN_PI_3)*radius;
A3=(-C3*M_COS_PI_3+C1*M_SIN_PI_3)*radius;
CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2];
CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6];
CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10];
CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3;
if(CONTACT(contact,ret*skip)->depth>0.f)
if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f &&
dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f &&
dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret;
A1=(-C1*M_COS_PI_3+C3*M_SIN_PI_3)*radius;
A3=(-C3*M_COS_PI_3-C1*M_SIN_PI_3)*radius;
CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2];
CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6];
CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10];
CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3;
if(CONTACT(contact,ret*skip)->depth>0.f)
if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f &&
dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f &&
dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret;
} else {
CONTACT(contact,ret*skip)->pos[0]=contact->pos[0]-2.f*(Q2>0 ? hlz*R[1]:-hlz*R[1]);
CONTACT(contact,ret*skip)->pos[1]=contact->pos[1]-2.f*(Q2>0 ? hlz*R[5]:-hlz*R[5]);
CONTACT(contact,ret*skip)->pos[2]=contact->pos[2]-2.f*(Q2>0 ? hlz*R[9]:-hlz*R[9]);
CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2);
if(CONTACT(contact,ret*skip)->depth>0.f)
if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f &&
dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f &&
dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret;
}
}
int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
dReal Length = dGeomRayGetLength(RayGeom);
dVector3 Diff;
Diff[0] = Origin[0] - Position[0];
Diff[1] = Origin[1] - Position[1];
Diff[2] = Origin[2] - Position[2];
Diff[3] = Origin[3] - Position[3];
Direction[0] *= Length;
Direction[1] *= Length;
Direction[2] *= Length;
Direction[3] *= Length;
dReal A = Length * Length;
dReal B = dDOT(Diff, Direction);
dReal C = dDOT(Diff, Diff) - (Radius * Radius);
dReal Discr = B * B - A * C;
if (Discr < REAL(0.0)){
return 0;
}
else if (Discr > REAL(0.0)){
dReal T[2];
dReal Root = dSqrt(Discr);
dReal InvA = REAL(1.0) / A;
T[0] = (-B - Root) * InvA;
T[1] = (-B + Root) * InvA;
if (T[0] >= REAL(0.0)){
dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
//Contact0->normal = 0;
Contact0->depth = 0.0f;
Contact0->g1 = RayGeom;
Contact0->g2 = SphereGeom;
dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact1->normal = 0;
Contact1->depth = 0.0f;
Contact1->g1 = RayGeom;
Contact1->g2 = SphereGeom;
return 2;
}
else if (T[1] >= REAL(0.0)){
dContactGeom* Contact = CONTACT(Flags, Contacts, 1, Stride);
Contact->pos[0] = Origin[0] + T[1] * Direction[0];
Contact->pos[1] = Origin[1] + T[1] * Direction[1];
Contact->pos[2] = Origin[2] + T[1] * Direction[2];
Contact->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = SphereGeom;
return 1;
}
else return 0;
}
else{
dReal T;
T = -B / A;
if (T >= REAL(0.0)){
dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
Contact->pos[0] = Origin[0] + T * Direction[0];
Contact->pos[1] = Origin[1] + T * Direction[1];
Contact->pos[2] = Origin[2] + T * Direction[2];
Contact->pos[3] = Origin[3] + T * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = SphereGeom;
return 1;
}
else return 0;
}
}
int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
int i;
const dReal tolerance = REAL(1e-5);
dxCapsule *cyl1 = (dxCapsule*) o1;
dxCapsule *cyl2 = (dxCapsule*) o2;
contact->g1 = o1;
contact->g2 = o2;
// copy out some variables, for convenience
dReal lz1 = cyl1->lz * REAL(0.5);
dReal lz2 = cyl2->lz * REAL(0.5);
dReal *pos1 = o1->final_posr->pos;
dReal *pos2 = o2->final_posr->pos;
dReal axis1[3],axis2[3];
axis1[0] = o1->final_posr->R[2];
axis1[1] = o1->final_posr->R[6];
axis1[2] = o1->final_posr->R[10];
axis2[0] = o2->final_posr->R[2];
axis2[1] = o2->final_posr->R[6];
axis2[2] = o2->final_posr->R[10];
// if the cylinder axes are close to parallel, we'll try to detect up to
// two contact points along the body of the cylinder. if we can't find any
// points then we'll fall back to the closest-points algorithm. note that
// we are not treating this special case for reasons of degeneracy, but
// because we want two contact points in some situations. the closet-points
// algorithm is robust in all casts, but it can return only one contact.
dVector3 sphere1,sphere2;
dReal a1a2 = dDOT (axis1,axis2);
dReal det = REAL(1.0)-a1a2*a1a2;
if (det < tolerance) {
// the cylinder axes (almost) parallel, so we will generate up to two
// contacts. alpha1 and alpha2 (line position parameters) are related by:
// alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2)
// or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2)
// first compute where the two cylinders overlap in alpha1 space:
if (a1a2 < 0) {
axis2[0] = -axis2[0];
axis2[1] = -axis2[1];
axis2[2] = -axis2[2];
}
dReal q[3];
for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
dReal k = dDOT (axis1,q);
dReal a1lo = -lz1;
dReal a1hi = lz1;
dReal a2lo = -lz2 - k;
dReal a2hi = lz2 - k;
dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
if (lo <= hi) {
int num_contacts = flags & NUMC_MASK;
if (num_contacts >= 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=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
int n1 = dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
if (n1) {
for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
dContactGeom *c2 = CONTACT(contact,skip);
int n2 = dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius, c2);
if (n2) {
c2->g1 = o1;
c2->g2 = o2;
return 2;
}
}
}
// just one contact to generate, so put it in the middle of
// the range
dReal alpha1 = (lo + hi) * REAL(0.5);
dReal alpha2 = alpha1 + k;
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
return dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
}
}
// use the closest point algorithm
dVector3 a1,a2,b1,b2;
a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1;
a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1;
a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1;
a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1;
a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1;
a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1;
b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2;
b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2;
b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2;
b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2;
b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2;
b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2;
dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2);
return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
}
int dcTriListCollider::dSortedTriCyl (
const dReal* triSideAx0,const dReal* triSideAx1,
const dReal* triAx,
//const dReal* v0,
//const dReal* v1,
//const dReal* v2,
CDB::TRI* T,
dReal dist,
dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip
)
{
VERIFY (dGeomGetClass(o1)== dCylinderClassUser);
const dReal *R = dGeomGetRotation(o1);
const dReal* p=dGeomGetPosition(o1);
dReal radius;
dReal hlz;
dGeomCylinderGetParams(o1,&radius,&hlz);
hlz/=2.f;
// find number of contacts requested
int maxc = flags & NUMC_MASK;
if (maxc < 1) maxc = 1;
if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed
dReal signum, outDepth,cos1,sin1;
////////////////////////////////////////////////////////////////////////////
//sepparation along tri plane normal;///////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
//cos0=dDOT14(triAx,R+0);
cos1=dFabs(dDOT14(triAx,R+1));
//cos2=dDOT14(triAx,R+2);
//sin1=_sqrt(cos0*cos0+cos2*cos2);
////////////////////////
//another way //////////
cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
sin1=_sqrt(REAL(1.)-cos1*cos1);
//////////////////////////////
dReal sidePr=cos1*hlz+sin1*radius;
if(dist>0.f)
return 0;
dReal depth=sidePr-dist;
outDepth=depth;
signum=-1.f;
int code=0;
if(depth<0.f) return 0;
dVector3 norm;
unsigned int ret=0;
dVector3 pos;
if(code==0){
norm[0]=triAx[0]*signum;
norm[1]=triAx[1]*signum;
norm[2]=triAx[2]*signum;
dReal Q1 = signum*dDOT14(triAx,R+0);
dReal Q2 = signum*dDOT14(triAx,R+1);
dReal Q3 = signum*dDOT14(triAx,R+2);
dReal factor =_sqrt(Q1*Q1+Q3*Q3);
dReal C1,C3;
dReal centerDepth;//depth in the cirle centre
if(factor>0.f)
{
C1=Q1/factor;
C3=Q3/factor;
}
else
{
C1=1.f;
C3=0.f;
}
dReal A1 = radius * C1;//cosinus
dReal A2 = hlz*Q2;
dReal A3 = radius * C3;//sinus
if(factor>0.f) centerDepth=outDepth-A1*Q1-A3*Q3; else centerDepth=outDepth;
pos[0]=p[0];
pos[1]=p[1];
pos[2]=p[2];
pos[0]+= A2>0 ? hlz*R[1]:-hlz*R[1];
pos[1]+= A2>0 ? hlz*R[5]:-hlz*R[5];
pos[2]+= A2>0 ? hlz*R[9]:-hlz*R[9];
ret=0;
contact->pos[0] = pos[0]+A1*R[0]+A3*R[2];
contact->pos[1] = pos[1]+A1*R[4]+A3*R[6];
contact->pos[2] = pos[2]+A1*R[8]+A3*R[10];
{
contact->depth = outDepth;
ret=1;
}
if(dFabs(Q2)>M_SQRT1_2){
A1=(-C1*M_COS_PI_3-C3*M_SIN_PI_3)*radius;
A3=(-C3*M_COS_PI_3+C1*M_SIN_PI_3)*radius;
CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2];
CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6];
CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10];
CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3;
if(CONTACT(contact,ret*skip)->depth>0.f)++ret;
A1=(-C1*M_COS_PI_3+C3*M_SIN_PI_3)*radius;
A3=(-C3*M_COS_PI_3-C1*M_SIN_PI_3)*radius;
CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2];
CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6];
CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10];
CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3;
if(CONTACT(contact,ret*skip)->depth>0.f)++ret;
} else {
CONTACT(contact,ret*skip)->pos[0]=contact->pos[0]-2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]);
CONTACT(contact,ret*skip)->pos[1]=contact->pos[1]-2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]);
CONTACT(contact,ret*skip)->pos[2]=contact->pos[2]-2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]);
CONTACT(contact,ret*skip)->depth=outDepth-Q2*2.f*A2;
if(CONTACT(contact,ret*skip)->depth>0.f)++ret;
}
}
if((int)ret>maxc) ret=(unsigned int)maxc;
for (unsigned int i=0; i<ret; ++i) {
CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o2);
CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o1);
CONTACT(contact,i*skip)->normal[0] = norm[0];
CONTACT(contact,i*skip)->normal[1] = norm[1];
CONTACT(contact,i*skip)->normal[2] = norm[2];
SURFACE(contact,i*skip)->mode=T->material;
}
if(ret&&dGeomGetUserData(o1)->callback)dGeomGetUserData(o1)->callback(T,contact);
return ret;
}
EXPORT_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 *return_code,
int maxc, dContactGeom *contact, int skip)
{
const dReal fudge_factor = REAL(1.05);
dVector3 p,pp,normalC;
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;
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] = dMUL(side1[0],REAL(0.5));
A[1] = dMUL(side1[1],REAL(0.5));
A[2] = dMUL(side1[2],REAL(0.5));
B[0] = dMUL(side2[0],REAL(0.5));
B[1] = dMUL(side2[1],REAL(0.5));
B[2] = dMUL(side2[2],REAL(0.5));
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(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.
#define TST(expr1,expr2,norm,cc) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + dMUL(B[0],Q11) + dMUL(B[1],Q12) + dMUL(B[2],Q13)),R1+0,1);
TST (pp[1],(A[1] + dMUL(B[0],Q21) + dMUL(B[1],Q22) + dMUL(B[2],Q23)),R1+1,2);
TST (pp[2],(A[2] + dMUL(B[0],Q31) + dMUL(B[1],Q32) + dMUL(B[2],Q33)),R1+2,3);
// separating axis = v1,v2,v3
TST (dDOT41(R2+0,p),(dMUL(A[0],Q11) + dMUL(A[1],Q21) + dMUL(A[2],Q31) + B[0]),R2+0,4);
TST (dDOT41(R2+1,p),(dMUL(A[0],Q12) + dMUL(A[1],Q22) + dMUL(A[2],Q32) + B[1]),R2+1,5);
TST (dDOT41(R2+2,p),(dMUL(A[0],Q13) + dMUL(A[1],Q23) + dMUL(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) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
l = dSqrt (dMUL((n1),(n1)) + dMUL((n2),(n2)) + dMUL((n3),(n3))); \
if (l > 0) { \
s2 = dDIV(s2,l); \
if (dMUL(s2,fudge_factor) > s) { \
s = s2; \
normalR = 0; \
normalC[0] = dDIV((n1),l); normalC[1] = dDIV((n2),l); normalC[2] = dDIV((n3),l); \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST((dMUL(pp[2],R21)-dMUL(pp[1],R31)),(dMUL(A[1],Q31)+dMUL(A[2],Q21)+dMUL(B[1],Q13)+dMUL(B[2],Q12)),0,-R31,R21,7);
TST((dMUL(pp[2],R22)-dMUL(pp[1],R32)),(dMUL(A[1],Q32)+dMUL(A[2],Q22)+dMUL(B[0],Q13)+dMUL(B[2],Q11)),0,-R32,R22,8);
TST((dMUL(pp[2],R23)-dMUL(pp[1],R33)),(dMUL(A[1],Q33)+dMUL(A[2],Q23)+dMUL(B[0],Q12)+dMUL(B[1],Q11)),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST((dMUL(pp[0],R31)-dMUL(pp[2],R11)),(dMUL(A[0],Q31)+dMUL(A[2],Q11)+dMUL(B[1],Q23)+dMUL(B[2],Q22)),R31,0,-R11,10);
TST((dMUL(pp[0],R32)-dMUL(pp[2],R12)),(dMUL(A[0],Q32)+dMUL(A[2],Q12)+dMUL(B[0],Q23)+dMUL(B[2],Q21)),R32,0,-R12,11);
TST((dMUL(pp[0],R33)-dMUL(pp[2],R13)),(dMUL(A[0],Q33)+dMUL(A[2],Q13)+dMUL(B[0],Q22)+dMUL(B[1],Q21)),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST((dMUL(pp[1],R11)-dMUL(pp[0],R21)),(dMUL(A[0],Q21)+dMUL(A[1],Q11)+dMUL(B[1],Q33)+dMUL(B[2],Q32)),-R21,R11,0,13);
TST((dMUL(pp[1],R12)-dMUL(pp[0],R22)),(dMUL(A[0],Q22)+dMUL(A[1],Q12)+dMUL(B[0],Q33)+dMUL(B[2],Q31)),-R22,R12,0,14);
TST((dMUL(pp[1],R13)-dMUL(pp[0],R23)),(dMUL(A[0],Q23)+dMUL(A[1],Q13)+dMUL(B[0],Q32)+dMUL(B[1],Q31)),-R23,R13,0,15);
#undef TST
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;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=0; i<3; i++) pa[i] += dMUL(sign,dMUL(A[j],R1[i*4+j]));
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=0; i<3; i++) pb[i] += dMUL(sign,dMUL(B[j],R2[i*4+j]));
}
dReal alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += dMUL(ua[i],alpha);
for (i=0; i<3; i++) pb[i] += dMUL(ub[i],beta);
for (i=0; i<3; i++) contact[0].pos[i] = dMUL(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).
const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
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];
}
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 indident face. the other axis numbers of the indicent 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] + dMUL(Sb[lanr],Rb[i*4+lanr]);
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - dMUL(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 = dDOT14 (center,Ra+code1);
c2 = dDOT14 (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 = dDOT44 (Ra+code1,Rb+a1);
m12 = dDOT44 (Ra+code1,Rb+a2);
m21 = dDOT44 (Ra+code2,Rb+a1);
m22 = dDOT44 (Ra+code2,Rb+a2);
{
dReal k1 = dMUL(m11,Sb[a1]);
dReal k2 = dMUL(m21,Sb[a1]);
dReal k3 = dMUL(m12,Sb[a2]);
dReal k4 = dMUL(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(dMUL(m11,m22) - dMUL(m12,m21));
m11 = dMUL(m11,det1);
m12 = dMUL(m12,det1);
m21 = dMUL(m21,det1);
m22 = dMUL(m22,det1);
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
dReal k1 = dMUL(m22,(ret[j*2]-c1)) - dMUL(m12,(ret[j*2+1]-c2));
dReal k2 = -dMUL(m21,(ret[j*2]-c1)) + dMUL(m11,(ret[j*2+1]-c2));
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + dMUL(k1,Rb[i*4+a1]) + dMUL(k2,Rb[i*4+a2]);
dep[cnum] = Sa[codeN] - dDOT(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 < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
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 {
// 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;
}
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 dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dxBox *box = (dxBox*) o1;
dxPlane *plane = (dxPlane*) o2;
contact->g1 = o1;
contact->g2 = o2;
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 = dDOT14(n,R+0);
dReal Q2 = dDOT14(n,R+1);
dReal Q3 = dDOT14(n,R+2);
dReal A1 = dMUL(box->side[0],Q1);
dReal A2 = dMUL(box->side[1],Q2);
dReal A3 = dMUL(box->side[2],Q3);
dReal B1 = dFabs(A1);
dReal B2 = dFabs(A2);
dReal B3 = dFabs(A3);
// early exit test
dReal depth = plane->p[3] + dMUL(REAL(0.5),(B1+B2+B3)) - dDOT(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;
if (maxc > 3) maxc = 3; // no more than 3 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 dMUL(REAL(0.5),dMUL(box->side[i],R[0+i])); \
p[1] op dMUL(REAL(0.5),dMUL(box->side[i],R[4+i])); \
p[2] op dMUL(REAL(0.5),dMUL(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->normal[0] = n[0];
contact->normal[1] = n[1];
contact->normal[2] = n[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 dMUL(box->side[j],R[0+j]); \
CONTACT(contact,i*skip)->pos[1] = p[1] op dMUL(box->side[j],R[4+j]); \
CONTACT(contact,i*skip)->pos[2] = p[2] op dMUL(box->side[j],R[8+j]);
#define BAR(ctact,side,sideinc) \
depth -= B ## sideinc; \
if (depth < 0) goto done; \
if (A ## sideinc > 0) { FOO(ctact,side,+) } else { FOO(ctact,side,-) } \
CONTACT(contact,ctact*skip)->depth = depth; \
ret++;
CONTACT(contact,skip)->normal[0] = n[0];
CONTACT(contact,skip)->normal[1] = n[1];
CONTACT(contact,skip)->normal[2] = n[2];
if (maxc == 3) {
CONTACT(contact,2*skip)->normal[0] = n[0];
CONTACT(contact,2*skip)->normal[1] = n[1];
CONTACT(contact,2*skip)->normal[2] = n[2];
}
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:
for (int i=0; i<ret; i++) {
CONTACT(contact,i*skip)->g1 = o1;
CONTACT(contact,i*skip)->g2 = o2;
}
return ret;
}
int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom);
dVector3 Extents;
dGeomBoxGetLengths(BoxGeom, Extents);
Extents[0] /= 2;
Extents[1] /= 2;
Extents[2] /= 2;
Extents[3] /= 2;
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
dReal Length = dGeomRayGetLength(RayGeom);
dVector3 Diff;
Diff[0] = Origin[0] - Position[0];
Diff[1] = Origin[1] - Position[1];
Diff[2] = Origin[2] - Position[2];
Diff[3] = Origin[3] - Position[3];
Direction[0] *= Length;
Direction[1] *= Length;
Direction[2] *= Length;
Direction[3] *= Length;
dVector3 Rot[3];
Decompose(Rotation, Rot);
dVector3 TransOrigin;
TransOrigin[0] = dDOT(Diff, Rot[0]);
TransOrigin[1] = dDOT(Diff, Rot[1]);
TransOrigin[2] = dDOT(Diff, Rot[2]);
TransOrigin[3] = REAL(0.0);
dVector3 TransDirection;
TransDirection[0] = dDOT(Direction, Rot[0]);
TransDirection[1] = dDOT(Direction, Rot[1]);
TransDirection[2] = dDOT(Direction, Rot[2]);
TransDirection[3] = REAL(0.0);
dReal T[2];
T[0] = 0.0f;
T[1] = dInfinity;
bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]);
if (Intersect){
if (T[0] > REAL(0.0)){
dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
//Contact0->normal = 0;
Contact0->depth = 0.0f;
Contact0->g1 = RayGeom;
Contact0->g2 = BoxGeom;
dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact1->normal = 0;
Contact1->depth = 0.0f;
Contact1->g1 = RayGeom;
Contact1->g2 = BoxGeom;
return 2;
}
else{
dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
Contact->pos[0] = Origin[0] + T[1] * Direction[0];
Contact->pos[1] = Origin[1] + T[1] * Direction[1];
Contact->pos[2] = Origin[2] + T[1] * Direction[2];
Contact->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = BoxGeom;
return 1;
}
}
else return 0;
}
