int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
                          dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dCapsuleClass);
    dIASSERT (o2->type == dPlaneClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    dxCapsule *ccyl = (dxCapsule*) o1;
    dxPlane *plane = (dxPlane*) o2;

    // collide the deepest capping sphere with the plane
    dReal sign = (dCalcVectorDot3_14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
    dVector3 p;
    p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2]  * ccyl->lz * REAL(0.5) * sign;
    p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6]  * ccyl->lz * REAL(0.5) * sign;
    p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;

    dReal k = dCalcVectorDot3 (p,plane->p);
    dReal depth = plane->p[3] - k + ccyl->radius;
    if (depth < 0) return 0;
    contact->normal[0] = plane->p[0];
    contact->normal[1] = plane->p[1];
    contact->normal[2] = plane->p[2];
    contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
    contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
    contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
    contact->depth = depth;

    int ncontacts = 1;
    if ((flags & NUMC_MASK) >= 2) {
        // collide the other capping sphere with the plane
        p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2]  * ccyl->lz * REAL(0.5) * sign;
        p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6]  * ccyl->lz * REAL(0.5) * sign;
        p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;

        k = dCalcVectorDot3 (p,plane->p);
        depth = plane->p[3] - k + ccyl->radius;
        if (depth >= 0) {
            dContactGeom *c2 = CONTACT(contact,skip);
            c2->normal[0] = plane->p[0];
            c2->normal[1] = plane->p[1];
            c2->normal[2] = plane->p[2];
            c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
            c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
            c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
            c2->depth = depth;
            ncontacts = 2;
        }
    }

    for (int i=0; i < ncontacts; i++) {
        dContactGeom *currContact = CONTACT(contact,i*skip);
        currContact->g1 = o1;
        currContact->g2 = o2;
        currContact->side1 = -1;
        currContact->side2 = -1;
    }
    return ncontacts;
}

int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
                      dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dRayClass);
    dIASSERT (o2->type == dPlaneClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    dxRay *ray = (dxRay*) o1;
    dxPlane *plane = (dxPlane*) o2;

    dReal alpha = plane->p[3] - dCalcVectorDot3 (plane->p,ray->final_posr->pos);
    // note: if alpha > 0 the starting point is below the plane
    dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0);
    dReal k = dCalcVectorDot3_14(plane->p,ray->final_posr->R+2);
    if (k==0) return 0;		// ray parallel to plane
    alpha /= k;
    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] = nsign*plane->p[0];
    contact->normal[1] = nsign*plane->p[1];
    contact->normal[2] = nsign*plane->p[2];
    contact->depth = alpha;
    contact->g1 = ray;
    contact->g2 = plane;
    contact->side1 = -1;
    contact->side2 = -1;
    return 1;
}

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 );
    }
}

dReal dJointGetHinge2Angle2Rate( dJointID j )
{
    dxJointHinge2* joint = ( dxJointHinge2* )j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Hinge2 );
    if ( joint->node[0].body && joint->node[1].body )
    {
        dVector3 axis;
        dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 );
        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
        if ( joint->node[1].body )
            rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
        return rate;
    }
    else return 0;
}

int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
                         dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dSphereClass);
    dIASSERT (o2->type == dPlaneClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    dxSphere *sphere = (dxSphere*) o1;
    dxPlane *plane = (dxPlane*) o2;

    contact->g1 = o1;
    contact->g2 = o2;
    contact->side1 = -1;
    contact->side2 = -1;

    dReal k = dCalcVectorDot3 (o1->final_posr->pos,plane->p);
    dReal depth = plane->p[3] - k + sphere->radius;
    if (depth >= 0) {
        contact->normal[0] = plane->p[0];
        contact->normal[1] = plane->p[1];
        contact->normal[2] = plane->p[2];
        contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius;
        contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius;
        contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius;
        contact->depth = depth;
        return 1;
    }
    else return 0;
}

dReal dJointGetHingeAngleRate( dJointID j )
{
    dxJointHinge* joint = ( dxJointHinge* )j;
    dAASSERT( joint );
    checktype( joint, Hinge );
    if ( joint->node[0].body )
    {
        dVector3 axis;
        dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 );
        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
        if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
        if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
        return rate;
    }
    else return 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 testPlaneSpace()
{
  HEADER;
  dVector3 n,p,q;
  int bad = 0;
  for (int i=0; i<1000; i++) {
    dMakeRandomVector (n,3,1.0);
    dNormalize3 (n);
    dPlaneSpace (n,p,q);
    if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1;
  }
  printf ("\t%s\n", bad ? "FAILED" : "passed");
}

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 );
}

