IoObject *IoODEBox_pointDepth(IoODEBox *self, IoObject *locals, IoMessage *m) { dReal lx = IoMessage_locals_doubleArgAt_(m, locals, 0); dReal ly = IoMessage_locals_doubleArgAt_(m, locals, 1); dReal lz = IoMessage_locals_doubleArgAt_(m, locals, 2); IoODEBox_assertHasBoxId(self, locals, m); return IONUMBER(dGeomBoxPointDepth(GEOMID, lx, ly, lz)); }
static gfloat soy_bodies_box_real_pointDepth (soybodiesBody* base, gfloat x, gfloat y, gfloat z) { soybodiesBox * self; gfloat result = 0.0F; struct dxGeom* _tmp0_; gfloat _tmp1_; gfloat _tmp2_; gfloat _tmp3_; dReal _tmp4_ = 0.0; self = (soybodiesBox*) base; _tmp0_ = ((soybodiesBody*) self)->geom; _tmp1_ = x; _tmp2_ = y; _tmp3_ = z; _tmp4_ = dGeomBoxPointDepth ((struct dxGeom*) _tmp0_, (dReal) _tmp1_, (dReal) _tmp2_, (dReal) _tmp3_); result = (gfloat) _tmp4_; return result; }
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_box_point_depth() { int i,j; dVector3 s,p,q,q2; // s = box sides dMatrix3 R; dReal ss,d; // ss = smallest side dSimpleSpace space(0); dGeomID box = dCreateBox (0,1,1,1); 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 center point has depth of smallest side ss = 1e9; for (j=0; j<3; j++) if (s[j] < ss) ss = s[j]; if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol) FAILED(); // ********** test point on surface has depth 0 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; i = dRandInt (3); if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i]; dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); for (j=0; j<3; j++) q2[j] += p[j]; if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED(); // ********** test points outside box have -ve depth for (j=0; j<3; j++) { q[j] = 0.5*s[j] + dRandReal() + 0.01; if (dRandReal() > 0.5) q[j] = -q[j]; } dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); for (j=0; j<3; j++) q2[j] += p[j]; if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED(); // ********** test points inside box have +ve depth for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5); dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); for (j=0; j<3; j++) q2[j] += p[j]; if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED(); // ********** test random depth of point aligned along axis (up to ss deep) i = dRandInt (3); for (j=0; j<3; j++) q[j] = 0; d = (dRandReal()*(ss*0.5+1)-1); q[i] = s[i]*0.5 - d; if (dRandReal() > 0.5) q[i] = -q[i]; dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); for (j=0; j<3; j++) q2[j] += p[j]; if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED(); PASSED(); }