예제 #1
0
void TransformedGeometryExtensionLocalParams(dGeomID geom_transform,const dReal* axis,float center_prg,dReal* local_axis,dReal& local_center_prg)
{
	R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform");
	const dReal* rot=dGeomGetRotation(geom_transform);
	const dReal* pos=dGeomGetPosition(geom_transform);
	dVector3	local_pos;

	dMULTIPLY1_331(local_axis,rot,axis);
	dMULTIPLY1_331(local_pos,rot,pos);
	local_center_prg=center_prg-dDOT(local_pos,local_axis);
}
예제 #2
0
//positive inside
dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z)
{
	dUASSERT (g && g->type == dConeClass,"argument not a cone");
	dxCone *cone = (dxCone*) g;
	dVector3 tmp,q;
	tmp[0] = x - cone->pos[0];
	tmp[1] = y - cone->pos[1];
	tmp[2] = z - cone->pos[2];
	dMULTIPLY1_331 (q,cone->R,tmp);

	dReal r = cone->radius;
	dReal h = cone->lz;

	dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]);
	dReal d1 = q[2];
	dReal d2 = h-q[2];
	
	if (d0 < d1) {
    if (d0 < d2) return d0; else return d2;
	}
	else {
	if (d1 < d2) return d1; else return d2;
	}
}
예제 #3
0
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;
}
예제 #4
0
EXPORT_C int dBoxBox (const dVector3 p1, const dMatrix3 R1,
	     const dVector3 side1, const dVector3 p2,
	     const dMatrix3 R2, const dVector3 side2,
	     dVector3 normal, dReal *depth, int *return_code,
	     int maxc, dContactGeom *contact, int skip)
{
  const dReal fudge_factor = REAL(1.05);
  dVector3 p,pp,normalC;
  const dReal *normalR = 0;
  dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
  int i,j,invert_normal,code;

  // get vector from centers of box 1 to box 2, relative to box 1
  p[0] = p2[0] - p1[0];
  p[1] = p2[1] - p1[1];
  p[2] = p2[2] - p1[2];
  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2
  A[0] = dMUL(side1[0],REAL(0.5));
  A[1] = dMUL(side1[1],REAL(0.5));
  A[2] = dMUL(side1[2],REAL(0.5));
  B[0] = dMUL(side2[0],REAL(0.5));
  B[1] = dMUL(side2[1],REAL(0.5));
  B[2] = dMUL(side2[2],REAL(0.5));

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);

  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);

  // for all 15 possible separating axes:
  //   * see if the axis separates the boxes. if so, return 0.
  //   * find the depth of the penetration along the separating axis (s2)
  //   * if this is the largest depth so far, record it.
  // the normal vector will be set to the separating axis with the smallest
  // depth. note: normalR is set to point to a column of R1 or R2 if that is
  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  // set to a vector relative to body 1. invert_normal is 1 if the sign of
  // the normal should be flipped.

#define TST(expr1,expr2,norm,cc) \
  s2 = dFabs(expr1) - (expr2); \
  if (s2 > 0) return 0; \
  if (s2 > s) { \
    s = s2; \
    normalR = norm; \
    invert_normal = ((expr1) < 0); \
    code = (cc); \
  }

  s = -dInfinity;
  invert_normal = 0;
  code = 0;

  // separating axis = u1,u2,u3
  TST (pp[0],(A[0] + dMUL(B[0],Q11) + dMUL(B[1],Q12) + dMUL(B[2],Q13)),R1+0,1);
  TST (pp[1],(A[1] + dMUL(B[0],Q21) + dMUL(B[1],Q22) + dMUL(B[2],Q23)),R1+1,2);
  TST (pp[2],(A[2] + dMUL(B[0],Q31) + dMUL(B[1],Q32) + dMUL(B[2],Q33)),R1+2,3);

  // separating axis = v1,v2,v3
  TST (dDOT41(R2+0,p),(dMUL(A[0],Q11) + dMUL(A[1],Q21) + dMUL(A[2],Q31) + B[0]),R2+0,4);
  TST (dDOT41(R2+1,p),(dMUL(A[0],Q12) + dMUL(A[1],Q22) + dMUL(A[2],Q32) + B[1]),R2+1,5);
  TST (dDOT41(R2+2,p),(dMUL(A[0],Q13) + dMUL(A[1],Q23) + dMUL(A[2],Q33) + B[2]),R2+2,6);

  // note: cross product axes need to be scaled when s is computed.
  // normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
  s2 = dFabs(expr1) - (expr2); \
  if (s2 > 0) return 0; \
  l = dSqrt (dMUL((n1),(n1)) + dMUL((n2),(n2)) + dMUL((n3),(n3))); \
  if (l > 0) { \
    s2 = dDIV(s2,l); \
    if (dMUL(s2,fudge_factor) > s) { \
      s = s2; \
      normalR = 0; \
      normalC[0] = dDIV((n1),l); normalC[1] = dDIV((n2),l); normalC[2] = dDIV((n3),l); \
      invert_normal = ((expr1) < 0); \
      code = (cc); \
    } \
  }

  // separating axis = u1 x (v1,v2,v3)
  TST((dMUL(pp[2],R21)-dMUL(pp[1],R31)),(dMUL(A[1],Q31)+dMUL(A[2],Q21)+dMUL(B[1],Q13)+dMUL(B[2],Q12)),0,-R31,R21,7);
  TST((dMUL(pp[2],R22)-dMUL(pp[1],R32)),(dMUL(A[1],Q32)+dMUL(A[2],Q22)+dMUL(B[0],Q13)+dMUL(B[2],Q11)),0,-R32,R22,8);
  TST((dMUL(pp[2],R23)-dMUL(pp[1],R33)),(dMUL(A[1],Q33)+dMUL(A[2],Q23)+dMUL(B[0],Q12)+dMUL(B[1],Q11)),0,-R33,R23,9);

  // separating axis = u2 x (v1,v2,v3)
  TST((dMUL(pp[0],R31)-dMUL(pp[2],R11)),(dMUL(A[0],Q31)+dMUL(A[2],Q11)+dMUL(B[1],Q23)+dMUL(B[2],Q22)),R31,0,-R11,10);
  TST((dMUL(pp[0],R32)-dMUL(pp[2],R12)),(dMUL(A[0],Q32)+dMUL(A[2],Q12)+dMUL(B[0],Q23)+dMUL(B[2],Q21)),R32,0,-R12,11);
  TST((dMUL(pp[0],R33)-dMUL(pp[2],R13)),(dMUL(A[0],Q33)+dMUL(A[2],Q13)+dMUL(B[0],Q22)+dMUL(B[1],Q21)),R33,0,-R13,12);

  // separating axis = u3 x (v1,v2,v3)
  TST((dMUL(pp[1],R11)-dMUL(pp[0],R21)),(dMUL(A[0],Q21)+dMUL(A[1],Q11)+dMUL(B[1],Q33)+dMUL(B[2],Q32)),-R21,R11,0,13);
  TST((dMUL(pp[1],R12)-dMUL(pp[0],R22)),(dMUL(A[0],Q22)+dMUL(A[1],Q12)+dMUL(B[0],Q33)+dMUL(B[2],Q31)),-R22,R12,0,14);
  TST((dMUL(pp[1],R13)-dMUL(pp[0],R23)),(dMUL(A[0],Q23)+dMUL(A[1],Q13)+dMUL(B[0],Q32)+dMUL(B[1],Q31)),-R23,R13,0,15);

