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 the
6 * source.
7 *
8 */
9
10
11 #include "math/floating.h"
12 #include "math/fvi.h"
13 #include "math/vecmat.h"
14
15
16 #define SMALL_NUM 1E-6
17
18 #define UNINITIALIZED_VALUE -1234567.8f
19 #define WARN_DIST 1.0
20
matrix_determinant_from_vectors(const vec3d * v1,const vec3d * v2,const vec3d * v3)21 static float matrix_determinant_from_vectors(const vec3d *v1, const vec3d *v2, const vec3d *v3)
22 {
23 float ans;
24 ans = v1->xyz.x * v2->xyz.y * v3->xyz.z;
25 ans += v2->xyz.x * v3->xyz.y * v1->xyz.z;
26 ans += v3->xyz.x * v1->xyz.y * v2->xyz.z;
27 ans -= v1->xyz.z * v2->xyz.y * v3->xyz.x;
28 ans -= v2->xyz.z * v3->xyz.y * v1->xyz.x;
29 ans -= v3->xyz.z * v1->xyz.y * v2->xyz.x;
30
31 return ans;
32 }
33
34 /**
35 * Finds the point on each line of closest approach (s and t) (lines need not intersect to get closest point)
36 * Taken from graphic gems I, p. 304
37 *
38 * lines: L1 = P1 + V1s and L2 = P2 + V2t
39 */
fvi_two_lines_in_3space(const vec3d * p1,const vec3d * v1,const vec3d * p2,const vec3d * v2,float * s,float * t)40 void fvi_two_lines_in_3space(const vec3d *p1, const vec3d *v1, const vec3d *p2, const vec3d *v2, float *s, float *t)
41 {
42 vec3d cross,delta;
43 vm_vec_cross(&cross, v1, v2);
44 vm_vec_sub(&delta, p2, p1);
45
46 float denominator = vm_vec_mag_squared(&cross);
47 float num_t, num_s;
48
49 if (denominator > 1e-10) {
50 num_s = matrix_determinant_from_vectors(&delta, v2, &cross);
51 *s = num_s / denominator;
52
53 num_t = matrix_determinant_from_vectors(&delta, v1, &cross);
54 *t = num_t / denominator;
55 } else {
56 // two lines are parallel
57 *s = FLT_MAX;
58 *t = FLT_MAX;
59 }
60 }
61
62 /**
63 * Tells distance from a plain to a point-Bobboau
64 *
65 * @param plane_pnt Plane description, a point
66 * @param plane_norm Plane description, a normal
67 * @param point A point to test
68 */
fvi_point_dist_plane(const vec3d * plane_pnt,const vec3d * plane_norm,const vec3d * point)69 float fvi_point_dist_plane(const vec3d *plane_pnt,const vec3d *plane_norm,
70 const vec3d *point
71 )
72 {
73 float dist,D;
74
75 D = -vm_vec_dot(plane_norm,plane_pnt);
76
77 dist = vm_vec_dot(plane_norm, point) + D;
78 return dist;
79 }
80
81 /**
82 * Finds the point on the specified plane where the infinite ray intersects.
83 *
84 * @param new_pnt A point to test
85 * @param plane_pnt Plane description, a point
86 * @param plane_norm Plane description, a normal
87 * @param ray_origin Ray description, an origin
88 * @param ray_direction Ray description, a direction
89 * @param rad Radius
90 *
91 * Returns scaled-distance plane is from the ray_origin (t), so
92 * P = O + t*D, where P is the point of intersection, O is the ray origin,
93 * and D is the ray's direction. So 0.0 would mean the intersection is
94 * exactly on the ray origin, 1.0 would be on the ray origin plus the ray
95 * direction vector, anything negative would be behind the ray's origin.
96 * If you pass a pointer to the new_pnt, this routine will perform the P=
97 * calculation to calculate the point of intersection and put the result
98 * in *new_pnt.
99 *
100 * If radius is anything other than 0.0, it assumes you want the intersection
101 * point to be that radius from the plane.
102 *
103 * Note that ray_direction doesn't have to be normalized unless you want
104 * the return value to be in units from the ray origin.
105 *
106 * Also note that new_pnt will always be filled in to some valid value,
107 * even if it is a point at infinity.
108 *
109 * If the plane and line are parallel, this will return the largest
110 * negative float number possible.
111 *
112 * So if you want to check a line segment from p0 to p1, you would pass
113 * p0 as ray_origin, p1-p0 as the ray_direction, and there would be an
114 * intersection if the return value is between 0 and 1.
115 */
fvi_ray_plane(vec3d * new_pnt,const vec3d * plane_pnt,const vec3d * plane_norm,const vec3d * ray_origin,const vec3d * ray_direction,float rad)116 float fvi_ray_plane(vec3d *new_pnt,
117 const vec3d *plane_pnt, const vec3d *plane_norm,
118 const vec3d *ray_origin, const vec3d *ray_direction,
119 float rad)
120 {
121 vec3d w;
122 float num,den,t;
123
124 vm_vec_sub(&w,ray_origin,plane_pnt);
125
126 den = -vm_vec_dot(plane_norm,ray_direction);
127 if ( den == 0.0f ) { // Ray & plane are parallel, so there is no intersection
128 if ( new_pnt ) {
129 new_pnt->xyz.x = -FLT_MAX;
130 new_pnt->xyz.y = -FLT_MAX;
131 new_pnt->xyz.z = -FLT_MAX;
132 }
133 return -FLT_MAX;
134 }
135
136 num = vm_vec_dot(plane_norm,&w);
137 num -= rad; //move point out by rad
138
139 t = num / den;
140
141 if ( new_pnt )
142 vm_vec_scale_add(new_pnt,ray_origin,ray_direction,t);
143
144 return t;
145 }
146
147 /**
148 * Find the point on the specified plane where the line intersects
149 *
150 * @param new_pnt The found point on the plane
151 * @param plane_pnt Plane description, a point
152 * @param plane_norm Plane description, a normal
153 * @param p0 The first end of the line
154 * @param p1 The second end of the line
155 * @param rad Radius
156 *
157 * @return true if point found, false if line parallel to plane
158 */
159
fvi_segment_plane(vec3d * new_pnt,const vec3d * plane_pnt,const vec3d * plane_norm,const vec3d * p0,const vec3d * p1,float rad)160 int fvi_segment_plane(vec3d *new_pnt,
161 const vec3d *plane_pnt, const vec3d *plane_norm,
162 const vec3d *p0, const vec3d *p1,float rad)
163 {
164 float t;
165 vec3d d;
166
167 vm_vec_sub( &d, p1, p0 );
168
169 t = fvi_ray_plane(new_pnt,
170 plane_pnt,plane_norm, // Plane description, a point and a normal
171 p0,&d, // Ray description, a point and a direction
172 rad);
173
174 if ( t < 0.0f ) return 0; // intersection lies behind p0
175 if ( t > 1.0f ) return 0; // intersection lies past p1
176
177 return 1; // They intersect!
178 }
179
180 /**
181 * Determine if and where a vector intersects with a sphere
182 *
183 * vector defined by p0,p1
184 * @return 1 if intersects, and fills in intp, else returns 0
185 */
fvi_segment_sphere(vec3d * intp,const vec3d * p0,const vec3d * p1,const vec3d * sphere_pos,float sphere_rad)186 int fvi_segment_sphere(vec3d *intp, const vec3d *p0, const vec3d *p1, const vec3d *sphere_pos, float sphere_rad)
187 {
188 vec3d d,dn,w,closest_point;
189 float mag_d,dist,w_dist,int_dist;
190
191 //this routine could be optimized if it's taking too much time!
192
193 vm_vec_sub(&d,p1,p0);
194 vm_vec_sub(&w,sphere_pos,p0);
195
196 mag_d = vm_vec_mag(&d);
197
198 if (mag_d <= 0.0f) {
199 int_dist = vm_vec_mag(&w);
200 *intp = *p0;
201 return (int_dist<sphere_rad)?1:0;
202 }
203
204 // normalize dn
205 dn.xyz.x = d.xyz.x / mag_d;
206 dn.xyz.y = d.xyz.y / mag_d;
207 dn.xyz.z = d.xyz.z / mag_d;
208
209 w_dist = vm_vec_dot(&dn,&w);
210
211 if (w_dist < -sphere_rad) //moving away from object
212 return 0;
213
214 if (w_dist > mag_d+sphere_rad)
215 return 0; //cannot hit
216
217 vm_vec_scale_add(&closest_point,p0,&dn,w_dist);
218
219 dist = vm_vec_dist(&closest_point,sphere_pos);
220
221 if (dist < sphere_rad) {
222 float dist2,radius2,shorten;
223
224 dist2 = dist*dist;
225 radius2 = sphere_rad*sphere_rad;
226
227 shorten = fl_sqrt(radius2 - dist2);
228
229 int_dist = w_dist-shorten;
230
231 if (int_dist > mag_d || int_dist < 0.0f) {
232 //past one or the other end of vector, which means we're inside
233
234 *intp = *p0; //don't move at all
235 return 1;
236 }
237
238 vm_vec_scale_add(intp,p0,&dn,int_dist); //calc intersection point
239
240 return 1;
241 }
242 else
243 return 0;
244 }
245
246 /**
247 * Determine if a cylinder *may* intersect with a sphere
248 *
249 * cylinder from p0 to p1 with radius cyl_rad
250 * sphere at sphere_pos with radius sphere_rad
251 * @return false if definitely not intersecting, returns true if intersection is possible
252 */
fvi_cylinder_sphere_may_collide(const vec3d * p0,const vec3d * p1,float cyl_rad,const vec3d * sphere_pos,float sphere_rad)253 bool fvi_cylinder_sphere_may_collide(const vec3d* p0, const vec3d* p1, float cyl_rad, const vec3d* sphere_pos, float sphere_rad)
254 {
255 vec3d cyl_rel = *p1 - *p0;
256 vec3d sphere_rel = *sphere_pos - *p0;
257
258 float cyl_len = vm_vec_mag(&cyl_rel);
259 if (cyl_len < SMALL_NUM)
260 // zero length cylinder, weird but ok, just do two sphere intersection
261 return vm_vec_mag_squared(&sphere_rel) <= (cyl_rad + sphere_rad) * (cyl_rad + sphere_rad);
262
263 vec3d cyl_dir = cyl_rel / cyl_len;
264 float intp_dist = vm_vec_dot(&cyl_dir, &sphere_rel);
265 if (intp_dist < -sphere_rad || intp_dist > cyl_len + sphere_rad)
266 // sphere is too far beyond either end of the cylinder
267 return false;
268
269 vec3d sphere_rel_intp = sphere_rel - cyl_dir * intp_dist;
270 return vm_vec_mag_squared(&sphere_rel_intp) <= (cyl_rad + sphere_rad) * (cyl_rad + sphere_rad);
271 }
272
273
274 /**
275 * Determine if and where a ray intersects with a sphere
276 *
277 * vector defined by p0,p1
278 * @return 1 if intersects, and fills in intp. else returns 0
279 */
fvi_ray_sphere(vec3d * intp,const vec3d * p0,const vec3d * p1,const vec3d * sphere_pos,float sphere_rad)280 int fvi_ray_sphere(vec3d *intp, const vec3d *p0, const vec3d *p1, const vec3d *sphere_pos,float sphere_rad)
281 {
282 vec3d d,dn,w,closest_point;
283 float mag_d,dist,w_dist,int_dist;
284
285 //this routine could be optimized if it's taking too much time!
286
287 vm_vec_sub(&d,p1,p0);
288 vm_vec_sub(&w,sphere_pos,p0);
289
290 mag_d = vm_vec_mag(&d);
291
292 if (mag_d <= 0.0f) {
293 int_dist = vm_vec_mag(&w);
294 *intp = *p0;
295 return (int_dist<sphere_rad)?1:0;
296 }
297
298 // normalize dn
299 dn.xyz.x = d.xyz.x / mag_d;
300 dn.xyz.y = d.xyz.y / mag_d;
301 dn.xyz.z = d.xyz.z / mag_d;
302
303 w_dist = vm_vec_dot(&dn,&w);
304
305 if (w_dist < -sphere_rad) //moving away from object
306 return 0;
307
308 vm_vec_scale_add(&closest_point,p0,&dn,w_dist);
309
310 dist = vm_vec_dist(&closest_point,sphere_pos);
311
312 if (dist < sphere_rad) {
313 float dist2, radius2, shorten;
314
315 dist2 = dist*dist;
316 radius2 = sphere_rad*sphere_rad;
317
318 shorten = fl_sqrt(radius2 - dist2);
319
320 int_dist = w_dist-shorten;
321
322 if (int_dist < 0.0f) {
323 //past one or the begining of vector, which means we're inside
324
325 *intp = *p0; //don't move at all
326 return 1;
327 }
328
329 vm_vec_scale_add(intp,p0,&dn,int_dist); //calc intersection point
330
331 return 1;
332 }
333 else
334 return 0;
335 }
336
337 /**
338 * Finds intersection of a ray and an axis-aligned bounding box
339 *
340 * Given a ray with origin at p0, and direction pdir, this function
341 * returns non-zero if that ray intersects an axis-aligned bounding box
342 * from min to max. If there was an intersection, then hitpt will contain
343 * the point where the ray begins inside the box.
344 * Fast ray-box intersection taken from Graphics Gems I, pages 395,736.
345 */
fvi_ray_boundingbox(const vec3d * min,const vec3d * max,const vec3d * p0,const vec3d * pdir,vec3d * hitpt)346 int fvi_ray_boundingbox(const vec3d *min, const vec3d *max, const vec3d * p0, const vec3d *pdir, vec3d *hitpt )
347 {
348 int middle = ((1<<0) | (1<<1) | (1<<2));
349 int i;
350 int which_plane;
351 float maxt[3];
352 float candidate_plane[3];
353
354 for (i = 0; i < 3; i++) {
355 if (p0->a1d[i] < min->a1d[i]) {
356 candidate_plane[i] = min->a1d[i];
357 middle &= ~(1<<i);
358 } else if (p0->a1d[i] > max->a1d[i]) {
359 candidate_plane[i] = max->a1d[i];
360 middle &= ~(1<<i);
361 }
362 }
363
364 // ray origin inside bounding box?
365 // (are all three bits still set?)
366 if (middle == ((1<<0) | (1<<1) | (1<<2))) {
367 *hitpt = *p0;
368 return 1;
369 }
370
371 // calculate T distances to candidate plane
372 for (i = 0; i < 3; i++) {
373 if ( (middle & (1<<i)) || (pdir->a1d[i] == 0.0f) ) {
374 maxt[i] = -1.0f;
375 } else {
376 maxt[i] = (candidate_plane[i] - p0->a1d[i]) / pdir->a1d[i];
377 }
378 }
379
380 // Get largest of the maxt's for final choice of intersection
381 which_plane = 0;
382 for (i = 1; i < 3; i++) {
383 if (maxt[which_plane] < maxt[i]) {
384 which_plane = i;
385 }
386 }
387
388 // check final candidate actually inside box
389 if (maxt[which_plane] < 0.0f) {
390 return 0;
391 }
392
393 for (i = 0; i < 3; i++) {
394 if (which_plane == i) {
395 hitpt->a1d[i] = candidate_plane[i];
396 } else {
397 hitpt->a1d[i] = (maxt[which_plane] * pdir->a1d[i]) + p0->a1d[i];
398
399 if ( (hitpt->a1d[i] < min->a1d[i]) || (hitpt->a1d[i] > max->a1d[i]) ) {
400 return 0;
401 }
402 }
403 }
404
405 return 1;
406 }
407
408 /**
409 * Given largest componant of normal, return i & j
410 * If largest componant is negative, swap i & j
411 */
412 static int ij_table[3][2] = {
413 {2,1}, //pos x biggest
414 {0,2}, //pos y biggest
415 {1,0}, //pos z biggest
416 };
417
418 /**
419 * See if a point in inside a face by projecting into 2d. Also
420 * finds uv's if uvls is not NULL.
421 *
422 * Returns 0 if point isn't on face, non-zero otherwise.
423 *
424 * From Graphics Gems I, "An efficient Ray-Polygon intersection", p390
425 *
426 * @param checkp The point to check
427 * @param nv How many verts in the poly
428 * @param verts The vertives for the polygon
429 * @param norm1 The polygon's normal
430 * @param u_out If not null and v_out not null and uvls not_null and point is on face, the uv's of where it hit
431 * @param 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
432 * @param uvls A list of uv pairs for each vertex
433 *
434 * This replaces the old check_point_to_face & find_hitpoint_uv
435 * WARNING!! In Gems, they use the code "if (u1==0)" in this function.
436 * I have found several cases where this will not detect collisions it should.
437 * I found two solutions:
438 * 1. Declare the 'beta' variable to be a double.
439 * 2. Instead of using 'if (u1==0)', compare it to a small value.
440 * I chose #2 because I would rather have our code work with all floats
441 * and never need doubles. -JAS Aug22,1997
442 */
443 #define delta 0.0001f
fvi_point_face(const vec3d * checkp,int nv,vec3d const * const * verts,const vec3d * norm1,float * u_out,float * v_out,const uv_pair * uvls)444 int fvi_point_face(const vec3d *checkp, int nv, vec3d const *const *verts, const vec3d * norm1, float *u_out,float *v_out, const uv_pair * uvls )
445 {
446 const float *norm;
447 const float *P;
448 vec3d t;
449 int i0, i1,i2;
450
451 norm = (float *)norm1;
452
453 //project polygon onto plane by finding largest component of normal
454 t.xyz.x = fl_abs(norm[0]);
455 t.xyz.y = fl_abs(norm[1]);
456 t.xyz.z = fl_abs(norm[2]);
457
458 if (t.xyz.x > t.xyz.y) if (t.xyz.x > t.xyz.z) i0=0; else i0=2;
459 else if (t.xyz.y > t.xyz.z) i0=1; else i0=2;
460
461 if (norm[i0] > 0.0f) {
462 i1 = ij_table[i0][0];
463 i2 = ij_table[i0][1];
464 }
465 else {
466 i1 = ij_table[i0][1];
467 i2 = ij_table[i0][0];
468 }
469
470 // Have i0, i1, i2
471 P = (float *)checkp;
472
473 float u0, u1, u2, v0, v1, v2, alpha = UNINITIALIZED_VALUE, gamma;
474 float beta;
475
476 int inter=0, i = 2;
477
478 u0 = P[i1] - verts[0]->a1d[i1];
479 v0 = P[i2] - verts[0]->a1d[i2];
480
481 do {
482
483 u1 = verts[i-1]->a1d[i1] - verts[0]->a1d[i1];
484 u2 = verts[i ]->a1d[i1] - verts[0]->a1d[i1];
485
486 v1 = verts[i-1]->a1d[i2] - verts[0]->a1d[i2];
487 v2 = verts[i ]->a1d[i2] - verts[0]->a1d[i2];
488
489
490 if ( (u1 >-delta) && (u1<delta) ) {
491 beta = u0 / u2;
492 if ((beta >=0.0f) && (beta<=1.0f)) {
493 alpha = (v0 - beta*v2)/v1;
494 inter = ((alpha>=0.0f)&&(alpha+beta<=1.0f));
495 }
496 } else {
497
498 beta = (v0*u1 - u0*v1) / (v2*u1 - u2*v1);
499 if ((beta >=0.0f) && (beta<=1.0f)) {
500 Assert(beta != UNINITIALIZED_VALUE);
501 alpha = (u0 - beta*u2)/u1;
502 inter = ((alpha>=0.0f)&&(alpha+beta<=1.0f));
503 }
504 }
505
506 } while ((!inter) && (++i < nv) );
507
508 if ( inter && uvls && u_out && v_out ) {
509 gamma = 1.0f - (alpha+beta);
510 *u_out = gamma * uvls[0].u + alpha*uvls[i-1].u + beta*uvls[i].u;
511 *v_out = gamma * uvls[0].v + alpha*uvls[i-1].v + beta*uvls[i].v;
512 }
513
514 return inter;
515 }
516
517 // ****************************************************************************
518 //
519 // SPHERE FACE INTERSECTION CODE
520 //
521 // ****************************************************************************
522
523 static int check_sphere_point(const vec3d *point, const vec3d *sphere_start, const vec3d *sphere_vel, float radius, float *collide_time );
524
525 /**
526 * Returns whether a sphere hits a given plane in the time [0,1]
527 * If two collisions occur, returns earliest legal time
528 * returns the intersection point on the plane
529 *
530 * @param intersect_point position on plane where sphere makes first contact [if hit_time in range 0-1]
531 * @param sphere_center_start initial sphere center
532 * @param sphere_velocity initial sphere velocity
533 * @param sphere_radius radius of sphere
534 * @param plane_normal normal to the colliding plane
535 * @param plane_point point in the colliding plane
536 * @param hit_time time surface of sphere first hits plane
537 * @param crossing_time time for sphere to cross plane (first to last contact)
538 *
539 * @return 1 if sphere may be in contact with plane in time range [0-1], 0 otherwise
540 */
fvi_sphere_plane(vec3d * intersect_point,const vec3d * sphere_center_start,const vec3d * sphere_velocity,float sphere_radius,const vec3d * plane_normal,const vec3d * plane_point,float * hit_time,float * crossing_time)541 int fvi_sphere_plane(vec3d *intersect_point, const vec3d *sphere_center_start, const vec3d *sphere_velocity, float sphere_radius,
542 const vec3d *plane_normal, const vec3d *plane_point, float *hit_time, float *crossing_time)
543 {
544 float D, xs0_dot_norm, vs_dot_norm;
545 float t1, t2;
546
547 // find the time and position of the ray-plane intersection
548 D = -vm_vec_dot( plane_normal, plane_point );
549 xs0_dot_norm = vm_vec_dot( plane_normal, sphere_center_start );
550 vs_dot_norm = vm_vec_dot( plane_normal, sphere_velocity);
551
552 // protect against divide by zero
553 if (fl_abs(vs_dot_norm) > 1e-30) {
554 t1 = (-D - xs0_dot_norm + sphere_radius) / vs_dot_norm;
555 t2 = (-D - xs0_dot_norm - sphere_radius) / vs_dot_norm;
556 } else {
557 return 0;
558 }
559
560 // sort t1 < t2
561 if (t2 < t1) {
562 float temp = t1;
563 t1 = t2;
564 t2 = temp;
565 }
566
567 *hit_time = t1;
568
569 // find hit pos if t1 in range 0-1
570 if (t1 > 0 && t1 < 1) {
571 vec3d v_temp;
572 vm_vec_scale_add( &v_temp, sphere_center_start, sphere_velocity, t1 );
573 vm_project_point_onto_plane( intersect_point, &v_temp, plane_normal, plane_point );
574 }
575
576 // get time to cross
577 *crossing_time = t2 - t1;
578
579 return ( (t1 < 1) && (t2 > 0) );
580 }
581
582 /**
583 * Returns whether a sphere hits and edge for the case the edge is perpendicular to sphere_velocity
584 * If two collisions occur, returns the earliest legal time
585 * returns the intersection point on the edge
586 *
587 * @param intersect_point position on plane where sphere makes first contact [RESULT]
588 * @param sphere_center_start initial sphere center
589 * @param sphere_velocity initial sphere velocity
590 * @param sphere_radius radius of sphere
591 * @param edge_point1 first edge point
592 * @param edge_point2 second edge point
593 * @param collide_time actual time of the collision
594 */
fvi_sphere_perp_edge(vec3d * intersect_point,const vec3d * sphere_center_start,const vec3d * sphere_velocity,float sphere_radius,vec3d * edge_point1,vec3d * edge_point2,float * collide_time)595 int fvi_sphere_perp_edge(vec3d *intersect_point, const vec3d *sphere_center_start, const vec3d *sphere_velocity,
596 float sphere_radius, vec3d *edge_point1, vec3d *edge_point2, float *collide_time)
597 {
598 // find the intersection in the plane normal to sphere velocity and edge velocity
599 // choose a plane point V0 (first vertex of the edge)
600 // project vectors and points into the plane
601 // find the projection of the intersection and see if it lies on the edge
602
603 vec3d edge_velocity;
604 vec3d V0, V1;
605 vec3d Xe_proj, Xs_proj;
606
607 V0 = *edge_point1;
608 V1 = *edge_point2;
609 vm_vec_sub(&edge_velocity, &V1, &V0);
610
611 // define a set of local unit vectors
612 vec3d x_hat, y_hat, z_hat;
613 float max_edge_parameter;
614
615 vm_vec_copy_normalize( &x_hat, &edge_velocity );
616 vm_vec_copy_normalize( &y_hat, sphere_velocity );
617 vm_vec_cross( &z_hat, &x_hat, &y_hat );
618 max_edge_parameter = vm_vec_mag( &edge_velocity );
619
620 vec3d temp;
621 // next two temp should be same as starting velocities
622 vm_vec_projection_onto_plane(&temp, sphere_velocity, &z_hat);
623 Assert ( !vm_vec_cmp(&temp, sphere_velocity) );
624 vm_vec_projection_onto_plane(&temp, &edge_velocity, &z_hat);
625 Assert ( !vm_vec_cmp(&temp, &edge_velocity) );
626
627 // should return V0
628 vm_project_point_onto_plane(&Xe_proj, &V0, &z_hat, &V0);
629 Assert ( !vm_vec_cmp(&Xe_proj, &V0) );
630
631 vm_project_point_onto_plane(&Xs_proj, sphere_center_start, &z_hat, &V0);
632
633 vec3d plane_coord;
634 plane_coord.xyz.x = vm_vec_dot(&Xs_proj, &x_hat);
635 plane_coord.xyz.y = vm_vec_dot(&Xe_proj, &y_hat);
636 plane_coord.xyz.z = vm_vec_dot(&Xe_proj, &z_hat);
637
638 // determime the position on the edge line
639 vm_vec_copy_scale( intersect_point, &x_hat, plane_coord.xyz.x );
640 vm_vec_scale_add2( intersect_point, &y_hat, plane_coord.xyz.y );
641 vm_vec_scale_add2( intersect_point, &z_hat, plane_coord.xyz.z );
642
643 // check if point is actually on edge
644 float edge_parameter;
645 vec3d temp_vec;
646
647 vm_vec_sub( &temp_vec, intersect_point, &V0 );
648 edge_parameter = vm_vec_dot( &temp_vec, &x_hat );
649
650 if ( edge_parameter < 0 || edge_parameter > max_edge_parameter ) {
651 return 0;
652 }
653
654 return ( check_sphere_point(intersect_point, sphere_center_start, sphere_velocity, sphere_radius, collide_time) );
655 }
656
657
658 /**
659 * Determines whether and where a moving sphere hits a point
660 *
661 * @param point point sphere collides with
662 * @param sphere_start initial sphere center
663 * @param sphere_vel velocity of sphere
664 * @param radius radius of sphere
665 * @param collide_time time of first collision with t >= 0
666 */
check_sphere_point(const vec3d * point,const vec3d * sphere_start,const vec3d * sphere_vel,float radius,float * collide_time)667 static int check_sphere_point(const vec3d *point, const vec3d *sphere_start, const vec3d *sphere_vel, float radius, float *collide_time )
668 {
669 vec3d delta_x;
670 float delta_x_sqr, vs_sqr, delta_x_dot_vs;
671
672 vm_vec_sub( &delta_x, sphere_start, point );
673 delta_x_sqr = vm_vec_mag_squared( &delta_x );
674 vs_sqr = vm_vec_mag_squared( sphere_vel );
675 delta_x_dot_vs = vm_vec_dot( &delta_x, sphere_vel );
676
677 float discriminant = delta_x_dot_vs*delta_x_dot_vs - vs_sqr*(delta_x_sqr - radius*radius);
678 if (discriminant < 0) {
679 return 0;
680 }
681
682 float radical, time1, time2;
683 radical = fl_sqrt(discriminant);
684 time1 = (-delta_x_dot_vs + radical) / vs_sqr;
685 time2 = (-delta_x_dot_vs - radical) / vs_sqr;
686
687 if (time1 > time2) {
688 float temp = time1;
689 time1 = time2;
690 time2 = temp;
691 }
692
693 if (time1 >= 0 && time1 <= 1.0) {
694 *collide_time = time1;
695 return 1;
696 }
697
698 if (time2 >= 0 && time2 <= 1.0) {
699 *collide_time = time2;
700 return 1;
701 }
702
703 return 0;
704 }
705
706 /**
707 * Given a polygon vertex list and a moving sphere, find the first contact the sphere makes with the edge, if any
708 *
709 * @param hit_point point on edge
710 * @param xs0 starting point for sphere
711 * @param vs sphere velocity
712 * @param Rs sphere radius
713 * @param nv number of vertices
714 * @param verts vertices making up polygon edges
715 * @param hit_time time the sphere hits an edge
716 *
717 * @return 1 if sphere hits polyedge, 0 if sphere misses
718 */
fvi_polyedge_sphereline(vec3d * hit_point,const vec3d * xs0,const vec3d * vs,float Rs,int nv,vec3d const * const * verts,float * hit_time)719 int fvi_polyedge_sphereline(vec3d *hit_point, const vec3d *xs0, const vec3d *vs, float Rs, int nv, vec3d const *const *verts, float *hit_time)
720 {
721 int i;
722 vec3d v0, v1;
723 vec3d ve; // edge velocity
724 float best_sphere_time; // earliest time sphere hits edge
725 vec3d delta_x;
726 float delta_x_dot_ve, delta_x_dot_vs, ve_dot_vs, ve_sqr, vs_sqr, delta_x_sqr;
727 vec3d temp_edge_hit, temp_sphere_hit;
728 float t_sphere_hit = 0.0f;
729 float A, B, C, temp, discriminant, invA;
730 float root, root1, root2;
731 float Rs2 = (Rs * Rs);
732 float Rs_point2 = (0.2f * Rs);
733
734 best_sphere_time = FLT_MAX;
735
736 vs_sqr = vm_vec_mag_squared(vs);
737
738 for (i = 0; i < nv; i++) {
739 // Get vertices of edge to check
740 v0 = *verts[i];
741 if (i+1 != nv) {
742 v1 = *verts[i+1];
743 } else {
744 v1 = *verts[0];
745 }
746
747 // Calculate edge velocity.
748 // Position along the edge is given by: P_edge = v0 + ve*t, where t is in the range [0,1]
749 vm_vec_sub(&ve, &v1, &v0);
750
751 // First find the closest intersection between the edge_line and the sphere_line.
752 vm_vec_sub(&delta_x, xs0, &v0);
753
754 delta_x_dot_ve = vm_vec_dot(&delta_x, &ve);
755 delta_x_dot_vs = vm_vec_dot(&delta_x, vs);
756 delta_x_sqr = vm_vec_mag_squared(&delta_x);
757 ve_dot_vs = vm_vec_dot(&ve, vs);
758 ve_sqr = vm_vec_mag_squared(&ve);
759
760 /*
761 * Solve for sphere time
762 *
763 * This code uses the following variation of the quadratic equation:
764 * A*x*x + 2*B*x + C = 0
765 *
766 * Solving for x yields ...
767 *
768 * -B +- sqrt(B*B - A*C)
769 * x = ---------------------
770 * A
771 */
772
773 A = ve_dot_vs*ve_dot_vs - ve_sqr*vs_sqr;
774 B = delta_x_dot_ve*ve_dot_vs - delta_x_dot_vs*ve_sqr;
775 C = delta_x_dot_ve*delta_x_dot_ve + Rs2*ve_sqr - delta_x_sqr*ve_sqr;
776
777 discriminant = B*B - A*C;
778 if (discriminant > 0.0f) {
779 invA = 1.0f / A;
780 root = sqrt(discriminant);
781 root1 = (-B + root) * invA;
782 root2 = (-B - root) * invA;
783
784 // sort root1 and root2
785 if (root2 < root1) {
786 temp = root1;
787 root1 = root2;
788 root2 = temp;
789 }
790
791 if ( (root1 < 0.0f) && (root1 >= -0.05f) )
792 root1 = 0.000001f;
793
794 // look only at first hit
795 if ( (root1 <= 1.0f) && (root1 >= 0.0f) ) {
796 t_sphere_hit = root1;
797 } else {
798 goto TryVertex;
799 }
800 } else {
801 // discriminant negative, so no hit possible
802 continue;
803 }
804
805 // check if best time with this edge is less than best so far
806 if (t_sphere_hit >= best_sphere_time)
807 continue;
808
809 vm_vec_scale_add( &temp_sphere_hit, xs0, vs, t_sphere_hit );
810
811 // solve for edge time
812 A *= ve_sqr;
813 B = ve_sqr * (delta_x_dot_ve*vs_sqr - delta_x_dot_vs*ve_dot_vs);
814 C = 2.0f*delta_x_dot_ve*delta_x_dot_vs*ve_dot_vs + Rs2*ve_dot_vs*ve_dot_vs
815 - delta_x_sqr*ve_dot_vs*ve_dot_vs - delta_x_dot_ve*delta_x_dot_ve*vs_sqr;
816
817 discriminant = B*B - A*C;
818 invA = 1.0f / A;
819
820 // guard against nearly perpendicular sphere edge velocities
821 if (discriminant < 0.0f)
822 root = 0.0f;
823 else
824 root = fl_sqrt(discriminant);
825
826 root1 = (-B + root) * invA;
827 root2 = (-B - root) * invA;
828
829 // given sphere position, find which edge time (position) allows a valid solution
830 if ( (root1 <= 1.0f) && (root1 >= 0.0f) ) {
831 // try edge root1
832 vm_vec_scale_add( &temp_edge_hit, &v0, &ve, root1 );
833 float q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
834 if ( fl_abs(q - Rs2) < Rs_point2 ) { // error less than 0.1m absolute (2*delta*Radius)
835 goto Hit;
836 }
837 }
838
839 if ( (root2 <= 1.0f) && (root2 >= 0.0f) ) {
840 // try edge root2
841 vm_vec_scale_add( &temp_edge_hit, &v0, &ve, root2 );
842 float q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
843 if ( fl_abs(q - Rs2) < Rs_point2 ) { // error less than 0.1m absolute
844 goto Hit;
845 }
846 } else {
847 // both root1 and root2 out of range so we have to check vertices
848 goto TryVertex;
849 }
850
851 TryVertex:
852 // try V0
853 A = vs_sqr;
854 B = delta_x_dot_vs;
855 C = delta_x_sqr - Rs2;
856 int v0_hit;
857 float sphere_v0, sphere_v1;
858
859 v0_hit = 0;
860 sphere_v0 = UNINITIALIZED_VALUE;
861 sphere_v1 = UNINITIALIZED_VALUE;
862
863 invA = 1.0f / A;
864 discriminant = B*B - A*C;
865 if (discriminant > 0.0f) {
866 root = fl_sqrt(discriminant);
867 root1 = (-B + root) * invA;
868 root2 = (-B - root) * invA;
869
870 if (root1 > root2) {
871 temp = root1;
872 root1 = root2;
873 root2 = temp;
874 }
875
876 // look only at the first hit (ignore negative first hit)
877 if ( (root1 < 1.0f) && (root1 > 0.0f) ) {
878 v0_hit = 1;
879 sphere_v0 = root1;
880 }
881 }
882
883 // try V1
884 vm_vec_sub( &delta_x, xs0, &v1 );
885 delta_x_sqr = vm_vec_mag_squared( &delta_x );
886 delta_x_dot_vs = vm_vec_dot( &delta_x, vs );
887 int v1_hit;
888
889 B = delta_x_dot_vs;
890 C = delta_x_sqr - Rs2;
891
892 v1_hit = 0;
893 discriminant = B*B - A*C;
894 if (discriminant > 0.0f) {
895 root = fl_sqrt(discriminant);
896 root1 = (-B + root) * invA;
897 root2 = (-B - root) * invA;
898
899 if (root1 > root2) {
900 temp = root1;
901 root1 = root2;
902 root2 = temp;
903 }
904
905 // look only at the first hit (ignore negative first hit)
906 if ( (root1 < 1.0f) && (root1 > 0.0f) ) {
907 v1_hit = 1;
908 sphere_v1 = root1;
909 }
910 }
911
912 // set hitpoint to closest vetex hit, if any
913 if ( v0_hit ) {
914 Assert(sphere_v0 != UNINITIALIZED_VALUE);
915 t_sphere_hit = sphere_v0;
916 temp_edge_hit = v0;
917
918 if (v1_hit) {
919 Assert( sphere_v1 != UNINITIALIZED_VALUE );
920 if (sphere_v1 < sphere_v0) {
921 t_sphere_hit = sphere_v1;
922 temp_edge_hit = v1;
923 }
924 }
925 } else if ( v1_hit ) {
926 Assert(sphere_v1 != UNINITIALIZED_VALUE);
927 t_sphere_hit = sphere_v1;
928 temp_edge_hit = v1;
929 } else {
930 continue;
931 }
932
933 //vm_vec_scale_add( &temp_sphere_hit, xs0, vs, t_sphere_hit );
934 //q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
935
936
937 Hit:
938 if (t_sphere_hit < best_sphere_time) {
939 best_sphere_time = t_sphere_hit;
940 *hit_point = temp_edge_hit;
941 }
942 } // end edge loop
943
944 if (best_sphere_time <= 1.0f) {
945 *hit_time = best_sphere_time;
946 return 1;
947 } else {
948 return 0;
949 }
950 }
951
952 /**
953 * Finds the closest point on a line to a given fixed point
954 *
955 * @param closest_point the closest point on the line
956 * @param fixed_point the fixed point
957 * @param line_point1 first point on the line
958 * @param line_point2 second point on the line
959 */
fvi_closest_point_on_line_segment(vec3d * closest_point,const vec3d * fixed_point,const vec3d * line_point1,const vec3d * line_point2)960 void fvi_closest_point_on_line_segment(vec3d *closest_point, const vec3d *fixed_point, const vec3d *line_point1, const vec3d *line_point2)
961 {
962 vec3d delta_x, line_velocity;
963 float t;
964
965 vm_vec_sub(&line_velocity, line_point2, line_point1);
966 vm_vec_sub(&delta_x, line_point1, fixed_point);
967 t = -vm_vec_dot(&delta_x, &line_velocity) / vm_vec_mag_squared(&line_velocity);
968
969 // Constrain t to be in range [0,1]
970 if (t < 0) {
971 t = 0.0f;
972 } else if (t > 1) {
973 t = 1.0f;
974 }
975
976 vm_vec_scale_add(closest_point, line_point1, &line_velocity, t);
977 }
978
979 /**
980 * checks whether two spheres hit given initial and starting positions and radii
981 * does not check whether sphere are already touching.
982 *
983 * @param x_p0 polymodel sphere, start point
984 * @param x_p1 polymodel sphere, end point
985 * @param x_s0 other sphere, start point
986 * @param x_s1 other sphere, end point
987 * @param radius_p radius of polymodel sphere
988 * @param radius_s radius of other sphere
989 * @param t1 time pointer 1
990 * @param t2 time pointer 2
991 *
992 * @return 1 if spheres overlap, 0 otherwise
993 */
fvi_check_sphere_sphere(const vec3d * x_p0,const vec3d * x_p1,const vec3d * x_s0,const vec3d * x_s1,float radius_p,float radius_s,float * t1,float * t2)994 int fvi_check_sphere_sphere(const vec3d *x_p0, const vec3d *x_p1, const vec3d *x_s0, const vec3d *x_s1, float radius_p, float radius_s, float *t1, float *t2)
995 {
996 vec3d delta_x, delta_v;
997 float discriminant, separation, delta_x_dot_delta_v, delta_v_sqr, delta_x_sqr;
998 float time1, time2;
999
1000 // Check that there are either 0 or 2 pointers to time
1001 Assert( (!(t1) && !(t2)) || (t1 && t2) );
1002
1003 vm_vec_sub(&delta_x, x_s0, x_p0);
1004 delta_x_sqr = vm_vec_mag_squared(&delta_x);
1005 separation = radius_p + radius_s;
1006
1007 // Check if already touching
1008 if (delta_x_sqr < separation*separation) {
1009 if ( !t1 ) {
1010 return 1;
1011 }
1012 }
1013
1014 // Find delta_v (in polymodel sphere frame of ref)
1015 // Note: x_p0 and x_p1 will be same for fixed polymodel
1016 vm_vec_sub(&delta_v, x_s1, x_s0);
1017 vm_vec_add2(&delta_v, x_p0);
1018 vm_vec_sub2(&delta_v, x_p1);
1019
1020 delta_x_dot_delta_v = vm_vec_dot(&delta_x, &delta_v);
1021 delta_v_sqr = vm_vec_mag_squared(&delta_v);
1022
1023 discriminant = delta_x_dot_delta_v*delta_x_dot_delta_v - delta_v_sqr*(delta_x_sqr - separation*separation);
1024
1025 if (discriminant < 0) {
1026 return 0;
1027 }
1028
1029 float radical = fl_sqrt(discriminant);
1030
1031 time1 = (-delta_x_dot_delta_v + radical) / delta_v_sqr;
1032 time2 = (-delta_x_dot_delta_v - radical) / delta_v_sqr;
1033
1034 // sort t1 < t2
1035 float temp;
1036 if (time1 > time2) {
1037 temp = time1;
1038 time1 = time2;
1039 time2 = temp;
1040 }
1041
1042 if ( t1 ) {
1043 *t1 = time1;
1044 *t2 = time2;
1045 }
1046
1047 // check whether the range from t1 to t2 intersects [0,1]
1048 if (time1 > 1 || time2 < 0) {
1049 return 0;
1050 } else {
1051 return 1;
1052 }
1053 }
1054
1055 /**
1056 * Culls polyfaces which moving sphere can not intersect
1057 *
1058 * Polygon face is characterized by a center and a radius. This routine checks whether it is
1059 * *impossible* for a moving sphere to intersect a fixed polygon face.
1060 *
1061 * @param poly_center center of polygon face to test
1062 * @param poly_r radius of polygon face in question
1063 * @param sphere_start start point of moving sphere
1064 * @param sphere_end end point of moving sphere
1065 * @param sphere_r radius of moving sphere
1066 *
1067 * @return 0 if no collision is possible, 1 if collision may be possible
1068 */
fvi_cull_polyface_sphere(const vec3d * poly_center,float poly_r,const vec3d * sphere_start,const vec3d * sphere_end,float sphere_r)1069 int fvi_cull_polyface_sphere(const vec3d *poly_center, float poly_r, const vec3d *sphere_start, const vec3d *sphere_end, float sphere_r)
1070 {
1071 vec3d closest_point, closest_separation;
1072 float max_sep;
1073
1074 fvi_closest_point_on_line_segment(&closest_point, poly_center, sphere_start, sphere_end);
1075 vm_vec_sub(&closest_separation, &closest_point, poly_center);
1076
1077 max_sep = vm_vec_mag(&closest_separation) + poly_r;
1078
1079 if ( max_sep > sphere_r ) {
1080 return 0;
1081 } else {
1082 return 1;
1083 }
1084 }
1085
1086 /**
1087 * Finds the closest points between two lines
1088 */
fvi_closest_line_line(const vec3d * x0,const vec3d * vx,const vec3d * y0,const vec3d * vy,float * x_time,float * y_time)1089 void fvi_closest_line_line(const vec3d *x0, const vec3d *vx, const vec3d *y0, const vec3d *vy, float *x_time, float *y_time )
1090 {
1091 vec3d vx_cross_vy, delta_l, delta_l_cross_vx, delta_l_cross_vy;
1092 float denominator;
1093
1094 vm_vec_sub(&delta_l, y0, x0);
1095
1096 vm_vec_cross(&vx_cross_vy, vx, vy);
1097 vm_vec_cross(&delta_l_cross_vx, &delta_l, vx);
1098 vm_vec_cross(&delta_l_cross_vy, &delta_l, vy);
1099
1100 denominator = vm_vec_mag_squared(&vx_cross_vy);
1101
1102 *x_time = vm_vec_dot(&delta_l_cross_vy, &vx_cross_vy) / denominator;
1103 *y_time = vm_vec_dot(&delta_l_cross_vx, &vx_cross_vy) / denominator;
1104 }
1105
1106 /**
1107 * Project point on bounding box
1108 *
1109 * NOTE: if a coordinate of start is *inside* the bbox, it is *not* moved to surface of bbox
1110 *
1111 * @param mins minimum extents of bbox
1112 * @param maxs maximum extents of bbox
1113 * @param start point in bbox reference frame
1114 * @param box_pt point in bbox reference frame.
1115 *
1116 * @return 1 if inside, 0 otherwise.
1117 */
project_point_onto_bbox(const vec3d * mins,const vec3d * maxs,const vec3d * start,vec3d * box_pt)1118 int project_point_onto_bbox(const vec3d *mins, const vec3d *maxs, const vec3d *start, vec3d *box_pt)
1119 {
1120 int inside = TRUE;
1121
1122 if (start->xyz.x > maxs->xyz.x) {
1123 box_pt->xyz.x = maxs->xyz.x;
1124 inside = FALSE;
1125 } else if (start->xyz.x < mins->xyz.x) {
1126 box_pt->xyz.x = mins->xyz.x;
1127 inside = FALSE;
1128 } else {
1129 box_pt->xyz.x = start->xyz.x;
1130 }
1131
1132 if (start->xyz.y > maxs->xyz.y) {
1133 box_pt->xyz.y = maxs->xyz.y;
1134 inside = FALSE;
1135 } else if (start->xyz.y < mins->xyz.y) {
1136 box_pt->xyz.y = mins->xyz.y;
1137 inside = FALSE;
1138 } else {
1139 box_pt->xyz.y = start->xyz.y;
1140 }
1141
1142 if (start->xyz.z > maxs->xyz.z) {
1143 box_pt->xyz.z = maxs->xyz.z;
1144 inside = FALSE;
1145 } else if (start->xyz.z < mins->xyz.z) {
1146 box_pt->xyz.z = mins->xyz.z;
1147 inside = FALSE;
1148 } else {
1149 box_pt->xyz.z = start->xyz.z;
1150 }
1151
1152 return inside;
1153 }
1154