Exemple #1
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 = dCalcVectorDot3_41( 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 = dCalcVectorDot3( q, q ) - ( cyl->radius * cyl->radius );

    // Compute the projection of ray direction normal onto cylinder direction normal.
    dReal uv = dCalcVectorDot3_44( 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 = dCalcVectorDot3( r, r );
    dReal B = 2 * dCalcVectorDot3( 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 = dCalcVectorDot3_14( 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;
}
Exemple #2
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 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
  R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
  R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(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 (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
    TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
    TST (dCalcVectorDot3_41(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 = (dCalcVectorDot3_14(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 = (dCalcVectorDot3_14(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 = dCalcVectorDot3_14 (center,Ra+code1);
  c2 = dCalcVectorDot3_14 (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 = dCalcVectorDot3_44 (Ra+code1,Rb+a1);
  m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2);
  m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1);
  m22 = dCalcVectorDot3_44 (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] - dCalcVectorDot3(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;
}
Exemple #3
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 = dCalcVectorDot3_41(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 = dCalcVectorDot3(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 = dCalcVectorDot3_44(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 = dCalcVectorDot3(r,r);
        dReal B = 2*dCalcVectorDot3(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 = dCalcVectorDot3_14(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);
}