void
dxJointContact::getInfo1( dxJoint::Info1 *info )
{
// make sure mu's >= 0, then calculate number of constraint rows and number
// of unbounded rows.
int m = 1, nub = 0;
if ( contact.surface.mu < 0 ) contact.surface.mu = 0;
if ( contact.surface.mode & dContactMu2 )
{
if ( contact.surface.mu > 0 ) m++;
if ( contact.surface.mu2 < 0 ) contact.surface.mu2 = 0;
if ( contact.surface.mu2 > 0 ) m++;
if (_dequal(contact.surface.mu, dInfinity)) nub ++;
if (_dequal(contact.surface.mu2, dInfinity)) nub ++;
}
else
{
if ( contact.surface.mu > 0 ) m += 2;
if (_dequal(contact.surface.mu, dInfinity)) nub += 2;
}
the_m = m;
info->m = m;
info->nub = nub;
}
dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)
{
dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
type = dBoxClass;
side[0] = lx;
side[1] = ly;
side[2] = lz;
updateZeroSizedFlag(_dequal(lx, 0.0) || _dequal(ly, 0.0) || _dequal(lz, 0.0));
}
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 dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
{
dUASSERT (g && g->type == dBoxClass,"argument not a box");
dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
dxBox *b = (dxBox*) g;
b->side[0] = lx;
b->side[1] = ly;
b->side[2] = lz;
b->updateZeroSizedFlag(_dequal(lx, 0.0) || _dequal(ly, 0.0) || _dequal(lz, 0.0));
dGeomMoved (g);
}
dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) :
dxGeom (space,1)
{
dAASSERT (_radius >= 0 && _length >= 0);
type = dCapsuleClass;
radius = _radius;
lz = _length;
updateZeroSizedFlag(_dequal(_radius, 0.0)/* || !_length -- zero length capsule is not a zero sized capsule*/);
}
void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length)
{
dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
dAASSERT (radius >= 0 && length >= 0);
dxCapsule *c = (dxCapsule*) g;
c->radius = radius;
c->lz = length;
c->updateZeroSizedFlag(_dequal(radius, 0.0)/* || !length -- zero length capsule is not a zero sized capsule*/);
dGeomMoved (g);
}
void dxPlane::computeAABB()
{
aabb[0] = -dInfinity;
aabb[1] = dInfinity;
aabb[2] = -dInfinity;
aabb[3] = dInfinity;
aabb[4] = -dInfinity;
aabb[5] = dInfinity;
// Planes that have normal vectors aligned along an axis can use a
// less comprehensive (half space) bounding box.
if (_dequal(p[1], 0.0f) && _dequal(p[2], 0.0f))
{
// normal aligned with x-axis
aabb[0] = (p[0] > 0) ? -dInfinity : -p[3];
aabb[1] = (p[0] > 0) ? p[3] : dInfinity;
} else
if (_dequal(p[0], 0.0f) && _dequal(p[2], 0.0f))
{
// normal aligned with y-axis
aabb[2] = (p[1] > 0) ? -dInfinity : -p[3];
aabb[3] = (p[1] > 0) ? p[3] : dInfinity;
} else
if (_dequal(p[0], 0.0f) && _dequal(p[1], 0.0f))
{
// normal aligned with z-axis
aabb[4] = (p[2] > 0) ? -dInfinity : -p[3];
aabb[5] = (p[2] > 0) ? p[3] : dInfinity;
}
}
/*
* This takes what is supposed to be a rotation matrix,
* and make sure it is correct.
* Note: this operates on rows, not columns, because for rotations
* both ways give equivalent results.
*/
void dOrthogonalizeR(dMatrix3 m)
{
dReal n0 = dCalcVectorLengthSquare3(m);
if (!_dequal(n0, 1.0))
dSafeNormalize3(m);
// project row[0] on row[1], should be zero
dReal proj = dCalcVectorDot3(m, m+4);
if (!_dequal(proj, 0.0)) {
// Gram-Schmidt step on row[1]
m[4] -= proj * m[0];
m[5] -= proj * m[1];
m[6] -= proj * m[2];
}
dReal n1 = dCalcVectorLengthSquare3(m+4);
if (!_dequal(n1, 1.0))
dSafeNormalize3(m+4);
/* just overwrite row[2], this makes sure the matrix is not
a reflection */
dCalcVectorCross3(m+8, m, m+4);
m[3] = m[4+3] = m[8+3] = 0;
}
void
dxJointContact::getInfo2( dxJoint::Info2 *info )
{
int s = info->rowskip;
int s2 = 2 * s;
// get normal, with sign adjusted for body1/body2 polarity
dVector3 normal;
if ( flags & dJOINT_REVERSE )
{
normal[0] = - contact.geom.normal[0];
normal[1] = - contact.geom.normal[1];
normal[2] = - contact.geom.normal[2];
}
else
{
normal[0] = contact.geom.normal[0];
normal[1] = contact.geom.normal[1];
normal[2] = contact.geom.normal[2];
}
normal[3] = 0; // @@@ hmmm
// c1,c2 = contact points with respect to body PORs
dVector3 c1, c2 = {0,0,0};
c1[0] = contact.geom.pos[0] - node[0].body->posr.pos[0];
c1[1] = contact.geom.pos[1] - node[0].body->posr.pos[1];
c1[2] = contact.geom.pos[2] - node[0].body->posr.pos[2];
// set jacobian for normal
info->J1l[0] = normal[0];
info->J1l[1] = normal[1];
info->J1l[2] = normal[2];
dCalcVectorCross3( info->J1a, c1, normal );
if ( node[1].body )
{
c2[0] = contact.geom.pos[0] - node[1].body->posr.pos[0];
c2[1] = contact.geom.pos[1] - node[1].body->posr.pos[1];
c2[2] = contact.geom.pos[2] - node[1].body->posr.pos[2];
info->J2l[0] = -normal[0];
info->J2l[1] = -normal[1];
info->J2l[2] = -normal[2];
dCalcVectorCross3( info->J2a, c2, normal );
dNegateVector3( info->J2a );
}
// set right hand side and cfm value for normal
dReal erp = info->erp;
if ( contact.surface.mode & dContactSoftERP )
erp = contact.surface.soft_erp;
dReal k = info->fps * erp;
// experimental - check relative acceleration at the contact
dReal depth;
dReal min_min_depth;
if (node[0].body->contactp != NULL && (node[1].body && node[1].body->contactp != NULL))
{
min_min_depth = std::min(node[0].body->contactp->min_depth,node[1].body->contactp->min_depth);
depth = contact.geom.depth - min_min_depth;
}
else if (node[0].body->contactp != NULL)
{
min_min_depth = node[0].body->contactp->min_depth;
depth = contact.geom.depth - min_min_depth;
}
else if (node[1].body && node[1].body->contactp != NULL)
{
min_min_depth = node[1].body->contactp->min_depth;
depth = contact.geom.depth - min_min_depth;
}
else
{
min_min_depth = world->contactp.min_depth;
depth = contact.geom.depth - min_min_depth;
}
if ( depth < 0 ) depth = 0;
if ( contact.surface.mode & dContactSoftCFM )
info->cfm[0] = contact.surface.soft_cfm;
dReal motionN = 0;
if ( contact.surface.mode & dContactMotionN )
motionN = contact.surface.motionN;
const dReal pushout = k * depth + motionN;
info->c[0] = pushout;
// note: this cap should not limit bounce velocity
// if contactp is not specified per body, use the global max_vel specified in world
// otherwise, use the body max_vel, but truncated by world max_vel.
dReal maxvel = world->contactp.max_vel;
if (node[0].body->contactp != NULL && (node[1].body && node[1].body->contactp != NULL))
maxvel = std::min(node[0].body->contactp->max_vel,node[1].body->contactp->max_vel);
else if (node[0].body && node[0].body->contactp != NULL)
maxvel = node[0].body->contactp->max_vel;
else if (node[1].body && node[1].body->contactp != NULL)
maxvel = node[1].body->contactp->max_vel;
// truncate everything by world max_vel
if (maxvel > world->contactp.max_vel)
maxvel = world->contactp.max_vel;
info->c_v_max[0] = maxvel;
// deal with bounce
if ( contact.surface.mode & dContactBounce )
{
// calculate outgoing velocity (-ve for incoming contact)
dReal outgoing = dCalcVectorDot3( info->J1l, node[0].body->lvel )
+ dCalcVectorDot3( info->J1a, node[0].body->avel );
if ( node[1].body )
{
outgoing += dCalcVectorDot3( info->J2l, node[1].body->lvel )
+ dCalcVectorDot3( info->J2a, node[1].body->avel );
}
outgoing -= motionN;
// only apply bounce if the outgoing velocity is greater than the
// threshold, and if the resulting c[0] exceeds what we already have.
if ( contact.surface.bounce_vel >= 0 &&
( -outgoing ) > contact.surface.bounce_vel )
{
dReal newc = - contact.surface.bounce * outgoing + motionN;
if ( newc > info->c[0] ) info->c[0] = newc;
}
}
// set LCP limits for normal
info->lo[0] = 0;
info->hi[0] = dInfinity;
info->findex[0] = -2;
// now do jacobian for tangential forces
dVector3 t1, t2; // two vectors tangential to normal
// first friction direction
if ( the_m >= 2 )
{
if ( contact.surface.mode & dContactFDir1 ) // use fdir1 ?
{
t1[0] = contact.fdir1[0];
t1[1] = contact.fdir1[1];
t1[2] = contact.fdir1[2];
dCalcVectorCross3( t2, normal, t1 );
// if fdir1 is parallel to normal, revert to dPlaneSpace
if (_dequal(t2[0], 0.0) &&
_dequal(t2[1], 0.0) &&
_dequal(t2[2], 0.0))
dPlaneSpace( normal, t1, t2 );
}
else
{
dPlaneSpace( normal, t1, t2 );
}
info->J1l[s+0] = t1[0];
info->J1l[s+1] = t1[1];
info->J1l[s+2] = t1[2];
dCalcVectorCross3( info->J1a + s, c1, t1 );
if ( node[1].body )
{
info->J2l[s+0] = -t1[0];
info->J2l[s+1] = -t1[1];
info->J2l[s+2] = -t1[2];
dReal *J2a_plus_s = info->J2a + s;
dCalcVectorCross3( J2a_plus_s, c2, t1 );
dNegateVector3( J2a_plus_s );
}
// set right hand side
if ( contact.surface.mode & dContactMotion1 )
{
info->c[1] = contact.surface.motion1;
}
// set LCP bounds and friction index. this depends on the approximation
// mode
info->lo[1] = -contact.surface.mu;
info->hi[1] = contact.surface.mu;
if ( contact.surface.mode & dContactApprox1_1 )
info->findex[1] = 0;
// set slip (constraint force mixing)
if ( contact.surface.mode & dContactSlip1 )
info->cfm[1] = contact.surface.slip1;
}
// second friction direction
if ( the_m >= 3 )
{
info->J1l[s2+0] = t2[0];
info->J1l[s2+1] = t2[1];
info->J1l[s2+2] = t2[2];
dCalcVectorCross3( info->J1a + s2, c1, t2 );
if ( node[1].body )
{
info->J2l[s2+0] = -t2[0];
info->J2l[s2+1] = -t2[1];
info->J2l[s2+2] = -t2[2];
dReal *J2a_plus_s2 = info->J2a + s2;
dCalcVectorCross3( J2a_plus_s2, c2, t2 );
dNegateVector3( J2a_plus_s2 );
}
// set right hand side
if ( contact.surface.mode & dContactMotion2 )
{
info->c[2] = contact.surface.motion2;
}
// set LCP bounds and friction index. this depends on the approximation
// mode
if ( contact.surface.mode & dContactMu2 )
{
info->lo[2] = -contact.surface.mu2;
info->hi[2] = contact.surface.mu2;
}
else
{
info->lo[2] = -contact.surface.mu;
info->hi[2] = contact.surface.mu;
}
if ( contact.surface.mode & dContactApprox1_2 )
info->findex[2] = 0;
// set slip (constraint force mixing)
if ( contact.surface.mode & dContactSlip2 )
info->cfm[2] = contact.surface.slip2;
}
}
int dxJointLimitMotor::addLimot( dxJoint *joint,
dxJoint::Info2 *info, int row,
const dVector3 ax1, int rotational )
{
int srow = row * info->rowskip;
// if the joint is powered, or has joint limits, add in the extra row
int powered = fmax > 0;
if ( powered || limit )
{
dReal *J1 = rotational ? info->J1a : info->J1l;
dReal *J2 = rotational ? info->J2a : info->J2l;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if ( joint->node[1].body )
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
// linear limot torque decoupling step:
//
// if this is a linear limot (e.g. from a slider), we have to be careful
// that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in powered or limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the point
// halfway between the body centers. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
if ( !rotational && joint->node[1].body )
{
dVector3 c;
c[0] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] );
c[1] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] );
c[2] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] );
dCalcVectorCross3( ltd, c, ax1 );
info->J1a[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2];
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if ( limit && (_dequal(lostop, histop) ) ) powered = 0;
if ( powered )
{
info->cfm[row] = normal_cfm;
if ( ! limit )
{
info->c[row] = vel;
info->lo[row] = -fmax;
info->hi[row] = fmax;
}
else
{
// the joint is at a limit, AND is being powered. if the joint is
// being powered into the limit then we apply the maximum motor force
// in that direction, because the motor is working against the
// immovable limit. if the joint is being powered away from the limit
// then we have problems because actually we need *two* lcp
// constraints to handle this case. so we fake it and apply some
// fraction of the maximum force. the fraction to use can be set as
// a fudge factor.
dReal fm = fmax;
if (( vel > 0 ) || (_dequal(vel, 0.0) && limit == 2 ) ) fm = -fm;
// if we're powering away from the limit, apply the fudge factor
if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
if ( rotational )
{
dBodyAddTorque( joint->node[0].body, -fm*ax1[0], -fm*ax1[1],
-fm*ax1[2] );
if ( joint->node[1].body )
dBodyAddTorque( joint->node[1].body, fm*ax1[0], fm*ax1[1], fm*ax1[2] );
}
else
{
dBodyAddForce( joint->node[0].body, -fm*ax1[0], -fm*ax1[1], -fm*ax1[2] );
if ( joint->node[1].body )
{
dBodyAddForce( joint->node[1].body, fm*ax1[0], fm*ax1[1], fm*ax1[2] );
// linear limot torque decoupling step: refer to above discussion
dBodyAddTorque( joint->node[0].body, -fm*ltd[0], -fm*ltd[1],
-fm*ltd[2] );
dBodyAddTorque( joint->node[1].body, -fm*ltd[0], -fm*ltd[1],
-fm*ltd[2] );
}
}
}
}
if ( limit )
{
dReal k = info->fps * stop_erp;
info->c[row] = -k * limit_err;
info->cfm[row] = stop_cfm;
if (_dequal(lostop, histop))
{
// limited low and high simultaneously
info->lo[row] = -dInfinity;
info->hi[row] = dInfinity;
}
else
{
if ( limit == 1 )
{
// low limit
info->lo[row] = 0;
info->hi[row] = dInfinity;
}
else
{
// high limit
info->lo[row] = -dInfinity;
info->hi[row] = 0;
}
// deal with bounce
if ( bounce > 0 )
{
// calculate joint velocity
dReal vvel;
if (rotational)
{
vvel = dCalcVectorDot3( joint->node[0].body->avel, ax1 );
if ( joint->node[1].body )
vvel -= dCalcVectorDot3( joint->node[1].body->avel, ax1 );
}
else
{
vvel = dCalcVectorDot3( joint->node[0].body->lvel, ax1 );
if ( joint->node[1].body )
vvel -= dCalcVectorDot3( joint->node[1].body->lvel, ax1 );
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
// low limit
if (vel < 0)
{
dReal newc = -bounce * vvel;
if ( newc > info->c[row] ) info->c[row] = newc;
}
}
else
{
// high limit - all those computations are reversed
if (vvel > 0)
{
dReal newc = -bounce * vvel;
if ( newc < info->c[row] ) info->c[row] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
void
dxJointContact::getInfo2( dxJoint::Info2 *info )
{
int s = info->rowskip;
int s2 = 2 * s;
int s3 = 3 * s;
// get normal, with sign adjusted for body1/body2 polarity
dVector3 normal;
if ( flags & dJOINT_REVERSE )
{
normal[0] = - contact.geom.normal[0];
normal[1] = - contact.geom.normal[1];
normal[2] = - contact.geom.normal[2];
}
else
{
normal[0] = contact.geom.normal[0];
normal[1] = contact.geom.normal[1];
normal[2] = contact.geom.normal[2];
}
normal[3] = 0; // @@@ hmmm
// c1,c2 = contact points with respect to body PORs
dVector3 c1, c2 = {0,0,0};
c1[0] = contact.geom.pos[0] - node[0].body->posr.pos[0];
c1[1] = contact.geom.pos[1] - node[0].body->posr.pos[1];
c1[2] = contact.geom.pos[2] - node[0].body->posr.pos[2];
// set jacobian for normal
info->J1l[0] = normal[0];
info->J1l[1] = normal[1];
info->J1l[2] = normal[2];
dCalcVectorCross3( info->J1a, c1, normal );
if ( node[1].body )
{
c2[0] = contact.geom.pos[0] - node[1].body->posr.pos[0];
c2[1] = contact.geom.pos[1] - node[1].body->posr.pos[1];
c2[2] = contact.geom.pos[2] - node[1].body->posr.pos[2];
info->J2l[0] = -normal[0];
info->J2l[1] = -normal[1];
info->J2l[2] = -normal[2];
dCalcVectorCross3( info->J2a, c2, normal );
dNegateVector3( info->J2a );
}
// experimental - check relative acceleration at the contact
dReal depth;
dReal min_min_depth;
if (node[0].body->contactp != NULL && (node[1].body && node[1].body->contactp != NULL))
{
min_min_depth = std::min(node[0].body->contactp->min_depth,node[1].body->contactp->min_depth);
depth = contact.geom.depth - min_min_depth;
}
else if (node[0].body->contactp != NULL)
{
min_min_depth = node[0].body->contactp->min_depth;
depth = contact.geom.depth - min_min_depth;
}
else if (node[1].body && node[1].body->contactp != NULL)
{
min_min_depth = node[1].body->contactp->min_depth;
depth = contact.geom.depth - min_min_depth;
}
else
{
min_min_depth = world->contactp.min_depth;
depth = contact.geom.depth - min_min_depth;
}
if ( depth < 0 ) depth = 0;
if ( contact.surface.mode & dContactSoftCFM )
info->cfm[0] = contact.surface.soft_cfm;
dReal motionN = 0;
if ( contact.surface.mode & dContactMotionN )
motionN = contact.surface.motionN;
// set right hand side and cfm value for normal
dReal local_erp = info->erp;
if ( contact.surface.mode & dContactSoftERP )
local_erp = contact.surface.soft_erp;
if ( contact.surface.mode & dContactEM )
{
// get patch radius for surface area calculation
dReal patch_radius;
if (!contact.surface.use_patch_radius)
{
if (contact.surface.surface_radius < 0)
contact.surface.surface_radius = 0;
patch_radius = sqrt(contact.surface.surface_radius * depth);
}
else
{
patch_radius = contact.surface.patch_radius;
}
// use elastic modulus
dReal e_star = contact.surface.elastic_modulus;
/// Using Hertzian contact, where stiffness term f = K*x^1.5.
/// Equation 5.23 form Contact Mechanics and Friction by Popov.
/// Split the penetration depth term to the 1.5 power
/// (x^1.5) as x(last iteration)^0.5 * x(current iteration)^1.3:
/// f = K*x^0.5 * x
/// Let linearized stiffness
/// K* = K*x^0.5
/// then,
/// f = K* * x
dReal khertz_sqrtx = 4.0 / 3.0 * e_star * sqrt(patch_radius * depth);
/// but note that this is not the linear spring stiffness
/// we are used to dealing with.
/// This is the Hertzian stiffness governed by
/// a non-linear equation (k*x^1.5).
// to convert stiffness to erp:
// 1) first recover kd from previous cfm and erp, then
// 2) compute new cfm and erp using new kp from
// elastic modulus calculation and kd from 1.
// get kd using: cfm = 1 / ( dt * kp + kd )
dReal kd = 1.0/info->cfm[0] - local_erp/info->fps;
// compute new erp using stiffness and kd
dReal kph = khertz_sqrtx/info->fps;
local_erp = (kph) / (kph + kd);
// compute new cfm given the new stiffness
info->cfm[0] = 1.0 / (kph + kd);
// debug, comparing stiffnesss, force and depth
// used to generate values for test/integration/elastic_modulus.cc:118
// printf("depth: %f, d: %f, k: %f k_linearized: %f, f: %f\n",
// depth, kd, 4.0 / 3.0 * e_star * sqrt(patch_radius),
// khertz_sqrtx, khertz_sqrtx*depth);
}
dReal k = info->fps * local_erp;
const dReal pushout = k * depth + motionN;
info->c[0] = pushout;
// note: this cap should not limit bounce velocity
// if contactp is not specified per body, use the global max_vel specified in world
// otherwise, use the body max_vel, but truncated by world max_vel.
dReal maxvel = world->contactp.max_vel;
if (node[0].body->contactp != NULL && (node[1].body && node[1].body->contactp != NULL))
maxvel = std::min(node[0].body->contactp->max_vel,node[1].body->contactp->max_vel);
else if (node[0].body && node[0].body->contactp != NULL)
maxvel = node[0].body->contactp->max_vel;
else if (node[1].body && node[1].body->contactp != NULL)
maxvel = node[1].body->contactp->max_vel;
// truncate everything by world max_vel
if (maxvel > world->contactp.max_vel)
maxvel = world->contactp.max_vel;
info->c_v_max[0] = maxvel;
// deal with bounce
if ( contact.surface.mode & dContactBounce )
{
// calculate outgoing velocity (-ve for incoming contact)
dReal outgoing = dCalcVectorDot3( info->J1l, node[0].body->lvel )
+ dCalcVectorDot3( info->J1a, node[0].body->avel );
if ( node[1].body )
{
outgoing += dCalcVectorDot3( info->J2l, node[1].body->lvel )
+ dCalcVectorDot3( info->J2a, node[1].body->avel );
}
outgoing -= motionN;
// only apply bounce if the outgoing velocity is greater than the
// threshold, and if the resulting c[0] exceeds what we already have.
if ( contact.surface.bounce_vel >= 0 &&
( -outgoing ) > contact.surface.bounce_vel )
{
dReal newc = - contact.surface.bounce * outgoing + motionN;
if ( newc > info->c[0] ) info->c[0] = newc;
}
}
// set LCP limits for normal
info->lo[0] = 0;
info->hi[0] = dInfinity;
info->findex[0] = -2;
// now do jacobian for tangential forces
dVector3 t1, t2; // two vectors tangential to normal
// first friction direction
if ( the_m >= 2 )
{
if ( contact.surface.mode & dContactFDir1 ) // use fdir1 ?
{
t1[0] = contact.fdir1[0];
t1[1] = contact.fdir1[1];
t1[2] = contact.fdir1[2];
dCalcVectorCross3( t2, normal, t1 );
// if fdir1 is parallel to normal, revert to dPlaneSpace
if (_dequal(t2[0], 0.0) &&
_dequal(t2[1], 0.0) &&
_dequal(t2[2], 0.0))
dPlaneSpace( normal, t1, t2 );
}
else
{
dPlaneSpace( normal, t1, t2 );
}
info->J1l[s+0] = t1[0];
info->J1l[s+1] = t1[1];
info->J1l[s+2] = t1[2];
dCalcVectorCross3( info->J1a + s, c1, t1 );
if ( node[1].body )
{
info->J2l[s+0] = -t1[0];
info->J2l[s+1] = -t1[1];
info->J2l[s+2] = -t1[2];
dReal *J2a_plus_s = info->J2a + s;
dCalcVectorCross3( J2a_plus_s, c2, t1 );
dNegateVector3( J2a_plus_s );
}
// set right hand side
if ( contact.surface.mode & dContactMotion1 )
{
info->c[1] = contact.surface.motion1;
}
// set LCP bounds and friction index. this depends on the approximation
// mode
info->lo[1] = -contact.surface.mu;
info->hi[1] = contact.surface.mu;
if ( contact.surface.mode & dContactApprox1_1 )
info->findex[1] = 0;
// set slip (constraint force mixing)
if ( contact.surface.mode & dContactSlip1 )
info->cfm[1] = contact.surface.slip1;
}
// second friction direction
if ( the_m >= 3 )
{
info->J1l[s2+0] = t2[0];
info->J1l[s2+1] = t2[1];
info->J1l[s2+2] = t2[2];
dCalcVectorCross3( info->J1a + s2, c1, t2 );
if ( node[1].body )
{
info->J2l[s2+0] = -t2[0];
info->J2l[s2+1] = -t2[1];
info->J2l[s2+2] = -t2[2];
dReal *J2a_plus_s2 = info->J2a + s2;
dCalcVectorCross3( J2a_plus_s2, c2, t2 );
dNegateVector3( J2a_plus_s2 );
}
// set right hand side
if ( contact.surface.mode & dContactMotion2 )
{
info->c[2] = contact.surface.motion2;
}
// set LCP bounds and friction index. this depends on the approximation
// mode
if ( contact.surface.mode & dContactMu2 )
{
info->lo[2] = -contact.surface.mu2;
info->hi[2] = contact.surface.mu2;
}
else
{
info->lo[2] = -contact.surface.mu;
info->hi[2] = contact.surface.mu;
}
if ( contact.surface.mode & dContactApprox1_2 )
info->findex[2] = 0;
// set slip (constraint force mixing)
if ( contact.surface.mode & dContactSlip2 )
info->cfm[2] = contact.surface.slip2;
}
// now do jacobian for rotational forces
// third friction direction (torsional)
// note that this will only be reachable if mu and mu2
// have positive values
if ( the_m >= 4 )
{
dVector3 t3 = {0, 0, 0};
// Linear, body 1
info->J1l[s3+0] = t3[0];
info->J1l[s3+1] = t3[1];
info->J1l[s3+2] = t3[2];
// Angular, body 1
info->J1a[s3+0] = normal[0];
info->J1a[s3+1] = normal[1];
info->J1a[s3+2] = normal[2];
if ( node[1].body )
{
// Linear, body 2
info->J2l[s3+0] = -t3[0];
info->J2l[s3+1] = -t3[1];
info->J2l[s3+2] = -t3[2];
// Angular, body 2
info->J2a[s3+0] = -normal[0];
info->J2a[s3+1] = -normal[1];
info->J2a[s3+2] = -normal[2];
}
// set LCP bounds and friction index. this depends on the approximation
// mode
if ( contact.surface.mode & dContactMu3 )
{
// Use user defined torsional patch radius
//
// M = torsional moment
// F = normal force
// a = patch radius
// R = surface radius
// d = depth
// mu = torsional friction coefficient
//
// M = (3 * pi * a * mu3)/16 * F
//
// When using radius:
//
// a = sqrt (R * d)
//
// M = (3 * pi * mu3 * sqrt (R * d))/16 * F
dReal patch_radius;
if (!contact.surface.use_patch_radius)
{
if (contact.surface.surface_radius < 0)
contact.surface.surface_radius = 0;
patch_radius = sqrt(contact.surface.surface_radius * depth);
}
else
{
patch_radius = contact.surface.patch_radius;
}
double rhs = (3 * M_PI * patch_radius * contact.surface.mu3)/16;
info->lo[3] = -rhs;
info->hi[3] = rhs;
// findex[3] must be zero in order for torsional friction moment
// to be proportional to normal force
if ( contact.surface.mode & dContactApprox3 )
info->findex[3] = 0;
// set slip (constraint force mixing)
if ( contact.surface.mode & dContactSlip3 )
info->cfm[3] = contact.surface.slip3;
}
}
}
void dSolveLCP (dxWorldProcessContext *context, int n, dReal *A, dReal *x, dReal *b,
dReal *outer_w/*=NULL*/, int nub, dReal *lo, dReal *hi, int *findex)
{
dAASSERT (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
# ifndef dNODEBUG
{
// check restrictions on lo and hi
for (int k=0; k<n; ++k) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
}
# endif
// if all the variables are unbounded then we can just factor, solve,
// and return
if (nub >= n) {
dReal *d = context->AllocateArray<dReal> (n);
dSetZero (d, n);
int nskip = dPAD(n);
dFactorLDLT (A, d, n, nskip);
dSolveLDLT (A, d, b, n, nskip);
memcpy (x, b, n*sizeof(dReal));
return;
}
const int nskip = dPAD(n);
dReal *L = context->AllocateArray<dReal> (n*nskip);
dReal *d = context->AllocateArray<dReal> (n);
dReal *w = outer_w ? outer_w : context->AllocateArray<dReal> (n);
dReal *delta_w = context->AllocateArray<dReal> (n);
dReal *delta_x = context->AllocateArray<dReal> (n);
dReal *Dell = context->AllocateArray<dReal> (n);
dReal *ell = context->AllocateArray<dReal> (n);
#ifdef ROWPTRS
dReal **Arows = context->AllocateArray<dReal *> (n);
#else
dReal **Arows = NULL;
#endif
int *p = context->AllocateArray<int> (n);
int *C = context->AllocateArray<int> (n);
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
bool *state = context->AllocateArray<bool> (n);
// create LCP object. note that tmp is set to delta_w to save space, this
// optimization relies on knowledge of how tmp is used, so be careful!
dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
int adj_nub = lcp.getNub();
// loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
// LCP conditions then i is added to the appropriate index set. otherwise
// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
// while driving x(i) we maintain the LCP conditions on the other variables
// 0..i-1. we do this by watching out for other x(i),w(i) values going
// outside the valid region, and then switching them between index sets
// when that happens.
bool hit_first_friction_index = false;
for (int i=adj_nub; i<n; ++i) {
bool s_error = false;
// the index i is the driving index and indexes i+1..n-1 are "dont care",
// i.e. when we make changes to the system those x's will be zero and we
// don't care what happens to those w's. in other words, we only consider
// an (i+1)*(i+1) sub-problem of A*x=b+w.
// if we've hit the first friction index, we have to compute the lo and
// hi values based on the values of x already computed. we have been
// permuting the indexes, so the values stored in the findex vector are
// no longer valid. thus we have to temporarily unpermute the x vector.
// for the purposes of this computation, 0*infinity = 0 ... so if the
// contact constraint's normal force is 0, there should be no tangential
// force applied.
if (!hit_first_friction_index && findex && findex[i] >= 0)
{
// un-permute x into delta_w, which is not being used at the moment
for (int j=0; j<n; ++j) delta_w[p[j]] = x[j];
// set lo and hi values
for (int k=i; k<n; ++k)
{
dReal wfk = delta_w[findex[k]];
if (_dequal(wfk, 0.0))
{
hi[k] = 0;
lo[k] = 0;
}
else
{
hi[k] = dFabs (hi[k] * wfk);
lo[k] = -hi[k];
}
}
hit_first_friction_index = true;
}
// thus far we have not even been computing the w values for indexes
// greater than i, so compute w[i] now.
w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
// if lo=hi=0 (which can happen for tangential friction when normals are
// 0) then the index will be assigned to set N with some state. however,
// set C's line has zero size, so the index will always remain in set N.
// with the "normal" switching logic, if w changed sign then the index
// would have to switch to set C and then back to set N with an inverted
// state. this is pointless, and also computationally expensive. to
// prevent this from happening, we use the rule that indexes with lo=hi=0
// will never be checked for set changes. this means that the state for
// these indexes may be incorrect, but that doesn't matter.
// see if x(i),w(i) is in a valid region
if (_dequal(lo[i], 0.0) && w[i] >= 0)
{
lcp.transfer_i_to_N (i);
state[i] = false;
}
else if (_dequal(hi[i], 0.0) && w[i] <= 0)
{
lcp.transfer_i_to_N (i);
state[i] = true;
}
else if (_dequal(w[i], 0.0))
{
// this is a degenerate case. by the time we get to this test we know
// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
// and similarly that hi > 0. this means that the line segment
// corresponding to set C is at least finite in extent, and we are on it.
// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
lcp.solve1 (delta_x,i,0,1);
lcp.transfer_i_to_C (i);
}
else {
// we must push x(i) and w(i)
for (;;) {
int dir;
dReal dirf;
// find direction to push on x(i)
if (w[i] <= 0) {
dir = 1;
dirf = REAL(1.0);
}
else {
dir = -1;
dirf = REAL(-1.0);
}
// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
lcp.solve1 (delta_x,i,dir);
// note that delta_x[i] = dirf, but we wont bother to set it
// compute: delta_w = A*delta_x ... note we only care about
// delta_w(N) and delta_w(i), the rest is ignored
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i,dir);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf;
// find largest step we can take (size=s), either to drive x(i),w(i)
// to the valid LCP region or to drive an already-valid variable
// outside the valid region.
int cmd = 1; // index switching command
int si = 0; // si = index to switch if cmd>3
dReal s = -w[i]/delta_w[i];
if (dir > 0) {
if (hi[i] < dInfinity) {
dReal s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i)
if (s2 < s) {
s = s2;
cmd = 3;
}
}
}
else {
if (lo[i] > -dInfinity) {
dReal s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i)
if (s2 < s) {
s = s2;
cmd = 2;
}
}
}
{
const int numN = lcp.numN();
for (int k=0; k < numN; ++k) {
const int indexN_k = lcp.indexN(k);
if (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) {
// don't bother checking if lo=hi=0
if (_dequal(lo[indexN_k], 0.0) && _dequal(hi[indexN_k], 0.0))
continue;
dReal s2 = -w[indexN_k] / delta_w[indexN_k];
if (s2 < s) {
s = s2;
cmd = 4;
si = indexN_k;
}
}
}
}
{
const int numC = lcp.numC();
for (int k=adj_nub; k < numC; ++k) {
const int indexC_k = lcp.indexC(k);
if (delta_x[indexC_k] < 0 && lo[indexC_k] > -dInfinity) {
dReal s2 = (lo[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 5;
si = indexC_k;
}
}
if (delta_x[indexC_k] > 0 && hi[indexC_k] < dInfinity) {
dReal s2 = (hi[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 6;
si = indexC_k;
}
}
}
}
//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
// "C->NL","C->NH"};
//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
// if s <= 0 then we've got a problem. if we just keep going then
// we're going to get stuck in an infinite loop. instead, just cross
// our fingers and exit with the current solution.
if (s <= REAL(0.0)) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",(double)s);
if (i < n) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
s_error = true;
break;
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x, s, delta_x);
x[i] += s * dirf;
// apply w = w + s * delta_w
lcp.pN_plusequals_s_times_qN (w, s, delta_w);
w[i] += s * delta_w[i];
void *tmpbuf;
// switch indexes between sets if necessary
switch (cmd)
{
case 1: // done
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
case 2: // done
x[i] = lo[i];
state[i] = false;
lcp.transfer_i_to_N (i);
break;
case 3: // done
x[i] = hi[i];
state[i] = true;
lcp.transfer_i_to_N (i);
break;
case 4: // keep going
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
break;
case 5: // keep going
x[si] = lo[si];
state[si] = false;
tmpbuf = context->PeekBufferRemainder();
lcp.transfer_i_from_C_to_N (si, tmpbuf);
break;
case 6: // keep going
x[si] = hi[si];
state[si] = true;
tmpbuf = context->PeekBufferRemainder();
lcp.transfer_i_from_C_to_N (si, tmpbuf);
break;
default:
break;
}
if (cmd <= 3) break;
} // for (;;)
} // else
if (s_error) {
break;
}
} // for (int i=adj_nub; i<n; ++i)
lcp.unpermute();
}
dLCP::dLCP (int _n, int _nskip, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
dReal *_Dell, dReal *_ell, dReal *_tmp,
bool *_state, int *_findex, int *_p, int *_C, dReal **Arows):
m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
# ifdef ROWPTRS
m_A(Arows),
#else
m_A(_Adata),
#endif
m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi),
m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
m_state(_state), m_findex(_findex), m_p(_p), m_C(_C)
{
{
dSetZero (m_x,m_n);
}
{
# ifdef ROWPTRS
// make matrix row pointers
dReal *aptr = _Adata;
ATYPE A = m_A;
const int n = m_n, nskip = m_nskip;
for (int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr;
# endif
}
{
int *p = m_p;
const int n = m_n;
for (int k=0; k<n; ++k) p[k]=k; // initially unpermuted
}
/*
// for testing, we can do some random swaps in the area i > nub
{
const int n = m_n;
const int nub = m_nub;
if (nub < n) {
for (int k=0; k<100; k++) {
int i1,i2;
do {
i1 = dRandInt(n-nub)+nub;
i2 = dRandInt(n-nub)+nub;
}
while (i1 > i2);
//printf ("--> %d %d\n",i1,i2);
swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,n,i1,i2,m_nskip,0);
}
}
*/
// permute the problem so that *all* the unbounded variables are at the
// start, i.e. look for unbounded variables not included in `nub'. we can
// potentially push up `nub' this way and get a bigger initial factorization.
// note that when we swap rows/cols here we must not just swap row pointers,
// as the initial factorization relies on the data being all in one chunk.
// variables that have findex >= 0 are *not* considered to be unbounded even
// if lo=-inf and hi=inf - this is because these limits may change during the
// solution process.
{
int *findex = m_findex;
dReal *lo = m_lo, *hi = m_hi;
const int n = m_n;
for (int k = m_nub; k<n; ++k) {
if (findex && findex[k] >= 0) continue;
if (_dequal(lo[k], -dInfinity) && _dequal(hi[k], dInfinity))
{
swapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0);
m_nub++;
}
}
}
// if there are unbounded variables at the start, factorize A up to that
// point and solve for x. this puts all indexes 0..nub-1 into C.
if (m_nub > 0) {
const int nub = m_nub;
{
dReal *Lrow = m_L;
const int nskip = m_nskip;
for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,AROW(j),(j+1)*sizeof(dReal));
}
dFactorLDLT (m_L,m_d,nub,m_nskip);
memcpy (m_x,m_b,nub*sizeof(dReal));
dSolveLDLT (m_L,m_d,m_x,nub,m_nskip);
dSetZero (m_w,nub);
{
int *C = m_C;
for (int k=0; k<nub; ++k) C[k] = k;
}
m_nC = nub;
}
// permute the indexes > nub such that all findex variables are at the end
if (m_findex) {
const int nub = m_nub;
int *findex = m_findex;
int num_at_end = 0;
for (int k=m_n-1; k >= nub; k--) {
if (findex[k] >= 0) {
swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,findex,m_n,k,m_n-1-num_at_end,m_nskip,1);
num_at_end++;
}
}
}
// print info about indexes
/*
{
const int n = m_n;
const int nub = m_nub;
for (int k=0; k<n; k++) {
if (k<nub) printf ("C");
else if (m_lo[k]==-dInfinity && m_hi[k]==dInfinity) printf ("c");
else printf (".");
}
printf ("\n");
}
*/
}
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
size_t memreq = EstimateTestSolveLCPMemoryReq(n);
dxWorldProcessContext *context = dxReallocateTemporayWorldProcessContext(NULL, memreq, NULL, NULL);
if (!context) {
return 0;
}
int i,nskip = dPAD(n);
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = context->AllocateArray<dReal> (n*nskip);
dReal *x = context->AllocateArray<dReal> (n);
dReal *b = context->AllocateArray<dReal> (n);
dReal *w = context->AllocateArray<dReal> (n);
dReal *lo = context->AllocateArray<dReal> (n);
dReal *hi = context->AllocateArray<dReal> (n);
dReal *A2 = context->AllocateArray<dReal> (n*nskip);
dReal *b2 = context->AllocateArray<dReal> (n);
dReal *lo2 = context->AllocateArray<dReal> (n);
dReal *hi2 = context->AllocateArray<dReal> (n);
dReal *tmp1 = context->AllocateArray<dReal> (n);
dReal *tmp2 = context->AllocateArray<dReal> (n);
double total_time = 0;
for (int count=0; count < 1000; count++) {
BEGIN_STATE_SAVE(context, saveInner) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dStopwatch sw;
dStopwatchReset (&sw);
dStopwatchStart (&sw);
dSolveLCP (context,n,A2,x,b2,w,nub,lo2,hi2,0);
dStopwatchStop (&sw);
double time = dStopwatchTime(&sw);
total_time += time;
double average = total_time / double(count+1) * 1000.0;
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i = 0; i < n; ++i)
tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol)
dDebug (0, "A*x = b+w, maximum difference = %.6e", diff);
int n1=0,n2=0,n3=0;
for (i = 0; i < n; ++i)
{
if (_dequal(x[i], lo[i]) && w[i] >= 0) {
n1++; // ok
}
else if (_dequal(x[i], hi[i]) && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && _dequal(w[i], 0.0))
{
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
} END_STATE_SAVE(context, saveInner);
}