int test_plane_point_depth() { int j; dVector3 n,p,q,a,b; // n = plane normal dReal d; dSimpleSpace space(0); dGeomID plane = dCreatePlane (0,0,0,1,0); 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 point on plane has depth 0 a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = 0; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED(); // ********** test arbitrary depth point 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]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol) FAILED(); // ********** test depth-1 point a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = -1; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED(); PASSED(); }
Real32 PhysicsPlaneGeom::getPointDepth(const Vec3f& p) const { return (Real32)dGeomPlanePointDepth(_GeomID, p.x(), p.y(), p.z()); }
Real32 PhysicsPlaneGeom::getPointDepth(const Vec3f& p) { PhysicsPlaneGeomPtr tmpPtr(*this); return (Real32)dGeomPlanePointDepth(tmpPtr->id, p.x(), p.y(), p.z()); }
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(); }