void dGeomRayMotionsSet (dGeomID g,const dReal* p,const dReal* d, dReal l)
{
dxRayMotions *c = (dxRayMotions*) dGeomGetClassData(g);
dGeomRaySetLength (c->ray, l);
dGeomRaySet (c->ray, p[0], p[1], p[2],
d[0], d[1], d[2]);
dGeomMoved(g);
}
int test_ray_and_plane()
{
int j;
dContactGeom contact;
dVector3 n,p,q,a,b,g,h; // n,d = plane parameters
dMatrix3 R;
dReal d;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,ray);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test finite length ray below plane
dGeomRaySetLength (ray,0.09);
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = -dRandReal()*0.5 - 0.1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
dGeomSetPosition (ray,b[0],b[1],b[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray above plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()*0.5 + 0.01;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal() + 0.01;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray that intersects plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()-0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal()-0.5;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) {
// test that contact is on plane surface
if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED();
// also check contact point depth
if (dFabs (dGeomPlanePointDepth
(plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test ray that just misses
for (j=0; j<3; j++) b[j] = (1+d)*n[j];
for (j=0; j<3; j++) h[j] = -n[j];
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,0.99);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray that just hits
dGeomRaySetLength (ray,1.01);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test polarity with typical ground plane
dGeomPlaneSetParams (plane,0,0,1,0);
for (j=0; j<3; j++) a[j] = 0.1;
for (j=0; j<3; j++) b[j] = 0;
a[2] = 1;
b[2] = -1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
a[2] = -1;
b[2] = 1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
PASSED();
}
int test_ray_and_box()
{
int i,j;
dContactGeom contact;
dVector3 s,p,q,n,q2,q3,q4; // s = box sides
dMatrix3 R;
dReal k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test zero length ray just inside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
for (j=0; j<3; j++) q4[j] += p[j];
for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
dNormalize3 (n);
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4));
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
dNormalize3 (q2);
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
for (j=0; j<3; j++) q2[j] = -q2[j];
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,k*0.99);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,k*1.01);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test contact point position for random rays
for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
dNormalize3 (q3);
dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomBoxPointDepth
(box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check position of contact point
for (j=0; j<3; j++) contact.pos[j] -= p[j];
dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
FAILED();
}
// also check normal signs
if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_ray_and_ccylinder()
{
int j;
dContactGeom contact;
dVector3 p,a,b,n;
dMatrix3 R;
dReal r,l,k,x,y;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,ccyl);
// ********** make a random capped cylinder
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test ray completely within ccyl
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
dNormalize3 (b);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
dGeomRaySetLength (ray,dCalcPointsDistance3(a,b));
for (j=0; j<3; j++) b[j] -= a[j];
dNormalize3 (b);
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just misses (between caps)
k = dRandReal()*2*M_PI;
x = sin(k);
y = cos(k);
for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (between caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test ray outside ccyl that just misses (caps)
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) < 0) {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
}
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test random rays
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
dNormalize3 (n);
dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_ray_and_sphere()
{
int j;
dContactGeom contact;
dVector3 p,q,q2,n,v1;
dMatrix3 R;
dReal r,k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID sphere = dCreateSphere (0,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,sphere);
// ********** make a random sphere of radius r at position p
r = dRandReal()+0.1;
dGeomSphereSetRadius (sphere,r);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (sphere,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (sphere,R);
// ********** test zero length ray just inside sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside that sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal();
for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j];
dMakeRandomVector (q2,3,1.0);
dNormalize3 (q2);
k = dRandReal();
for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j];
for (j=0; j<3; j++) n[j] = q2[j] - q[j];
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q,q2));
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
do {
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
}
while (dCalcVectorDot3(n,q) < 0); // make sure normal goes away from sphere
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) n[j] = -q[j];
for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j];
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,0.99*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,1.01*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
for (j=0; j<3; j++) q2[j] = r * q[j] + p[j];
if (dCalcPointsDistance3 (contact.pos,q2) > tol) FAILED();
// ********** test contact point distance for random rays
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal()+0.5;
for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j];
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) {
k = dCalcPointsDistance3 (contact.pos,dGeomGetPosition(sphere));
if (dFabs(k - r) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
// also check depth of contact point
if (dFabs (dGeomSpherePointDepth
(sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test tangential grazing - miss
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test tangential grazing - hit
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
PASSED();
}
