static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
dxQuickStepParameters *qs)
{
const int num_iterations = qs->num_iterations;
const dReal sor_w = qs->w; // SOR over-relaxation parameter
int i,j;
#ifdef WARM_STARTING
// for warm starting, this seems to be necessary to prevent
// jerkiness in motor-driven joints. i have no idea why this works.
for (i=0; i<m; i++) lambda[i] *= 0.9;
#else
dSetZero (lambda,m);
#endif
// the lambda computed at the previous iteration.
// this is used to measure error for when we are reordering the indexes.
dRealAllocaArray (last_lambda,m);
// a copy of the 'hi' vector in case findex[] is being used
dRealAllocaArray (hicopy,m);
memcpy (hicopy,hi,m*sizeof(dReal));
// precompute iMJ = inv(M)*J'
dRealAllocaArray (iMJ,m*12);
compute_invM_JT (m,J,iMJ,jb,body,invI);
// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
// as we change lambda.
#ifdef WARM_STARTING
multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
#else
dSetZero (fc,nb*6);
#endif
// precompute 1 / diagonals of A
dRealAllocaArray (Ad,m);
dRealPtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
dReal sum = 0;
for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
if (jb[i*2+1] >= 0) {
for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
}
iMJ_ptr += 12;
J_ptr += 12;
Ad[i] = sor_w / (sum + cfm[i]);
}
// scale J and b by Ad
J_ptr = J;
for (i=0; i<m; i++) {
for (j=0; j<12; j++) {
J_ptr[0] *= Ad[i];
J_ptr++;
}
b[i] *= Ad[i];
}
// scale Ad by CFM
for (i=0; i<m; i++) Ad[i] *= cfm[i];
// order to solve constraint rows in
IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
#ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first.
j=0;
for (i=0; i<m; i++) if (findex[i] < 0) order[j++].index = i;
for (i=0; i<m; i++) if (findex[i] >= 0) order[j++].index = i;
dIASSERT (j==m);
#endif
for (int iteration=0; iteration < num_iterations; iteration++) {
#ifdef REORDER_CONSTRAINTS
// constraints with findex < 0 always come first.
if (iteration < 2) {
// for the first two iterations, solve the constraints in
// the given order
for (i=0; i<m; i++) {
order[i].error = i;
order[i].findex = findex[i];
order[i].index = i;
}
}
else {
// sort the constraints so that the ones converging slowest
// get solved last. use the absolute (not relative) error.
for (i=0; i<m; i++) {
dReal v1 = dFabs (lambda[i]);
dReal v2 = dFabs (last_lambda[i]);
dReal max = (v1 > v2) ? v1 : v2;
if (max > 0) {
//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
order[i].error = dFabs(lambda[i]-last_lambda[i]);
}
else {
order[i].error = dInfinity;
}
order[i].findex = findex[i];
order[i].index = i;
}
}
qsort (order,m,sizeof(IndexError),&compare_index_error);
#endif
#ifdef RANDOMLY_REORDER_CONSTRAINTS
if ((iteration & 7) == 0) {
for (i=1; i<m; ++i) {
IndexError tmp = order[i];
int swapi = dRandInt(i+1);
order[i] = order[swapi];
order[swapi] = tmp;
}
}
#endif
//@@@ potential optimization: swap lambda and last_lambda pointers rather
// than copying the data. we must make sure lambda is properly
// returned to the caller
memcpy (last_lambda,lambda,m*sizeof(dReal));
for (int i=0; i<m; i++) {
// @@@ potential optimization: we could pre-sort J and iMJ, thereby
// linearizing access to those arrays. hmmm, this does not seem
// like a win, but we should think carefully about our memory
// access pattern.
int index = order[i].index;
J_ptr = J + index*12;
iMJ_ptr = iMJ + index*12;
// set the limits for this constraint. note that 'hicopy' is used.
// this is the place where the QuickStep method differs from the
// direct LCP solving method, since that method only performs this
// limit adjustment once per time step, whereas this method performs
// once per iteration per constraint row.
// the constraints are ordered so that all lambda[] values needed have
// already been computed.
if (findex[index] >= 0) {
hi[index] = dFabs (hicopy[index] * lambda[findex[index]]);
lo[index] = -hi[index];
}
int b1 = jb[index*2];
int b2 = jb[index*2+1];
dReal delta = b[index] - lambda[index]*Ad[index];
dRealMutablePtr fc_ptr = fc + 6*b1;
// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = fc + 6*b2;
delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
}
// compute lambda and clamp it to [lo,hi].
// @@@ potential optimization: does SSE have clamping instructions
// to save test+jump penalties here?
dReal new_lambda = lambda[index] + delta;
if (new_lambda < lo[index]) {
delta = lo[index]-lambda[index];
lambda[index] = lo[index];
}
else if (new_lambda > hi[index]) {
delta = hi[index]-lambda[index];
lambda[index] = hi[index];
}
else {
lambda[index] = new_lambda;
}
//@@@ a trick that may or may not help
//dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations));
//delta *= ramp;
// update fc.
// @@@ potential optimization: SIMD for this and the b2 >= 0 case
fc_ptr = fc + 6*b1;
fc_ptr[0] += delta * iMJ_ptr[0];
fc_ptr[1] += delta * iMJ_ptr[1];
fc_ptr[2] += delta * iMJ_ptr[2];
fc_ptr[3] += delta * iMJ_ptr[3];
fc_ptr[4] += delta * iMJ_ptr[4];
fc_ptr[5] += delta * iMJ_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = fc + 6*b2;
fc_ptr[0] += delta * iMJ_ptr[6];
fc_ptr[1] += delta * iMJ_ptr[7];
fc_ptr[2] += delta * iMJ_ptr[8];
fc_ptr[3] += delta * iMJ_ptr[9];
fc_ptr[4] += delta * iMJ_ptr[10];
fc_ptr[5] += delta * iMJ_ptr[11];
}
}
}
}
bool solvePGS(int n, int nskip, int /*nub*/, double * A, double * x, double * b,
double * lo, double * hi, int * findex, PGSOption * option)
{
// LDLT solver will work !!!
//if (nub == n)
//{
// return LDLTSolver(n,nskip,A,x,b)
//}
int i, j, iter, idx, n_new;
bool sentinel;
double old_x, new_x, hi_tmp, lo_tmp, dummy, ea;
double * A_ptr;
double one_minus_sor_w = 1.0 - (option->sor_w);
//--- ORDERING & SCALING & INITIAL LOOP & Test
int* order = new int[n];
n_new = 0;
sentinel = true;
for (i = 0 ; i < n ; i++)
{
// ORDERING
if ( A[nskip*i + i] < option->eps_div )
{
x[i] = 0.0;
continue;
}
order[n_new++] = i;
// INITIAL LOOP
A_ptr = A + nskip*i;
new_x = b[i];
old_x = x[i];
for (j = 0 ; j < i ; j++)
new_x -= A_ptr[j]*x[j];
for (j = i + 1 ; j < n ; j++)
new_x -= A_ptr[j]*x[j];
new_x = new_x/A[nskip*i + i];
if (findex[i] >= 0) // friction index
{
hi_tmp = hi[i] * x[findex[i]];
lo_tmp = -hi_tmp;
if (new_x > hi_tmp)
x[i] = hi_tmp;
else if (new_x < lo_tmp)
x[i] = lo_tmp;
else
x[i] = new_x;
}
else // no friction index
{
if (new_x > hi[i])
x[i] = hi[i];
else if (new_x < lo[i])
x[i] = lo[i];
else
x[i] = new_x;
}
// TEST
if (sentinel)
{
ea = fabs(x[i] - old_x);
if (ea > option->eps_res)
sentinel = false;
}
}
if (sentinel)
{
delete[] order;
return true;
}
// SCALING
for (i = 0 ; i < n_new ; i++)
{
idx = order[i];
dummy = 1.0/A[nskip*idx + idx]; // diagonal element
b[idx] *= dummy;
for (j = 0 ; j < n ; j++)
A[nskip*idx + j] *= dummy;
}
//--- ITERATION LOOP
for (iter = 1 ; iter < option->itermax ; iter++)
{
//--- RANDOMLY_REORDER_CONSTRAINTS
#if LCP_PGS_RANDOMLY_REORDER_CONSTRAINTS
if ((iter & 7)==0)
{
int tmp, swapi;
for (i = 1 ; i < n_new ; i++)
{
tmp = order[i];
swapi = dRandInt(i+1);
order[i] = order[swapi];
order[swapi] = tmp;
}
}
#endif
sentinel = true;
//-- ONE LOOP
for (i = 0 ; i < n_new ; i++)
{
idx = order[i];
A_ptr = A + nskip*idx;
new_x = b[idx];
old_x = x[idx];
for (j = 0 ; j < idx ; j++)
new_x -= A_ptr[j]*x[j];
for (j = idx + 1 ; j < n ; j++)
new_x -= A_ptr[j]*x[j];
new_x = (option->sor_w * new_x) + (one_minus_sor_w * old_x);
if (findex[idx] >= 0) // friction index
{
hi_tmp = hi[idx] * x[findex[idx]];
lo_tmp = -hi_tmp;
if (new_x > hi_tmp)
x[idx] = hi_tmp;
else if (new_x < lo_tmp)
x[idx] = lo_tmp;
else
x[idx] = new_x;
}
else // no friction index
{
if (new_x > hi[idx])
x[idx] = hi[idx];
else if (new_x < lo[idx])
x[idx] = lo[idx];
else
x[idx] = new_x;
}
if ( sentinel && fabs(x[idx]) > option->eps_div)
{
ea = fabs((x[idx] - old_x)/x[idx]);
if (ea > option->eps_ea)
sentinel = false;
}
}
if (sentinel)
break;
}
delete[] order;
return sentinel;
}
void
dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations)
{
dxBody *bodyPair[2], *body;
dReal *GIPair[2], *GinvIPair[2];
dxJoint *joint;
int iter, b, j, i;
dReal ministep = stepsize / maxiterations;
// make a local copy of the joint array, because we might want to modify it.
// (the "dxJoint *const*" declaration says we're allowed to modify the joints
// but not the joint array, because the caller might need it unchanged).
dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *));
memcpy (joints, _joints, nj * sizeof (dxJoint *));
// get m = total constraint dimension, nub = number of unbounded variables.
// create constraint offset array and number-of-rows array for all joints.
// the constraints are re-ordered as follows: the purely unbounded
// constraints, the mixed unbounded + LCP constraints, and last the purely
// LCP constraints. this assists the LCP solver to put all unbounded
// variables at the start for a quick factorization.
//
// joints with m=0 are inactive and are removed from the joints array
// entirely, so that the code that follows does not consider them.
// also number all active joints in the joint list (set their tag values).
// inactive joints receive a tag value of -1.
int m = 0;
dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1));
int *ofs = (int *) ALLOCA (nj * sizeof (int));
for (i = 0, j = 0; j < nj; j++)
{ // i=dest, j=src
joints[j]->vtable->getInfo1 (joints[j], info + i);
if (info[i].m > 0)
{
joints[i] = joints[j];
joints[i]->tag = i;
i++;
}
else
{
joints[j]->tag = -1;
}
}
nj = i;
// the purely unbounded constraints
for (i = 0; i < nj; i++)
{
ofs[i] = m;
m += info[i].m;
}
dReal *c = NULL;
dReal *cfm = NULL;
dReal *lo = NULL;
dReal *hi = NULL;
int *findex = NULL;
dReal *J = NULL;
dxJoint::Info2 * Jinfo = NULL;
if (m)
{
// create a constraint equation right hand side vector `c', a constraint
// force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector.
c = (dReal *) ALLOCA (m * sizeof (dReal));
cfm = (dReal *) ALLOCA (m * sizeof (dReal));
lo = (dReal *) ALLOCA (m * sizeof (dReal));
hi = (dReal *) ALLOCA (m * sizeof (dReal));
findex = (int *) ALLOCA (m * sizeof (int));
dSetZero (c, m);
dSetValue (cfm, m, world->global_cfm);
dSetValue (lo, m, -dInfinity);
dSetValue (hi, m, dInfinity);
for (i = 0; i < m; i++)
findex[i] = -1;
// get jacobian data from constraints. a (2*m)x8 matrix will be created
// to store the two jacobian blocks from each constraint. it has this
// format:
//
// l l l 0 a a a 0 \ .
// l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
// l l l 0 a a a 0 /
// l l l 0 a a a 0 \ .
// l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
// l l l 0 a a a 0 /
// l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
// l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
// etc...
//
// (lll) = linear jacobian data
// (aaa) = angular jacobian data
//
J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal));
dSetZero (J, 2 * m * 8);
Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2));
for (i = 0; i < nj; i++)
{
Jinfo[i].rowskip = 8;
Jinfo[i].fps = dRecip (stepsize);
Jinfo[i].erp = world->global_erp;
Jinfo[i].J1l = J + 2 * 8 * ofs[i];
Jinfo[i].J1a = Jinfo[i].J1l + 4;
Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m;
Jinfo[i].J2a = Jinfo[i].J2l + 4;
Jinfo[i].c = c + ofs[i];
Jinfo[i].cfm = cfm + ofs[i];
Jinfo[i].lo = lo + ofs[i];
Jinfo[i].hi = hi + ofs[i];
Jinfo[i].findex = findex + ofs[i];
//joints[i]->vtable->getInfo2 (joints[i], Jinfo+i);
}
}
dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
for (b = 0; b < nb; b++)
{
for (i = 0; i < 4; i++)
{
saveFacc[b * 4 + i] = bodies[b]->facc[i];
saveTacc[b * 4 + i] = bodies[b]->tacc[i];
}
bodies[b]->tag = b;
}
for (iter = 0; iter < maxiterations; iter++)
{
dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
for (b = 0; b < nb; b++)
{
body = bodies[b];
// for all bodies, compute the inertia tensor and its inverse in the global
// frame, and compute the rotational force and add it to the torque
// accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
// @@@ check computation of rotational force.
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp, body->mass.I, body->posr.R);
dMULTIPLY0_333 (globalI + b * 12, body->posr.R, tmp);
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp, body->invI, body->posr.R);
dMULTIPLY0_333 (globalInvI + b * 12, body->posr.R, tmp);
for (i = 0; i < 4; i++)
body->tacc[i] = saveTacc[b * 4 + i];
// add the gravity force to all bodies
if ((body->flags & dxBodyNoGravity) == 0)
{
body->facc[0] = saveFacc[b * 4 + 0] + dMUL(body->mass.mass,world->gravity[0]);
body->facc[1] = saveFacc[b * 4 + 1] + dMUL(body->mass.mass,world->gravity[1]);
body->facc[2] = saveFacc[b * 4 + 2] + dMUL(body->mass.mass,world->gravity[2]);
body->facc[3] = 0;
} else {
body->facc[0] = saveFacc[b * 4 + 0];
body->facc[1] = saveFacc[b * 4 + 1];
body->facc[2] = saveFacc[b * 4 + 2];
body->facc[3] = 0;
}
}
#ifdef RANDOM_JOINT_ORDER
//randomize the order of the joints by looping through the array
//and swapping the current joint pointer with a random one before it.
for (j = 0; j < nj; j++)
{
joint = joints[j];
dxJoint::Info1 i1 = info[j];
dxJoint::Info2 i2 = Jinfo[j];
const int r = dRandInt(j+1);
joints[j] = joints[r];
info[j] = info[r];
Jinfo[j] = Jinfo[r];
joints[r] = joint;
info[r] = i1;
Jinfo[r] = i2;
}
#endif
//now iterate through the random ordered joint array we created.
for (j = 0; j < nj; j++)
{
joint = joints[j];
bodyPair[0] = joint->node[0].body;
bodyPair[1] = joint->node[1].body;
if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled))
bodyPair[0] = 0;
if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled))
bodyPair[1] = 0;
//if this joint is not connected to any enabled bodies, skip it.
if (!bodyPair[0] && !bodyPair[1])
continue;
if (bodyPair[0])
{
GIPair[0] = globalI + bodyPair[0]->tag * 12;
GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12;
}
if (bodyPair[1])
{
GIPair[1] = globalI + bodyPair[1]->tag * 12;
GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12;
}
joints[j]->vtable->getInfo2 (joints[j], Jinfo + j);
//dInternalStepIslandFast is an exact copy of the old routine with one
//modification: the calculated forces are added back to the facc and tacc
//vectors instead of applying them to the bodies and moving them.
if (info[j].m > 0)
{
dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep);
}
}
// }
//Now we can simulate all the free floating bodies, and move them.
for (b = 0; b < nb; b++)
{
body = bodies[b];
for (i = 0; i < 4; i++)
{
body->facc[i] = dMUL(body->facc[i],ministep);
body->tacc[i] = dMUL(body->tacc[i],ministep);
}
//apply torque
dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc);
//apply force
for (i = 0; i < 3; i++)
body->lvel[i] += dMUL(body->invMass,body->facc[i]);
//move It!
moveAndRotateBody (body, ministep);
}
}
for (b = 0; b < nb; b++)
for (j = 0; j < 4; j++)
bodies[b]->facc[j] = bodies[b]->tacc[j] = 0;
}
void createTest()
{
int i,j;
if (world) dWorldDestroy (world);
world = dWorldCreate();
// create random bodies
for (i=0; i<NUM; i++) {
// create bodies at random position and orientation
body[i] = dBodyCreate (world);
// dBodySetPosition (body[i],dRandReal()*2-1,dRandReal()*2-1,
// dRandReal()*2+RADIUS);
dBodySetPosition (body[i],dRandReal()*SIDE-1,dRandReal()*SIDE-1,
dRandReal()*SIDE+RADIUS);
dReal q[4];
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dBodySetQuaternion (body[i],q);
// set random velocity
dBodySetLinearVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
dBodySetAngularVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
// set random mass (random diagonal mass rotated by a random amount)
dMass m;
dMatrix3 R;
dMassSetBox (&m,1,dRandReal()+0.1,dRandReal()+0.1,dRandReal()+0.1);
dMassAdjust (&m,dRandReal()+1);
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dQtoR (q,R);
dMassRotate (&m,R);
dBodySetMass (body[i],&m);
}
// create ball-n-socket joints at random positions, linking random bodies
// (but make sure not to link the same pair of bodies twice)
for (i=0; i<NUM*NUM; i++) linked[i] = 0;
for (i=0; i<NUMJ; i++) {
int b1,b2;
do {
b1 = dRandInt (NUM);
b2 = dRandInt (NUM);
} while (linked[b1*NUM + b2] || b1==b2);
linked[b1*NUM + b2] = 1;
linked[b2*NUM + b1] = 1;
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[b1],body[b2]);
dJointSetBallAnchor (joint[i],dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*2+RADIUS);
}
for (i=0; i<NUM; i++) {
// move bodies a bit to get some joint error
const dReal *pos = dBodyGetPosition (body[i]);
dBodySetPosition (body[i],pos[0]+dRandReal()*0.2-0.1,
pos[1]+dRandReal()*0.2-0.1,pos[2]+dRandReal()*0.2-0.1);
}
}
int test_ray_and_box()
{
int i,j;
dContactGeom contact;
dVector3 s,p,q,n,q2,q3,q4; // s = box sides
dMatrix3 R;
dReal k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test zero length ray just inside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
for (j=0; j<3; j++) q4[j] += p[j];
for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
dNormalize3 (n);
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4));
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
dNormalize3 (q2);
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
for (j=0; j<3; j++) q2[j] = -q2[j];
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,k*0.99);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,k*1.01);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test contact point position for random rays
for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
dNormalize3 (q3);
dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomBoxPointDepth
(box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check position of contact point
for (j=0; j<3; j++) contact.pos[j] -= p[j];
dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
FAILED();
}
// also check normal signs
if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_box_point_depth()
{
int i,j;
dVector3 s,p,q,q2; // s = box sides
dMatrix3 R;
dReal ss,d; // ss = smallest side
dSimpleSpace space(0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test center point has depth of smallest side
ss = 1e9;
for (j=0; j<3; j++) if (s[j] < ss) ss = s[j];
if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol)
FAILED();
// ********** test point on surface has depth 0
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED();
// ********** test points outside box have -ve depth
for (j=0; j<3; j++) {
q[j] = 0.5*s[j] + dRandReal() + 0.01;
if (dRandReal() > 0.5) q[j] = -q[j];
}
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED();
// ********** test points inside box have +ve depth
for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5);
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED();
// ********** test random depth of point aligned along axis (up to ss deep)
i = dRandInt (3);
for (j=0; j<3; j++) q[j] = 0;
d = (dRandReal()*(ss*0.5+1)-1);
q[i] = s[i]*0.5 - d;
if (dRandReal() > 0.5) q[i] = -q[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED();
PASSED();
}