From f205de7847da7ae1c10212d82e7042d0100b4ce0 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:24:38 +0000 Subject: from the start... checking in ode-0.9 --- libraries/ode-0.9/ode/src/ray.cpp | 686 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 686 insertions(+) create mode 100644 libraries/ode-0.9/ode/src/ray.cpp (limited to 'libraries/ode-0.9/ode/src/ray.cpp') diff --git a/libraries/ode-0.9/ode/src/ray.cpp b/libraries/ode-0.9/ode/src/ray.cpp new file mode 100644 index 0000000..adf61ac --- /dev/null +++ b/libraries/ode-0.9/ode/src/ray.cpp @@ -0,0 +1,686 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// ray public API + +dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1) +{ + type = dRayClass; + length = _length; +} + + +void dxRay::computeAABB() +{ + dVector3 e; + e[0] = final_posr->pos[0] + final_posr->R[0*4+2]*length; + e[1] = final_posr->pos[1] + final_posr->R[1*4+2]*length; + e[2] = final_posr->pos[2] + final_posr->R[2*4+2]*length; + + if (final_posr->pos[0] < e[0]){ + aabb[0] = final_posr->pos[0]; + aabb[1] = e[0]; + } + else{ + aabb[0] = e[0]; + aabb[1] = final_posr->pos[0]; + } + + if (final_posr->pos[1] < e[1]){ + aabb[2] = final_posr->pos[1]; + aabb[3] = e[1]; + } + else{ + aabb[2] = e[1]; + aabb[3] = final_posr->pos[1]; + } + + if (final_posr->pos[2] < e[2]){ + aabb[4] = final_posr->pos[2]; + aabb[5] = e[2]; + } + else{ + aabb[4] = e[2]; + aabb[5] = final_posr->pos[2]; + } +} + + +dGeomID dCreateRay (dSpaceID space, dReal length) +{ + return new dxRay (space,length); +} + + +void dGeomRaySetLength (dGeomID g, dReal length) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + r->length = length; + dGeomMoved (g); +} + + +dReal dGeomRayGetLength (dGeomID g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + return r->length; +} + + +void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, + dReal dx, dReal dy, dReal dz) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + g->recomputePosr(); + dReal* rot = g->final_posr->R; + dReal* pos = g->final_posr->pos; + dVector3 n; + pos[0] = px; + pos[1] = py; + pos[2] = pz; + + n[0] = dx; + n[1] = dy; + n[2] = dz; + dNormalize3(n); + rot[0*4+2] = n[0]; + rot[1*4+2] = n[1]; + rot[2*4+2] = n[2]; + dGeomMoved (g); +} + + +void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + g->recomputePosr(); + start[0] = g->final_posr->pos[0]; + start[1] = g->final_posr->pos[1]; + start[2] = g->final_posr->pos[2]; + dir[0] = g->final_posr->R[0*4+2]; + dir[1] = g->final_posr->R[1*4+2]; + dir[2] = g->final_posr->R[2*4+2]; +} + + +void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + if (FirstContact){ + g->gflags |= RAY_FIRSTCONTACT; + } + else g->gflags &= ~RAY_FIRSTCONTACT; + + if (BackfaceCull){ + g->gflags |= RAY_BACKFACECULL; + } + else g->gflags &= ~RAY_BACKFACECULL; +} + + +void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + (*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0); + (*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0); +} + + +void dGeomRaySetClosestHit (dxGeom *g, int closestHit) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + if (closestHit){ + g->gflags |= RAY_CLOSEST_HIT; + } + else g->gflags &= ~RAY_CLOSEST_HIT; +} + + +int dGeomRayGetClosestHit (dxGeom *g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + return ((g->gflags & RAY_CLOSEST_HIT) != 0); +} + + + +// if mode==1 then use the sphere exit contact, not the entry contact + +static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, + dContactGeom *contact, int mode) +{ + dVector3 q; + q[0] = ray->final_posr->pos[0] - sphere_pos[0]; + q[1] = ray->final_posr->pos[1] - sphere_pos[1]; + q[2] = ray->final_posr->pos[2] - sphere_pos[2]; + dReal B = dDOT14(q,ray->final_posr->R+2); + dReal C = dDOT(q,q) - radius*radius; + // note: if C <= 0 then the start of the ray is inside the sphere + dReal k = B*B - C; + if (k < 0) return 0; + k = dSqrt(k); + dReal alpha; + if (mode && C >= 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + else { + alpha = -B - k; + if (alpha < 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + } + if (alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0); + contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]); + contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]); + contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; +} + + +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxSphere *sphere = (dxSphere*) o2; + contact->g1 = ray; + contact->g2 = sphere; + return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0); +} + + +int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = ray; + contact->g2 = box; + + int i; + + // compute the start and delta of the ray relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,v; + tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0]; + tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1]; + tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2]; + dMULTIPLY1_331 (s,box->final_posr->R,tmp); + tmp[0] = ray->final_posr->R[0*4+2]; + tmp[1] = ray->final_posr->R[1*4+2]; + tmp[2] = ray->final_posr->R[2*4+2]; + dMULTIPLY1_331 (v,box->final_posr->R,tmp); + + // mirror the line so that v has all components >= 0 + dVector3 sign; + for (i=0; i<3; i++) { + if (v[i] < 0) { + s[i] = -s[i]; + v[i] = -v[i]; + sign[i] = 1; + } + else sign[i] = -1; + } + + // compute the half-sides of the box + dReal h[3]; + h[0] = REAL(0.5) * box->side[0]; + h[1] = REAL(0.5) * box->side[1]; + h[2] = REAL(0.5) * box->side[2]; + + // do a few early exit tests + if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] || + (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] || + (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] || + (v[0] == 0 && v[1] == 0 && v[2] == 0)) { + return 0; + } + + // compute the t=[lo..hi] range for where s+v*t intersects the box + dReal lo = -dInfinity; + dReal hi = dInfinity; + int nlo = 0, nhi = 0; + for (i=0; i<3; i++) { + if (v[i] != 0) { + dReal k = (-h[i] - s[i])/v[i]; + if (k > lo) { + lo = k; + nlo = i; + } + k = (h[i] - s[i])/v[i]; + if (k < hi) { + hi = k; + nhi = i; + } + } + } + + // check if the ray intersects + if (lo > hi) return 0; + dReal alpha; + int n; + if (lo >= 0) { + alpha = lo; + n = nlo; + } + else { + alpha = hi; + n = nhi; + } + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + contact->normal[0] = box->final_posr->R[0*4+n] * sign[n]; + contact->normal[1] = box->final_posr->R[1*4+n] * sign[n]; + contact->normal[2] = box->final_posr->R[2*4+n] * sign[n]; + contact->depth = alpha; + return 1; +} + + +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; + 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); +} + + +int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxPlane *plane = (dxPlane*) o2; + + dReal alpha = plane->p[3] - dDOT (plane->p,ray->final_posr->pos); + // note: if alpha > 0 the starting point is below the plane + dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0); + dReal k = dDOT14(plane->p,ray->final_posr->R+2); + if (k==0) return 0; // ray parallel to plane + alpha /= k; + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + contact->normal[0] = nsign*plane->p[0]; + contact->normal[1] = nsign*plane->p[1]; + contact->normal[2] = nsign*plane->p[2]; + contact->depth = alpha; + contact->g1 = ray; + contact->g2 = plane; + return 1; +} + +// 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; + + 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; +} + -- cgit v1.1