Exemplo n.º 1
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
// Ray - Cylinder collider by David Walters (June 2006)
int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
{
	dIASSERT( skip >= (int)sizeof( dContactGeom ) );
	dIASSERT( o1->type == dRayClass );
	dIASSERT( o2->type == dCylinderClass );
	dIASSERT( (flags & NUMC_MASK) >= 1 );

	dxRay* ray = (dxRay*)( o1 );
	dxCylinder* cyl = (dxCylinder*)( o2 );

	// Fill in contact information.
	contact->g1 = ray;
	contact->g2 = cyl;
	contact->side1 = -1;
	contact->side2 = -1;

	const dReal half_length = cyl->lz * REAL( 0.5 );

	//
	// Compute some useful info
	//

	dVector3 q, r;
	dReal d, C, k;

	// Vector 'r', line segment from C to R (ray start) ( r = R - C )
	r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0];
	r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1];
	r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2];

	// Distance that ray start is along cyl axis ( Z-axis direction )
	d = dDOT41( cyl->final_posr->R + 2, r );

	//
	// Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz).
	//
	// Point on axis ( in world space ):	cp = ( d * Cz ) + C
	//
	// Line 'q' from R to cp:				q = cp - R
	//										q = ( d * Cz ) + C - R
	//										q = ( d * Cz ) - ( R - C )

	q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ];
	q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ];
	q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ];


	// Compute square length of 'q'. Subtract from radius squared to
	// get square distance 'C' between the line q and the radius.

	// if C < 0 then ray start position is within infinite extension of cylinder

	C = dDOT( q, q ) - ( cyl->radius * cyl->radius );

	// Compute the projection of ray direction normal onto cylinder direction normal.
	dReal uv = dDOT44( cyl->final_posr->R+2, ray->final_posr->R+2 );



	//
	// Find ray collision with infinite cylinder
	//

	// Compute vector from end of ray direction normal to projection on cylinder direction normal.
	r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2];
	r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2];
	r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2];


	// Quadratic Formula Magic
	// Compute discriminant 'k':

	// k < 0 : No intersection
	// k = 0 : Tangent
	// k > 0 : Intersection

	dReal A = dDOT( r, r );
	dReal B = 2 * dDOT( q, r );

	k = B*B - 4*A*C;




	//
	// Collision with Flat Caps ?
	//

	// No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases )
	if ( k < dEpsilon && C <= 0 )
	{
		// The ray does not intersect the edge of the infinite cylinder,
		// but the ray start is inside and so must run parallel to the axis.
		// It may yet intersect an end cap. The following cases are valid:

		//        -ve-cap , -half              centre               +half , +ve-cap
		//  <<================|-------------------|------------->>>---|================>>
		//                    |                                       |
		//                    |                              d------------------->    1.
		//   2.    d------------------>                               |
		//   3.    <------------------d                               |
		//                    |                              <-------------------d    4.
		//                    |                                       |
		//  <<================|-------------------|------------->>>---|===============>>

		// Negative if the ray and cylinder axes point in opposite directions.
		const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );

		// Negative if the ray start is inside the cylinder
		const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 );

		// Ray and Cylinder axes run in the same direction ( cases 1, 2 )
		// Ray and Cylinder axes run in opposite directions ( cases 3, 4 )
		if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) ||
		     ( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) )
		{
			return 0; // No intersection with caps or curved surface.
		}

		// Compute depth (distance from ray to cylinder)
		contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) );

		// Compute contact point.
		contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] );
		contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] );
		contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] );

		// Compute reflected contact normal.
		contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] );
		contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] );
		contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] );

		// Contact!
		return 1;
	}



	//
	// Collision with Curved Edge ?
	//

	if ( k > 0 )
	{
		// Finish off quadratic formula to get intersection co-efficient
		k = dSqrt( k );
		A = dRecip( 2 * A );

		// Compute distance along line to contact point.
		dReal alpha = ( -B - k ) * A;
		if ( alpha < 0 )
		{
			// Flip in the other direction.
			alpha = ( -B + k ) * A;
		}

		// Intersection point is within ray length?
		if ( alpha >= 0 && alpha <= ray->length )
		{
			// The ray intersects the infinite cylinder!

			// Compute contact point.
			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] );

			// q is the vector from the cylinder centre to the contact point.
			q[0] = contact->pos[0] - cyl->final_posr->pos[0];
			q[1] = contact->pos[1] - cyl->final_posr->pos[1];
			q[2] = contact->pos[2] - cyl->final_posr->pos[2];

			// Compute the distance along the cylinder axis of this contact point.
			d = dDOT14( q, cyl->final_posr->R+2 );

			// Check to see if the intersection point is between the flat end caps
			if ( d >= -half_length && d <= +half_length )
			{
				// Flip the normal if the start point is inside the cylinder.
				const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );

				// Compute contact normal.
				contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2]));
				contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2]));
				contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2]));
				dNormalize3( contact->normal );

				// Store depth.
				contact->depth = alpha;

				// Contact!
				return 1;
			}
		}
	}

	// No contact with anything.
	return 0;
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
			  int flags, dContactGeom *contact, int skip)
{
  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (o1->type == dRayClass);
  dIASSERT (o2->type == dCapsuleClass);
  dIASSERT ((flags & NUMC_MASK) >= 1);

  dxRay *ray = (dxRay*) o1;
  dxCapsule *ccyl = (dxCapsule*) o2;

  contact->g1 = ray;
  contact->g2 = ccyl;
  contact->side1 = -1;
  contact->side2 = -1;
  
  dReal lz2 = ccyl->lz * REAL(0.5);

  // compute some useful info
  dVector3 cs,q,r;
  dReal C,k;
  cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0];
  cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1];
  cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2];
  k = dDOT41(ccyl->final_posr->R+2,cs);	// position of ray start along ccyl axis
  q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0];
  q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1];
  q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2];
  C = dDOT(q,q) - ccyl->radius*ccyl->radius;
  // if C < 0 then ray start position within infinite extension of cylinder

  // see if ray start position is inside the capped cylinder
  int inside_ccyl = 0;
  if (C < 0) {
    if (k < -lz2) k = -lz2;
    else if (k > lz2) k = lz2;
    r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
    r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
    r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
    if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) +
	(ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) +
	(ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) {
      inside_ccyl = 1;
    }
  }

  // compute ray collision with infinite cylinder, except for the case where
  // the ray is outside the capped cylinder but within the infinite cylinder
  // (it that case the ray can only hit endcaps)
  if (!inside_ccyl && C < 0) {
    // set k to cap position to check
    if (k < 0) k = -lz2; else k = lz2;
  }
  else {
    dReal uv = dDOT44(ccyl->final_posr->R+2,ray->final_posr->R+2);
    r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2];
    r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2];
    r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2];
    dReal A = dDOT(r,r);
    dReal B = 2*dDOT(q,r);
    k = B*B-4*A*C;
    if (k < 0) {
      // the ray does not intersect the infinite cylinder, but if the ray is
      // inside and parallel to the cylinder axis it may intersect the end
      // caps. set k to cap position to check.
      if (!inside_ccyl) return 0;
      if (uv < 0) k = -lz2; else k = lz2;
    }
    else {
      k = dSqrt(k);
      A = dRecip (2*A);
      dReal alpha = (-B-k)*A;
      if (alpha < 0) {
	alpha = (-B+k)*A;
	if (alpha < 0) return 0;
      }
      if (alpha > ray->length) return 0;

      // the ray intersects the infinite cylinder. check to see if the
      // intersection point is between the caps
      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];
      q[0] = contact->pos[0] - ccyl->final_posr->pos[0];
      q[1] = contact->pos[1] - ccyl->final_posr->pos[1];
      q[2] = contact->pos[2] - ccyl->final_posr->pos[2];
      k = dDOT14(q,ccyl->final_posr->R+2);
      dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0);
      if (k >= -lz2 && k <= lz2) {
	contact->normal[0] = nsign * (contact->pos[0] -
				      (ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]));
	contact->normal[1] = nsign * (contact->pos[1] -
				      (ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]));
	contact->normal[2] = nsign * (contact->pos[2] -
				      (ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]));
	dNormalize3 (contact->normal);
	contact->depth = alpha;
	return 1;
      }

      // the infinite cylinder intersection point is not between the caps.
      // set k to cap position to check.
      if (k < 0) k = -lz2; else k = lz2;
    }
  }

  // check for ray intersection with the caps. k must indicate the cap
  // position to check
  q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
  q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
  q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
  return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl);
}