void
dxJointHinge2::makeV1andV2()
{
if ( node[0].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, v;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) ||
(_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) ||
(_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2])))
return;
// modify axis 2 so it's perpendicular to axis 1
dReal k = dCalcVectorDot3( ax1, ax2 );
for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
dNormalize3( ax2 );
// make v1 = modified axis2, v2 = axis1 x (modified axis2)
dCalcVectorCross3( v, ax1, ax2 );
dMultiply1_331( v1, node[0].body->posr.R, ax2 );
dMultiply1_331( v2, node[0].body->posr.R, v );
}
}
void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
dVector3 anchor1, dVector3 anchor2 )
{
if ( j->node[0].body )
{
dReal q[4];
q[0] = x - j->node[0].body->posr.pos[0];
q[1] = y - j->node[0].body->posr.pos[1];
q[2] = z - j->node[0].body->posr.pos[2];
q[3] = 0;
dMultiply1_331( anchor1, j->node[0].body->posr.R, q );
if ( j->node[1].body )
{
q[0] = x - j->node[1].body->posr.pos[0];
q[1] = y - j->node[1].body->posr.pos[1];
q[2] = z - j->node[1].body->posr.pos[2];
q[3] = 0;
dMultiply1_331( anchor2, j->node[1].body->posr.R, q );
}
else
{
anchor2[0] = x;
anchor2[1] = y;
anchor2[2] = z;
}
}
anchor1[3] = 0;
anchor2[3] = 0;
}
void
dxJointHinge2::makeW1andW2()
{
if ( node[1].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, w;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) ||
( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;
// modify axis 1 so it's perpendicular to axis 2
dReal k = dCalcVectorDot3( ax2, ax1 );
for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i];
dNormalize3( ax1 );
// make w1 = modified axis1, w2 = axis2 x (modified axis1)
dCalcVectorCross3( w, ax2, ax1 );
dMultiply1_331( w1, node[1].body->posr.R, ax1 );
dMultiply1_331( w2, node[1].body->posr.R, w );
}
}
void
dxJointAMotor::setEulerReferenceVectors()
{
if ( node[0].body && node[1].body )
{
dVector3 r; // axis[2] and axis[0] in global coordinates
dMultiply0_331( r, node[1].body->posr.R, axis[2] );
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
dMultiply1_331( reference2, node[1].body->posr.R, r );
}
else // jds
{
// else if (j->node[0].body) {
// dMultiply1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]);
// dMultiply0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]);
// We want to handle angular motors attached to passive geoms
dVector3 r; // axis[2] and axis[0] in global coordinates
r[0] = axis[2][0];
r[1] = axis[2][1];
r[2] = axis[2][2];
r[3] = axis[2][3];
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
reference2[0] += r[0];
reference2[1] += r[1];
reference2[2] += r[2];
reference2[3] += r[3];
}
}
void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
dVector3 axis1, dVector3 axis2 )
{
if ( j->node[0].body )
{
dReal q[4];
q[0] = x;
q[1] = y;
q[2] = z;
q[3] = 0;
dNormalize3( q );
if ( axis1 )
{
dMultiply1_331( axis1, j->node[0].body->posr.R, q );
axis1[3] = 0;
}
if ( axis2 )
{
if ( j->node[1].body )
{
dMultiply1_331( axis2, j->node[1].body->posr.R, q );
}
else
{
axis2[0] = x;
axis2[1] = y;
axis2[2] = z;
}
axis2[3] = 0;
}
}
}
void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
checktype( joint, AMotor );
dUASSERT( !( !joint->node[1].body && ( joint->flags & dJOINT_REVERSE ) && rel == 1 ), "no first body, can't set axis rel=1" );
dUASSERT( !( !joint->node[1].body && !( joint->flags & dJOINT_REVERSE ) && rel == 2 ), "no second body, can't set axis rel=2" );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
// adjust rel to match the internal body order
if ( !joint->node[1].body && rel == 2 ) rel = 1;
joint->rel[anum] = rel;
// x,y,z is always in global coordinates regardless of rel, so we may have
// to convert it to be relative to a body
dVector3 r;
r[0] = x;
r[1] = y;
r[2] = z;
r[3] = 0;
if ( rel > 0 )
{
if ( rel == 1 )
{
dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
}
else
{
// don't assert; handle the case of attachment to a bodiless geom
if ( joint->node[1].body ) // jds
{
dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
}
else
{
joint->axis[anum][0] = r[0];
joint->axis[anum][1] = r[1];
joint->axis[anum][2] = r[2];
joint->axis[anum][3] = r[3];
}
}
}
else
{
joint->axis[anum][0] = r[0];
joint->axis[anum][1] = r[1];
joint->axis[anum][2] = r[2];
}
dNormalize3( joint->axis[anum] );
if ( joint->mode == dAMotorEuler ) joint->setEulerReferenceVectors();
}
void dJointSetFixed ( dJointID j )
{
dxJointFixed* joint = ( dxJointFixed* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Fixed );
int i;
// This code is taken from dJointSetSliderAxis(), we should really put the
// common code in its own function.
// compute the offset between the bodies
if ( joint->node[0].body )
{
if ( joint->node[1].body )
{
dReal ofs[4];
for ( i = 0; i < 4; i++ )
ofs[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i];
dMultiply1_331 ( joint->offset, joint->node[0].body->posr.R, ofs );
}
else
{
joint->offset[0] = joint->node[0].body->posr.pos[0];
joint->offset[1] = joint->node[0].body->posr.pos[1];
joint->offset[2] = joint->node[0].body->posr.pos[2];
}
}
joint->computeInitialRelativeRotation();
}
dReal
dxJointHinge2::measureAngle() const
{
dVector3 a1, a2;
dMultiply0_331( a1, node[1].body->posr.R, axis2 );
dMultiply1_331( a2, node[0].body->posr.R, a1 );
dReal x = dCalcVectorDot3( v1, a2 );
dReal y = dCalcVectorDot3( v2, a2 );
return -dAtan2( y, x );
}
void dJointSetLMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
{
dxJointLMotor* joint = ( dxJointLMotor* )j;
//for now we are ignoring rel!
dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
checktype( joint, LMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
if ( !joint->node[1].body && rel == 2 ) rel = 1; //ref 1
joint->rel[anum] = rel;
dVector3 r;
r[0] = x;
r[1] = y;
r[2] = z;
r[3] = 0;
if ( rel > 0 )
{
if ( rel == 1 )
{
dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
}
else
{
//second body has to exists thanks to ref 1 line
dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
}
}
else
{
joint->axis[anum][0] = r[0];
joint->axis[anum][1] = r[1];
joint->axis[anum][2] = r[2];
}
dNormalize3( joint->axis[anum] );
}
void
dxJointAMotor::setEulerReferenceVectors()
{
if ( node[0].body && node[1].body )
{
dVector3 r; // axis[2] and axis[0] in global coordinates
dMultiply0_331( r, node[1].body->posr.R, axis[2] );
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
dMultiply1_331( reference2, node[1].body->posr.R, r );
} else {
// We want to handle angular motors attached to passive geoms
// Replace missing node.R with identity
if (node[0].body) {
dMultiply1_331( reference1, node[0].body->posr.R, axis[2] );
dMultiply0_331( reference2, node[0].body->posr.R, axis[0] );
} else if (node[1].body) {
dMultiply0_331( reference1, node[1].body->posr.R, axis[2] );
dMultiply1_331( reference2, node[1].body->posr.R, axis[0] );
}
}
}
void dJointSetScrewAnchorDelta( dJointID j, dReal x, dReal y, dReal z,
dReal dx, dReal dy, dReal dz )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Screw );
if ( joint->node[0].body )
{
dReal q[4];
q[0] = x - joint->node[0].body->posr.pos[0];
q[1] = y - joint->node[0].body->posr.pos[1];
q[2] = z - joint->node[0].body->posr.pos[2];
q[3] = 0;
dMultiply1_331( joint->anchor1, joint->node[0].body->posr.R, q );
if ( joint->node[1].body )
{
q[0] = x - joint->node[1].body->posr.pos[0];
q[1] = y - joint->node[1].body->posr.pos[1];
q[2] = z - joint->node[1].body->posr.pos[2];
q[3] = 0;
dMultiply1_331( joint->anchor2, joint->node[1].body->posr.R, q );
}
else
{
// Move the relative displacement between the passive body and the
// anchor in the same direction as the passive body has just moved
joint->anchor2[0] = x + dx;
joint->anchor2[1] = y + dy;
joint->anchor2[2] = z + dz;
}
}
joint->anchor1[3] = 0;
joint->anchor2[3] = 0;
joint->computeInitialRelativeRotation();
}
void testSmallMatrixMultiply()
{
dMatrix3 A,B,C,A2;
dVector3 a,a2,x;
HEADER;
dMakeRandomMatrix (A,3,3,1.0);
dMakeRandomMatrix (B,3,3,1.0);
dMakeRandomMatrix (C,3,3,1.0);
dMakeRandomMatrix (x,3,1,1.0);
// dMultiply0_331()
dMultiply0_331 (a,B,x);
dMultiply0 (a2,B,x,3,3,1);
printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
"passed");
// dMultiply1_331()
dMultiply1_331 (a,B,x);
dMultiply1 (a2,B,x,3,3,1);
printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
"passed");
// dMultiply0_133
dMultiply0_133 (a,x,B);
dMultiply0 (a2,x,B,1,3,3);
printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" :
"passed");
// dMultiply0_333()
dMultiply0_333 (A,B,C);
dMultiply0 (A2,B,C,3,3,3);
printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
"passed");
// dMultiply1_333()
dMultiply1_333 (A,B,C);
dMultiply1 (A2,B,C,3,3,3);
printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
"passed");
// dMultiply2_333()
dMultiply2_333 (A,B,C);
dMultiply2 (A2,B,C,3,3,3);
printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
"passed");
}
/// Compute center of body1 w.r.t body 2
void
dxJointScrew::computeOffset()
{
if ( node[1].body )
{
dVector3 cgdiff;
cgdiff[0] = node[0].body->posr.pos[0] - node[1].body->posr.pos[0];
cgdiff[1] = node[0].body->posr.pos[1] - node[1].body->posr.pos[1];
cgdiff[2] = node[0].body->posr.pos[2] - node[1].body->posr.pos[2];
dMultiply1_331 ( offset, node[1].body->posr.R, cgdiff );
}
else if ( node[0].body )
{
offset[0] = node[0].body->posr.pos[0];
offset[1] = node[0].body->posr.pos[1];
offset[2] = node[0].body->posr.pos[2];
}
}
dReal
dxJointHinge2::measureAngle1() const
{
// bring axis 2 into first body's reference frame
dVector3 p, q;
if (node[1].body)
dMultiply0_331( p, node[1].body->posr.R, axis2 );
else
dCopyVector3(p, axis2);
if (node[0].body)
dMultiply1_331( q, node[0].body->posr.R, p );
else
dCopyVector3(q, p);
dReal x = dCalcVectorDot3( v1, q );
dReal y = dCalcVectorDot3( v2, q );
return -dAtan2( y, x );
}
void dGeomVectorFromWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result)
{
dAASSERT (g);
if ((g->gflags & GEOM_PLACEABLE) == 0) {
result[0] = px;
result[1] = py;
result[2] = pz;
return;
}
g->recomputePosr();
dVector3 p;
p[0] = px;
p[1] = py;
p[2] = pz;
p[3] = 0;
dMultiply1_331 (result,g->final_posr->R,p);
}
dReal
dxJointHinge2::measureAngle2() const
{
// bring axis 1 into second body's reference frame
dVector3 p, q;
if (node[0].body)
dMultiply0_331( p, node[0].body->posr.R, axis1 );
else
dCopyVector3r4(p, axis1);
if (node[1].body)
dMultiply1_331( q, node[1].body->posr.R, p );
else
dCopyVector3r4(q, p);
dReal x = dCalcVectorDot3( w1, q );
dReal y = dCalcVectorDot3( w2, q );
return -dAtan2( y, x );
}
void dGeomGetPosRelPoint (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result)
{
dAASSERT (g);
if ((g->gflags & GEOM_PLACEABLE) == 0) {
result[0] = px;
result[1] = py;
result[2] = pz;
return;
}
g->recomputePosr();
dVector3 prel;
prel[0] = px - g->final_posr->pos[0];
prel[1] = py - g->final_posr->pos[1];
prel[2] = pz - g->final_posr->pos[2];
prel[3] = 0;
dMultiply1_331 (result,g->final_posr->R,prel);
}
/// Compute center of body1 w.r.t body 2
void
dxJointSlider::computeOffset()
{
if ( node[1].body )
{
dVector3 c;
c[0] = node[0].body->posr.pos[0] - node[1].body->posr.pos[0];
c[1] = node[0].body->posr.pos[1] - node[1].body->posr.pos[1];
c[2] = node[0].body->posr.pos[2] - node[1].body->posr.pos[2];
dMultiply1_331 ( offset, node[1].body->posr.R, c );
}
else if ( node[0].body )
{
offset[0] = node[0].body->posr.pos[0];
offset[1] = node[0].body->posr.pos[1];
offset[2] = node[0].body->posr.pos[2];
}
}
// Ray-Cylinder collider by Joseph Cooper (2011)
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 );
/* Possible collision cases:
* Ray origin between/outside caps
* Ray origin within/outside radius
* Ray direction left/right/perpendicular
* Ray direction parallel/perpendicular/other
*
* Ray origin cases (ignoring origin on surface)
*
* A B
* /-\-----------\
* C ( ) D )
* \_/___________/
*
* Cases A and D can collide with caps or cylinder
* Case C can only collide with the caps
* Case B can only collide with the cylinder
* Case D will produce inverted normals
* If the ray is perpendicular, only check the cylinder
* If the ray is parallel to cylinder axis,
* we can only check caps
* If the ray points right,
* Case A,C Check left cap
* Case D Check right cap
* If the ray points left
* Case A,C Check right cap
* Case D Check left cap
* Case B, check only first possible cylinder collision
* Case D, check only second possible cylinder collision
*/
// Find the ray in the cylinder coordinate frame:
dVector3 tmp;
dVector3 pos; // Ray origin in cylinder frame
dVector3 dir; // Ray direction in cylinder frame
// Translate ray start by inverse cyl
dSubtractVectors3(tmp,ray->final_posr->pos,cyl->final_posr->pos);
// Rotate ray start by inverse cyl
dMultiply1_331(pos,cyl->final_posr->R,tmp);
// Get the ray's direction
tmp[0] = ray->final_posr->R[2];
tmp[1] = ray->final_posr->R[6];
tmp[2] = ray->final_posr->R[10];
// Rotate the ray direction by inverse cyl
dMultiply1_331(dir,cyl->final_posr->R,tmp);
// Is the ray origin inside of the (extended) cylinder?
dReal r2 = cyl->radius*cyl->radius;
dReal C = pos[0]*pos[0] + pos[1]*pos[1] - r2;
// Find the different cases
// Is ray parallel to the cylinder length?
int parallel = (dir[0]==0 && dir[1]==0);
// Is ray perpendicular to the cylinder length?
int perpendicular = (dir[2]==0);
// Is ray origin within the radius of the caps?
int inRadius = (C<=0);
// Is ray origin between the top and bottom caps?
int inCaps = (dFabs(pos[2])<=half_length);
int checkCaps = (!perpendicular && (!inCaps || inRadius));
int checkCyl = (!parallel && (!inRadius || inCaps));
int flipNormals = (inCaps&&inRadius);
dReal tt=-dInfinity; // Depth to intersection
dVector3 tmpNorm = {dNaN, dNaN, dNaN}; // ensure we don't leak garbage
if (checkCaps) {
// Make it so we only need to check one cap
int flipDir = 0;
// Wish c had logical xor...
if ((dir[2]<0 && flipNormals) || (dir[2]>0 && !flipNormals)) {
flipDir = 1;
dir[2]=-dir[2];
pos[2]=-pos[2];
}
// The cap is half the cylinder's length
// from the cylinder's origin
// We only checkCaps if dir[2]!=0
tt = (half_length-pos[2])/dir[2];
if (tt>=0 && tt<=ray->length) {
tmp[0] = pos[0] + tt*dir[0];
tmp[1] = pos[1] + tt*dir[1];
// Ensure collision point is within cap circle
if (tmp[0]*tmp[0] + tmp[1]*tmp[1] <= r2) {
// Successful collision
tmp[2] = (flipDir)?-half_length:half_length;
tmpNorm[0]=0;
tmpNorm[1]=0;
tmpNorm[2]=(flipDir!=flipNormals)?-1:1;
checkCyl = 0; // Short circuit cylinder check
} else {
// Ray hits cap plane outside of cap circle
tt=-dInfinity; // No collision yet
}
} else {
// The cap plane is beyond (or behind) the ray length
tt=-dInfinity; // No collision yet
}
if (flipDir) {
// Flip back
dir[2]=-dir[2];
pos[2]=-pos[2];
}
}
if (checkCyl) {
// Compute quadratic formula for parametric ray equation
dReal A = dir[0]*dir[0] + dir[1]*dir[1];
dReal B = 2*(pos[0]*dir[0] + pos[1]*dir[1]);
// Already computed C
dReal k = B*B - 4*A*C;
// Check collision with infinite cylinder
// k<0 means the ray passes outside the cylinder
// k==0 means ray is tangent to cylinder (or parallel)
//
// Our quadratic formula: tt = (-B +- sqrt(k))/(2*A)
//
// A must be positive (otherwise we wouldn't be checking
// cylinder because ray is parallel)
// if (k<0) ray doesn't collide with sphere
// if (B > sqrt(k)) then both times are negative
// -- don't calculate
// if (B<-sqrt(k)) then both times are positive (Case A or B)
// -- only calculate first, if first isn't valid
// -- second can't be without first going through a cap
// otherwise (fabs(B)<=sqrt(k)) then C<=0 (ray-origin inside/on cylinder)
// -- only calculate second collision
if (k>=0 && (B<0 || B*B<=k)) {
k = dSqrt(k);
A = dRecip(2*A);
if (dFabs(B)<=k) {
tt = (-B + k)*A; // Second solution
// If ray origin is on surface and pointed out, we
// can get a tt=0 solution...
} else {
tt = (-B - k)*A; // First solution
}
if (tt<=ray->length) {
tmp[2] = pos[2] + tt*dir[2];
if (dFabs(tmp[2])<=half_length) {
// Valid solution
tmp[0] = pos[0] + tt*dir[0];
tmp[1] = pos[1] + tt*dir[1];
tmpNorm[0] = tmp[0]/cyl->radius;
tmpNorm[1] = tmp[1]/cyl->radius;
tmpNorm[2] = 0;
if (flipNormals) {
// Ray origin was inside cylinder
tmpNorm[0] = -tmpNorm[0];
tmpNorm[1] = -tmpNorm[1];
}
} else {
// Ray hits cylinder outside of caps
tt=-dInfinity;
}
} else {
// Ray doesn't reach the cylinder
tt=-dInfinity;
}
}
}
if (tt>0) {
contact->depth = tt;
// Transform the point back to world coordinates
tmpNorm[3]=0;
tmp[3] = 0;
dMultiply0_331(contact->normal,cyl->final_posr->R,tmpNorm);
dMultiply0_331(contact->pos,cyl->final_posr->R,tmp);
contact->pos[0]+=cyl->final_posr->pos[0];
contact->pos[1]+=cyl->final_posr->pos[1];
contact->pos[2]+=cyl->final_posr->pos[2];
return 1;
}
// No contact with anything.
return 0;
}
int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxRay *ray = (dxRay*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = ray;
contact->g2 = box;
contact->side1 = -1;
contact->side2 = -1;
int i;
// compute the start and delta of the ray relative to the box.
// we will do all subsequent computations in this box-relative coordinate
// system. we have to do a translation and rotation for each point.
dVector3 tmp,s,v;
tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0];
tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1];
tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2];
dMultiply1_331 (s,box->final_posr->R,tmp);
tmp[0] = ray->final_posr->R[0*4+2];
tmp[1] = ray->final_posr->R[1*4+2];
tmp[2] = ray->final_posr->R[2*4+2];
dMultiply1_331 (v,box->final_posr->R,tmp);
// mirror the line so that v has all components >= 0
dVector3 sign;
for (i=0; i<3; i++) {
if (v[i] < 0) {
s[i] = -s[i];
v[i] = -v[i];
sign[i] = 1;
}
else sign[i] = -1;
}
// compute the half-sides of the box
dReal h[3];
h[0] = REAL(0.5) * box->side[0];
h[1] = REAL(0.5) * box->side[1];
h[2] = REAL(0.5) * box->side[2];
// do a few early exit tests
if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] ||
(s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] ||
(s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] ||
(v[0] == 0 && v[1] == 0 && v[2] == 0)) {
return 0;
}
// compute the t=[lo..hi] range for where s+v*t intersects the box
dReal lo = -dInfinity;
dReal hi = dInfinity;
int nlo = 0, nhi = 0;
for (i=0; i<3; i++) {
if (v[i] != 0) {
dReal k = (-h[i] - s[i])/v[i];
if (k > lo) {
lo = k;
nlo = i;
}
k = (h[i] - s[i])/v[i];
if (k < hi) {
hi = k;
nhi = i;
}
}
}
// check if the ray intersects
if (lo > hi) return 0;
dReal alpha;
int n;
if (lo >= 0) {
alpha = lo;
n = nlo;
}
else {
alpha = hi;
n = nhi;
}
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] = box->final_posr->R[0*4+n] * sign[n];
contact->normal[1] = box->final_posr->R[1*4+n] * sign[n];
contact->normal[2] = box->final_posr->R[2*4+n] * sign[n];
contact->depth = alpha;
return 1;
}
dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
dUASSERT (g && g->type == dBoxClass,"argument not a box");
g->recomputePosr();
dxBox *b = (dxBox*) g;
// Set p = (x,y,z) relative to box center
//
// This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)
dVector3 p,q;
p[0] = x - b->final_posr->pos[0];
p[1] = y - b->final_posr->pos[1];
p[2] = z - b->final_posr->pos[2];
// Rotate p into box's coordinate frame, so we can
// treat the OBB as an AABB
dMultiply1_331 (q,b->final_posr->R,p);
// Record distance from point to each successive box side, and see
// if the point is inside all six sides
dReal dist[6];
int i;
bool inside = true;
for (i=0; i < 3; i++) {
dReal side = b->side[i] * REAL(0.5);
dist[i ] = side - q[i];
dist[i+3] = side + q[i];
if ((dist[i] < 0) || (dist[i+3] < 0)) {
inside = false;
}
}
// If point is inside the box, the depth is the smallest positive distance
// to any side
if (inside) {
dReal smallest_dist = (dReal) (unsigned) -1;
for (i=0; i < 6; i++) {
if (dist[i] < smallest_dist) smallest_dist = dist[i];
}
return smallest_dist;
}
// Otherwise, if point is outside the box, the depth is the largest
// distance to any side. This is an approximation to the 'proper'
// solution (the proper solution may be larger in some cases).
dReal largest_dist = 0;
for (i=0; i < 6; i++) {
if (dist[i] > largest_dist) largest_dist = dist[i];
}
return -largest_dist;
}
void
dxJointDBall::getInfo2( dxJoint::Info2 *info )
{
info->erp = erp;
info->cfm[0] = cfm;
dVector3 globalA1, globalA2;
dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
if (node[1].body)
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
else
dCopyVector3(globalA2, anchor2);
dVector3 q;
dSubtractVectors3(q, globalA1, globalA2);
#ifdef dSINGLE
const dReal MIN_LENGTH = REAL(1e-7);
#else
const dReal MIN_LENGTH = REAL(1e-12);
#endif
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// too small, let's choose an arbitrary direction
// heuristic: difference in velocities at anchors
dVector3 v1, v2;
dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
if (node[1].body)
dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
else
dSetZero(v2, 3);
dSubtractVectors3(q, v1, v2);
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// this direction is as good as any
q[0] = 1;
q[1] = 0;
q[2] = 0;
}
}
dNormalize3(q);
info->J1l[0] = q[0];
info->J1l[1] = q[1];
info->J1l[2] = q[2];
dVector3 relA1;
dBodyVectorToWorld(node[0].body,
anchor1[0], anchor1[1], anchor1[2],
relA1);
dMatrix3 a1m;
dSetZero(a1m, 12);
dSetCrossMatrixMinus(a1m, relA1, 4);
dMultiply1_331(info->J1a, a1m, q);
if (node[1].body) {
info->J2l[0] = -q[0];
info->J2l[1] = -q[1];
info->J2l[2] = -q[2];
dVector3 relA2;
dBodyVectorToWorld(node[1].body,
anchor2[0], anchor2[1], anchor2[2],
relA2);
dMatrix3 a2m;
dSetZero(a2m, 12);
dSetCrossMatrixPlus(a2m, relA2, 4);
dMultiply1_331(info->J2a, a2m, q);
}
const dReal k = info->fps * info->erp;
info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));
}
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;
}