dReal dJointGetPRPositionRate( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );
    // get axis1 in global coordinates
    dVector3 ax1;
    dMultiply0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 );

    if ( joint->node[1].body )
    {
        dVector3 lv2;
        dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 );
        return dCalcVectorDot3( ax1, joint->node[0].body->lvel ) - dCalcVectorDot3( ax1, lv2 );
    }
    else
    {
        dReal rate = dCalcVectorDot3( ax1, joint->node[0].body->lvel );
        return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
    }
}

void testNormalize3()
{
  HEADER;
  int i,j,bad=0;
  dVector3 n1,n2;
  for (i=0; i<1000; i++) {
    dMakeRandomVector (n1,3,1.0);
    for (j=0; j<3; j++) n2[j]=n1[j];
    dNormalize3 (n2);
    if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1;
    if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2;
    if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4;
    if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8;
    if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16;
    if (bad) {
      printf ("\tFAILED (code=%x)\n",bad);
      return;
    }
  }
  printf ("\tpassed\n");
}

dReal dJointGetPUAngle2Rate( dJointID j )
{
    dxJointPU* joint = ( dxJointPU* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PU );

    if ( joint->node[0].body )
    {
        dVector3 axis;

        if ( joint->flags & dJOINT_REVERSE )
            getAxis( joint, axis, joint->axis1 );
        else
            getAxis2( joint, axis, joint->axis2 );

        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
        if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
        return rate;
    }
    return 0;
}

void makeRandomRotation (dMatrix3 R)
{
  dReal *u1 = R, *u2=R+4, *u3=R+8;
  dMakeRandomVector (u1,3,1.0);
  dNormalize3 (u1);
  dMakeRandomVector (u2,3,1.0);
  dReal d = dCalcVectorDot3(u1,u2);
  u2[0] -= d*u1[0];
  u2[1] -= d*u1[1];
  u2[2] -= d*u1[2];
  dNormalize3(u2);
  dCalcVectorCross3(u3,u1,u2);
}

dReal dJointGetScrewPositionRate ( dJointID j )
{
    dxJointScrew* joint = ( dxJointScrew* ) j;
    dUASSERT ( joint, "bad joint argument" );
    checktype ( joint, Screw );

    // get axis1 in global coordinates
    dVector3 ax1;
    dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );

    if ( joint->node[1].body )
    {
        return dCalcVectorDot3 ( ax1, joint->node[0].body->lvel ) -
               dCalcVectorDot3 ( ax1, joint->node[1].body->lvel );
    }
    else
    {
        dReal rate = dCalcVectorDot3 ( ax1, joint->node[0].body->lvel );
        if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
        return rate;
    }
}

dReal dJointGetPRPosition( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );

    dVector3 q;
    // get the offset in global coordinates
    dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );

    if ( joint->node[1].body )
    {
        dVector3 anchor2;

        // get the anchor2 in global coordinates
        dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );

    }
    else
    {
        //N.B. When there is no body 2 the joint->anchor2 is already in
        //     global coordinates

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->anchor2[2] ) );

        if ( joint->flags & dJOINT_REVERSE )
        {
            q[0] = -q[0];
            q[1] = -q[1];
            q[2] = -q[2];
        }
    }

    dVector3 axP;
    // get prismatic axis in global coordinates
    dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );

    return dCalcVectorDot3( axP, q );
}

// Generate contact info for movement 1
static void contactplat_2(dContact &contact)
{
    /*
      For arbitrary contact directions we need to project the moving
      geom's velocity against the contact normal and fdir1, fdir2
      (obtained with dPlaneSpace()). Assuming moving geom=g2
      (so the contact joint is in the moving geom's reference frame):
      motion1 = dCalcVectorDot3(fdir1, vel);
      motion2 = dCalcVectorDot3(fdir2, vel);
      motionN = dCalcVectorDot3(normal, vel);

      For geom=g1 just negate motionN and motion2. fdir1 is an arbitrary
      vector, so there's no need to negate motion1.

    */
    contact.surface.mode |= 
        dContactMotionN |                   // velocity along normal
        dContactMotion1 | dContactMotion2 | // and along the contact plane
        dContactFDir1;                      // don't forget to set the direction 1


    // This is a convenience function: given a vector, it finds other 2 perpendicular vectors
    dVector3 motiondir1, motiondir2;
    dPlaneSpace(contact.geom.normal, motiondir1, motiondir2);
    for (int i=0; i<3; ++i)
        contact.fdir1[i] = motiondir1[i];
    

    dReal inv = 1;
    if (contact.geom.g1 == platform)
        inv = -1;
    
    contact.surface.motion1 = dCalcVectorDot3(mov2_vel, motiondir1);
    contact.surface.motion2 = inv * dCalcVectorDot3(mov2_vel, motiondir2);
    contact.surface.motionN = inv * dCalcVectorDot3(mov2_vel, contact.geom.normal);

}

