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