mirror of
https://github.com/scp-fs2open/fs2open.github.com.git
synced 2026-11-08 07:28:22 -10:00
1154 lines
32 KiB
C++
1154 lines
32 KiB
C++
/*
|
|
* Copyright (C) Volition, Inc. 1999. All rights reserved.
|
|
*
|
|
* All source code herein is the property of Volition, Inc. You may not sell
|
|
* or otherwise commercially exploit the source or things you created based on the
|
|
* source.
|
|
*
|
|
*/
|
|
|
|
|
|
#include "math/floating.h"
|
|
#include "math/fvi.h"
|
|
#include "math/vecmat.h"
|
|
|
|
|
|
#define SMALL_NUM 1E-6
|
|
|
|
#define UNINITIALIZED_VALUE -1234567.8f
|
|
#define WARN_DIST 1.0
|
|
|
|
static float matrix_determinant_from_vectors(const vec3d *v1, const vec3d *v2, const vec3d *v3)
|
|
{
|
|
float ans;
|
|
ans = v1->xyz.x * v2->xyz.y * v3->xyz.z;
|
|
ans += v2->xyz.x * v3->xyz.y * v1->xyz.z;
|
|
ans += v3->xyz.x * v1->xyz.y * v2->xyz.z;
|
|
ans -= v1->xyz.z * v2->xyz.y * v3->xyz.x;
|
|
ans -= v2->xyz.z * v3->xyz.y * v1->xyz.x;
|
|
ans -= v3->xyz.z * v1->xyz.y * v2->xyz.x;
|
|
|
|
return ans;
|
|
}
|
|
|
|
/**
|
|
* Finds the point on each line of closest approach (s and t) (lines need not intersect to get closest point)
|
|
* Taken from graphic gems I, p. 304
|
|
*
|
|
* lines: L1 = P1 + V1s and L2 = P2 + V2t
|
|
*/
|
|
void fvi_two_lines_in_3space(const vec3d *p1, const vec3d *v1, const vec3d *p2, const vec3d *v2, float *s, float *t)
|
|
{
|
|
vec3d cross,delta;
|
|
vm_vec_cross(&cross, v1, v2);
|
|
vm_vec_sub(&delta, p2, p1);
|
|
|
|
float denominator = vm_vec_mag_squared(&cross);
|
|
float num_t, num_s;
|
|
|
|
if (denominator > 1e-10) {
|
|
num_s = matrix_determinant_from_vectors(&delta, v2, &cross);
|
|
*s = num_s / denominator;
|
|
|
|
num_t = matrix_determinant_from_vectors(&delta, v1, &cross);
|
|
*t = num_t / denominator;
|
|
} else {
|
|
// two lines are parallel
|
|
*s = FLT_MAX;
|
|
*t = FLT_MAX;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Tells distance from a plain to a point-Bobboau
|
|
*
|
|
* @param plane_pnt Plane description, a point
|
|
* @param plane_norm Plane description, a normal
|
|
* @param point A point to test
|
|
*/
|
|
float fvi_point_dist_plane(const vec3d *plane_pnt,const vec3d *plane_norm,
|
|
const vec3d *point
|
|
)
|
|
{
|
|
float dist,D;
|
|
|
|
D = -vm_vec_dot(plane_norm,plane_pnt);
|
|
|
|
dist = vm_vec_dot(plane_norm, point) + D;
|
|
return dist;
|
|
}
|
|
|
|
/**
|
|
* Finds the point on the specified plane where the infinite ray intersects.
|
|
*
|
|
* @param new_pnt A point to test
|
|
* @param plane_pnt Plane description, a point
|
|
* @param plane_norm Plane description, a normal
|
|
* @param ray_origin Ray description, an origin
|
|
* @param ray_direction Ray description, a direction
|
|
* @param rad Radius
|
|
*
|
|
* Returns scaled-distance plane is from the ray_origin (t), so
|
|
* P = O + t*D, where P is the point of intersection, O is the ray origin,
|
|
* and D is the ray's direction. So 0.0 would mean the intersection is
|
|
* exactly on the ray origin, 1.0 would be on the ray origin plus the ray
|
|
* direction vector, anything negative would be behind the ray's origin.
|
|
* If you pass a pointer to the new_pnt, this routine will perform the P=
|
|
* calculation to calculate the point of intersection and put the result
|
|
* in *new_pnt.
|
|
*
|
|
* If radius is anything other than 0.0, it assumes you want the intersection
|
|
* point to be that radius from the plane.
|
|
*
|
|
* Note that ray_direction doesn't have to be normalized unless you want
|
|
* the return value to be in units from the ray origin.
|
|
*
|
|
* Also note that new_pnt will always be filled in to some valid value,
|
|
* even if it is a point at infinity.
|
|
*
|
|
* If the plane and line are parallel, this will return the largest
|
|
* negative float number possible.
|
|
*
|
|
* So if you want to check a line segment from p0 to p1, you would pass
|
|
* p0 as ray_origin, p1-p0 as the ray_direction, and there would be an
|
|
* intersection if the return value is between 0 and 1.
|
|
*/
|
|
float fvi_ray_plane(vec3d *new_pnt,
|
|
const vec3d *plane_pnt, const vec3d *plane_norm,
|
|
const vec3d *ray_origin, const vec3d *ray_direction,
|
|
float rad)
|
|
{
|
|
vec3d w;
|
|
float num,den,t;
|
|
|
|
vm_vec_sub(&w,ray_origin,plane_pnt);
|
|
|
|
den = -vm_vec_dot(plane_norm,ray_direction);
|
|
if ( den == 0.0f ) { // Ray & plane are parallel, so there is no intersection
|
|
if ( new_pnt ) {
|
|
new_pnt->xyz.x = -FLT_MAX;
|
|
new_pnt->xyz.y = -FLT_MAX;
|
|
new_pnt->xyz.z = -FLT_MAX;
|
|
}
|
|
return -FLT_MAX;
|
|
}
|
|
|
|
num = vm_vec_dot(plane_norm,&w);
|
|
num -= rad; //move point out by rad
|
|
|
|
t = num / den;
|
|
|
|
if ( new_pnt )
|
|
vm_vec_scale_add(new_pnt,ray_origin,ray_direction,t);
|
|
|
|
return t;
|
|
}
|
|
|
|
/**
|
|
* Find the point on the specified plane where the line intersects
|
|
*
|
|
* @param new_pnt The found point on the plane
|
|
* @param plane_pnt Plane description, a point
|
|
* @param plane_norm Plane description, a normal
|
|
* @param p0 The first end of the line
|
|
* @param p1 The second end of the line
|
|
* @param rad Radius
|
|
*
|
|
* @return true if point found, false if line parallel to plane
|
|
*/
|
|
|
|
int fvi_segment_plane(vec3d *new_pnt,
|
|
const vec3d *plane_pnt, const vec3d *plane_norm,
|
|
const vec3d *p0, const vec3d *p1,float rad)
|
|
{
|
|
float t;
|
|
vec3d d;
|
|
|
|
vm_vec_sub( &d, p1, p0 );
|
|
|
|
t = fvi_ray_plane(new_pnt,
|
|
plane_pnt,plane_norm, // Plane description, a point and a normal
|
|
p0,&d, // Ray description, a point and a direction
|
|
rad);
|
|
|
|
if ( t < 0.0f ) return 0; // intersection lies behind p0
|
|
if ( t > 1.0f ) return 0; // intersection lies past p1
|
|
|
|
return 1; // They intersect!
|
|
}
|
|
|
|
/**
|
|
* Determine if and where a vector intersects with a sphere
|
|
*
|
|
* vector defined by p0,p1
|
|
* @return 1 if intersects, and fills in intp, else returns 0
|
|
*/
|
|
int fvi_segment_sphere(vec3d *intp, const vec3d *p0, const vec3d *p1, const vec3d *sphere_pos, float sphere_rad)
|
|
{
|
|
vec3d d,dn,w,closest_point;
|
|
float mag_d,dist,w_dist,int_dist;
|
|
|
|
//this routine could be optimized if it's taking too much time!
|
|
|
|
vm_vec_sub(&d,p1,p0);
|
|
vm_vec_sub(&w,sphere_pos,p0);
|
|
|
|
mag_d = vm_vec_mag(&d);
|
|
|
|
if (mag_d <= 0.0f) {
|
|
int_dist = vm_vec_mag(&w);
|
|
*intp = *p0;
|
|
return (int_dist<sphere_rad)?1:0;
|
|
}
|
|
|
|
// normalize dn
|
|
dn.xyz.x = d.xyz.x / mag_d;
|
|
dn.xyz.y = d.xyz.y / mag_d;
|
|
dn.xyz.z = d.xyz.z / mag_d;
|
|
|
|
w_dist = vm_vec_dot(&dn,&w);
|
|
|
|
if (w_dist < -sphere_rad) //moving away from object
|
|
return 0;
|
|
|
|
if (w_dist > mag_d+sphere_rad)
|
|
return 0; //cannot hit
|
|
|
|
vm_vec_scale_add(&closest_point,p0,&dn,w_dist);
|
|
|
|
dist = vm_vec_dist(&closest_point,sphere_pos);
|
|
|
|
if (dist < sphere_rad) {
|
|
float dist2,radius2,shorten;
|
|
|
|
dist2 = dist*dist;
|
|
radius2 = sphere_rad*sphere_rad;
|
|
|
|
shorten = fl_sqrt(radius2 - dist2);
|
|
|
|
int_dist = w_dist-shorten;
|
|
|
|
if (int_dist > mag_d || int_dist < 0.0f) {
|
|
//past one or the other end of vector, which means we're inside
|
|
|
|
*intp = *p0; //don't move at all
|
|
return 1;
|
|
}
|
|
|
|
vm_vec_scale_add(intp,p0,&dn,int_dist); //calc intersection point
|
|
|
|
return 1;
|
|
}
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Determine if a cylinder *may* intersect with a sphere
|
|
*
|
|
* cylinder from p0 to p1 with radius cyl_rad
|
|
* sphere at sphere_pos with radius sphere_rad
|
|
* @return false if definitely not intersecting, returns true if intersection is possible
|
|
*/
|
|
bool fvi_cylinder_sphere_may_collide(const vec3d* p0, const vec3d* p1, float cyl_rad, const vec3d* sphere_pos, float sphere_rad)
|
|
{
|
|
vec3d cyl_rel = *p1 - *p0;
|
|
vec3d sphere_rel = *sphere_pos - *p0;
|
|
|
|
float cyl_len = vm_vec_mag(&cyl_rel);
|
|
if (cyl_len < SMALL_NUM)
|
|
// zero length cylinder, weird but ok, just do two sphere intersection
|
|
return vm_vec_mag_squared(&sphere_rel) <= (cyl_rad + sphere_rad) * (cyl_rad + sphere_rad);
|
|
|
|
vec3d cyl_dir = cyl_rel / cyl_len;
|
|
float intp_dist = vm_vec_dot(&cyl_dir, &sphere_rel);
|
|
if (intp_dist < -sphere_rad || intp_dist > cyl_len + sphere_rad)
|
|
// sphere is too far beyond either end of the cylinder
|
|
return false;
|
|
|
|
vec3d sphere_rel_intp = sphere_rel - cyl_dir * intp_dist;
|
|
return vm_vec_mag_squared(&sphere_rel_intp) <= (cyl_rad + sphere_rad) * (cyl_rad + sphere_rad);
|
|
}
|
|
|
|
|
|
/**
|
|
* Determine if and where a ray intersects with a sphere
|
|
*
|
|
* vector defined by p0,p1
|
|
* @return 1 if intersects, and fills in intp. else returns 0
|
|
*/
|
|
int fvi_ray_sphere(vec3d *intp, const vec3d *p0, const vec3d *p1, const vec3d *sphere_pos,float sphere_rad)
|
|
{
|
|
vec3d d,dn,w,closest_point;
|
|
float mag_d,dist,w_dist,int_dist;
|
|
|
|
//this routine could be optimized if it's taking too much time!
|
|
|
|
vm_vec_sub(&d,p1,p0);
|
|
vm_vec_sub(&w,sphere_pos,p0);
|
|
|
|
mag_d = vm_vec_mag(&d);
|
|
|
|
if (mag_d <= 0.0f) {
|
|
int_dist = vm_vec_mag(&w);
|
|
*intp = *p0;
|
|
return (int_dist<sphere_rad)?1:0;
|
|
}
|
|
|
|
// normalize dn
|
|
dn.xyz.x = d.xyz.x / mag_d;
|
|
dn.xyz.y = d.xyz.y / mag_d;
|
|
dn.xyz.z = d.xyz.z / mag_d;
|
|
|
|
w_dist = vm_vec_dot(&dn,&w);
|
|
|
|
if (w_dist < -sphere_rad) //moving away from object
|
|
return 0;
|
|
|
|
vm_vec_scale_add(&closest_point,p0,&dn,w_dist);
|
|
|
|
dist = vm_vec_dist(&closest_point,sphere_pos);
|
|
|
|
if (dist < sphere_rad) {
|
|
float dist2, radius2, shorten;
|
|
|
|
dist2 = dist*dist;
|
|
radius2 = sphere_rad*sphere_rad;
|
|
|
|
shorten = fl_sqrt(radius2 - dist2);
|
|
|
|
int_dist = w_dist-shorten;
|
|
|
|
if (int_dist < 0.0f) {
|
|
//past one or the begining of vector, which means we're inside
|
|
|
|
*intp = *p0; //don't move at all
|
|
return 1;
|
|
}
|
|
|
|
vm_vec_scale_add(intp,p0,&dn,int_dist); //calc intersection point
|
|
|
|
return 1;
|
|
}
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Finds intersection of a ray and an axis-aligned bounding box
|
|
*
|
|
* Given a ray with origin at p0, and direction pdir, this function
|
|
* returns non-zero if that ray intersects an axis-aligned bounding box
|
|
* from min to max. If there was an intersection, then hitpt will contain
|
|
* the point where the ray begins inside the box.
|
|
* Fast ray-box intersection taken from Graphics Gems I, pages 395,736.
|
|
*/
|
|
int fvi_ray_boundingbox(const vec3d *min, const vec3d *max, const vec3d * p0, const vec3d *pdir, vec3d *hitpt )
|
|
{
|
|
int middle = ((1<<0) | (1<<1) | (1<<2));
|
|
int i;
|
|
int which_plane;
|
|
float maxt[3];
|
|
float candidate_plane[3];
|
|
|
|
for (i = 0; i < 3; i++) {
|
|
if (p0->a1d[i] < min->a1d[i]) {
|
|
candidate_plane[i] = min->a1d[i];
|
|
middle &= ~(1<<i);
|
|
} else if (p0->a1d[i] > max->a1d[i]) {
|
|
candidate_plane[i] = max->a1d[i];
|
|
middle &= ~(1<<i);
|
|
}
|
|
}
|
|
|
|
// ray origin inside bounding box?
|
|
// (are all three bits still set?)
|
|
if (middle == ((1<<0) | (1<<1) | (1<<2))) {
|
|
*hitpt = *p0;
|
|
return 1;
|
|
}
|
|
|
|
// calculate T distances to candidate plane
|
|
for (i = 0; i < 3; i++) {
|
|
if ( (middle & (1<<i)) || (pdir->a1d[i] == 0.0f) ) {
|
|
maxt[i] = -1.0f;
|
|
} else {
|
|
maxt[i] = (candidate_plane[i] - p0->a1d[i]) / pdir->a1d[i];
|
|
}
|
|
}
|
|
|
|
// Get largest of the maxt's for final choice of intersection
|
|
which_plane = 0;
|
|
for (i = 1; i < 3; i++) {
|
|
if (maxt[which_plane] < maxt[i]) {
|
|
which_plane = i;
|
|
}
|
|
}
|
|
|
|
// check final candidate actually inside box
|
|
if (maxt[which_plane] < 0.0f) {
|
|
return 0;
|
|
}
|
|
|
|
for (i = 0; i < 3; i++) {
|
|
if (which_plane == i) {
|
|
hitpt->a1d[i] = candidate_plane[i];
|
|
} else {
|
|
hitpt->a1d[i] = (maxt[which_plane] * pdir->a1d[i]) + p0->a1d[i];
|
|
|
|
if ( (hitpt->a1d[i] < min->a1d[i]) || (hitpt->a1d[i] > max->a1d[i]) ) {
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
/**
|
|
* Given largest componant of normal, return i & j
|
|
* If largest componant is negative, swap i & j
|
|
*/
|
|
static int ij_table[3][2] = {
|
|
{2,1}, //pos x biggest
|
|
{0,2}, //pos y biggest
|
|
{1,0}, //pos z biggest
|
|
};
|
|
|
|
/**
|
|
* See if a point in inside a face by projecting into 2d. Also
|
|
* finds uv's if uvls is not NULL.
|
|
*
|
|
* Returns 0 if point isn't on face, non-zero otherwise.
|
|
*
|
|
* From Graphics Gems I, "An efficient Ray-Polygon intersection", p390
|
|
*
|
|
* @param checkp The point to check
|
|
* @param nv How many verts in the poly
|
|
* @param verts The vertives for the polygon
|
|
* @param norm1 The polygon's normal
|
|
* @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
|
|
* @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
|
|
* @param uvls A list of uv pairs for each vertex
|
|
*
|
|
* This replaces the old check_point_to_face & find_hitpoint_uv
|
|
* WARNING!! In Gems, they use the code "if (u1==0)" in this function.
|
|
* I have found several cases where this will not detect collisions it should.
|
|
* I found two solutions:
|
|
* 1. Declare the 'beta' variable to be a double.
|
|
* 2. Instead of using 'if (u1==0)', compare it to a small value.
|
|
* I chose #2 because I would rather have our code work with all floats
|
|
* and never need doubles. -JAS Aug22,1997
|
|
*/
|
|
#define delta 0.0001f
|
|
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 )
|
|
{
|
|
const float *norm;
|
|
const float *P;
|
|
vec3d t;
|
|
int i0, i1,i2;
|
|
|
|
norm = (float *)norm1;
|
|
|
|
//project polygon onto plane by finding largest component of normal
|
|
t.xyz.x = fl_abs(norm[0]);
|
|
t.xyz.y = fl_abs(norm[1]);
|
|
t.xyz.z = fl_abs(norm[2]);
|
|
|
|
if (t.xyz.x > t.xyz.y) if (t.xyz.x > t.xyz.z) i0=0; else i0=2;
|
|
else if (t.xyz.y > t.xyz.z) i0=1; else i0=2;
|
|
|
|
if (norm[i0] > 0.0f) {
|
|
i1 = ij_table[i0][0];
|
|
i2 = ij_table[i0][1];
|
|
}
|
|
else {
|
|
i1 = ij_table[i0][1];
|
|
i2 = ij_table[i0][0];
|
|
}
|
|
|
|
// Have i0, i1, i2
|
|
P = (float *)checkp;
|
|
|
|
float u0, u1, u2, v0, v1, v2, alpha = UNINITIALIZED_VALUE, gamma;
|
|
float beta;
|
|
|
|
int inter=0, i = 2;
|
|
|
|
u0 = P[i1] - verts[0]->a1d[i1];
|
|
v0 = P[i2] - verts[0]->a1d[i2];
|
|
|
|
do {
|
|
|
|
u1 = verts[i-1]->a1d[i1] - verts[0]->a1d[i1];
|
|
u2 = verts[i ]->a1d[i1] - verts[0]->a1d[i1];
|
|
|
|
v1 = verts[i-1]->a1d[i2] - verts[0]->a1d[i2];
|
|
v2 = verts[i ]->a1d[i2] - verts[0]->a1d[i2];
|
|
|
|
|
|
if ( (u1 >-delta) && (u1<delta) ) {
|
|
beta = u0 / u2;
|
|
if ((beta >=0.0f) && (beta<=1.0f)) {
|
|
alpha = (v0 - beta*v2)/v1;
|
|
inter = ((alpha>=0.0f)&&(alpha+beta<=1.0f));
|
|
}
|
|
} else {
|
|
|
|
beta = (v0*u1 - u0*v1) / (v2*u1 - u2*v1);
|
|
if ((beta >=0.0f) && (beta<=1.0f)) {
|
|
Assert(beta != UNINITIALIZED_VALUE);
|
|
alpha = (u0 - beta*u2)/u1;
|
|
inter = ((alpha>=0.0f)&&(alpha+beta<=1.0f));
|
|
}
|
|
}
|
|
|
|
} while ((!inter) && (++i < nv) );
|
|
|
|
if ( inter && uvls && u_out && v_out ) {
|
|
gamma = 1.0f - (alpha+beta);
|
|
*u_out = gamma * uvls[0].u + alpha*uvls[i-1].u + beta*uvls[i].u;
|
|
*v_out = gamma * uvls[0].v + alpha*uvls[i-1].v + beta*uvls[i].v;
|
|
}
|
|
|
|
return inter;
|
|
}
|
|
|
|
// ****************************************************************************
|
|
//
|
|
// SPHERE FACE INTERSECTION CODE
|
|
//
|
|
// ****************************************************************************
|
|
|
|
static int check_sphere_point(const vec3d *point, const vec3d *sphere_start, const vec3d *sphere_vel, float radius, float *collide_time );
|
|
|
|
/**
|
|
* Returns whether a sphere hits a given plane in the time [0,1]
|
|
* If two collisions occur, returns earliest legal time
|
|
* returns the intersection point on the plane
|
|
*
|
|
* @param intersect_point position on plane where sphere makes first contact [if hit_time in range 0-1]
|
|
* @param sphere_center_start initial sphere center
|
|
* @param sphere_velocity initial sphere velocity
|
|
* @param sphere_radius radius of sphere
|
|
* @param plane_normal normal to the colliding plane
|
|
* @param plane_point point in the colliding plane
|
|
* @param hit_time time surface of sphere first hits plane
|
|
* @param crossing_time time for sphere to cross plane (first to last contact)
|
|
*
|
|
* @return 1 if sphere may be in contact with plane in time range [0-1], 0 otherwise
|
|
*/
|
|
int 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)
|
|
{
|
|
float D, xs0_dot_norm, vs_dot_norm;
|
|
float t1, t2;
|
|
|
|
// find the time and position of the ray-plane intersection
|
|
D = -vm_vec_dot( plane_normal, plane_point );
|
|
xs0_dot_norm = vm_vec_dot( plane_normal, sphere_center_start );
|
|
vs_dot_norm = vm_vec_dot( plane_normal, sphere_velocity);
|
|
|
|
// protect against divide by zero
|
|
if (fl_abs(vs_dot_norm) > 1e-30) {
|
|
t1 = (-D - xs0_dot_norm + sphere_radius) / vs_dot_norm;
|
|
t2 = (-D - xs0_dot_norm - sphere_radius) / vs_dot_norm;
|
|
} else {
|
|
return 0;
|
|
}
|
|
|
|
// sort t1 < t2
|
|
if (t2 < t1) {
|
|
float temp = t1;
|
|
t1 = t2;
|
|
t2 = temp;
|
|
}
|
|
|
|
*hit_time = t1;
|
|
|
|
// find hit pos if t1 in range 0-1
|
|
if (t1 > 0 && t1 < 1) {
|
|
vec3d v_temp;
|
|
vm_vec_scale_add( &v_temp, sphere_center_start, sphere_velocity, t1 );
|
|
vm_project_point_onto_plane( intersect_point, &v_temp, plane_normal, plane_point );
|
|
}
|
|
|
|
// get time to cross
|
|
*crossing_time = t2 - t1;
|
|
|
|
return ( (t1 < 1) && (t2 > 0) );
|
|
}
|
|
|
|
/**
|
|
* Returns whether a sphere hits and edge for the case the edge is perpendicular to sphere_velocity
|
|
* If two collisions occur, returns the earliest legal time
|
|
* returns the intersection point on the edge
|
|
*
|
|
* @param intersect_point position on plane where sphere makes first contact [RESULT]
|
|
* @param sphere_center_start initial sphere center
|
|
* @param sphere_velocity initial sphere velocity
|
|
* @param sphere_radius radius of sphere
|
|
* @param edge_point1 first edge point
|
|
* @param edge_point2 second edge point
|
|
* @param collide_time actual time of the collision
|
|
*/
|
|
int 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)
|
|
{
|
|
// find the intersection in the plane normal to sphere velocity and edge velocity
|
|
// choose a plane point V0 (first vertex of the edge)
|
|
// project vectors and points into the plane
|
|
// find the projection of the intersection and see if it lies on the edge
|
|
|
|
vec3d edge_velocity;
|
|
vec3d V0, V1;
|
|
vec3d Xe_proj, Xs_proj;
|
|
|
|
V0 = *edge_point1;
|
|
V1 = *edge_point2;
|
|
vm_vec_sub(&edge_velocity, &V1, &V0);
|
|
|
|
// define a set of local unit vectors
|
|
vec3d x_hat, y_hat, z_hat;
|
|
float max_edge_parameter;
|
|
|
|
vm_vec_copy_normalize( &x_hat, &edge_velocity );
|
|
vm_vec_copy_normalize( &y_hat, sphere_velocity );
|
|
vm_vec_cross( &z_hat, &x_hat, &y_hat );
|
|
max_edge_parameter = vm_vec_mag( &edge_velocity );
|
|
|
|
vec3d temp;
|
|
// next two temp should be same as starting velocities
|
|
vm_vec_projection_onto_plane(&temp, sphere_velocity, &z_hat);
|
|
Assert ( !vm_vec_cmp(&temp, sphere_velocity) );
|
|
vm_vec_projection_onto_plane(&temp, &edge_velocity, &z_hat);
|
|
Assert ( !vm_vec_cmp(&temp, &edge_velocity) );
|
|
|
|
// should return V0
|
|
vm_project_point_onto_plane(&Xe_proj, &V0, &z_hat, &V0);
|
|
Assert ( !vm_vec_cmp(&Xe_proj, &V0) );
|
|
|
|
vm_project_point_onto_plane(&Xs_proj, sphere_center_start, &z_hat, &V0);
|
|
|
|
vec3d plane_coord;
|
|
plane_coord.xyz.x = vm_vec_dot(&Xs_proj, &x_hat);
|
|
plane_coord.xyz.y = vm_vec_dot(&Xe_proj, &y_hat);
|
|
plane_coord.xyz.z = vm_vec_dot(&Xe_proj, &z_hat);
|
|
|
|
// determime the position on the edge line
|
|
vm_vec_copy_scale( intersect_point, &x_hat, plane_coord.xyz.x );
|
|
vm_vec_scale_add2( intersect_point, &y_hat, plane_coord.xyz.y );
|
|
vm_vec_scale_add2( intersect_point, &z_hat, plane_coord.xyz.z );
|
|
|
|
// check if point is actually on edge
|
|
float edge_parameter;
|
|
vec3d temp_vec;
|
|
|
|
vm_vec_sub( &temp_vec, intersect_point, &V0 );
|
|
edge_parameter = vm_vec_dot( &temp_vec, &x_hat );
|
|
|
|
if ( edge_parameter < 0 || edge_parameter > max_edge_parameter ) {
|
|
return 0;
|
|
}
|
|
|
|
return ( check_sphere_point(intersect_point, sphere_center_start, sphere_velocity, sphere_radius, collide_time) );
|
|
}
|
|
|
|
|
|
/**
|
|
* Determines whether and where a moving sphere hits a point
|
|
*
|
|
* @param point point sphere collides with
|
|
* @param sphere_start initial sphere center
|
|
* @param sphere_vel velocity of sphere
|
|
* @param radius radius of sphere
|
|
* @param collide_time time of first collision with t >= 0
|
|
*/
|
|
static int check_sphere_point(const vec3d *point, const vec3d *sphere_start, const vec3d *sphere_vel, float radius, float *collide_time )
|
|
{
|
|
vec3d delta_x;
|
|
float delta_x_sqr, vs_sqr, delta_x_dot_vs;
|
|
|
|
vm_vec_sub( &delta_x, sphere_start, point );
|
|
delta_x_sqr = vm_vec_mag_squared( &delta_x );
|
|
vs_sqr = vm_vec_mag_squared( sphere_vel );
|
|
delta_x_dot_vs = vm_vec_dot( &delta_x, sphere_vel );
|
|
|
|
float discriminant = delta_x_dot_vs*delta_x_dot_vs - vs_sqr*(delta_x_sqr - radius*radius);
|
|
if (discriminant < 0) {
|
|
return 0;
|
|
}
|
|
|
|
float radical, time1, time2;
|
|
radical = fl_sqrt(discriminant);
|
|
time1 = (-delta_x_dot_vs + radical) / vs_sqr;
|
|
time2 = (-delta_x_dot_vs - radical) / vs_sqr;
|
|
|
|
if (time1 > time2) {
|
|
float temp = time1;
|
|
time1 = time2;
|
|
time2 = temp;
|
|
}
|
|
|
|
if (time1 >= 0 && time1 <= 1.0) {
|
|
*collide_time = time1;
|
|
return 1;
|
|
}
|
|
|
|
if (time2 >= 0 && time2 <= 1.0) {
|
|
*collide_time = time2;
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Given a polygon vertex list and a moving sphere, find the first contact the sphere makes with the edge, if any
|
|
*
|
|
* @param hit_point point on edge
|
|
* @param xs0 starting point for sphere
|
|
* @param vs sphere velocity
|
|
* @param Rs sphere radius
|
|
* @param nv number of vertices
|
|
* @param verts vertices making up polygon edges
|
|
* @param hit_time time the sphere hits an edge
|
|
*
|
|
* @return 1 if sphere hits polyedge, 0 if sphere misses
|
|
*/
|
|
int fvi_polyedge_sphereline(vec3d *hit_point, const vec3d *xs0, const vec3d *vs, float Rs, int nv, vec3d const *const *verts, float *hit_time)
|
|
{
|
|
int i;
|
|
vec3d v0, v1;
|
|
vec3d ve; // edge velocity
|
|
float best_sphere_time; // earliest time sphere hits edge
|
|
vec3d delta_x;
|
|
float delta_x_dot_ve, delta_x_dot_vs, ve_dot_vs, ve_sqr, vs_sqr, delta_x_sqr;
|
|
vec3d temp_edge_hit, temp_sphere_hit;
|
|
float t_sphere_hit = 0.0f;
|
|
float A, B, C, temp, discriminant, invA;
|
|
float root, root1, root2;
|
|
float Rs2 = (Rs * Rs);
|
|
float Rs_point2 = (0.2f * Rs);
|
|
|
|
best_sphere_time = FLT_MAX;
|
|
|
|
vs_sqr = vm_vec_mag_squared(vs);
|
|
|
|
for (i = 0; i < nv; i++) {
|
|
// Get vertices of edge to check
|
|
v0 = *verts[i];
|
|
if (i+1 != nv) {
|
|
v1 = *verts[i+1];
|
|
} else {
|
|
v1 = *verts[0];
|
|
}
|
|
|
|
// Calculate edge velocity.
|
|
// Position along the edge is given by: P_edge = v0 + ve*t, where t is in the range [0,1]
|
|
vm_vec_sub(&ve, &v1, &v0);
|
|
|
|
// First find the closest intersection between the edge_line and the sphere_line.
|
|
vm_vec_sub(&delta_x, xs0, &v0);
|
|
|
|
delta_x_dot_ve = vm_vec_dot(&delta_x, &ve);
|
|
delta_x_dot_vs = vm_vec_dot(&delta_x, vs);
|
|
delta_x_sqr = vm_vec_mag_squared(&delta_x);
|
|
ve_dot_vs = vm_vec_dot(&ve, vs);
|
|
ve_sqr = vm_vec_mag_squared(&ve);
|
|
|
|
/*
|
|
* Solve for sphere time
|
|
*
|
|
* This code uses the following variation of the quadratic equation:
|
|
* A*x*x + 2*B*x + C = 0
|
|
*
|
|
* Solving for x yields ...
|
|
*
|
|
* -B +- sqrt(B*B - A*C)
|
|
* x = ---------------------
|
|
* A
|
|
*/
|
|
|
|
A = ve_dot_vs*ve_dot_vs - ve_sqr*vs_sqr;
|
|
B = delta_x_dot_ve*ve_dot_vs - delta_x_dot_vs*ve_sqr;
|
|
C = delta_x_dot_ve*delta_x_dot_ve + Rs2*ve_sqr - delta_x_sqr*ve_sqr;
|
|
|
|
discriminant = B*B - A*C;
|
|
if (discriminant > 0.0f) {
|
|
invA = 1.0f / A;
|
|
root = sqrt(discriminant);
|
|
root1 = (-B + root) * invA;
|
|
root2 = (-B - root) * invA;
|
|
|
|
// sort root1 and root2
|
|
if (root2 < root1) {
|
|
temp = root1;
|
|
root1 = root2;
|
|
root2 = temp;
|
|
}
|
|
|
|
if ( (root1 < 0.0f) && (root1 >= -0.05f) )
|
|
root1 = 0.000001f;
|
|
|
|
// look only at first hit
|
|
if ( (root1 <= 1.0f) && (root1 >= 0.0f) ) {
|
|
t_sphere_hit = root1;
|
|
} else {
|
|
goto TryVertex;
|
|
}
|
|
} else {
|
|
// discriminant negative, so no hit possible
|
|
continue;
|
|
}
|
|
|
|
// check if best time with this edge is less than best so far
|
|
if (t_sphere_hit >= best_sphere_time)
|
|
continue;
|
|
|
|
vm_vec_scale_add( &temp_sphere_hit, xs0, vs, t_sphere_hit );
|
|
|
|
// solve for edge time
|
|
A *= ve_sqr;
|
|
B = ve_sqr * (delta_x_dot_ve*vs_sqr - delta_x_dot_vs*ve_dot_vs);
|
|
C = 2.0f*delta_x_dot_ve*delta_x_dot_vs*ve_dot_vs + Rs2*ve_dot_vs*ve_dot_vs
|
|
- delta_x_sqr*ve_dot_vs*ve_dot_vs - delta_x_dot_ve*delta_x_dot_ve*vs_sqr;
|
|
|
|
discriminant = B*B - A*C;
|
|
invA = 1.0f / A;
|
|
|
|
// guard against nearly perpendicular sphere edge velocities
|
|
if (discriminant < 0.0f)
|
|
root = 0.0f;
|
|
else
|
|
root = fl_sqrt(discriminant);
|
|
|
|
root1 = (-B + root) * invA;
|
|
root2 = (-B - root) * invA;
|
|
|
|
// given sphere position, find which edge time (position) allows a valid solution
|
|
if ( (root1 <= 1.0f) && (root1 >= 0.0f) ) {
|
|
// try edge root1
|
|
vm_vec_scale_add( &temp_edge_hit, &v0, &ve, root1 );
|
|
float q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
|
|
if ( fl_abs(q - Rs2) < Rs_point2 ) { // error less than 0.1m absolute (2*delta*Radius)
|
|
goto Hit;
|
|
}
|
|
}
|
|
|
|
if ( (root2 <= 1.0f) && (root2 >= 0.0f) ) {
|
|
// try edge root2
|
|
vm_vec_scale_add( &temp_edge_hit, &v0, &ve, root2 );
|
|
float q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
|
|
if ( fl_abs(q - Rs2) < Rs_point2 ) { // error less than 0.1m absolute
|
|
goto Hit;
|
|
}
|
|
} else {
|
|
// both root1 and root2 out of range so we have to check vertices
|
|
goto TryVertex;
|
|
}
|
|
|
|
TryVertex:
|
|
// try V0
|
|
A = vs_sqr;
|
|
B = delta_x_dot_vs;
|
|
C = delta_x_sqr - Rs2;
|
|
int v0_hit;
|
|
float sphere_v0, sphere_v1;
|
|
|
|
v0_hit = 0;
|
|
sphere_v0 = UNINITIALIZED_VALUE;
|
|
sphere_v1 = UNINITIALIZED_VALUE;
|
|
|
|
invA = 1.0f / A;
|
|
discriminant = B*B - A*C;
|
|
if (discriminant > 0.0f) {
|
|
root = fl_sqrt(discriminant);
|
|
root1 = (-B + root) * invA;
|
|
root2 = (-B - root) * invA;
|
|
|
|
if (root1 > root2) {
|
|
temp = root1;
|
|
root1 = root2;
|
|
root2 = temp;
|
|
}
|
|
|
|
// look only at the first hit (ignore negative first hit)
|
|
if ( (root1 < 1.0f) && (root1 > 0.0f) ) {
|
|
v0_hit = 1;
|
|
sphere_v0 = root1;
|
|
}
|
|
}
|
|
|
|
// try V1
|
|
vm_vec_sub( &delta_x, xs0, &v1 );
|
|
delta_x_sqr = vm_vec_mag_squared( &delta_x );
|
|
delta_x_dot_vs = vm_vec_dot( &delta_x, vs );
|
|
int v1_hit;
|
|
|
|
B = delta_x_dot_vs;
|
|
C = delta_x_sqr - Rs2;
|
|
|
|
v1_hit = 0;
|
|
discriminant = B*B - A*C;
|
|
if (discriminant > 0.0f) {
|
|
root = fl_sqrt(discriminant);
|
|
root1 = (-B + root) * invA;
|
|
root2 = (-B - root) * invA;
|
|
|
|
if (root1 > root2) {
|
|
temp = root1;
|
|
root1 = root2;
|
|
root2 = temp;
|
|
}
|
|
|
|
// look only at the first hit (ignore negative first hit)
|
|
if ( (root1 < 1.0f) && (root1 > 0.0f) ) {
|
|
v1_hit = 1;
|
|
sphere_v1 = root1;
|
|
}
|
|
}
|
|
|
|
// set hitpoint to closest vetex hit, if any
|
|
if ( v0_hit ) {
|
|
Assert(sphere_v0 != UNINITIALIZED_VALUE);
|
|
t_sphere_hit = sphere_v0;
|
|
temp_edge_hit = v0;
|
|
|
|
if (v1_hit) {
|
|
Assert( sphere_v1 != UNINITIALIZED_VALUE );
|
|
if (sphere_v1 < sphere_v0) {
|
|
t_sphere_hit = sphere_v1;
|
|
temp_edge_hit = v1;
|
|
}
|
|
}
|
|
} else if ( v1_hit ) {
|
|
Assert(sphere_v1 != UNINITIALIZED_VALUE);
|
|
t_sphere_hit = sphere_v1;
|
|
temp_edge_hit = v1;
|
|
} else {
|
|
continue;
|
|
}
|
|
|
|
//vm_vec_scale_add( &temp_sphere_hit, xs0, vs, t_sphere_hit );
|
|
//q = vm_vec_dist_squared(&temp_edge_hit, &temp_sphere_hit);
|
|
|
|
|
|
Hit:
|
|
if (t_sphere_hit < best_sphere_time) {
|
|
best_sphere_time = t_sphere_hit;
|
|
*hit_point = temp_edge_hit;
|
|
}
|
|
} // end edge loop
|
|
|
|
if (best_sphere_time <= 1.0f) {
|
|
*hit_time = best_sphere_time;
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Finds the closest point on a line to a given fixed point
|
|
*
|
|
* @param closest_point the closest point on the line
|
|
* @param fixed_point the fixed point
|
|
* @param line_point1 first point on the line
|
|
* @param line_point2 second point on the line
|
|
*/
|
|
void fvi_closest_point_on_line_segment(vec3d *closest_point, const vec3d *fixed_point, const vec3d *line_point1, const vec3d *line_point2)
|
|
{
|
|
vec3d delta_x, line_velocity;
|
|
float t;
|
|
|
|
vm_vec_sub(&line_velocity, line_point2, line_point1);
|
|
vm_vec_sub(&delta_x, line_point1, fixed_point);
|
|
t = -vm_vec_dot(&delta_x, &line_velocity) / vm_vec_mag_squared(&line_velocity);
|
|
|
|
// Constrain t to be in range [0,1]
|
|
if (t < 0) {
|
|
t = 0.0f;
|
|
} else if (t > 1) {
|
|
t = 1.0f;
|
|
}
|
|
|
|
vm_vec_scale_add(closest_point, line_point1, &line_velocity, t);
|
|
}
|
|
|
|
/**
|
|
* checks whether two spheres hit given initial and starting positions and radii
|
|
* does not check whether sphere are already touching.
|
|
*
|
|
* @param x_p0 polymodel sphere, start point
|
|
* @param x_p1 polymodel sphere, end point
|
|
* @param x_s0 other sphere, start point
|
|
* @param x_s1 other sphere, end point
|
|
* @param radius_p radius of polymodel sphere
|
|
* @param radius_s radius of other sphere
|
|
* @param t1 time pointer 1
|
|
* @param t2 time pointer 2
|
|
*
|
|
* @return 1 if spheres overlap, 0 otherwise
|
|
*/
|
|
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)
|
|
{
|
|
vec3d delta_x, delta_v;
|
|
float discriminant, separation, delta_x_dot_delta_v, delta_v_sqr, delta_x_sqr;
|
|
float time1, time2;
|
|
|
|
// Check that there are either 0 or 2 pointers to time
|
|
Assert( (!(t1) && !(t2)) || (t1 && t2) );
|
|
|
|
vm_vec_sub(&delta_x, x_s0, x_p0);
|
|
delta_x_sqr = vm_vec_mag_squared(&delta_x);
|
|
separation = radius_p + radius_s;
|
|
|
|
// Check if already touching
|
|
if (delta_x_sqr < separation*separation) {
|
|
if ( !t1 ) {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
// Find delta_v (in polymodel sphere frame of ref)
|
|
// Note: x_p0 and x_p1 will be same for fixed polymodel
|
|
vm_vec_sub(&delta_v, x_s1, x_s0);
|
|
vm_vec_add2(&delta_v, x_p0);
|
|
vm_vec_sub2(&delta_v, x_p1);
|
|
|
|
delta_x_dot_delta_v = vm_vec_dot(&delta_x, &delta_v);
|
|
delta_v_sqr = vm_vec_mag_squared(&delta_v);
|
|
|
|
discriminant = delta_x_dot_delta_v*delta_x_dot_delta_v - delta_v_sqr*(delta_x_sqr - separation*separation);
|
|
|
|
if (discriminant < 0) {
|
|
return 0;
|
|
}
|
|
|
|
float radical = fl_sqrt(discriminant);
|
|
|
|
time1 = (-delta_x_dot_delta_v + radical) / delta_v_sqr;
|
|
time2 = (-delta_x_dot_delta_v - radical) / delta_v_sqr;
|
|
|
|
// sort t1 < t2
|
|
float temp;
|
|
if (time1 > time2) {
|
|
temp = time1;
|
|
time1 = time2;
|
|
time2 = temp;
|
|
}
|
|
|
|
if ( t1 ) {
|
|
*t1 = time1;
|
|
*t2 = time2;
|
|
}
|
|
|
|
// check whether the range from t1 to t2 intersects [0,1]
|
|
if (time1 > 1 || time2 < 0) {
|
|
return 0;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Culls polyfaces which moving sphere can not intersect
|
|
*
|
|
* Polygon face is characterized by a center and a radius. This routine checks whether it is
|
|
* *impossible* for a moving sphere to intersect a fixed polygon face.
|
|
*
|
|
* @param poly_center center of polygon face to test
|
|
* @param poly_r radius of polygon face in question
|
|
* @param sphere_start start point of moving sphere
|
|
* @param sphere_end end point of moving sphere
|
|
* @param sphere_r radius of moving sphere
|
|
*
|
|
* @return 0 if no collision is possible, 1 if collision may be possible
|
|
*/
|
|
int fvi_cull_polyface_sphere(const vec3d *poly_center, float poly_r, const vec3d *sphere_start, const vec3d *sphere_end, float sphere_r)
|
|
{
|
|
vec3d closest_point, closest_separation;
|
|
float max_sep;
|
|
|
|
fvi_closest_point_on_line_segment(&closest_point, poly_center, sphere_start, sphere_end);
|
|
vm_vec_sub(&closest_separation, &closest_point, poly_center);
|
|
|
|
max_sep = vm_vec_mag(&closest_separation) + poly_r;
|
|
|
|
if ( max_sep > sphere_r ) {
|
|
return 0;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Finds the closest points between two lines
|
|
*/
|
|
void fvi_closest_line_line(const vec3d *x0, const vec3d *vx, const vec3d *y0, const vec3d *vy, float *x_time, float *y_time )
|
|
{
|
|
vec3d vx_cross_vy, delta_l, delta_l_cross_vx, delta_l_cross_vy;
|
|
float denominator;
|
|
|
|
vm_vec_sub(&delta_l, y0, x0);
|
|
|
|
vm_vec_cross(&vx_cross_vy, vx, vy);
|
|
vm_vec_cross(&delta_l_cross_vx, &delta_l, vx);
|
|
vm_vec_cross(&delta_l_cross_vy, &delta_l, vy);
|
|
|
|
denominator = vm_vec_mag_squared(&vx_cross_vy);
|
|
|
|
*x_time = vm_vec_dot(&delta_l_cross_vy, &vx_cross_vy) / denominator;
|
|
*y_time = vm_vec_dot(&delta_l_cross_vx, &vx_cross_vy) / denominator;
|
|
}
|
|
|
|
/**
|
|
* Project point on bounding box
|
|
*
|
|
* NOTE: if a coordinate of start is *inside* the bbox, it is *not* moved to surface of bbox
|
|
*
|
|
* @param mins minimum extents of bbox
|
|
* @param maxs maximum extents of bbox
|
|
* @param start point in bbox reference frame
|
|
* @param box_pt point in bbox reference frame.
|
|
*
|
|
* @return 1 if inside, 0 otherwise.
|
|
*/
|
|
int project_point_onto_bbox(const vec3d *mins, const vec3d *maxs, const vec3d *start, vec3d *box_pt)
|
|
{
|
|
int inside = TRUE;
|
|
|
|
if (start->xyz.x > maxs->xyz.x) {
|
|
box_pt->xyz.x = maxs->xyz.x;
|
|
inside = FALSE;
|
|
} else if (start->xyz.x < mins->xyz.x) {
|
|
box_pt->xyz.x = mins->xyz.x;
|
|
inside = FALSE;
|
|
} else {
|
|
box_pt->xyz.x = start->xyz.x;
|
|
}
|
|
|
|
if (start->xyz.y > maxs->xyz.y) {
|
|
box_pt->xyz.y = maxs->xyz.y;
|
|
inside = FALSE;
|
|
} else if (start->xyz.y < mins->xyz.y) {
|
|
box_pt->xyz.y = mins->xyz.y;
|
|
inside = FALSE;
|
|
} else {
|
|
box_pt->xyz.y = start->xyz.y;
|
|
}
|
|
|
|
if (start->xyz.z > maxs->xyz.z) {
|
|
box_pt->xyz.z = maxs->xyz.z;
|
|
inside = FALSE;
|
|
} else if (start->xyz.z < mins->xyz.z) {
|
|
box_pt->xyz.z = mins->xyz.z;
|
|
inside = FALSE;
|
|
} else {
|
|
box_pt->xyz.z = start->xyz.z;
|
|
}
|
|
|
|
return inside;
|
|
}
|