#undef TST

  if (!code) return 0;

  // if we get to this point, the boxes interpenetrate. compute the normal
  // in global coordinates.
  if (normalR) {
    normal[0] = normalR[0];
    normal[1] = normalR[4];
    normal[2] = normalR[8];
  }
  else {
    dMULTIPLY0_331 (normal,R1,normalC);
  }
  if (invert_normal) {
    normal[0] = -normal[0];
    normal[1] = -normal[1];
    normal[2] = -normal[2];
  }
  *depth = -s;

  // compute contact point(s)

  if (code > 6) {
    // an edge from box 1 touches an edge from box 2.
    // find a point pa on the intersecting edge of box 1
    dVector3 pa;
    dReal sign;
    for (i=0; i<3; i++) pa[i] = p1[i];
    for (j=0; j<3; j++) {
      sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
      for (i=0; i<3; i++) pa[i] += dMUL(sign,dMUL(A[j],R1[i*4+j]));
    }

    // find a point pb on the intersecting edge of box 2
    dVector3 pb;
    for (i=0; i<3; i++) pb[i] = p2[i];
    for (j=0; j<3; j++) {
      sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
      for (i=0; i<3; i++) pb[i] += dMUL(sign,dMUL(B[j],R2[i*4+j]));
    }

    dReal alpha,beta;
    dVector3 ua,ub;
    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];

    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
    for (i=0; i<3; i++) pa[i] += dMUL(ua[i],alpha);
    for (i=0; i<3; i++) pb[i] += dMUL(ub[i],beta);

    for (i=0; i<3; i++) contact[0].pos[i] = dMUL(REAL(0.5),(pa[i]+pb[i]));
    contact[0].depth = *depth;
    *return_code = code;
    return 1;
  }

  // okay, we have a face-something intersection (because the separating
  // axis is perpendicular to a face). define face 'a' to be the reference
  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
  // the incident face (the closest face of the other box).

  const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
  if (code <= 3) {
    Ra = R1;
    Rb = R2;
    pa = p1;
    pb = p2;
    Sa = A;
    Sb = B;
  }
  else {
    Ra = R2;
    Rb = R1;
    pa = p2;
    pb = p1;
    Sa = B;
    Sb = A;
  }

  // nr = normal vector of reference face dotted with axes of incident box.
  // anr = absolute values of nr.
  dVector3 normal2,nr,anr;
  if (code <= 3) {
    normal2[0] = normal[0];
    normal2[1] = normal[1];
    normal2[2] = normal[2];
  }
  else {
    normal2[0] = -normal[0];
    normal2[1] = -normal[1];
    normal2[2] = -normal[2];
  }
  dMULTIPLY1_331 (nr,Rb,normal2);
  anr[0] = dFabs (nr[0]);
  anr[1] = dFabs (nr[1]);
  anr[2] = dFabs (nr[2]);

  // find the largest compontent of anr: this corresponds to the normal
  // for the indident face. the other axis numbers of the indicent face
  // are stored in a1,a2.
  int lanr,a1,a2;
  if (anr[1] > anr[0]) {
    if (anr[1] > anr[2]) {
      a1 = 0;
      lanr = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }
  else {
    if (anr[0] > anr[2]) {
      lanr = 0;
      a1 = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }

  // compute center point of incident face, in reference-face coordinates
  dVector3 center;
  if (nr[lanr] < 0) {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + dMUL(Sb[lanr],Rb[i*4+lanr]);
  }
  else {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - dMUL(Sb[lanr],Rb[i*4+lanr]);
  }

  // find the normal and non-normal axis numbers of the reference box
  int codeN,code1,code2;
  if (code <= 3) codeN = code-1; else codeN = code-4;
  if (codeN==0) {
    code1 = 1;
    code2 = 2;
  }
  else if (codeN==1) {
    code1 = 0;
    code2 = 2;
  }
  else {
    code1 = 0;
    code2 = 1;
  }

  // find the four corners of the incident face, in reference-face coordinates
  dReal quad[8];	// 2D coordinate of incident face (x,y pairs)
  dReal c1,c2,m11,m12,m21,m22;
  c1 = dDOT14 (center,Ra+code1);
  c2 = dDOT14 (center,Ra+code2);
  // optimize this? - we have already computed this data above, but it is not
  // stored in an easy-to-index format. for now it's quicker just to recompute
  // the four dot products.
  m11 = dDOT44 (Ra+code1,Rb+a1);
  m12 = dDOT44 (Ra+code1,Rb+a2);
  m21 = dDOT44 (Ra+code2,Rb+a1);
  m22 = dDOT44 (Ra+code2,Rb+a2);
  {
    dReal k1 = dMUL(m11,Sb[a1]);
    dReal k2 = dMUL(m21,Sb[a1]);
    dReal k3 = dMUL(m12,Sb[a2]);
    dReal k4 = dMUL(m22,Sb[a2]);
    quad[0] = c1 - k1 - k3;
    quad[1] = c2 - k2 - k4;
    quad[2] = c1 - k1 + k3;
    quad[3] = c2 - k2 + k4;
    quad[4] = c1 + k1 + k3;
    quad[5] = c2 + k2 + k4;
    quad[6] = c1 + k1 - k3;
    quad[7] = c2 + k2 - k4;
  }

  // find the size of the reference face
  dReal rect[2];
  rect[0] = Sa[code1];
  rect[1] = Sa[code2];

  // intersect the incident and reference faces
  dReal ret[16];
  int n = intersectRectQuad (rect,quad,ret);
  if (n < 1) return 0;		// this should never happen

  // convert the intersection points into reference-face coordinates,
  // and compute the contact position and depth for each point. only keep
  // those points that have a positive (penetrating) depth. delete points in
  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
  dReal point[3*8];		// penetrating contact points
  dReal dep[8];			// depths for those points
  dReal det1 = dRecip(dMUL(m11,m22) - dMUL(m12,m21));
  m11 = dMUL(m11,det1);
  m12 = dMUL(m12,det1);
  m21 = dMUL(m21,det1);
  m22 = dMUL(m22,det1);
  int cnum = 0;			// number of penetrating contact points found
  for (j=0; j < n; j++) {
    dReal k1 =  dMUL(m22,(ret[j*2]-c1)) - dMUL(m12,(ret[j*2+1]-c2));
    dReal k2 = -dMUL(m21,(ret[j*2]-c1)) + dMUL(m11,(ret[j*2+1]-c2));
    for (i=0; i<3; i++) point[cnum*3+i] =
			  center[i] + dMUL(k1,Rb[i*4+a1]) + dMUL(k2,Rb[i*4+a2]);
    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
    if (dep[cnum] >= 0) {
      ret[cnum*2] = ret[j*2];
      ret[cnum*2+1] = ret[j*2+1];
      cnum++;
    }
  }
  if (cnum < 1) return 0;	// this should never happen

  // we can't generate more contacts than we actually have
  if (maxc > cnum) maxc = cnum;
  if (maxc < 1) maxc = 1;

  if (cnum <= maxc) {
    // we have less contacts than we need, so we use them all
    for (j=0; j < cnum; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
      con->depth = dep[j];
    }
  }
  else {
    // we have more contacts than are wanted, some of them must be culled.
    // find the deepest point, it is always the first contact.
    int i1 = 0;
    dReal maxdepth = dep[0];
    for (i=1; i<cnum; i++) {
      if (dep[i] > maxdepth) {
	maxdepth = dep[i];
	i1 = i;
      }
    }

    int iret[8];
    cullPoints (cnum,ret,maxc,i1,iret);

    for (j=0; j < maxc; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
      con->depth = dep[iret[j]];
    }
    cnum = maxc;
  }

  *return_code = code;
  return cnum;
}
예제 #5
0
파일: collision.c 프로젝트: mewbak/vu
int dBoxBox2 (const btVector3 p1, const dMatrix3 R1,
		const btVector3 side1, const btVector3 p2,
		const dMatrix3 R2, const btVector3 side2,
		BoxBoxResults *results)
{
	const btScalar fudge_factor = 1.05;
	btVector3 p,pp,normalC;
	normalC[0] = 0.f;
	normalC[1] = 0.f;
	normalC[2] = 0.f;
	const btScalar *normalR = 0;
	btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
			 Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,normal[3],depth;
	int i,j,invert_normal,code;

	// get vector from centers of box 1 to box 2, relative to box 1
	p[0] = p2[0] - p1[0];
	p[1] = p2[1] - p1[1];
	p[2] = p2[2] - p1[2];
	dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1

	// get side lengths (already specified as half lengths)
	A[0] = side1[0];
	A[1] = side1[1];
	A[2] = side1[2];
	B[0] = side2[0];
	B[1] = side2[1];
	B[2] = side2[2];

	// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
	R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
	R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
	R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);

	Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
	Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
	Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);

	// for all 15 possible separating axes:
	//   * see if the axis separates the boxes. if so, return 0.
	//   * find the depth of the penetration along the separating axis (s2)
	//   * if this is the largest depth so far, record it.
	// the normal vector will be set to the separating axis with the smallest
	// depth. note: normalR is set to point to a column of R1 or R2 if that is
	// the smallest depth normal so far. otherwise normalR is 0 and normalC is
	// set to a vector relative to body 1. invert_normal is 1 if the sign of
	// the normal should be flipped.