dReal dJointGetScrewPosition ( dJointID j )
{
    dxJointScrew* joint = ( dxJointScrew* ) j;
    dUASSERT ( joint, "bad joint argument" );
    checktype ( joint, Screw );

    // get axis1 in global coordinates
    dVector3 ax1, q;
    if (!joint->node[0].body)
      return 0;

    dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );

    if (joint->node[1].body)
    {
        // get body2 + offset point in global coordinates
        dMultiply0_331 ( q, joint->node[1].body->posr.R, joint->offset );
        for (int i = 0; i < 3; ++i )
            q[i] = joint->node[0].body->posr.pos[i]
                   - q[i]
                   - joint->node[1].body->posr.pos[i];
    }
    else
    {
        q[0] = joint->node[0].body->posr.pos[0] - joint->offset[0];
        q[1] = joint->node[0].body->posr.pos[1] - joint->offset[1];
        q[2] = joint->node[0].body->posr.pos[2] - joint->offset[2];

        if ( joint->flags & dJOINT_REVERSE )
        {
            // N.B. it could have been simplier to only inverse the sign of
            //      the dCalcVectorDot3 result but this case is exceptional and
            //      doing the check for all case can decrease the performance.
            ax1[0] = -ax1[0];
            ax1[1] = -ax1[1];
            ax1[2] = -ax1[2];
        }
    }

    return dCalcVectorDot3 ( ax1, q );
}

dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )
{
    // the angle between the two bodies is extracted from the quaternion that
    // represents the relative rotation between them. recall that a quaternion
    // q is:
    //    [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
    // where s is a scalar and v is a 3-vector. u is a unit length axis and
    // theta is a rotation along that axis. we can get theta/2 by:
    //    theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
    // but we can't get sin(theta/2) directly, only its absolute value, i.e.:
    //    |v| = |sin(theta/2)| * |u|
    //        = |sin(theta/2)|
    // using this value will have a strange effect. recall that there are two
    // quaternion representations of a given rotation, q and -q. typically as
    // a body rotates along the axis it will go through a complete cycle using
    // one representation and then the next cycle will use the other
    // representation. this corresponds to u pointing in the direction of the
    // hinge axis and then in the opposite direction. the result is that theta
    // will appear to go "backwards" every other cycle. here is a fix: if u
    // points "away" from the direction of the hinge (motor) axis (i.e. more
    // than 90 degrees) then use -q instead of q. this represents the same
    // rotation, but results in the cos(theta/2) value being sign inverted.

    // extract the angle from the quaternion. cost2 = cos(theta/2),
    // sint2 = |sin(theta/2)|
    dReal cost2 = qrel[0];
    dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );
    dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumptions
                  ( 2 * dAtan2( sint2, cost2 ) ) :  // if u points in direction of axis
                  ( 2 * dAtan2( sint2, -cost2 ) );  // if u points in opposite direction

    // the angle we get will be between 0..2*pi, but we want to return angles
    // between -pi..pi
    if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );

    // the angle we've just extracted has the wrong sign
    theta = -theta;

    return theta;
}

int _dSafeNormalize4 (dVector4 a)
{
  dAASSERT (a);

  dReal l = dCalcVectorDot3(a,a)+a[3]*a[3];

  if (l > 0) {
    l = dRecipSqrt(l);
    a[0] *= l;
    a[1] *= l;
    a[2] *= l;
    a[3] *= l;
	return 1;
  }
  else {
    a[0] = 1;
    a[1] = 0;
    a[2] = 0;
    a[3] = 0;
    return 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 (n0 != 1)
        dSafeNormalize3(m);

    // project row[0] on row[1], should be zero
    dReal proj = dCalcVectorDot3(m, m+4);
    if (proj != 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 (n1 != 1)
        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;
}