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