#define TST(expr1,expr2,norm,cc) \
	s2 = btFabs(expr1) - (expr2); \
	if (s2 > 0) return 0; \
	if (s2 > s) { \
		s = s2; \
		normalR = norm; \
		invert_normal = ((expr1) < 0); \
		code = (cc); \
	}

	s = -dInfinity;
	invert_normal = 0;
	code = 0;

	// separating axis = u1,u2,u3
	TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
	TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
	TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);

	// separating axis = v1,v2,v3
	TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
	TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
	TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);

	// note: cross product axes need to be scaled when s is computed.
	// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
	s2 = btFabs(expr1) - (expr2); \
	if (s2 > SIMD_EPSILON) return 0; \
	l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
	if (l > SIMD_EPSILON) { \
		s2 /= l; \
		if (s2*fudge_factor > s) { \
			s = s2; \
			normalR = 0; \
			normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
			invert_normal = ((expr1) < 0); \
			code = (cc); \
		} \
	}

	btScalar fudge2 = 1.0e-5f;

	Q11 += fudge2;
	Q12 += fudge2;
	Q13 += fudge2;

	Q21 += fudge2;
	Q22 += fudge2;
	Q23 += fudge2;

	Q31 += fudge2;
	Q32 += fudge2;
	Q33 += fudge2;

	// separating axis = u1 x (v1,v2,v3)
	TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
	TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
	TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);

	// separating axis = u2 x (v1,v2,v3)
	TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
	TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
	TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);

	// separating axis = u3 x (v1,v2,v3)
	TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
	TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
	TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);

