From 0d2e078eebb2827e0ed49e55dd10df406c4e3af1 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:22:50 +0000 Subject: trying to fix my screwup part deux --- "libraries/ode-0.9\\/ode/src/ray.cpp" | 686 ---------------------------------- 1 file changed, 686 deletions(-) delete mode 100755 "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" deleted file mode 100755 index adf61ac..0000000 --- "a/libraries/ode-0.9\\/ode/src/ray.cpp" +++ /dev/null @@ -1,686 +0,0 @@ -/************************************************************************* - * * - * 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