Beispiel #1
0
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();
}
Real32 PhysicsCapsuleGeom::getPointDepth(const Vec3f& p) const
{
	return (Real32)dGeomCapsulePointDepth(_GeomID, p.x(), p.y(), p.z());
}
Beispiel #3
0
int test_ccylinder_point_depth()
{
  int j;
  dVector3 p,a;
  dMatrix3 R;
  dReal r,l,beta,x,y,d;

  dSimpleSpace space(0);
  dGeomID ccyl = dCreateCapsule (0,1,1);
  dSpaceAdd (space,ccyl);

  // ********** make a random ccyl

  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 point on axis has depth of 'radius'

  beta = dRandReal()-0.5;
  for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2];
  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol)
    FAILED();

  // ********** test point on surface (excluding caps) has depth 0

  beta = dRandReal()*2*M_PI;
  x = r*sin(beta);
  y = r*cos(beta);
  beta = dRandReal()-0.5;
  for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();

  // ********** test point on surface of caps has depth 0

  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++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2];
  }
  else {
    for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2];
  }
  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();

  // ********** test point inside ccyl has positive depth

  for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
  dNormalize3 (a);
  beta = dRandReal()-0.5;
  for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2];
  if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED();

  // ********** test point depth (1)

  d = (dRandReal()*2-1) * r;
  beta = dRandReal()*2*M_PI;
  x = (r-d)*sin(beta);
  y = (r-d)*cos(beta);
  beta = dRandReal()-0.5;
  for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
    FAILED();

  // ********** test point depth (2)

  d = (dRandReal()*2-1) * r;
  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++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2];
  }
  else {
    for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2];
  }
  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
    FAILED();

  PASSED();
}