#undef TST

	if (!code) return 0;
	results->code = code;

	// if we get to this point, the boxes interpenetrate. compute the normal
	// in global coordinates.
	if (normalR) {
		normal[0] = normalR[0];
		normal[1] = normalR[4];
		normal[2] = normalR[8];
	}
	else {
		dMULTIPLY0_331 (normal,R1,normalC);
	}
	if (invert_normal) {
		normal[0] = -normal[0];
		normal[1] = -normal[1];
		normal[2] = -normal[2];
	}
	depth = -s;

	// compute contact point(s)

	if (code > 6) {
		// an edge from box 1 touches an edge from box 2.
		// find a point pa on the intersecting edge of box 1
		btVector3 pa;
		btScalar sign;
		for (i=0; i<3; i++) pa[i] = p1[i];
		for (j=0; j<3; j++) {
			sign = (dDOT14(normal,R1+j) > 0) ? 1.0 : -1.0;
			for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
		}

		// find a point pb on the intersecting edge of box 2
		btVector3 pb;
		for (i=0; i<3; i++) pb[i] = p2[i];
		for (j=0; j<3; j++) {
			sign = (dDOT14(normal,R2+j) > 0) ? -1.0 : 1.0;
			for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
		}

		btScalar alpha,beta;
		btVector3 ua,ub;
		for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
		for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];

		dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
		for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
		for (i=0; i<3; i++) pb[i] += ub[i]*beta;

		{
			btVector3 pointInWorld;
			addContactPoint(results,normal,pb,depth);
		}
		return 1;
	}

	// okay, we have a face-something intersection (because the separating
	// axis is perpendicular to a face). define face 'a' to be the reference
	// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
	// the incident face (the closest face of the other box).

	const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
	if (code <= 3) {
		Ra = R1;
		Rb = R2;
		pa = p1;
		pb = p2;
		Sa = A;
		Sb = B;
	}
	else {
		Ra = R2;
		Rb = R1;
		pa = p2;
		pb = p1;
		Sa = B;
		Sb = A;
	}

	// nr = normal vector of reference face dotted with axes of incident box.
	// anr = absolute values of nr.
	btVector3 normal2,nr,anr;
	if (code <= 3) {
		normal2[0] = normal[0];
		normal2[1] = normal[1];
		normal2[2] = normal[2];
	}
	else {
		normal2[0] = -normal[0];
		normal2[1] = -normal[1];
		normal2[2] = -normal[2];
	}
	dMULTIPLY1_331 (nr,Rb,normal2);
	anr[0] = btFabs (nr[0]);
	anr[1] = btFabs (nr[1]);
	anr[2] = btFabs (nr[2]);

	// find the largest compontent of anr: this corresponds to the normal
	// for the indident face. the other axis numbers of the indicent face
	// are stored in a1,a2.
	int lanr,a1,a2;
	if (anr[1] > anr[0]) {
		if (anr[1] > anr[2]) {
			a1 = 0;
			lanr = 1;
			a2 = 2;
		}
		else {
			a1 = 0;
			a2 = 1;
			lanr = 2;
		}
	}
	else {
		if (anr[0] > anr[2]) {
			lanr = 0;
			a1 = 1;
			a2 = 2;
		}
		else {
			a1 = 0;
			a2 = 1;
			lanr = 2;
		}
	}

	// compute center point of incident face, in reference-face coordinates
	btVector3 center;
	if (nr[lanr] < 0) {
		for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
	}
	else {
		for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
	}

	// find the normal and non-normal axis numbers of the reference box
	int codeN,code1,code2;
	if (code <= 3) codeN = code-1; else codeN = code-4;
	if (codeN==0) {
		code1 = 1;
		code2 = 2;
	}
	else if (codeN==1) {
		code1 = 0;
		code2 = 2;
	}
	else {
		code1 = 0;
		code2 = 1;
	}

	// find the four corners of the incident face, in reference-face coordinates
	btScalar quad[8];	// 2D coordinate of incident face (x,y pairs)
	btScalar c1,c2,m11,m12,m21,m22;
	c1 = dDOT14 (center,Ra+code1);
	c2 = dDOT14 (center,Ra+code2);
	// optimize this? - we have already computed this data above, but it is not
	// stored in an easy-to-index format. for now it's quicker just to recompute
	// the four dot products.
	m11 = dDOT44 (Ra+code1,Rb+a1);
	m12 = dDOT44 (Ra+code1,Rb+a2);
	m21 = dDOT44 (Ra+code2,Rb+a1);
	m22 = dDOT44 (Ra+code2,Rb+a2);
	{
		btScalar k1 = m11*Sb[a1];
		btScalar k2 = m21*Sb[a1];
		btScalar k3 = m12*Sb[a2];
		btScalar k4 = m22*Sb[a2];
		quad[0] = c1 - k1 - k3;
		quad[1] = c2 - k2 - k4;
		quad[2] = c1 - k1 + k3;
		quad[3] = c2 - k2 + k4;
		quad[4] = c1 + k1 + k3;
		quad[5] = c2 + k2 + k4;
		quad[6] = c1 + k1 - k3;
		quad[7] = c2 + k2 - k4;
	}

	// find the size of the reference face
	btScalar rect[2];
	rect[0] = Sa[code1];
	rect[1] = Sa[code2];

	// intersect the incident and reference faces
	btScalar ret[16];
	int n = intersectRectQuad2 (rect,quad,ret);
	if (n < 1) return 0;		// this should never happen

	// convert the intersection points into reference-face coordinates,
	// and compute the contact position and depth for each point. only keep
	// those points that have a positive (penetrating) depth. delete points in
	// the 'ret' array as necessary so that 'point' and 'ret' correspond.
	btScalar point[3*8];		// penetrating contact points
	btScalar dep[8];			// depths for those points
	btScalar det1 = 1.f/(m11*m22 - m12*m21);
	m11 *= det1;
	m12 *= det1;
	m21 *= det1;
	m22 *= det1;
	int cnum = 0;			// number of penetrating contact points found
	for (j=0; j < n; j++) {
		btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
		btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
		for (i=0; i<3; i++) point[cnum*3+i] =
			center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
		dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
		if (dep[cnum] >= 0) {
			ret[cnum*2] = ret[j*2];
			ret[cnum*2+1] = ret[j*2+1];
			cnum++;
		}
	}
	if (cnum < 1) return 0;	// this should never happen

	// we can't generate more contacts than we actually have
	int maxc = 4;
	if (maxc > cnum) maxc = cnum;
	if (maxc < 1) maxc = 1;

	if (cnum <= maxc)
	{
		if (code<4)
		{
			// we have less contacts than we need, so we use them all
			for (j=0; j < cnum; j++)
			{
				btVector3 pointInWorld;
				for (i=0; i<3; i++)
					pointInWorld[i] = point[j*3+i] + pa[i];
				addContactPoint(results,normal,pointInWorld,dep[j]);
			}
		}
		else
		{
			// we have less contacts than we need, so we use them all
			for (j=0; j < cnum; j++)
			{
				btVector3 pointInWorld;
				for (i=0; i<3; i++)
					pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
				addContactPoint(results,normal,pointInWorld,dep[j]);
			}
		}
	}
	else {
		// we have more contacts than are wanted, some of them must be culled.
		// find the deepest point, it is always the first contact.
		int i1 = 0;
		btScalar maxdepth = dep[0];
		for (i=1; i<cnum; i++)
		{
			if (dep[i] > maxdepth)
			{
				maxdepth = dep[i];
				i1 = i;
			}
		}

		int iret[8];
		cullPoints2 (cnum,ret,maxc,i1,iret);

		for (j=0; j < maxc; j++)
		{
			btVector3 posInWorld;
			for (i=0; i<3; i++)
				posInWorld[i] = point[iret[j]*3+i] + pa[i];
			if (code<4)
			{
				addContactPoint(results, normal,posInWorld,dep[iret[j]]);
			}
			else
			{
				posInWorld[0] -= normal[0]*dep[iret[j]];
				posInWorld[1] -= normal[1]*dep[iret[j]];
				posInWorld[2] -= normal[2]*dep[iret[j]];
				addContactPoint(results, normal,posInWorld,dep[iret[j]]);
			}
		}
		cnum = maxc;
	}
	return cnum;
}
예제 #6
0
파일: DARTCollide.cpp 프로젝트: dtbinh/dart
void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
                            const dVector3 c, const dMatrix3 R,
                            const dVector3 side,
                            dVector3 lret, dVector3 bret)
{
  int i;

  // compute the start and delta of the line p1-p2 relative to the box.
  // we will do all subsequent computations in this box-relative coordinate
  // system. we have to do a translation and rotation for each point.
  dVector3 tmp,s,v;
  tmp[0] = p1[0] - c[0];
  tmp[1] = p1[1] - c[1];
  tmp[2] = p1[2] - c[2];
  dMULTIPLY1_331 (s,R,tmp);
  tmp[0] = p2[0] - p1[0];
  tmp[1] = p2[1] - p1[1];
  tmp[2] = p2[2] - p1[2];
  dMULTIPLY1_331 (v,R,tmp);

  // mirror the line so that v has all components >= 0
  dVector3 sign;
  for (i=0; i<3; i++) {
    if (v[i] < 0) {
      s[i] = -s[i];
      v[i] = -v[i];
      sign[i] = -1;
    }
    else sign[i] = 1;
  }

  // compute v^2
  dVector3 v2;
  v2[0] = v[0]*v[0];
  v2[1] = v[1]*v[1];
  v2[2] = v[2]*v[2];

  // compute the half-sides of the box
  double h[3];
  h[0] = side[0];
  h[1] = side[1];
  h[2] = side[2];

  // region is -1,0,+1 depending on which side of the box planes each
  // coordinate is on. tanchor is the next t value at which there is a
  // transition, or the last one if there are no more.
  int region[3];
  double tanchor[3];

  // find the region and tanchor values for p1
  for (i=0; i<3; i++) {
    if (v[i] > 0) {
      if (s[i] < -h[i]) {
        region[i] = -1;
        tanchor[i] = (-h[i]-s[i])/v[i];
      }
      else {
        region[i] = (s[i] > h[i]);
        tanchor[i] = (h[i]-s[i])/v[i];
      }
    }
    else {
      region[i] = 0;
      tanchor[i] = 2;		// this will never be a valid tanchor
    }
  }

  // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
  double t=0;
  double dd2dt = 0;
  for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i];
  if (dd2dt >= 0) goto got_answer;

  do {
    // find the point on the line that is at the next clip plane boundary
    double next_t = 1;
    for (i=0; i<3; i++) {
      if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t)
        next_t = tanchor[i];
    }

    // compute d|d|^2/dt for the next t
    double next_dd2dt = 0;
    for (i=0; i<3; i++) {
      next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]);
    }

    // if the sign of d|d|^2/dt has changed, solution = the crossover point
    if (next_dd2dt >= 0) {
      double m = (next_dd2dt-dd2dt)/(next_t - t);
      t -= dd2dt/m;
      goto got_answer;
    }

    // advance to the next anchor point / region
    for (i=0; i<3; i++) {
      if (tanchor[i] == next_t) {
        tanchor[i] = (h[i]-s[i])/v[i];
        region[i]++;
      }
    }
    t = next_t;
    dd2dt = next_dd2dt;
  }
  while (t < 1);
  t = 1;

