// 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;
}
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;
}
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);
}