ode/src/ray.cpp

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  *************************************************************************/
  22
  23 /*
  24
  25 standard ODE geometry primitives: public API and pairwise collision functions.
  26
  27 the rule is that only the low level primitive collision functions should set
  28 dContactGeom::g1 and dContactGeom::g2.
  29
  30 */
  31
  32 #include <ode/common.h>
  33 #include <ode/collision.h>
  34 #include <ode/matrix.h>
  35 #include <ode/rotation.h>
  36 #include <ode/odemath.h>
  37 #include "collision_kernel.h"
  38 #include "collision_std.h"
  39 #include "collision_util.h"
  40
  41 #ifdef _MSC_VER
  42 #pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
  43 #endif
  44
  45 //****************************************************************************
  46 // ray public API
  47
  48 dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1)
  49 {
  50   type = dRayClass;
  51   length = _length;
  52 }
  53
  54
  55 void dxRay::computeAABB()
  56 {
  57   dVector3 e;
  58   e[0] = final_posr->pos[0] + final_posr->R[0*4+2]*length;
  59   e[1] = final_posr->pos[1] + final_posr->R[1*4+2]*length;
  60   e[2] = final_posr->pos[2] + final_posr->R[2*4+2]*length;
  61
  62   if (final_posr->pos[0] < e[0]){
  63     aabb[0] = final_posr->pos[0];
  64     aabb[1] = e[0];
  65   }
  66   else{
  67     aabb[0] = e[0];
  68     aabb[1] = final_posr->pos[0];
  69   }
  70
  71   if (final_posr->pos[1] < e[1]){
  72     aabb[2] = final_posr->pos[1];
  73     aabb[3] = e[1];
  74   }
  75   else{
  76     aabb[2] = e[1];
  77     aabb[3] = final_posr->pos[1];
  78   }
  79
  80   if (final_posr->pos[2] < e[2]){
  81     aabb[4] = final_posr->pos[2];
  82     aabb[5] = e[2];
  83   }
  84   else{
  85     aabb[4] = e[2];
  86     aabb[5] = final_posr->pos[2];
  87   }
  88 }
  89
  90
  91 dGeomID dCreateRay (dSpaceID space, dReal length)
  92 {
  93   return new dxRay (space,length);
  94 }
  95
  96
  97 void dGeomRaySetLength (dGeomID g, dReal length)
  98 {
  99   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 100   dxRay *r = (dxRay*) g;
 101   r->length = length;
 102   dGeomMoved (g);
 103 }
 104
 105
 106 dReal dGeomRayGetLength (dGeomID g)
 107 {
 108   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 109   dxRay *r = (dxRay*) g;
 110   return r->length;
 111 }
 112
 113
 114 void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz,
 115                   dReal dx, dReal dy, dReal dz)
 116 {
 117   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 118   g->recomputePosr();
 119   dReal* rot = g->final_posr->R;
 120   dReal* pos = g->final_posr->pos;
 121   dVector3 n;
 122   pos[0] = px;
 123   pos[1] = py;
 124   pos[2] = pz;
 125
 126   n[0] = dx;
 127   n[1] = dy;
 128   n[2] = dz;
 129   dNormalize3(n);
 130   rot[0*4+2] = n[0];
 131   rot[1*4+2] = n[1];
 132   rot[2*4+2] = n[2];
 133   dGeomMoved (g);
 134 }
 135
 136
 137 void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir)
 138 {
 139   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 140   g->recomputePosr();
 141   start[0] = g->final_posr->pos[0];
 142   start[1] = g->final_posr->pos[1];
 143   start[2] = g->final_posr->pos[2];
 144   dir[0] = g->final_posr->R[0*4+2];
 145   dir[1] = g->final_posr->R[1*4+2];
 146   dir[2] = g->final_posr->R[2*4+2];
 147 }
 148
 149
 150 void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull)
 151 {
 152   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 153
 154   if (FirstContact){
 155     g->gflags |= RAY_FIRSTCONTACT;
 156   }
 157   else g->gflags &= ~RAY_FIRSTCONTACT;
 158
 159   if (BackfaceCull){
 160     g->gflags |= RAY_BACKFACECULL;
 161   }
 162   else g->gflags &= ~RAY_BACKFACECULL;
 163 }
 164
 165
 166 void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull)
 167 {
 168   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 169
 170   (*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0);
 171   (*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0);
 172 }
 173
 174
 175 void dGeomRaySetClosestHit (dxGeom *g, int closestHit)
 176 {
 177   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 178   if (closestHit){
 179     g->gflags |= RAY_CLOSEST_HIT;
 180   }
 181   else g->gflags &= ~RAY_CLOSEST_HIT;
 182 }
 183
 184
 185 int dGeomRayGetClosestHit (dxGeom *g)
 186 {
 187   dUASSERT (g && g->type == dRayClass,"argument not a ray");
 188   return ((g->gflags & RAY_CLOSEST_HIT) != 0);
 189 }
 190
 191
 192
 193 // if mode==1 then use the sphere exit contact, not the entry contact
 194
 195 static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius,
 196                               dContactGeom *contact, int mode)
 197 {
 198   dVector3 q;
 199   q[0] = ray->final_posr->pos[0] - sphere_pos[0];
 200   q[1] = ray->final_posr->pos[1] - sphere_pos[1];
 201   q[2] = ray->final_posr->pos[2] - sphere_pos[2];
 202   dReal B = dDOT14(q,ray->final_posr->R+2);
 203   dReal C = dDOT(q,q) - radius*radius;
 204   // note: if C <= 0 then the start of the ray is inside the sphere
 205   dReal k = B*B - C;
 206   if (k < 0) return 0;
 207   k = dSqrt(k);
 208   dReal alpha;
 209   if (mode && C >= 0) {
 210     alpha = -B + k;
 211     if (alpha < 0) return 0;
 212   }
 213   else {
 214     alpha = -B - k;
 215     if (alpha < 0) {
 216       alpha = -B + k;
 217       if (alpha < 0) return 0;
 218     }
 219   }
 220   if (alpha > ray->length) return 0;
 221   contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
 222   contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
 223   contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
 224   dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0);
 225   contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]);
 226   contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]);
 227   contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]);
 228   dNormalize3 (contact->normal);
 229   contact->depth = alpha;
 230   return 1;
 231 }
 232
 233
 234 int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags,
 235                        dContactGeom *contact, int skip)
 236 {
 237   dIASSERT (skip >= (int)sizeof(dContactGeom));
 238   dIASSERT (o1->type == dRayClass);
 239   dIASSERT (o2->type == dSphereClass);
 240   dIASSERT ((flags & NUMC_MASK) >= 1);
 241
 242   dxRay *ray = (dxRay*) o1;
 243   dxSphere *sphere = (dxSphere*) o2;
 244   contact->g1 = ray;
 245   contact->g2 = sphere;
 246   return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0);
 247 }
 248
 249
 250 int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
 251                     dContactGeom *contact, int skip)
 252 {
 253   dIASSERT (skip >= (int)sizeof(dContactGeom));
 254   dIASSERT (o1->type == dRayClass);
 255   dIASSERT (o2->type == dBoxClass);
 256   dIASSERT ((flags & NUMC_MASK) >= 1);
 257
 258   dxRay *ray = (dxRay*) o1;
 259   dxBox *box = (dxBox*) o2;
 260
 261   contact->g1 = ray;
 262   contact->g2 = box;
 263
 264   int i;
 265
 266   // compute the start and delta of the ray relative to the box.
 267   // we will do all subsequent computations in this box-relative coordinate
 268   // system. we have to do a translation and rotation for each point.
 269   dVector3 tmp,s,v;
 270   tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0];
 271   tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1];
 272   tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2];
 273   dMULTIPLY1_331 (s,box->final_posr->R,tmp);
 274   tmp[0] = ray->final_posr->R[0*4+2];
 275   tmp[1] = ray->final_posr->R[1*4+2];
 276   tmp[2] = ray->final_posr->R[2*4+2];
 277   dMULTIPLY1_331 (v,box->final_posr->R,tmp);
 278
 279   // mirror the line so that v has all components >= 0
 280   dVector3 sign;
 281   for (i=0; i<3; i++) {
 282     if (v[i] < 0) {
 283       s[i] = -s[i];
 284       v[i] = -v[i];
 285       sign[i] = 1;
 286     }
 287     else sign[i] = -1;
 288   }
 289
 290   // compute the half-sides of the box
 291   dReal h[3];
 292   h[0] = REAL(0.5) * box->side[0];
 293   h[1] = REAL(0.5) * box->side[1];
 294   h[2] = REAL(0.5) * box->side[2];
 295
 296   // do a few early exit tests
 297   if ((s[0] < -h[0] && v[0] <= 0) || s[0] >  h[0] ||
 298       (s[1] < -h[1] && v[1] <= 0) || s[1] >  h[1] ||
 299       (s[2] < -h[2] && v[2] <= 0) || s[2] >  h[2] ||
 300       (v[0] == 0 && v[1] == 0 && v[2] == 0)) {
 301     return 0;
 302   }
 303
 304   // compute the t=[lo..hi] range for where s+v*t intersects the box
 305   dReal lo = -dInfinity;
 306   dReal hi = dInfinity;
 307   int nlo = 0, nhi = 0;
 308   for (i=0; i<3; i++) {
 309     if (v[i] != 0) {
 310       dReal k = (-h[i] - s[i])/v[i];
 311       if (k > lo) {
 312         lo = k;
 313         nlo = i;
 314       }
 315       k = (h[i] - s[i])/v[i];
 316       if (k < hi) {
 317         hi = k;
 318         nhi = i;
 319       }
 320     }
 321   }
 322
 323   // check if the ray intersects
 324   if (lo > hi) return 0;
 325   dReal alpha;
 326   int n;
 327   if (lo >= 0) {
 328     alpha = lo;
 329     n = nlo;
 330   }
 331   else {
 332     alpha = hi;
 333     n = nhi;
 334   }
 335   if (alpha < 0 || alpha > ray->length) return 0;
 336   contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
 337   contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
 338   contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
 339   contact->normal[0] = box->final_posr->R[0*4+n] * sign[n];
 340   contact->normal[1] = box->final_posr->R[1*4+n] * sign[n];
 341   contact->normal[2] = box->final_posr->R[2*4+n] * sign[n];
 342   contact->depth = alpha;
 343   return 1;
 344 }
 345
 346
 347 int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
 348                           int flags, dContactGeom *contact, int skip)
 349 {
 350   dIASSERT (skip >= (int)sizeof(dContactGeom));
 351   dIASSERT (o1->type == dRayClass);
 352   dIASSERT (o2->type == dCapsuleClass);
 353   dIASSERT ((flags & NUMC_MASK) >= 1);
 354
 355   dxRay *ray = (dxRay*) o1;
 356   dxCapsule *ccyl = (dxCapsule*) o2;
 357
 358   contact->g1 = ray;
 359   contact->g2 = ccyl;
 360   dReal lz2 = ccyl->lz * REAL(0.5);
 361
 362   // compute some useful info
 363   dVector3 cs,q,r;
 364   dReal C,k;
 365   cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0];
 366   cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1];
 367   cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2];
 368   k = dDOT41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis
 369   q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0];
 370   q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1];
 371   q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2];
 372   C = dDOT(q,q) - ccyl->radius*ccyl->radius;
 373   // if C < 0 then ray start position within infinite extension of cylinder
 374
 375   // see if ray start position is inside the capped cylinder
 376   int inside_ccyl = 0;
 377   if (C < 0) {
 378     if (k < -lz2) k = -lz2;
 379     else if (k > lz2) k = lz2;
 380     r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
 381     r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
 382     r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
 383     if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) +
 384         (ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) +
 385         (ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) {
 386       inside_ccyl = 1;
 387     }
 388   }
 389
 390   // compute ray collision with infinite cylinder, except for the case where
 391   // the ray is outside the capped cylinder but within the infinite cylinder
 392   // (it that case the ray can only hit endcaps)
 393   if (!inside_ccyl && C < 0) {
 394     // set k to cap position to check
 395     if (k < 0) k = -lz2; else k = lz2;
 396   }
 397   else {
 398     dReal uv = dDOT44(ccyl->final_posr->R+2,ray->final_posr->R+2);
 399     r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2];
 400     r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2];
 401     r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2];
 402     dReal A = dDOT(r,r);
 403     dReal B = 2*dDOT(q,r);
 404     k = B*B-4*A*C;
 405     if (k < 0) {
 406       // the ray does not intersect the infinite cylinder, but if the ray is
 407       // inside and parallel to the cylinder axis it may intersect the end
 408       // caps. set k to cap position to check.
 409       if (!inside_ccyl) return 0;
 410       if (uv < 0) k = -lz2; else k = lz2;
 411     }
 412     else {
 413       k = dSqrt(k);
 414       A = dRecip (2*A);
 415       dReal alpha = (-B-k)*A;
 416       if (alpha < 0) {
 417         alpha = (-B+k)*A;
 418         if (alpha < 0) return 0;
 419       }
 420       if (alpha > ray->length) return 0;
 421
 422       // the ray intersects the infinite cylinder. check to see if the
 423       // intersection point is between the caps
 424       contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
 425       contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
 426       contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
 427       q[0] = contact->pos[0] - ccyl->final_posr->pos[0];
 428       q[1] = contact->pos[1] - ccyl->final_posr->pos[1];
 429       q[2] = contact->pos[2] - ccyl->final_posr->pos[2];
 430       k = dDOT14(q,ccyl->final_posr->R+2);
 431       dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0);
 432       if (k >= -lz2 && k <= lz2) {
 433         contact->normal[0] = nsign * (contact->pos[0] -
 434                                       (ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]));
 435         contact->normal[1] = nsign * (contact->pos[1] -
 436                                       (ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]));
 437         contact->normal[2] = nsign * (contact->pos[2] -
 438                                       (ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]));
 439         dNormalize3 (contact->normal);
 440         contact->depth = alpha;
 441         return 1;
 442       }
 443
 444       // the infinite cylinder intersection point is not between the caps.
 445       // set k to cap position to check.
 446       if (k < 0) k = -lz2; else k = lz2;
 447     }
 448   }
 449
 450   // check for ray intersection with the caps. k must indicate the cap
 451   // position to check
 452   q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
 453   q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
 454   q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
 455   return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl);
 456 }
 457
 458
 459 int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
 460                       dContactGeom *contact, int skip)
 461 {
 462   dIASSERT (skip >= (int)sizeof(dContactGeom));
 463   dIASSERT (o1->type == dRayClass);
 464   dIASSERT (o2->type == dPlaneClass);
 465   dIASSERT ((flags & NUMC_MASK) >= 1);
 466
 467   dxRay *ray = (dxRay*) o1;
 468   dxPlane *plane = (dxPlane*) o2;
 469
 470   dReal alpha = plane->p[3] - dDOT (plane->p,ray->final_posr->pos);
 471   // note: if alpha > 0 the starting point is below the plane
 472   dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0);
 473   dReal k = dDOT14(plane->p,ray->final_posr->R+2);
 474   if (k==0) return 0;           // ray parallel to plane
 475   alpha /= k;
 476   if (alpha < 0 || alpha > ray->length) return 0;
 477   contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
 478   contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
 479   contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
 480   contact->normal[0] = nsign*plane->p[0];
 481   contact->normal[1] = nsign*plane->p[1];
 482   contact->normal[2] = nsign*plane->p[2];
 483   contact->depth = alpha;
 484   contact->g1 = ray;
 485   contact->g2 = plane;
 486   return 1;
 487 }
 488
 489 // Ray - Cylinder collider by David Walters (June 2006)
 490 int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
 491 {
 492         dIASSERT( skip >= (int)sizeof( dContactGeom ) );
 493         dIASSERT( o1->type == dRayClass );
 494         dIASSERT( o2->type == dCylinderClass );
 495         dIASSERT( (flags & NUMC_MASK) >= 1 );
 496
 497         dxRay* ray = (dxRay*)( o1 );
 498         dxCylinder* cyl = (dxCylinder*)( o2 );
 499
 500         // Fill in contact information.
 501         contact->g1 = ray;
 502         contact->g2 = cyl;
 503
 504         const dReal half_length = cyl->lz * REAL( 0.5 );
 505
 506         //
 507         // Compute some useful info
 508         //
 509
 510         dVector3 q, r;
 511         dReal d, C, k;
 512
 513         // Vector 'r', line segment from C to R (ray start) ( r = R - C )
 514         r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0];
 515         r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1];
 516         r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2];
 517
 518         // Distance that ray start is along cyl axis ( Z-axis direction )
 519         d = dDOT41( cyl->final_posr->R + 2, r );
 520
 521         //
 522         // Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz).
 523         //
 524         // Point on axis ( in world space ):    cp = ( d * Cz ) + C
 525         //
 526         // Line 'q' from R to cp:                               q = cp - R
 527         //                                                                              q = ( d * Cz ) + C - R
 528         //                                                                              q = ( d * Cz ) - ( R - C )
 529
 530         q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ];
 531         q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ];
 532         q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ];
 533
 534
 535         // Compute square length of 'q'. Subtract from radius squared to
 536         // get square distance 'C' between the line q and the radius.
 537
 538         // if C < 0 then ray start position is within infinite extension of cylinder
 539
 540         C = dDOT( q, q ) - ( cyl->radius * cyl->radius );
 541
 542         // Compute the projection of ray direction normal onto cylinder direction normal.
 543         dReal uv = dDOT44( cyl->final_posr->R+2, ray->final_posr->R+2 );
 544
 545
 546
 547         //
 548         // Find ray collision with infinite cylinder
 549         //
 550
 551         // Compute vector from end of ray direction normal to projection on cylinder direction normal.
 552         r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2];
 553         r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2];
 554         r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2];
 555
 556
 557         // Quadratic Formula Magic
 558         // Compute discriminant 'k':
 559
 560         // k < 0 : No intersection
 561         // k = 0 : Tangent
 562         // k > 0 : Intersection
 563
 564         dReal A = dDOT( r, r );
 565         dReal B = 2 * dDOT( q, r );
 566
 567         k = B*B - 4*A*C;
 568
 569
 570
 571
 572         //
 573         // Collision with Flat Caps ?
 574         //
 575
 576         // No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases )
 577         if ( k < dEpsilon && C <= 0 )
 578         {
 579                 // The ray does not intersect the edge of the infinite cylinder,
 580                 // but the ray start is inside and so must run parallel to the axis.
 581                 // It may yet intersect an end cap. The following cases are valid:
 582
 583                 //        -ve-cap , -half              centre               +half , +ve-cap
 584                 //  <<================|-------------------|------------->>>---|================>>
 585                 //                    |                                       |
 586                 //                    |                              d------------------->    1.
 587                 //   2.    d------------------>                               |
 588                 //   3.    <------------------d                               |
 589                 //                    |                              <-------------------d    4.
 590                 //                    |                                       |
 591                 //  <<================|-------------------|------------->>>---|===============>>
 592
 593                 // Negative if the ray and cylinder axes point in opposite directions.
 594                 const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
 595
 596                 // Negative if the ray start is inside the cylinder
 597                 const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 );
 598
 599                 // Ray and Cylinder axes run in the same direction ( cases 1, 2 )
 600                 // Ray and Cylinder axes run in opposite directions ( cases 3, 4 )
 601                 if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) ||
 602                      ( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) )
 603                 {
 604                         return 0; // No intersection with caps or curved surface.
 605                 }
 606
 607                 // Compute depth (distance from ray to cylinder)
 608                 contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) );
 609
 610                 // Compute contact point.
 611                 contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] );
 612                 contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] );
 613                 contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] );
 614
 615                 // Compute reflected contact normal.
 616                 contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] );
 617                 contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] );
 618                 contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] );
 619
 620                 // Contact!
 621                 return 1;
 622         }
 623
 624
 625
 626         //
 627         // Collision with Curved Edge ?
 628         //
 629
 630         if ( k > 0 )
 631         {
 632                 // Finish off quadratic formula to get intersection co-efficient
 633                 k = dSqrt( k );
 634                 A = dRecip( 2 * A );
 635
 636                 // Compute distance along line to contact point.
 637                 dReal alpha = ( -B - k ) * A;
 638                 if ( alpha < 0 )
 639                 {
 640                         // Flip in the other direction.
 641                         alpha = ( -B + k ) * A;
 642                 }
 643
 644                 // Intersection point is within ray length?
 645                 if ( alpha >= 0 && alpha <= ray->length )
 646                 {
 647                         // The ray intersects the infinite cylinder!
 648
 649                         // Compute contact point.
 650                         contact->pos[0] = ray->final_posr->pos[0] + ( alpha * ray->final_posr->R[0*4+2] );
 651                         contact->pos[1] = ray->final_posr->pos[1] + ( alpha * ray->final_posr->R[1*4+2] );
 652                         contact->pos[2] = ray->final_posr->pos[2] + ( alpha * ray->final_posr->R[2*4+2] );
 653
 654                         // q is the vector from the cylinder centre to the contact point.
 655                         q[0] = contact->pos[0] - cyl->final_posr->pos[0];
 656                         q[1] = contact->pos[1] - cyl->final_posr->pos[1];
 657                         q[2] = contact->pos[2] - cyl->final_posr->pos[2];
 658
 659                         // Compute the distance along the cylinder axis of this contact point.
 660                         d = dDOT14( q, cyl->final_posr->R+2 );
 661
 662                         // Check to see if the intersection point is between the flat end caps
 663                         if ( d >= -half_length && d <= +half_length )
 664                         {
 665                                 // Flip the normal if the start point is inside the cylinder.
 666                                 const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
 667
 668                                 // Compute contact normal.
 669                                 contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2]));
 670                                 contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2]));
 671                                 contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2]));
 672                                 dNormalize3( contact->normal );
 673
 674                                 // Store depth.
 675                                 contact->depth = alpha;
 676
 677                                 // Contact!
 678                                 return 1;
 679                         }
 680                 }
 681         }
 682
 683         // No contact with anything.
 684         return 0;
 685 }
 686