src/math/fvi.cpp

   1 /*
   2  * Copyright (C) Volition, Inc. 1999.  All rights reserved.
   3  *
   4  * All source code herein is the property of Volition, Inc. You may not sell
   5  * or otherwise commercially exploit the source or things you created based on
   6  * the source.
   7  */
   8
   9 /*
  10  * $Logfile: /Freespace2/code/Math/Fvi.cpp $
  11  * $Revision$
  12  * $Date$
  13  * $Author$
  14  *
  15  * Routines to find intersections of various 3d things.
  16  *
  17  * $Log$
  18  * Revision 1.4  2002/06/17 06:33:09  relnev
  19  * ryan's struct patch for gcc 2.95
  20  *
  21  * Revision 1.3  2002/06/09 04:41:22  relnev
  22  * added copyright header
  23  *
  24  * Revision 1.2  2002/05/07 03:16:46  theoddone33
  25  * The Great Newline Fix
  26  *
  27  * Revision 1.1.1.1  2002/05/03 03:28:09  root
  28  * Initial import.
  29  *
  30  *
  31  * 5     8/16/99 8:19a Andsager
  32  * Add project_point_onto_bbox() to fvi and include in aicode
  33  *
  34  * 4     5/17/99 5:38p Mattf
  35  * Removed bogus assert.
  36  *
  37  * 3     11/13/98 11:10a Andsager
  38  * Add fvi_two_lines_in_3space() - returns closest point of intersection
  39  *
  40  * 2     10/07/98 10:53a Dave
  41  * Initial checkin.
  42  *
  43  * 1     10/07/98 10:49a Dave
  44  *
  45  * 37    3/27/98 2:03p Andsager
  46  * Removed rarely hit warning message.
  47  *
  48  * 36    3/18/98 3:04p Andsager
  49  * Increase collision error warning distance
  50  *
  51  * 35    3/09/98 12:11a Andsager
  52  * Allow spher-model collisions at slightly negative times.  Increase
  53  * error tolerance between estimated radius and true radius.
  54  *
  55  * 34    1/20/98 9:47a Mike
  56  * Suppress optimized compiler warnings.
  57  * Some secondary weapon work.
  58  *
  59  * 33    1/19/98 9:30a Andsager
  60  * Changed warning to mprintf.  Increased error tolerance.
  61  *
  62  * 32    1/15/98 5:55p Andsager
  63  * increase error tolerance in polyedge_sphereline
  64  *
  65  * 31    1/15/98 10:47a Andsager
  66  * Reduced tolerance in polyedge_sphereline to 0.01m absolute distance.
  67  * Changed asserts to warning
  68  *
  69  * 30    1/12/98 9:20p Andsager
  70  * Modify calling procedure to fvi_sphere_plane.  Modify
  71  * fvi_polyedge_sphereline to find the first instance when the edge is
  72  * hit.
  73  *
  74  * 29    1/09/98 10:08a Mike
  75  * Comment out warning for QA rev.
  76  *
  77  * 28    1/05/98 2:59p Andsager
  78  * Relaxed error tolerance in fvi_polyedge_sphereline().  Improved control
  79  * flow.
  80  *
  81  * 27    12/23/97 9:39a Andsager
  82  * Slight optimization and improved error checking in polyedge_sphereline
  83  *
  84  * 26    12/22/97 9:58p Andsager
  85  * Work around numerical precision problem in polyedge_sphereline
  86  *
  87  * 25    12/15/97 5:54p Andsager
  88  * Yet another version of fvi_polyedge_sphereline
  89  *
  90  * 24    12/05/97 5:17p Andsager
  91  * Fixed stupid bug missing some poly:sphere  edge collisions.
  92  *
  93  * 23    11/14/97 5:30p Andsager
  94  * Include modified version of polyedge_sphereline
  95  *
  96  * 22    11/05/97 5:36p Andsager
  97  * Fixed bug in fvi_polyedge_sphereline causing incorrect hit points to be
  98  * returned,
  99  *
 100  * 21    11/03/97 11:16p Andsager
 101  * Implemented third (and hopefully last) sphere-edge collision detection
 102  *
 103  * 20    10/19/97 9:42p Andsager
 104  * add fvi_sphere_plane to header
 105  *
 106  * 19    10/19/97 9:27p Andsager
 107  * fixed bug in sphere_plane.  Improved control flow and comments in
 108  * polyedge_sphereline
 109  *
 110  * 18    10/17/97 1:21a Andsager
 111  * add new function to check sphere-polgon edge collision
 112  *
 113  * 17    10/11/97 1:42p Mike
 114  * Remove warning message by deleting definition of plane_normal.
 115  *
 116  * 16    10/03/97 5:06p Andsager
 117  * added sphere polygon intersection code
 118  *
 119  * 15    9/28/97 2:16p Andsager
 120  * added fvi_moving_sphere_intersect_plane
 121  *
 122  * 14    8/22/97 11:42a John
 123  * fixed bug in fvi_point_face that was failing near some vertices.
 124  *
 125  * 13    7/21/97 2:39p John
 126  * fixed same thing as previous comment for fvi_ray_sphere.
 127  *
 128  * 12    7/21/97 2:24p John
 129  * fixed bug with fvi_segment_sphere in case where p0 is in sphere and p1
 130  * is out, but p0 is past center, where the code would fail.
 131  *
 132  * 11    6/23/97 11:11a John
 133  * made fvi_segment_sphere not get 0 length vector in normalize error.
 134  *
 135  * 10    4/01/97 1:03p John
 136  * Changed fvi_ray_plane to take a dir, not two points.
 137  *
 138  * 9     3/27/97 11:36a Mike
 139  * Fix fvi_ray_plane code.  Enables using fvi_ray_plane to place objects
 140  * arbitrarily far away from the viewer (in Fred).
 141  *
 142  * 8     3/26/97 10:48a Hoffoss
 143  * JAS: Added fvi_ray_sphere
 144  *
 145  * 7     3/24/97 3:55p John
 146  * renamed fvi functions so rays and segments aren't confusing.
 147  *
 148  * 6     3/24/97 3:26p John
 149  * Cleaned up and restructured model_collide code and fvi code.  In fvi
 150  * made code that finds uvs work..  Added bm_get_pixel to BmpMan.
 151  *
 152  * 5     2/17/97 5:18p John
 153  * Added a bunch of RCS headers to a bunch of old files that don't have
 154  * them.
 155  *
 156  * $NoKeywords: $
 157  */
 158
 159 #include <float.h>      // For FLT_MAX
 160
 161 #include "pstypes.h"
 162 #include "vecmat.h"
 163 #include "floating.h"
 164 #include "fvi.h"
 165
 166 #define SMALL_NUM       1E-6
 167 void accurate_square_root( float A, float B, float C, float discriminant, float *root1, float *root2 );
 168
 169
 170 float matrix_determinant_from_vectors(vector *v1,vector *v2,vector *v3)
 171 {
 172         float ans;
 173         ans=v1->xyz.x*v2->xyz.y*v3->xyz.z;
 174         ans+=v2->xyz.x*v3->xyz.y*v1->xyz.z;
 175         ans+=v3->xyz.x*v1->xyz.y*v2->xyz.z;
 176         ans-=v1->xyz.z*v2->xyz.y*v3->xyz.x;
 177         ans-=v2->xyz.z*v3->xyz.y*v1->xyz.x;
 178         ans-=v3->xyz.z*v1->xyz.y*v2->xyz.x;
 179
 180         return ans;
 181 }
 182
 183 // lines: L1 = P1 + V1s  and   L2 = P2 + V2t
 184 // finds the point on each line of closest approach (s and t) (lines need not intersect to get closest point)
 185 // taken from graphic gems I, p. 304
 186 //
 187 void fvi_two_lines_in_3space(vector *p1, vector *v1, vector *p2, vector *v2, float *s, float *t)
 188 {
 189         vector cross,delta;
 190         vm_vec_crossprod(&cross, v1, v2);
 191         vm_vec_sub(&delta, p2, p1);
 192
 193         float denominator = vm_vec_mag_squared(&cross);
 194         float num_t, num_s;
 195
 196         if (denominator > 1e-10) {
 197                 num_s = matrix_determinant_from_vectors(&delta, v2, &cross);
 198                 *s = num_s / denominator;
 199
 200                 num_t = matrix_determinant_from_vectors(&delta, v1, &cross);
 201                 *t = num_t / denominator;
 202         } else {
 203                 // two lines are parallel
 204                 *s = FLT_MAX;
 205                 *t = FLT_MAX;
 206         }
 207 }
 208
 209
 210 //--------------------------------------------------------------------
 211 // fvi_ray_plane - Finds the point on the specified plane where the
 212 // infinite ray intersects.
 213 //
 214 // Returns scaled-distance plane is from the ray_origin (t), so
 215 // P = O + t*D, where P is the point of intersection, O is the ray origin,
 216 // and D is the ray's direction.  So 0.0 would mean the intersection is
 217 // exactly on the ray origin, 1.0 would be on the ray origin plus the ray
 218 // direction vector, anything negative would be behind the ray's origin.
 219 // If you pass a pointer to the new_pnt, this routine will perform the P=
 220 // calculation to calculate the point of intersection and put the result
 221 // in *new_pnt.
 222 //
 223 // If radius is anything other than 0.0, it assumes you want the intersection
 224 // point to be that radius from the plane.
 225 //
 226 // Note that ray_direction doesn't have to be normalized unless you want
 227 // the return value to be in units from the ray origin.
 228 //
 229 // Also note that new_pnt will always be filled in to some valid value,
 230 // even if it is a point at infinity.
 231 //
 232 // If the plane and line are parallel, this will return the largest
 233 // negative float number possible.
 234 //
 235 // So if you want to check a line segment from p0 to p1, you would pass
 236 // p0 as ray_origin, p1-p0 as the ray_direction, and there would be an
 237 // intersection if the return value is between 0 and 1.
 238
 239 float fvi_ray_plane(vector *new_pnt,
 240                     vector *plane_pnt,vector *plane_norm,               // Plane description, a point and a normal
 241                     vector *ray_origin,vector *ray_direction,   // Ray description, a point and a direction
 242                                                   float rad)
 243 {
 244         vector w;
 245         float num,den,t;
 246
 247         vm_vec_sub(&w,ray_origin,plane_pnt);
 248
 249         den = -vm_vec_dot(plane_norm,ray_direction);
 250         if ( den == 0.0f ) {    // Ray & plane are parallel, so there is no intersection
 251                 if ( new_pnt )  {
 252                         new_pnt->xyz.x = -FLT_MAX;
 253                         new_pnt->xyz.y = -FLT_MAX;
 254                         new_pnt->xyz.z = -FLT_MAX;
 255                 }
 256                 return -FLT_MAX;
 257         }
 258
 259         num =  vm_vec_dot(plane_norm,&w);
 260         num -= rad;                     //move point out by rad
 261
 262         t = num / den;
 263
 264         if ( new_pnt )
 265                 vm_vec_scale_add(new_pnt,ray_origin,ray_direction,t);
 266
 267         return t;
 268 }
 269
 270
 271 //find the point on the specified plane where the line intersects
 272 //returns true if point found, false if line parallel to plane
 273 //new_pnt is the found point on the plane
 274 //plane_pnt & plane_norm describe the plane
 275 //p0 & p1 are the ends of the line.
 276 int fvi_segment_plane(vector *new_pnt,
 277                                                                                 vector *plane_pnt,vector *plane_norm,
 278                                  vector *p0,vector *p1,float rad)
 279 {
 280         float t;
 281         vector d;
 282
 283         vm_vec_sub( &d, p1, p0 );
 284
 285         t = fvi_ray_plane(new_pnt,
 286                     plane_pnt,plane_norm,               // Plane description, a point and a normal
 287                     p0,&d,      // Ray description, a point and a direction
 288                                                   rad);
 289
 290         if ( t < 0.0f ) return 0;               // intersection lies behind p0
 291         if ( t > 1.0f ) return 0;               // intersection lies past p1
 292
 293         return 1;               // They intersect!
 294 }
 295
 296
 297
 298
 299 //maybe this routine should just return the distance and let the caller
 300 //decide it it's close enough to hit
 301 //determine if and where a vector intersects with a sphere
 302 //vector defined by p0,p1
 303 //returns 1 if intersects, and fills in intp
 304 //else returns 0
 305 int fvi_segment_sphere(vector *intp,vector *p0,vector *p1,vector *sphere_pos,float sphere_rad)
 306 {
 307         vector d,dn,w,closest_point;
 308         float mag_d,dist,w_dist,int_dist;
 309
 310         //this routine could be optimized if it's taking too much time!
 311
 312         vm_vec_sub(&d,p1,p0);
 313         vm_vec_sub(&w,sphere_pos,p0);
 314
 315         mag_d = vm_vec_mag(&d);
 316
 317         if (mag_d <= 0.0f) {
 318                 int_dist = vm_vec_mag(&w);
 319                 *intp = *p0;
 320                 return (int_dist<sphere_rad)?1:0;
 321                 // int_dist is dist
 322         }
 323
 324         // normalize dn
 325         dn.xyz.x = d.xyz.x / mag_d;
 326         dn.xyz.y = d.xyz.y / mag_d;
 327         dn.xyz.z = d.xyz.z / mag_d;
 328
 329         w_dist = vm_vec_dot(&dn,&w);
 330
 331         if (w_dist < -sphere_rad)               //moving away from object
 332                  return 0;
 333
 334         if (w_dist > mag_d+sphere_rad)
 335                 return 0;               //cannot hit
 336
 337         vm_vec_scale_add(&closest_point,p0,&dn,w_dist);
 338
 339         dist = vm_vec_dist(&closest_point,sphere_pos);
 340
 341         if (dist < sphere_rad) {
 342                 float dist2,rad2,shorten;
 343
 344                 dist2 = dist*dist;
 345                 rad2 = sphere_rad*sphere_rad;
 346
 347                 shorten = fl_sqrt(rad2 - dist2);
 348
 349                 int_dist = w_dist-shorten;
 350
 351                 if (int_dist > mag_d || int_dist < 0.0f) {
 352                         //past one or the other end of vector, which means we're inside
 353
 354                         *intp = *p0;            //don't move at all
 355                         return 1;
 356                 }
 357
 358                 vm_vec_scale_add(intp,p0,&dn,int_dist);         //calc intersection point
 359
 360 //              {
 361 //                      fix dd = vm_vec_dist(intp,sphere_pos);
 362 //                      Assert(dd == sphere_rad);
 363 //                      mprintf(0,"dd=%x, rad=%x, delta=%x\n",dd,sphere_rad,dd-sphere_rad);
 364 //              }
 365
 366                 return 1;
 367         }
 368         else
 369                 return 0;
 370 }
 371
 372
 373 //determine if and where a ray intersects with a sphere
 374 //vector defined by p0,p1
 375 //returns 1 if intersects, and fills in intp
 376 //else returns 0
 377 int fvi_ray_sphere(vector *intp,vector *p0,vector *p1,vector *sphere_pos,float sphere_rad)
 378 {
 379         vector d,dn,w,closest_point;
 380         float mag_d,dist,w_dist,int_dist;
 381
 382         //this routine could be optimized if it's taking too much time!
 383
 384         vm_vec_sub(&d,p1,p0);
 385         vm_vec_sub(&w,sphere_pos,p0);
 386
 387         mag_d = vm_vec_mag(&d);
 388
 389         if (mag_d <= 0.0f) {
 390                 int_dist = vm_vec_mag(&w);
 391                 *intp = *p0;
 392                 return (int_dist<sphere_rad)?1:0;
 393                 // int_dist is dist
 394         }
 395
 396         // normalize dn
 397         dn.xyz.x = d.xyz.x / mag_d;
 398         dn.xyz.y = d.xyz.y / mag_d;
 399         dn.xyz.z = d.xyz.z / mag_d;
 400
 401         w_dist = vm_vec_dot(&dn,&w);
 402
 403         if (w_dist < -sphere_rad)               //moving away from object
 404                  return 0;
 405
 406 //      if (w_dist > mag_d+sphere_rad)
 407 //              return 0;               //cannot hit
 408
 409         vm_vec_scale_add(&closest_point,p0,&dn,w_dist);
 410
 411         dist = vm_vec_dist(&closest_point,sphere_pos);
 412
 413         if (dist < sphere_rad) {
 414                 float dist2,rad2,shorten;
 415
 416                 dist2 = dist*dist;
 417                 rad2 = sphere_rad*sphere_rad;
 418
 419                 shorten = fl_sqrt(rad2 - dist2);
 420
 421                 int_dist = w_dist-shorten;
 422
 423 //              if (int_dist > mag_d || int_dist < 0.0f) {
 424                 if (int_dist < 0.0f) {
 425                         //past one or the begining of vector, which means we're inside
 426
 427                         *intp = *p0;            //don't move at all
 428                         return 1;
 429                 }
 430
 431                 vm_vec_scale_add(intp,p0,&dn,int_dist);         //calc intersection point
 432
 433 //              {
 434 //                      fix dd = vm_vec_dist(intp,sphere_pos);
 435 //                      Assert(dd == sphere_rad);
 436 //                      mprintf(0,"dd=%x, rad=%x, delta=%x\n",dd,sphere_rad,dd-sphere_rad);
 437 //              }
 438
 439                 return 1;
 440         }
 441         else
 442                 return 0;
 443 }
 444
 445
 446 //==============================================================
 447 // Finds intersection of a ray and an axis-aligned bounding box
 448 // Given a ray with origin at p0, and direction pdir, this function
 449 // returns non-zero if that ray intersects an axis-aligned bounding box
 450 // from min to max.   If there was an intersection, then hitpt will contain
 451 // the point where the ray begins inside the box.
 452 // Fast ray-box intersection taken from Graphics Gems I, pages 395,736.
 453 int fvi_ray_boundingbox( vector *min, vector *max, vector * p0, vector *pdir, vector *hitpt )
 454 {
 455         float *origin = (float *)&p0->xyz.x;
 456         float *dir = (float *)&pdir->xyz.x;
 457         float *minB = (float *)min;
 458         float *maxB = (float *)max;
 459         float *coord = (float *)&hitpt->xyz.x;
 460         int inside = 1;
 461         int middle[3];
 462         int i;
 463         int which_plane;
 464         float maxt[3];
 465         float candidate_plane[3];
 466
 467         for (i=0; i<3; i++ )    {
 468                 if ( origin[i] < minB[i] )      {
 469                         middle[i] = 0;
 470                         candidate_plane[i] = minB[i];
 471                         inside = 0;
 472                 } else if (origin[i] > maxB[i] )        {
 473                         middle[i] = 0;
 474                         candidate_plane[i] = maxB[i];
 475                         inside = 0;
 476                 } else {
 477                         middle[i] = 1;
 478                 }
 479         }
 480
 481         // ray origin inside bounding box
 482         if ( inside )   {
 483                 *hitpt = *p0;
 484                 return 1;
 485         }
 486
 487         // calculate T distances to canditate plane
 488         for (i=0; i<3; i++ )    {
 489                 if ( (!middle[i]) && (dir[i] != 0.0f ))
 490                         maxt[i] = (candidate_plane[i]-origin[i]) / dir[i];
 491                 else
 492                         maxt[i] = -1.0f;
 493         }
 494
 495         // Get largest of the maxt's for final choice of intersection
 496         which_plane = 0;
 497         for (i=1; i<3; i++ )
 498                 if (maxt[which_plane] < maxt[i] )
 499                         which_plane = i;
 500
 501         // check final candidate actually inside box
 502         if ( maxt[which_plane] < 0.0f ) return 0;
 503
 504         for (i=0; i<3; i++ )    {
 505                 if (which_plane != i )  {
 506                         coord[i] = origin[i] + maxt[which_plane]*dir[i];
 507                         if ( coord[i] < minB[i] || coord[i] > maxB[i] )
 508                                 return 0;
 509                 } else {
 510                         coord[i] = candidate_plane[i];
 511                 }
 512         }
 513         return 1;
 514 }
 515
 516
 517
 518 //given largest componant of normal, return i & j
 519 //if largest componant is negative, swap i & j
 520 int ij_table[3][2] =        {
 521                                                         {2,1},          //pos x biggest
 522                                                         {0,2},          //pos y biggest
 523                                                         {1,0},          //pos z biggest
 524                                                 };
 525
 526 // fvi_point_face
 527 // see if a point in inside a face by projecting into 2d. Also
 528 // Finds uv's if uvls is not NULL.  Returns 0 if point isn't on
 529 // face, non-zero otherwise.
 530 // From Graphics Gems I, "An efficient Ray-Polygon intersection", p390
 531 // checkp - The point to check
 532 // nv - how many verts in the poly
 533 // verts - the vertives for the polygon
 534 // norm1 - the polygon's normal
 535 // u_out,vout - if not null and v_out not null and uvls not_null and point is on face, the uv's of where it hit
 536 // uvls - a list of uv pairs for each vertex
 537 // This replaces the old check_point_to_face & find_hitpoint_uv
 538 // WARNING!!   In Gems, they use the code "if (u1==0)" in this function.
 539 // I have found several cases where this will not detect collisions it should.
 540 // I found two solutions:
 541 //   1. Declare the 'beta' variable to be a double.
 542 //   2. Instead of using 'if (u1==0)', compare it to a small value.
 543 // I chose #2 because I would rather have our code work with all floats
 544 // and never need doubles.   -JAS Aug22,1997
 545 #define delta 0.0001f
 546 #define UNINITIALIZED_VALUE     -1234567.8f
 547
 548 int fvi_point_face(vector *checkp, int nv, vector **verts, vector * norm1, float *u_out,float *v_out, uv_pair * uvls )
 549 {
 550         float *norm, *P;
 551         vector t;
 552         int i0, i1,i2;
 553
 554         norm = (float *)norm1;
 555
 556         //project polygon onto plane by finding largest component of normal
 557         t.xyz.x = fl_abs(norm[0]);
 558         t.xyz.y = fl_abs(norm[1]);
 559         t.xyz.z = fl_abs(norm[2]);
 560
 561         if (t.xyz.x > t.xyz.y) if (t.xyz.x > t.xyz.z) i0=0; else i0=2;
 562         else if (t.xyz.y > t.xyz.z) i0=1; else i0=2;
 563
 564         if (norm[i0] > 0.0f) {
 565                 i1 = ij_table[i0][0];
 566                 i2 = ij_table[i0][1];
 567         }
 568         else {
 569                 i1 = ij_table[i0][1];
 570                 i2 = ij_table[i0][0];
 571         }
 572
 573         // Have i0, i1, i2
 574         P = (float *)checkp;
 575         vectora **V = (vectora **)verts;
 576
 577         float u0, u1, u2, v0, v1, v2, alpha = UNINITIALIZED_VALUE, gamma;
 578         float beta;
 579
 580         int inter=0, i = 2;
 581
 582         u0 = P[i1] - V[0]->xyz[i1];
 583         v0 = P[i2] - V[0]->xyz[i2];
 584
 585         do {
 586
 587                 u1 = V[i-1]->xyz[i1] - V[0]->xyz[i1];
 588                 u2 = V[i  ]->xyz[i1] - V[0]->xyz[i1];
 589
 590                 v1 = V[i-1]->xyz[i2] - V[0]->xyz[i2];
 591                 v2 = V[i  ]->xyz[i2] - V[0]->xyz[i2];
 592
 593
 594                 if ( (u1 >-delta) && (u1<delta) )       {
 595                         beta = u0 / u2;
 596                         if ((beta >=0.0f) && (beta<=1.0f))      {
 597                                 alpha = (v0 - beta*v2)/v1;
 598                                 inter = ((alpha>=0.0f)&&(alpha+beta<=1.0f));
 599                         }
 600                 } else {
 601
 602                         beta = (v0*u1 - u0*v1) / (v2*u1 - u2*v1);
 603                         if ((beta >=0.0f) && (beta<=1.0f))      {
 604                                 Assert(beta != UNINITIALIZED_VALUE);
 605                                 alpha = (u0 - beta*u2)/u1;
 606                                 inter = ((alpha>=0.0f)&&(alpha+beta<=1.0f));
 607                         }
 608
 609
 610                 }
 611
 612 /*
 613                 // This is test code that I used to detect failures.  See the
 614                 // comments above this function for details.
 615                 double betad;
 616                 float alphad;
 617                 int interd=0;
 618
 619                 betad = (v0*u1 - u0*v1) / (v2*u1 - u2*v1);
 620                 if ((betad >=0.0f) && (betad<=1.0f))    {
 621                         alphad = (u0 - betad*u2)/u1;
 622                         interd = ((alphad>=0.0f)&&(alphad+betad<=1.0f));
 623                 }
 624
 625                 if ( interd != inter )  {
 626                         mprintf(( "u0=%.4f, u1=%.16f, u2=%.4f\n", u0, u1, u2 ));
 627                         mprintf(( "v0=%.4f, v1=%.16f, v2=%.4f\n", v0, v1, v2 ));
 628                 }
 629 */
 630
 631         } while ((!inter) && (++i < nv) );
 632
 633         if ( inter &&  uvls && u_out && v_out ) {
 634                 // Assert(alpha != 1.0f);
 635                 gamma = 1.0f - (alpha+beta);
 636                 *u_out = gamma * uvls[0].u + alpha*uvls[i-1].u + beta*uvls[i].u;
 637                 *v_out = gamma * uvls[0].v + alpha*uvls[i-1].v + beta*uvls[i].v;
 638         }
 639
 640         return inter;
 641 }
 642
 643 // ****************************************************************************
 644 //
 645 // SPHERE FACE INTERSECTION CODE
 646 //
 647 // ****************************************************************************
 648
 649 int check_sphere_point( vector *point, vector *sphere_start, vector *sphere_vel, float radius, float *collide_time );
 650
 651 // ----------------------------------------------------------------------------
 652 // fvi_sphere_plane()
 653 // returns whether a sphere hits a given plane in the time [0,1]
 654 // if two collisions occur, returns earliest legal time
 655 // returns the intersection point on the plane
 656 //
 657 //              inputs: intersect_point         =>              position on plane where sphere makes first contact [if hit_time in range 0-1]
 658 //                                      sphere_center_start     =>              initial sphere center
 659 //                                      sphere_velocity         =>              initial sphere velocity
 660 //                                      sphere_radius                   =>              radius of sphere
 661 //                                      plane_normal                    =>              normal to the colliding plane
 662 //                                      plane_point                             =>              point in the colliding plane
 663 //                                      hit_time                                        =>              time surface of sphere first hits plane
 664 //                                      delta_t                                 =>              time for sphere to cross plane (first to last contact)
 665 //
 666 //              return: 1 if sphere may be in contact with plane in time range [0-1], 0 otherwise
 667 //
 668
 669 int fvi_sphere_plane(vector *intersect_point, vector *sphere_center_start, vector *sphere_velocity, float sphere_radius,
 670                                                         vector *plane_normal, vector *plane_point, float *hit_time, float *crossing_time)
 671 {
 672         float   D, xs0_dot_norm, vs_dot_norm;
 673         float t1, t2;
 674
 675         // find the time and position of the ray-plane intersection
 676         D = -vm_vec_dotprod( plane_normal, plane_point );
 677         xs0_dot_norm = vm_vec_dotprod( plane_normal, sphere_center_start );
 678         vs_dot_norm  = vm_vec_dotprod( plane_normal, sphere_velocity);
 679
 680         // protect against divide by zero
 681         if (fl_abs(vs_dot_norm) > 1e-30) {
 682                 t1 = (-D - xs0_dot_norm + sphere_radius) / vs_dot_norm;
 683                 t2 = (-D - xs0_dot_norm - sphere_radius) / vs_dot_norm;
 684         } else {
 685                 return 0;
 686         }
 687
 688         // sort t1 < t2
 689         if (t2 < t1) {
 690                 float temp = t1;
 691                 t1 = t2;
 692                 t2 = temp;
 693         }
 694
 695         *hit_time = t1;
 696
 697         // find hit pos if t1 in range 0-1
 698         if (t1 > 0 && t1 < 1) {
 699                 vector v_temp;
 700                 vm_vec_scale_add( &v_temp, sphere_center_start, sphere_velocity, t1 );
 701                 vm_project_point_onto_plane( intersect_point, &v_temp, plane_normal, plane_point );
 702         }
 703
 704         // get time to cross
 705         *crossing_time = t2 - t1;
 706
 707         return ( (t1 < 1) && (t2 > 0) );
 708 }
 709
 710 // ----------------------------------------------------------------------------
 711 // fvi_sphere_perp_edge()
 712 //      returns whether a sphere hits and edge for the case the edge is perpendicular to sphere_velocity
 713 // if two collisions occur, returns the earliest legal time
 714 // returns the intersection point on the edge
 715 //
 716 //              inputs: intersect_point         =>              position on plane where sphere makes first contact [RESULT]
 717 //                                      sphere_center_start     =>              initial sphere center
 718 //                                      sphere_velocity         =>              initial sphere velocity
 719 //                                      sphere_radius                   =>              radius of sphere
 720 //                                      edge_point1                             =>              first edge point
 721 //                                      edge_point2                             =>              second edge point
 722 //                                      max_time                                        =>              maximum legal time at which collision can occur
 723 //                                      collide_time                    =>              actual time of the collision
 724 //
 725 int fvi_sphere_perp_edge(vector *intersect_point, vector *sphere_center_start, vector *sphere_velocity,
 726                                                                  float sphere_radius, vector *edge_point1, vector *edge_point2, float *collide_time)
 727 {
 728         // find the intersection in the plane normal to sphere velocity and edge velocity
 729         // choose a plane point V0 (first vertex of the edge)
 730         // project vectors and points into the plane
 731         // find the projection of the intersection and see if it lies on the edge
 732
 733         vector edge_velocity;
 734         vector V0, V1;
 735         vector Xe_proj, Xs_proj;
 736
 737         V0 = *edge_point1;
 738         V1 = *edge_point2;
 739         vm_vec_sub(&edge_velocity, &V1, &V0);
 740
 741         // define a set of local unit vectors
 742         vector x_hat, y_hat, z_hat;
 743         float max_edge_parameter;
 744
 745         vm_vec_copy_normalize( &x_hat, &edge_velocity );
 746         vm_vec_copy_normalize( &y_hat, sphere_velocity );
 747         vm_vec_crossprod( &z_hat, &x_hat, &y_hat );
 748         max_edge_parameter = vm_vec_mag( &edge_velocity );
 749
 750         vector temp;
 751         // next two temp should be same as starting velocities
 752         vm_vec_projection_onto_plane(&temp, sphere_velocity, &z_hat);
 753         Assert ( !vm_vec_cmp(&temp, sphere_velocity) );
 754         vm_vec_projection_onto_plane(&temp, &edge_velocity,  &z_hat);
 755         Assert ( !vm_vec_cmp(&temp, &edge_velocity) );
 756
 757         // should return V0
 758         vm_project_point_onto_plane(&Xe_proj, &V0, &z_hat, &V0);
 759         Assert ( !vm_vec_cmp(&Xe_proj, &V0) );
 760
 761         vm_project_point_onto_plane(&Xs_proj, sphere_center_start, &z_hat, &V0);
 762
 763         vector plane_coord;
 764         plane_coord.xyz.x = vm_vec_dotprod(&Xs_proj, &x_hat);
 765         plane_coord.xyz.y = vm_vec_dotprod(&Xe_proj, &y_hat);
 766         plane_coord.xyz.z = vm_vec_dotprod(&Xe_proj, &z_hat);
 767
 768         // determime the position on the edge line
 769         vm_vec_copy_scale( intersect_point, &x_hat, plane_coord.xyz.x );
 770         vm_vec_scale_add2( intersect_point, &y_hat, plane_coord.xyz.y );
 771         vm_vec_scale_add2( intersect_point, &z_hat, plane_coord.xyz.z );
 772
 773         // check if point is actually on edge
 774         float edge_parameter;
 775         vector temp_vec;
 776
 777         vm_vec_sub( &temp_vec, intersect_point, &V0 );
 778         edge_parameter = vm_vec_dotprod( &temp_vec, &x_hat );
 779
 780         if ( edge_parameter < 0 || edge_parameter > max_edge_parameter ) {
 781                 return 0;
 782         }
 783
 784         return ( check_sphere_point(intersect_point, sphere_center_start, sphere_velocity, sphere_radius, collide_time) );
 785 }
 786
 787
 788 // ----------------------------------------------------------------------------
 789 // check_sphere_point()
 790 // determines whether and where a moving sphere hits a point
 791 //
 792 //                      inputs:         point                           =>              point sphere collides with
 793 //                                                      sphere_start    =>              initial sphere center
 794 //                                                      sphere_vel              =>              velocity of sphere
 795 //                                                      radius                  =>              radius of sphere
 796 //                                                      collide_time    =>              time of first collision with t >= 0
 797 //
 798 int check_sphere_point( vector *point, vector *sphere_start, vector *sphere_vel, float radius, float *collide_time )
 799 {
 800         vector delta_x;
 801         float delta_x_sqr, vs_sqr, delta_x_dot_vs;
 802
 803         vm_vec_sub( &delta_x, sphere_start, point );
 804         delta_x_sqr = vm_vec_mag_squared( &delta_x );
 805         vs_sqr = vm_vec_mag_squared( sphere_vel );
 806         delta_x_dot_vs = vm_vec_dotprod( &delta_x, sphere_vel );
 807
 808 //      a = vs_sqr;
 809 //      b = 2.0f*delta_x_dot_vs;
 810 //      c = delta_x_sqr - radius*radius;
 811
 812         float discriminant = delta_x_dot_vs*delta_x_dot_vs - vs_sqr*(delta_x_sqr - radius*radius);
 813         if (discriminant < 0) {
 814                 return 0;
 815         }
 816
 817         float radical, time1, time2;
 818         radical = fl_sqrt(discriminant);
 819         time1 = (-delta_x_dot_vs + radical) / vs_sqr;
 820         time2 = (-delta_x_dot_vs - radical) / vs_sqr;
 821
 822         if (time1 > time2) {
 823                 float temp = time1;
 824                 time1 = time2;
 825                 time2 = temp;
 826         }
 827
 828         if (time1 >= 0 && time1 <= 1.0) {
 829                 *collide_time = time1;
 830                 return 1;
 831         }
 832
 833         if (time2 >= 0 && time2 <= 1.0) {
 834                 *collide_time = time2;
 835                 return 1;
 836         }
 837
 838         return 0;
 839 }
 840
 841
 842
 843 // ----------------------------------------------------------------------------
 844 //
 845 // fvi_polyedge_sphereline()
 846 //
 847 // Given a polygon vertex list and a moving sphere, find the first contact the sphere makes with the edge, if any
 848 //
 849 //              Inputs: hit_point               =>              point on edge
 850 //                                      xs0                             =>              starting point for sphere
 851 //                                      vs                                      =>              sphere velocity
 852 //                                      Rs                                      =>              sphere radius
 853 //                                      nv                                      =>              number of vertices
 854 //                                      verts                           =>              vertices making up polygon edges
 855 //                                      hit_time                        =>              time the sphere hits an edge
 856 //
 857 //              Return: 1 if sphere hits polyedge, 0 if sphere misses
 858 /*
 859 #define TOL 1E-3
 860
 861 int fvi_polyedge_sphereline(vector *hit_point, vector *xs0, vector *vs, float Rs, int nv, vector **verts, float *hit_time)
 862 {
 863         int i;
 864         vector v0, v1;
 865         vector ve;                                              // edge velocity
 866         float best_sphere_time;         // earliest time sphere hits edge
 867         vector delta_x;
 868         float   delta_x_dot_ve, delta_x_dot_vs, ve_dot_vs, ve_sqr, vs_sqr;
 869         float denominator;
 870         float time_el, time_sl;         // times for edge_line and sphere_line at closest approach
 871         vector temp_edge_hit, temp_sphere_hit;
 872         vector best_edge_hit;           // edge position for earliest edge hit
 873
 874         best_sphere_time = FLT_MAX;
 875
 876         for (i=0; i<nv; i++) {
 877                 // Get vertices of edge to check
 878                 v0 = *verts[i];
 879                 if (i+1 != nv) {
 880                         v1 = *verts[i+1];
 881                 } else {
 882                         v1 = *verts[0];
 883                 }
 884
 885                 // Calculate edge velocity.
 886                 // Position along the edge is given by: P_edge = v0 + ve*t, where t is in the range [0,1]
 887                 vm_vec_sub(&ve, &v1, &v0);
 888
 889                 // First find the closest intersection between the edge_line and the sphere_line.
 890                 vm_vec_sub(&delta_x, &v0, xs0);
 891                 delta_x_dot_ve = vm_vec_dotprod(&delta_x, &ve);
 892
 893
 894                 delta_x_dot_vs = vm_vec_dotprod(&delta_x, vs);
 895                 ve_dot_vs = vm_vec_dotprod(&ve, vs);
 896                 ve_sqr = vm_vec_mag_squared(&ve);
 897                 vs_sqr = vm_vec_mag_squared(vs);
 898
 899
 900                 denominator = ve_dot_vs*ve_dot_vs - ve_sqr*vs_sqr;
 901                 if (fl_abs(denominator) < SMALL_NUM) {          // check for parallel linesg
 902                         continue;                                                                               // would have to hit at vertex
 903                 }                                                                                                               // and another edge will not be so parallel
 904
 905                 // Compute time (and position) along the edge_line and sphere_line at closest approach.
 906                 time_el = (delta_x_dot_ve*vs_sqr - ve_dot_vs*delta_x_dot_vs) / denominator;
 907                 time_sl = (delta_x_dot_vs + ve_dot_vs*time_el) / vs_sqr;
 908
 909                 // DA: 12/5/97  I checked above procedure for calculating line intersection against fvi_closest_line_line()
 910                 // fvi_closest_line_line appears to have much lower accuracy as many edges collisions were missed
 911
 912                 vm_vec_scale_add(&temp_edge_hit, &v0, &ve, time_el);
 913                 vm_vec_scale_add(&temp_sphere_hit, xs0, vs, time_sl);
 914
 915                 // Compute distance squared at closest approach.
 916                 vector diff;
 917                 float  d0_sqr;
 918                 vm_vec_sub(&diff, &temp_sphere_hit, &temp_edge_hit);
 919                 d0_sqr = vm_vec_mag_squared(&diff);
 920
 921                 if (d0_sqr > Rs*Rs) {
 922                         continue;
 923                 }
 924
 925                 // Starting from the positions of closest approach, back up the sphere until it is just touching the edge_line.
 926                 // Check this edge_line point against the range of the edge.  If not in range, check if the sphere hits the
 927                 // extreme of the edge.
 928
 929                 //      time_e1 - the edge_line position of closest approach
 930                 //      time_e  - the edge position where sphere makes first contact
 931                 float dist_e_sqr, dist_s_sqr, delta_te, delta_ts, cos_sqr;
 932                 float time_s;
 933                 float time_em, time_ep, time_sm, time_sp;
 934                 float te_per_ts;
 935                 cos_sqr = (ve_dot_vs*ve_dot_vs) / (ve_sqr*vs_sqr);
 936                 dist_s_sqr = (Rs*Rs - d0_sqr) / (1.0f - cos_sqr);
 937                 delta_ts = fl_sqrt(dist_s_sqr / vs_sqr);
 938                 time_sm = time_sl - delta_ts;
 939                 time_sp = time_sl + delta_ts;
 940
 941                 if (time_sm > 1 || time_sp < 0) {
 942                         continue;
 943                 }
 944
 945                 dist_e_sqr = (Rs*Rs - d0_sqr) * cos_sqr / (1.0f - cos_sqr);
 946                 delta_te = fl_sqrt(dist_e_sqr / ve_sqr);
 947                 time_em = time_el - delta_te;
 948                 time_ep = time_el + delta_te;
 949
 950                 if (cos_sqr > 0.5f) {
 951                         if (time_em > 1 || time_ep < 0) {
 952                                 continue;
 953                         }
 954                 } else {
 955                         delta_te = fl_sqrt( (Rs*Rs - d0_sqr) / ve_sqr );
 956                         if ((time_el - delta_te) > 1 || (time_el + delta_te) <  0) {
 957                                 continue;
 958                         }
 959                 }
 960
 961                 // Check if we already have a hit
 962                 // Move sphere back to time_sm.  If ve_dot_vs > 0 edge to time_em, < 0 time_ep.
 963
 964                 if (time_sm >= 0 && time_sm <= 1) {
 965                         if (ve_dot_vs > 0) {
 966                                 if (time_em >= 0 && time_em <= 1) {
 967                                         vm_vec_scale_add( &temp_edge_hit, &v0, &ve, time_em );
 968                                         time_s = time_sm;
 969                                         goto Hit;
 970                                 }
 971                         } else {
 972                                 if (time_ep >= 0 && time_ep <= 1) {
 973                                         vm_vec_scale_add( &temp_edge_hit, &v0, &ve, time_ep );
 974                                         time_s = time_sm;
 975                                         goto Hit;
 976                                 }
 977                         }
 978                 }
 979
 980                 // Find the ratio of times between sphere_line and edge_line.
 981                 te_per_ts = fl_sqrt( vs_sqr / ve_sqr );
 982
 983                 // Check the location of the closest point on the edge line corresponding to the first valid point on sphere_line.
 984                 // First valid sphere_line point is the greater of (1) t = 0 or (2) t = time_sm.
 985                 // If the corresponding edge interval is left, we check against the rightmost edgepoint.
 986                 // If the corresponding edge interval is right, we check against the leftmost edgepoint.
 987                 // If the corresponding edge interval contains this point, then we are already penetrating.
 988                 float first_valid_sphere_time;
 989                 float closest_edge_time;
 990
 991                 // Find first valid sphere time
 992                 if (time_sm < 0) {
 993                         first_valid_sphere_time = 0.0f;
 994                 } else {
 995                         first_valid_sphere_time = time_sm;
 996                         Assert( time_sm <= 1.0f );
 997                 }
 998
 999                 if (ve_dot_vs > 0) {
1000
1001                         // Find corresponding edge time
1002                         closest_edge_time = time_el - te_per_ts * (time_sl - first_valid_sphere_time) * fl_sqrt(cos_sqr);
1003                         if (time_em < 0) {
1004                                 time_em = 0.0f;
1005                         }
1006                         if (time_ep > 1) {
1007                                 time_ep = 1.0f;
1008                         }
1009
1010                         if (time_em > closest_edge_time - SMALL_NUM) {
1011                                 // edge interval is right so test against left edge
1012                                 vm_vec_scale_add( &temp_edge_hit, &v0, &ve, time_em );
1013                                 if ( !check_sphere_point( &temp_edge_hit, xs0, vs, Rs, &time_s ) ) {
1014                                         continue;
1015                                 }
1016
1017                         } else if (time_ep < closest_edge_time + SMALL_NUM) {
1018                                 // edge interval is left so test against right edge
1019                                 vm_vec_scale_add( &temp_edge_hit, &v0, &ve, time_ep );
1020                                 if ( !check_sphere_point( &temp_edge_hit, xs0, vs, Rs, &time_s ) ) {
1021                                         continue;
1022                                 }
1023
1024                         } else {
1025                                 // edge interval contains point, so already penetrating
1026                                 continue;
1027                         }
1028                 } else {
1029                         // Sphere and edge have opposite velocities
1030
1031                         // Find corresponding edge time
1032                         closest_edge_time = time_el + te_per_ts * (time_sl - first_valid_sphere_time) * fl_sqrt(cos_sqr);
1033                         if (time_em < 0) {
1034                                 time_em = 0.0f;
1035                         }
1036                         if (time_ep > 1) {
1037                                 time_ep = 1.0f;
1038                         }
1039
1040                         if (closest_edge_time > time_ep - SMALL_NUM) {
1041                                 // edge interval is right so test against left edge
1042                                 vm_vec_scale_add( &temp_edge_hit, &v0, &ve, time_ep );
1043                                 if ( !check_sphere_point( &temp_edge_hit, xs0, vs, Rs, &time_s ) ) {
1044                                         continue;
1045                                 }
1046
1047                         } else if (closest_edge_time < time_em + SMALL_NUM) {
1048                                 // edge interval is left so test against right edge
1049                                 vm_vec_scale_add( &temp_edge_hit, &v0, &ve, time_em );
1050                                 if ( !check_sphere_point( &temp_edge_hit, xs0, vs, Rs, &time_s ) ) {
1051                                         continue;
1052                                 }
1053
1054                         } else {
1055                                 // edge interval contains point, so already penetrating
1056                                 continue;
1057                         }
1058                 }
1059
1060 Hit:
1061 //              vector temp;
1062 //              vm_vec_scale_add( &temp, xs0, vs, time_s);
1063 //              float q = vm_vec_dist( &temp, &temp_edge_hit );
1064 //              if (q > Rs + .003 || q < Rs - .003) {
1065 //                      Int3();
1066 //              }
1067                 if (time_s < best_sphere_time) {
1068                         best_sphere_time = time_s;
1069                         best_edge_hit = temp_edge_hit;
1070                 }
1071         }       // end edge loop
1072
1073         if (best_sphere_time <= 1.0f) {
1074                 *hit_time = best_sphere_time;
1075                 *hit_point = best_edge_hit;
1076                 return 1;
1077         } else {
1078                 return 0;
1079         }
1080 }
1081 */
1082 // ----------------------------------------------------------------------------
1083 //
1084 // fvi_polyedge_sphereline()
1085 //
1086 // Given a polygon vertex list and a moving sphere, find the first contact the sphere makes with the edge, if any
1087 //
1088 //              Inputs: hit_point               =>              point on edge
1089 //                                      xs0                             =>              starting point for sphere
1090 //                                      vs                                      =>              sphere velocity
1091 //                                      Rs                                      =>              sphere radius
1092 //                                      nv                                      =>              number of vertices
1093 //                                      verts                           =>              vertices making up polygon edges
1094 //                                      hit_time                        =>              time the sphere hits an edge
1095 //
1096 //              Return: 1 if sphere hits polyedge, 0 if sphere misses
1097
1098 #define WARN_DIST       1.0
1099
1100 int fvi_polyedge_sphereline(vector *hit_point, vector *xs0, vector *vs, float Rs, int nv, vector **verts, float *hit_time)
1101 {
1102         int i;
1103         vector v0, v1;
1104         vector ve;                                              // edge velocity
1105         float best_sphere_time;         // earliest time sphere hits edge
1106         vector delta_x;
1107         float   delta_x_dot_ve, delta_x_dot_vs, ve_dot_vs, ve_sqr, vs_sqr, delta_x_sqr;
1108         vector temp_edge_hit, temp_sphere_hit;
1109
1110         best_sphere_time = FLT_MAX;
1111
1112         for (i=0; i<nv; i++) {
1113                 // Get vertices of edge to check
1114                 v0 = *verts[i];
1115                 if (i+1 != nv) {
1116                         v1 = *verts[i+1];
1117                 } else {
1118                         v1 = *verts[0];
1119                 }
1120
1121                 // Calculate edge velocity.
1122                 // Position along the edge is given by: P_edge = v0 + ve*t, where t is in the range [0,1]
1123                 vm_vec_sub(&ve, &v1, &v0);
1124
1125                 // First find the closest intersection between the edge_line and the sphere_line.
1126                 vm_vec_sub(&delta_x, xs0, &v0);
1127
1128                 delta_x_dot_ve = vm_vec_dotprod(&delta_x, &ve);
1129                 delta_x_dot_vs = vm_vec_dotprod(&delta_x, vs);
1130                 delta_x_sqr = vm_vec_mag_squared(&delta_x);
1131                 ve_dot_vs = vm_vec_dotprod(&ve, vs);
1132                 ve_sqr = vm_vec_mag_squared(&ve);
1133                 vs_sqr = vm_vec_mag_squared(vs);
1134
1135                 float t_sphere_hit, temp;
1136
1137                 // solve for sphere time
1138                 double A, B, C, root, discriminant;
1139                 float root1, root2;
1140                 A = ve_dot_vs*ve_dot_vs - ve_sqr*vs_sqr;
1141                 B = 2 * (delta_x_dot_ve*ve_dot_vs - delta_x_dot_vs*ve_sqr);
1142                 C = delta_x_dot_ve*delta_x_dot_ve + Rs*Rs*ve_sqr - delta_x_sqr*ve_sqr;
1143
1144                 discriminant = B*B - 4*A*C;
1145                 if (discriminant > 0) {
1146                         root = fl_sqrt(discriminant);
1147                         root1 = (float) ((-B + root)/(2*A));
1148                         root2 = (float) ((-B - root)/(2*A));
1149
1150
1151                         // sort root1 and root2
1152                         if (root2 < root1) {
1153                                 temp = root1;
1154                                 root1 = root2;
1155                                 root2 = temp;
1156                         }
1157
1158                         if (root1 >= -0.05f && root1 < 0.0f) {
1159                                 root1 = 0.000001f;
1160                         }
1161
1162                         // look only at first hit
1163                         if ( (root1 >= 0) && (root1 <= 1) ) {
1164                                 t_sphere_hit = root1;
1165                         } else {
1166                                 goto TryVertex;
1167                         }
1168                 } else {
1169                         // discriminant negative, so no hit possible
1170                         continue;
1171                 }
1172
1173                 // check if best time with this edge is less than best so far
1174                 if (t_sphere_hit >= best_sphere_time)
1175                         continue;
1176
1177                 vm_vec_scale_add( &temp_sphere_hit, xs0, vs, t_sphere_hit );
1178                 float q;
1179                 // solve for edge time
1180                 A = ve_sqr * (ve_dot_vs*ve_dot_vs - ve_sqr*vs_sqr);
1181                 B = 2*ve_sqr * (delta_x_dot_ve*vs_sqr - delta_x_dot_vs*ve_dot_vs);
1182                 C = 2*delta_x_dot_ve*delta_x_dot_vs*ve_dot_vs + Rs*Rs*ve_dot_vs*ve_dot_vs
1183                          - delta_x_sqr*ve_dot_vs*ve_dot_vs - delta_x_dot_ve*delta_x_dot_ve*vs_sqr;
1184
1185                 discriminant = B*B - 4*A*C;
1186
1187                 // guard against nearly perpendicular sphere edge velocities
1188                 if ( (discriminant < 0)  ) {
1189                         discriminant = 0;
1190                 }
1191
1192                 root = fl_sqrt(discriminant);
1193                 root1 = (float) ((-B + root)/(2*A));
1194                 root2 = (float) ((-B - root)/(2*A));
1195
1196                 // given sphere position, find which edge time (position) allows a valid solution
1197                 if ( (root1 >= 0) && (root1 <= 1) ) {
1198                         // try edge root1
1199                         vm_vec_scale_add( &temp_edge_hit, &v0, &ve, root1 );
1200                         q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
1201                         if ( fl_abs(q - Rs*Rs) < 0.2*Rs ) {     // error less than 0.1m absolute (2*delta*Radius)
1202                                 goto Hit;
1203                         }
1204                 }
1205
1206                 if ( (root2 >= 0) && (root2 <= 1) ) {
1207                         // try edge root2
1208                         vm_vec_scale_add( &temp_edge_hit, &v0, &ve, root2 );
1209                         q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
1210                         if ( fl_abs(q - Rs*Rs) < 0.2*Rs ) {     // error less than 0.1m absolute
1211                                 goto Hit;
1212                         }
1213                 } else {
1214                         // both root1 and root2 out of range so we have to check vertices
1215                         goto TryVertex;
1216                 }
1217
1218                 // Misses EDGE, so try ENDPOINTS
1219                 // Not exactly sure about this part (ie, which endpoint to check)
1220
1221                 // CHECK V0, we don't need to recalculate delta_x
1222                 // CHECK V1, we *need* to recalculate delat_x
1223
1224                 // check end points
1225
1226 TryVertex:
1227                 // try V0
1228                 A = vs_sqr;
1229                 B = 2*delta_x_dot_vs;
1230                 C = delta_x_sqr - Rs*Rs;
1231                 int v0_hit;
1232                 float sphere_v0, sphere_v1;
1233
1234                 sphere_v0 = UNINITIALIZED_VALUE;
1235                 sphere_v1 = UNINITIALIZED_VALUE;
1236
1237                 v0_hit = 0;
1238                 discriminant = B*B - 4*A*C;
1239                 if (discriminant > 0) {
1240                         root = fl_sqrt(discriminant);
1241                         root1 = (float) ((-B + root)/(2*A));
1242                         root2 = (float) ((-B - root)/(2*A));
1243
1244                         if (root1 > root2) {
1245                                 temp = root1;
1246                                 root1 = root2;
1247                                 root2 = temp;
1248                         }
1249
1250                         // look only at the fist hit  (ignore negative first hit)
1251                         if ( (root1 > 0) && (root1 < 1) ) {
1252                                 v0_hit = 1;
1253                                 sphere_v0 = root1;
1254                                 vm_vec_scale_add( &temp_sphere_hit, xs0, vs, root1 );
1255                         //      q = vm_vec_dist_squared( &v0, &temp_sphere_hit );       // debug
1256                         //      if ( fl_abs(q - Rs*Rs) > 2*WARN_DIST*Rs )
1257                         //              mprintf(("Estimated radius error: Estimate %f, actual %f  Get Dave A.\n", fl_sqrt(q), Rs));
1258                         }
1259                 }
1260
1261                 // try V1
1262                 vm_vec_sub( &delta_x, xs0, &v1 );
1263                 delta_x_sqr = vm_vec_mag_squared( &delta_x );
1264                 delta_x_dot_vs = vm_vec_dotprod( &delta_x, vs );
1265                 int v1_hit;
1266
1267                 B = 2*delta_x_dot_vs;
1268                 C = delta_x_sqr - Rs*Rs;
1269
1270                 v1_hit = 0;
1271                 discriminant = B*B - 4*A*C;
1272                 if (discriminant > 0) {
1273                         root = fl_sqrt(discriminant);
1274                         root1 = (float) ((-B + root)/(2*A));
1275                         root2 = (float) ((-B - root)/(2*A));
1276
1277                         if (root1 > root2) {
1278                                 temp = root1;
1279                                 root1 = root2;
1280                                 root2 = temp;
1281                         }
1282
1283                         // look only at the first hit (ignore negative first hit)
1284                         if ( (root1 > 0) && (root1 < 1) ) {
1285                                 v1_hit = 1;
1286                                 sphere_v1 = root1;
1287                                 vm_vec_scale_add( &temp_sphere_hit, xs0, vs, root1 );
1288                         //      q = vm_vec_dist_squared( &v1, &temp_sphere_hit );
1289                         //      if ( fl_abs(q - Rs*Rs) > 2*WARN_DIST*Rs )
1290                         //              mprintf(("Estimated radius error: Estimate %f, actual %f  Get Dave A.\n", fl_sqrt(q), Rs));
1291                         }
1292                 }
1293
1294                 // set hitpoint to closest vetex hit, if any
1295                 if ( v0_hit ) {
1296                         Assert(sphere_v0 != UNINITIALIZED_VALUE);
1297                         t_sphere_hit = sphere_v0;
1298                         temp_edge_hit = v0;
1299
1300                         if (v1_hit) {
1301                                 Assert(sphere_v1 != UNINITIALIZED_VALUE);
1302                                 if (sphere_v1 < sphere_v0) {
1303                                         t_sphere_hit = sphere_v1;
1304                                         temp_edge_hit = v1;
1305                                 }
1306                         }
1307                 } else if ( v1_hit ) {
1308                         Assert(sphere_v1 != UNINITIALIZED_VALUE);
1309                         t_sphere_hit = sphere_v1;
1310                         temp_edge_hit = v1;
1311                 } else {
1312                         continue;
1313                 }
1314
1315                 vm_vec_scale_add( &temp_sphere_hit, xs0, vs, t_sphere_hit );
1316                 q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
1317                 // if ( fl_abs(q - Rs*Rs) > 2*WARN_DIST*Rs ) {
1318                 //      mprintf(("Estimated radius error: Estimate %f, actual %f  Get Dave A.\n", fl_sqrt(q), Rs));
1319                 // }
1320
1321 Hit:
1322 //              vector temp;
1323 //              vm_vec_scale_add( &temp, xs0, vs, time_s);
1324 //              float q = vm_vec_dist( &temp, &temp_edge_hit );
1325 //              if (q > Rs + .003 || q < Rs - .003) {
1326 //                      Int3();
1327 //              }
1328                 if (t_sphere_hit < best_sphere_time) {
1329                         best_sphere_time = t_sphere_hit;
1330                         *hit_point = temp_edge_hit;
1331                 }
1332         }       // end edge loop
1333
1334         if (best_sphere_time <= 1.0f) {
1335                 *hit_time = best_sphere_time;
1336                 return 1;
1337         } else {
1338                 return 0;
1339         }
1340 }
1341
1342
1343 // ----------------------------------------------------------------------------
1344 //
1345 //      fvi_closest_point_on_line_segment()
1346 //
1347 // Finds the closest point on a line to a given fixed point
1348 //
1349 //              Inputs: closest_point   =>              the closest point on the line
1350 //                                      fixed_point             =>              the fixed point
1351 //                                      line_point1             =>              first point on the line
1352 //                                      line_point2             =>              second point on the line
1353 //
1354 void fvi_closest_point_on_line_segment(vector *closest_point, vector *fixed_point, vector *line_point1, vector *line_point2)
1355 {
1356         vector delta_x, line_velocity;
1357         float t;
1358
1359         vm_vec_sub(&line_velocity, line_point2, line_point1);
1360         vm_vec_sub(&delta_x, line_point1, fixed_point);
1361         t = -vm_vec_dotprod(&delta_x, &line_velocity) / vm_vec_mag_squared(&line_velocity);
1362
1363         // Constrain t to be in range [0,1]
1364         if (t < 0) {
1365                 t = 0.0f;
1366         } else if (t > 1) {
1367                 t = 1.0f;
1368         }
1369
1370         vm_vec_scale_add(closest_point, line_point1, &line_velocity, t);
1371 }
1372
1373
1374 // ----------------------------------------------------------------------------
1375 //
1376 //      fvi_check_sphere_sphere()
1377 //
1378 //      checks whether two spheres hit given initial and starting positions and radii
1379 // does not check whether sphere are already touching.
1380 //
1381 //              Inputs  x_p0                    =>              polymodel sphere, start point
1382 //                                      x_p1                    =>              polymodel sphere, end point
1383 //                                      x_s0                    =>              other sphere, start point
1384 //                                      x_s1                    =>              other sphere, end point
1385 //                                      radius_p                =>              radius of polymodel sphere
1386 //                                      radius_s                =>              radius of other sphere
1387 //
1388 //              returns 1 if spheres overlap, 0 otherwise
1389 //
1390 int fvi_check_sphere_sphere(vector *x_p0, vector *x_p1, vector *x_s0, vector *x_s1, float radius_p, float radius_s, float *t1, float *t2)
1391 {
1392         vector delta_x, delta_v;
1393         float discriminant, separation, delta_x_dot_delta_v, delta_v_sqr, delta_x_sqr;
1394         float time1, time2;
1395
1396         // Check that there are either 0 or 2 pointers to time
1397         Assert( (!(t1) && !(t2)) || (t1 && t2) );
1398
1399         vm_vec_sub(&delta_x, x_s0, x_p0);
1400         delta_x_sqr = vm_vec_mag_squared(&delta_x);
1401         separation = radius_p + radius_s;
1402
1403         // Check if already touching
1404         if (delta_x_sqr < separation*separation) {
1405                  if ( !t1 ) {
1406                         return 1;
1407                  }
1408         }
1409
1410         // Find delta_v (in polymodel sphere frame of ref)
1411         // Note: x_p0 and x_p1 will be same for fixed polymodel
1412         vm_vec_sub(&delta_v, x_s1, x_s0);
1413         vm_vec_add2(&delta_v, x_p0);
1414         vm_vec_sub2(&delta_v, x_p1);
1415
1416         delta_x_dot_delta_v = vm_vec_dotprod(&delta_x, &delta_v);
1417         delta_v_sqr = vm_vec_mag_squared(&delta_v);
1418
1419         discriminant = delta_x_dot_delta_v*delta_x_dot_delta_v - delta_v_sqr*(delta_x_sqr - separation*separation);
1420
1421         if (discriminant < 0) {
1422                 return 0;
1423         }
1424
1425         float radical = fl_sqrt(discriminant);
1426
1427         time1 = (-delta_x_dot_delta_v + radical) / delta_v_sqr;
1428         time2 = (-delta_x_dot_delta_v - radical) / delta_v_sqr;
1429
1430         // sort t1 < t2
1431         float temp;
1432         if (time1 > time2) {
1433                 temp  = time1;
1434                 time1 = time2;
1435                 time2 = temp;
1436         }
1437
1438         if ( t1 ) {
1439                 *t1 = time1;
1440                 *t2 = time2;
1441         }
1442
1443         // check whether the range from t1 to t2 intersects [0,1]
1444         if (time1 > 1 || time2 < 0) {
1445                 return 0;
1446         } else {
1447                 return 1;
1448         }
1449 }
1450
1451 // ----------------------------------------------------------------------------
1452 //
1453 //      fvi_cull_polyface_sphere()
1454 //
1455 // Culls polyfaces which moving sphere can not intersect
1456 //
1457 //              Inputs:         poly_center             =>              center of polygon face to test
1458 //                                              poly_r                  =>              radius of polygon face in question
1459 //                                              sphere_start    =>              start point of moving sphere
1460 //                                              sphere_end              =>              end point of moving sphere
1461 //                                              sphere_r                        =>              radius of moving sphere
1462 //
1463 //              Output:         returns 0 if no collision is possible, 1 if collision may be possible
1464 //
1465 //              Polygon face is characterized by a center and a radius.  This routine checks whether it is
1466 //              *impossible* for a moving sphere to intersect a fixed polygon face.
1467 int fvi_cull_polyface_sphere(vector *poly_center, float poly_r, vector *sphere_start, vector *sphere_end, float sphere_r)
1468 {
1469         vector closest_point, closest_separation;
1470         float max_sep;
1471
1472         fvi_closest_point_on_line_segment(&closest_point, poly_center, sphere_start, sphere_end);
1473         vm_vec_sub(&closest_separation, &closest_point, poly_center);
1474
1475         max_sep = vm_vec_mag(&closest_separation) + poly_r;
1476
1477         if ( max_sep > sphere_r ) {
1478                 return 0;
1479         } else {
1480                 return 1;
1481         }
1482 }
1483
1484 // ---------------------------------------------------------------------------------------------------------------------
1485 // fvi_closest_line_line
1486 // finds the closest points between two lines
1487 //
1488 void fvi_closest_line_line( vector *x0, vector *vx, vector *y0, vector *vy, float *x_time, float *y_time )
1489 {
1490         vector vx_cross_vy, delta_l, delta_l_cross_vx, delta_l_cross_vy;
1491         float denominator;
1492
1493         vm_vec_sub(&delta_l, y0, x0);
1494
1495         vm_vec_crossprod(&vx_cross_vy, vx, vy);
1496         vm_vec_crossprod(&delta_l_cross_vx, &delta_l, vx);
1497         vm_vec_crossprod(&delta_l_cross_vy, &delta_l, vy);
1498
1499         denominator = vm_vec_mag_squared(&vx_cross_vy);
1500
1501         *x_time = vm_vec_dotprod(&delta_l_cross_vy, &vx_cross_vy) / denominator;
1502         *y_time = vm_vec_dotprod(&delta_l_cross_vx, &vx_cross_vy) / denominator;
1503
1504 //      vector x_result, y_result;
1505 //      vm_vec_scale_add(&x_result, x0, vx, *x_time);
1506 //      vm_vec_scale_add(&y_result, y0, vy, *y_time);
1507 //      *dist_sqr = vm_vec_dist_squared(&x_result, &y_result);
1508
1509 }
1510
1511 // --------------------------------------------------------------------------------------------------------------------
1512 void accurate_square_root( float A, float B, float C, float discriminant, float *root1, float *root2 )
1513 {
1514         float root = fl_sqrt(discriminant);
1515
1516         if (B > 0) {
1517                 *root1 = 2.0f*C / (-B - root);
1518                 *root2 = (-B - root) / (2.0f*A);
1519         } else {        // B < 0
1520                 *root1 = (-B + root) / (2.0f*A);
1521                 *root2 = 2.0f*C / (-B + root);
1522         }
1523 }
1524
1525 // vector mins - minimum extents of bbox
1526 // vector maxs - maximum extents of bbox
1527 // vector start - point in bbox reference frame
1528 // vector box_pt - point in bbox reference frame.
1529 // NOTE: if a coordinate of start is *inside* the bbox, it is *not* moved to surface of bbox
1530 // return: 1 if inside, 0 otherwise.
1531 int project_point_onto_bbox(vector *mins, vector *maxs, vector *start, vector *box_pt)
1532 {
1533         int inside = TRUE;
1534
1535         if (start->xyz.x > maxs->xyz.x) {
1536                 box_pt->xyz.x = maxs->xyz.x;
1537                 inside = FALSE;
1538         } else if (start->xyz.x < mins->xyz.x) {
1539                 box_pt->xyz.x = mins->xyz.x;
1540                 inside = FALSE;
1541         } else {
1542                 box_pt->xyz.x = start->xyz.x;
1543         }
1544
1545         if (start->xyz.y > maxs->xyz.y) {
1546                 box_pt->xyz.y = maxs->xyz.y;
1547                 inside = FALSE;
1548         } else if (start->xyz.y < mins->xyz.y) {
1549                 box_pt->xyz.y = mins->xyz.y;
1550                 inside = FALSE;
1551         } else {
1552                 box_pt->xyz.y = start->xyz.y;
1553         }
1554
1555         if (start->xyz.z > maxs->xyz.z) {
1556                 box_pt->xyz.z = maxs->xyz.z;
1557                 inside = FALSE;
1558         } else if (start->xyz.z < mins->xyz.z) {
1559                 box_pt->xyz.z = mins->xyz.z;
1560                 inside = FALSE;
1561         } else {
1562                 box_pt->xyz.z = start->xyz.z;
1563         }
1564
1565         return inside;
1566 }
1567