src/math/fvi.cpp

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