got_answer:

  // compute closest point on the line
  for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i];	// note: tmp=p2-p1

  // compute closest point on the box
  for (i=0; i<3; i++) {
    tmp[i] = sign[i] * (s[i] + t*v[i]);
    if (tmp[i] < -h[i]) tmp[i] = -h[i];
    else if (tmp[i] > h[i]) tmp[i] = h[i];
  }
  dMULTIPLY0_331 (s,R,tmp);
  for (i=0; i<3; i++) bret[i] = s[i] + c[i];
}
dReal doStuffAndGetError (int n)
{
  switch (n) {

  // ********** fixed joint

  case 0: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the orientations are the same
    const dReal *R1 = dBodyGetRotation (body[0]);
    const dReal *R2 = dBodyGetRotation (body[1]);
    dReal err1 = dMaxDifference (R1,R2,3,3);
    // check the body offset is correct
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return (err1 + length (pp)) * 300;
  }

  case 1: {			// 1 body to static env
    addOscillatingTorque (0.1);

    // check the orientation is the identity
    dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return (err1 + length (p)) * 1e6;
  }

  case 2: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the body offset is correct
    // Should really check body rotation too.  Oh well.
    const dReal *R1 = dBodyGetRotation (body[0]);
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return length(pp) * 300;
  }

  case 3: {			// 1 body to static env with relative rotation
    addOscillatingTorque (0.1);

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return  length (p) * 1e6;
  }


  // ********** hinge joint

  case 200:			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    return dInfinity;

  case 220:			// hinge angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHingeAngle (joint);
      if (a > 0.5 && a < 1) return 0; else return 10;
    }
    return 0;

  case 221: {			// hinge angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHingeAngle (joint);
    dReal r = dJointGetHingeAngleRate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er) * 4e4;
  }

  case 230:			// hinge motor rate (and polarity) test
  case 231: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHingeAngleRate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHingeParam (joint,dParamVel,cos(a));
    if (n==231) return dInfinity;
    return err * 1e6;
  }

  // ********** slider joint

  case 300:			// 2 body
    addOscillatingTorque (0.05);
    dampRotationalMotion (0.1);
    addSpringForce (0.5);
    return dInfinity;

  case 320:			// slider angle polarity test
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    if (iteration == 40) {
      dReal a = dJointGetSliderPosition (joint);
      if (a > 0.2 && a < 0.5) return 0; else return 10;
      return a;
    }
    return 0;

  case 321: {			// slider angle rate test
    static dReal last_pos = 0;
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    dReal p = dJointGetSliderPosition (joint);
    dReal r = dJointGetSliderPositionRate (joint);
    dReal er = (p-last_pos)/STEPSIZE;	// estimated rate (almost exact)
    last_pos = p;
    return fabs(r-er) * 1e9;
  }

  case 330:			// slider motor rate (and polarity) test
  case 331: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetSliderPositionRate (joint);
    dReal err = fabs (0.7*cos(a) - r);
    if (a < 0.04) err = 0;
    a += 0.03;
    dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
    if (n==331) return dInfinity;
    return err * 1e6;
  }

  // ********** hinge-2 joint

  case 420:			// hinge-2 steering angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHinge2Angle1 (joint);
      if (a > 0.5 && a < 0.6) return 0; else return 10;
    }
    return 0;

  case 421: {			// hinge-2 steering angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHinge2Angle1 (joint);
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er)*2e4;
  }

  case 430:			// hinge 2 steering motor rate (+polarity) test
  case 431: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel,cos(a));
    if (n==431) return dInfinity;
    return err * 1e6;
  }

  case 432: {			// hinge 2 wheel motor rate (+polarity) test
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle2Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel2,cos(a));
    return err * 1e6;
  }

  // ********** angular motor joint

  case 600: {			// test euler angle calculations
    // desired euler angles from last iteration
    static dReal a1,a2,a3;

    // find actual euler angles
    dReal aa1 = dJointGetAMotorAngle (joint,0);
    dReal aa2 = dJointGetAMotorAngle (joint,1);
    dReal aa3 = dJointGetAMotorAngle (joint,2);
    // printf ("actual  = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);

    dReal err = dInfinity;
    if (iteration > 0) {
      err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
      err *= 1e10;
    }

    // get random base rotation for both bodies
    dMatrix3 Rbase;
    dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
			3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
    dBodySetRotation (body[0],Rbase);

    // rotate body 2 by random euler angles w.r.t. body 1
    a1 = 3.14 * 2 * (dRandReal()-0.5);
    a2 = 1.57 * 2 * (dRandReal()-0.5);
    a3 = 3.14 * 2 * (dRandReal()-0.5);
    dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
    dRFromAxisAndAngle (R1,0,0,1,-a1);
    dRFromAxisAndAngle (R2,0,1,0,a2);
    dRFromAxisAndAngle (R3,1,0,0,-a3);
    dMultiply0 (Rtmp1,R2,R3,3,3,3);
    dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
    dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
    dBodySetRotation (body[1],Rtmp1);
    // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);

    return err;
  }

  // ********** universal joint

  case 700: {		// 2 body: joint constraint
    dVector3 ax1, ax2;

    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 701: {		// 2 body: angle 1 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 702: {		// 2 body: angle 2 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 720: {		// universal transmit torque test: constraint error
    dVector3 ax1, ax2;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 721: {		// universal transmit torque test: angle1 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 722: {		// universal transmit torque test: angle2 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 730:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 731:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 2e3;
  }

  case 732:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 740:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 741:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 742:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e4;
  }
  }

  return dInfinity;
}
예제 #8
0
파일: box.cpp 프로젝트: 4nakin/awesomeball
int dBoxBox (const dVector3 p1, const dMatrix3 R1,
	     const dVector3 side1, const dVector3 p2,
	     const dMatrix3 R2, const dVector3 side2,
	     dVector3 normal, dReal *depth, int *return_code,
	     int flags, dContactGeom *contact, int skip)
{
  const dReal fudge_factor = REAL(1.05);
  dVector3 p,pp,normalC={0,0,0};
  const dReal *normalR = 0;
  dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
  int i,j,invert_normal,code;

  // get vector from centers of box 1 to box 2, relative to box 1
  p[0] = p2[0] - p1[0];
  p[1] = p2[1] - p1[1];
  p[2] = p2[2] - p1[2];
  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2
  A[0] = side1[0]*REAL(0.5);
  A[1] = side1[1]*REAL(0.5);
  A[2] = side1[2]*REAL(0.5);
  B[0] = side2[0]*REAL(0.5);
  B[1] = side2[1]*REAL(0.5);
  B[2] = side2[2]*REAL(0.5);

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);

  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);

  // for all 15 possible separating axes:
  //   * see if the axis separates the boxes. if so, return 0.
  //   * find the depth of the penetration along the separating axis (s2)
  //   * if this is the largest depth so far, record it.
  // the normal vector will be set to the separating axis with the smallest
  // depth. note: normalR is set to point to a column of R1 or R2 if that is
  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  // set to a vector relative to body 1. invert_normal is 1 if the sign of
  // the normal should be flipped.

  do {
#define TST(expr1,expr2,norm,cc) \
    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
    s2 = dFabs(expr1_val) - (expr2); \
    if (s2 > 0) return 0; \
    if (s2 > s) { \
      s = s2; \
      normalR = norm; \
      invert_normal = ((expr1_val) < 0); \
      code = (cc); \
	  if (flags & CONTACTS_UNIMPORTANT) break; \
	}

    s = -dInfinity;
    invert_normal = 0;
    code = 0;

    // separating axis = u1,u2,u3
    TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
    TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
    TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);

    // separating axis = v1,v2,v3
    TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
    TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
    TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);

    // note: cross product axes need to be scaled when s is computed.
    // normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
    s2 = dFabs(expr1_val) - (expr2); \
    if (s2 > 0) return 0; \
    l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
    if (l > 0) { \
      s2 /= l; \
      if (s2*fudge_factor > s) { \
        s = s2; \
        normalR = 0; \
        normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
        invert_normal = ((expr1_val) < 0); \
        code = (cc); \
        if (flags & CONTACTS_UNIMPORTANT) break; \
	  } \
	}

    // We only need to check 3 edges per box 
    // since parallel edges are equivalent.

    // separating axis = u1 x (v1,v2,v3)
    TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
    TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
    TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);

    // separating axis = u2 x (v1,v2,v3)
    TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
    TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
    TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);

    // separating axis = u3 x (v1,v2,v3)
    TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
    TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
    TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
  } while (0);

  if (!code) return 0;

  // if we get to this point, the boxes interpenetrate. compute the normal
  // in global coordinates.
  if (normalR) {
    normal[0] = normalR[0];
    normal[1] = normalR[4];
    normal[2] = normalR[8];
  }
  else {
    dMULTIPLY0_331 (normal,R1,normalC);
  }
  if (invert_normal) {
    normal[0] = -normal[0];
    normal[1] = -normal[1];
    normal[2] = -normal[2];
  }
  *depth = -s;

  // compute contact point(s)

  if (code > 6) {
    // An edge from box 1 touches an edge from box 2.
    // find a point pa on the intersecting edge of box 1
    dVector3 pa;
    dReal sign;
    // Copy p1 into pa
    for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy?
    // Get world position of p2 into pa
    for (j=0; j<3; j++) {
      sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
    }

    // find a point pb on the intersecting edge of box 2
    dVector3 pb;
    // Copy p2 into pb
    for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy?
    // Get world position of p2 into pb
    for (j=0; j<3; j++) {
      sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
    }
    
    dReal alpha,beta;
    dVector3 ua,ub;
    // Get direction of first edge
    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
    // Get direction of second edge
    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
    // Get closest points between edges (one at each)
    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);    
    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
    // Set the contact point as halfway between the 2 closest points
    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
    contact[0].depth = *depth;
    *return_code = code;
    return 1;
  }

  // okay, we have a face-something intersection (because the separating
  // axis is perpendicular to a face). define face 'a' to be the reference
  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
  // the incident face (the closest face of the other box).
  // Note: Unmodified parameter values are being used here
  const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
  if (code <= 3) { // One of the faces of box 1 is the reference face
    Ra = R1; // Rotation of 'a'
    Rb = R2; // Rotation of 'b'
    pa = p1; // Center (location) of 'a'
    pb = p2; // Center (location) of 'b'
    Sa = A;  // Side Lenght of 'a'
    Sb = B;  // Side Lenght of 'b'
  }
  else { // One of the faces of box 2 is the reference face
    Ra = R2; // Rotation of 'a'
    Rb = R1; // Rotation of 'b'
    pa = p2; // Center (location) of 'a'
    pb = p1; // Center (location) of 'b'
    Sa = B;  // Side Lenght of 'a'
    Sb = A;  // Side Lenght of 'b'
  }

  // nr = normal vector of reference face dotted with axes of incident box.
  // anr = absolute values of nr.
  /*
	The normal is flipped if necessary so it always points outward from box 'a',
	box 'b' is thus always the incident box
  */
  dVector3 normal2,nr,anr;
  if (code <= 3) {
    normal2[0] = normal[0];
    normal2[1] = normal[1];
    normal2[2] = normal[2];
  }
  else {
    normal2[0] = -normal[0];
    normal2[1] = -normal[1];
    normal2[2] = -normal[2];
  }
  // Rotate normal2 in incident box opposite direction
  dMULTIPLY1_331 (nr,Rb,normal2);
  anr[0] = dFabs (nr[0]);
  anr[1] = dFabs (nr[1]);
  anr[2] = dFabs (nr[2]);

  // find the largest compontent of anr: this corresponds to the normal
  // for the incident face. the other axis numbers of the incident face
  // are stored in a1,a2.
  int lanr,a1,a2;
  if (anr[1] > anr[0]) {
    if (anr[1] > anr[2]) {
      a1 = 0;
      lanr = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }
  else {
    if (anr[0] > anr[2]) {
      lanr = 0;
      a1 = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }

  // compute center point of incident face, in reference-face coordinates
  dVector3 center;
  if (nr[lanr] < 0) {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
  }
  else {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
  }

  // find the normal and non-normal axis numbers of the reference box
  int codeN,code1,code2;
  if (code <= 3) codeN = code-1; else codeN = code-4;
  if (codeN==0) {
    code1 = 1;
    code2 = 2;
  }
  else if (codeN==1) {
    code1 = 0;
    code2 = 2;
  }
  else {
    code1 = 0;
    code2 = 1;
  }

  // find the four corners of the incident face, in reference-face coordinates
  dReal quad[8];	// 2D coordinate of incident face (x,y pairs)
  dReal c1,c2,m11,m12,m21,m22;
  c1 = dDOT14 (center,Ra+code1);
  c2 = dDOT14 (center,Ra+code2);
  // optimize this? - we have already computed this data above, but it is not
  // stored in an easy-to-index format. for now it's quicker just to recompute
  // the four dot products.
  m11 = dDOT44 (Ra+code1,Rb+a1);
  m12 = dDOT44 (Ra+code1,Rb+a2);
  m21 = dDOT44 (Ra+code2,Rb+a1);
  m22 = dDOT44 (Ra+code2,Rb+a2);
  {
    dReal k1 = m11*Sb[a1];
    dReal k2 = m21*Sb[a1];
    dReal k3 = m12*Sb[a2];
    dReal k4 = m22*Sb[a2];
    quad[0] = c1 - k1 - k3;
    quad[1] = c2 - k2 - k4;
    quad[2] = c1 - k1 + k3;
    quad[3] = c2 - k2 + k4;
    quad[4] = c1 + k1 + k3;
    quad[5] = c2 + k2 + k4;
    quad[6] = c1 + k1 - k3;
    quad[7] = c2 + k2 - k4;
  }

  // find the size of the reference face
  dReal rect[2];
  rect[0] = Sa[code1];
  rect[1] = Sa[code2];

  // intersect the incident and reference faces
  dReal ret[16];
  int n = intersectRectQuad (rect,quad,ret);
  if (n < 1) return 0;		// this should never happen

  // convert the intersection points into reference-face coordinates,
  // and compute the contact position and depth for each point. only keep
  // those points that have a positive (penetrating) depth. delete points in
  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
  dReal point[3*8];		// penetrating contact points
  dReal dep[8];			// depths for those points
  dReal det1 = dRecip(m11*m22 - m12*m21);
  m11 *= det1;
  m12 *= det1;
  m21 *= det1;
  m22 *= det1;
  int cnum = 0;			// number of penetrating contact points found
  for (j=0; j < n; j++) {
    dReal k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
    dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
    for (i=0; i<3; i++) point[cnum*3+i] =
			  center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
    if (dep[cnum] >= 0) {
      ret[cnum*2] = ret[j*2];
      ret[cnum*2+1] = ret[j*2+1];
      cnum++;
	  if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
		  break;
	  }
    }
  }
  if (cnum < 1) { 
	  return 0;	// this should not happen, yet does at times (demo_plane2d single precision).
  }

  // we can't generate more contacts than we actually have
  int maxc = flags & NUMC_MASK;
  if (maxc > cnum) maxc = cnum;
  if (maxc < 1) maxc = 1;	// Even though max count must not be zero this check is kept for backward compatibility as this is a public function

  if (cnum <= maxc) {
    // we have less contacts than we need, so we use them all
    for (j=0; j < cnum; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
      con->depth = dep[j];
    }
  }
  else {
    dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
    // we have more contacts than are wanted, some of them must be culled.
    // find the deepest point, it is always the first contact.
    int i1 = 0;
    dReal maxdepth = dep[0];
    for (i=1; i<cnum; i++) {
      if (dep[i] > maxdepth) {
	maxdepth = dep[i];
	i1 = i;
      }
    }

    int iret[8];
    cullPoints (cnum,ret,maxc,i1,iret);

    for (j=0; j < maxc; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
      con->depth = dep[iret[j]];
    }
    cnum = maxc;
  }

  *return_code = code;
  return cnum;
}
예제 #9
0
파일: ray.cpp 프로젝트: 4nakin/awesomeball
int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
		    dContactGeom *contact, int skip)
{
  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (o1->type == dRayClass);
  dIASSERT (o2->type == dBoxClass);
  dIASSERT ((flags & NUMC_MASK) >= 1);

  dxRay *ray = (dxRay*) o1;
  dxBox *box = (dxBox*) o2;

  contact->g1 = ray;
  contact->g2 = box;
  contact->side1 = -1;
  contact->side2 = -1;

  int i;

  // compute the start and delta of the ray relative to the box.
  // we will do all subsequent computations in this box-relative coordinate
  // system. we have to do a translation and rotation for each point.
  dVector3 tmp,s,v;
  tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0];
  tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1];
  tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2];
  dMULTIPLY1_331 (s,box->final_posr->R,tmp);
  tmp[0] = ray->final_posr->R[0*4+2];
  tmp[1] = ray->final_posr->R[1*4+2];
  tmp[2] = ray->final_posr->R[2*4+2];
  dMULTIPLY1_331 (v,box->final_posr->R,tmp);

  // mirror the line so that v has all components >= 0
  dVector3 sign;
  for (i=0; i<3; i++) {
    if (v[i] < 0) {
      s[i] = -s[i];
      v[i] = -v[i];
      sign[i] = 1;
    }
    else sign[i] = -1;
  }

  // compute the half-sides of the box
  dReal h[3];
  h[0] = REAL(0.5) * box->side[0];
  h[1] = REAL(0.5) * box->side[1];
  h[2] = REAL(0.5) * box->side[2];

  // do a few early exit tests
  if ((s[0] < -h[0] && v[0] <= 0) || s[0] >  h[0] ||
      (s[1] < -h[1] && v[1] <= 0) || s[1] >  h[1] ||
      (s[2] < -h[2] && v[2] <= 0) || s[2] >  h[2] ||
      (v[0] == 0 && v[1] == 0 && v[2] == 0)) {
    return 0;
  }

  // compute the t=[lo..hi] range for where s+v*t intersects the box
  dReal lo = -dInfinity;
  dReal hi = dInfinity;
  int nlo = 0, nhi = 0;
  for (i=0; i<3; i++) {
    if (v[i] != 0) {
      dReal k = (-h[i] - s[i])/v[i];
      if (k > lo) {
	lo = k;
	nlo = i;
      }
      k = (h[i] - s[i])/v[i];
      if (k < hi) {
	hi = k;
	nhi = i;
      }
    }
  }

  // check if the ray intersects
  if (lo > hi) return 0;
  dReal alpha;
  int n;
  if (lo >= 0) {
    alpha = lo;
    n = nlo;
  }
  else {
    alpha = hi;
    n = nhi;
  }
  if (alpha < 0 || alpha > ray->length) return 0;
  contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
  contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
  contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
  contact->normal[0] = box->final_posr->R[0*4+n] * sign[n];
  contact->normal[1] = box->final_posr->R[1*4+n] * sign[n];
  contact->normal[2] = box->final_posr->R[2*4+n] * sign[n];
  contact->depth = alpha;
  return 1;
}