void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir)
{
    dUASSERT (g && g->type == dRayClass,"argument not a ray");
    g->recomputePosr();
    start[0] = g->final_posr->pos[0];
    start[1] = g->final_posr->pos[1];
    start[2] = g->final_posr->pos[2];
    dir[0] = g->final_posr->R[0*4+2];
    dir[1] = g->final_posr->R[1*4+2];
    dir[2] = g->final_posr->R[2*4+2];
}

dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
  dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
  g->recomputePosr();
  
  dxSphere *s = (dxSphere*) g;
  dReal * pos = s->final_posr->pos;
  return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) +
			    (y-pos[1])*(y-pos[1]) +
			    (z-pos[2])*(z-pos[2]));
}

void dGeomVectorFromWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result)
{
    dAASSERT (g);
    if ((g->gflags & GEOM_PLACEABLE) == 0) {
        result[0] = px;
        result[1] = py;
        result[2] = pz;
        return;
    }

    g->recomputePosr();

    dVector3 p;
    p[0] = px;
    p[1] = py;
    p[2] = pz;
    p[3] = 0;
    dMultiply1_331 (result,g->final_posr->R,p);
}

void dGeomGetPosRelPoint (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result)
{
    dAASSERT (g);
    if ((g->gflags & GEOM_PLACEABLE) == 0) {
        result[0] = px;
        result[1] = py;
        result[2] = pz;
        return;
    }

    g->recomputePosr();

    dVector3 prel;
    prel[0] = px - g->final_posr->pos[0];
    prel[1] = py - g->final_posr->pos[1];
    prel[2] = pz - g->final_posr->pos[2];
    prel[3] = 0;
    dMultiply1_331 (result,g->final_posr->R,prel);
}

void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz,
                  dReal dx, dReal dy, dReal dz)
{
    dUASSERT (g && g->type == dRayClass,"argument not a ray");
    g->recomputePosr();
    dReal* rot = g->final_posr->R;
    dReal* pos = g->final_posr->pos;
    dVector3 n;
    pos[0] = px;
    pos[1] = py;
    pos[2] = pz;

    n[0] = dx;
    n[1] = dy;
    n[2] = dz;
    dNormalize3(n);
    rot[0*4+2] = n[0];
    rot[1*4+2] = n[1];
    rot[2*4+2] = n[2];
    dGeomMoved (g);
}

EXPORT_C dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
  g->recomputePosr();
  dxCapsule *c = (dxCapsule*) g;

  const dReal* R = g->final_posr->R;
  const dReal* pos = g->final_posr->pos;
  
  dVector3 a;
  a[0] = x - pos[0];
  a[1] = y - pos[1];
  a[2] = z - pos[2];
  dReal beta = dDOT14(a,R+2);
  dReal lz2 = dMUL(c->lz,REAL(0.5));
  if (beta < -lz2) beta = -lz2;
  else if (beta > lz2) beta = lz2;
  a[0] = c->final_posr->pos[0] + dMUL(beta,R[0*4+2]);
  a[1] = c->final_posr->pos[1] + dMUL(beta,R[1*4+2]);
  a[2] = c->final_posr->pos[2] + dMUL(beta,R[2*4+2]);
  return c->radius -
    dSqrt (dMUL((x-a[0]),(x-a[0])) + dMUL((y-a[1]),(y-a[1])) + dMUL((z-a[2]),(z-a[2])));
}

dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
  dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
  g->recomputePosr();
  dxCapsule *c = (dxCapsule*) g;

  const dReal* R = g->final_posr->R;
  const dReal* pos = g->final_posr->pos;
  
  dVector3 a;
  a[0] = x - pos[0];
  a[1] = y - pos[1];
  a[2] = z - pos[2];
  dReal beta = dDOT14(a,R+2);
  dReal lz2 = c->lz*REAL(0.5);
  if (beta < -lz2) beta = -lz2;
  else if (beta > lz2) beta = lz2;
  a[0] = c->final_posr->pos[0] + beta*R[0*4+2];
  a[1] = c->final_posr->pos[1] + beta*R[1*4+2];
  a[2] = c->final_posr->pos[2] + beta*R[2*4+2];
  return c->radius -
    dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2]));
}

void dGeomGetRelPointPos (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result)
{
    dAASSERT (g);

    if ((g->gflags & GEOM_PLACEABLE) == 0) {
        result[0] = px;
        result[1] = py;
        result[2] = pz;
        return;
    }

    g->recomputePosr();

    dVector3 prel,p;
    prel[0] = px;
    prel[1] = py;
    prel[2] = pz;
    prel[3] = 0;
    dMultiply0_331 (p,g->final_posr->R,prel);
    result[0] = p[0] + g->final_posr->pos[0];
    result[1] = p[1] + g->final_posr->pos[1];
    result[2] = p[2] + g->final_posr->pos[2];
}

EXPORT_C dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
  g->recomputePosr();
  dxBox *b = (dxBox*) g;

  // Set p = (x,y,z) relative to box center
  //
  // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)

  dVector3 p,q;

  p[0] = x - b->final_posr->pos[0];
  p[1] = y - b->final_posr->pos[1];
  p[2] = z - b->final_posr->pos[2];

  // Rotate p into box's coordinate frame, so we can
  // treat the OBB as an AABB

  dMULTIPLY1_331 (q,b->final_posr->R,p);

  // Record distance from point to each successive box side, and see
  // if the point is inside all six sides

  dReal dist[6];
  int   i;

  bool inside = true;

  for (i=0; i < 3; i++) {
    dReal side = dMUL(b->side[i],REAL(0.5));

    dist[i  ] = side - q[i];
    dist[i+3] = side + q[i];

    if ((dist[i] < 0) || (dist[i+3] < 0)) {
      inside = false;
    }
  }

  // If point is inside the box, the depth is the smallest positive distance
  // to any side

  if (inside) {
    dReal smallest_dist = (dReal) (unsigned) -1;

    for (i=0; i < 6; i++) {
      if (dist[i] < smallest_dist) smallest_dist = dist[i];
    }

    return smallest_dist;
  }

  // Otherwise, if point is outside the box, the depth is the largest
  // distance to any side.  This is an approximation to the 'proper'
  // solution (the proper solution may be larger in some cases).

  dReal largest_dist = 0;

  for (i=0; i < 6; i++) {
    if (dist[i] > largest_dist) largest_dist = dist[i];
  }

  return -largest_dist;
}

void dGeomMoved(dGeomID g) { g->Move(); }