Example #1
0
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;
}
Example #2
0
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));
}
Example #3
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 );
    }
}
Example #4
0
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);
}
Example #5
0
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*/);
}
Example #6
0
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);
}
Example #7
0
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;
      }
}
Example #8
0
/*
* 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;
}
Example #9
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;
    }
}
Example #10
0
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;
}
Example #11
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;
        }
    }
}
Example #12
0
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();
}
Example #13
0
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");
  }
  */
}
Example #14
0
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);
  }