fvi.cpp

Go to the documentation of this file.

/*
 * 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 void accurate_square_root( float A, float B, float C, float discriminant, float *root1, float *root2 ) __UNUSED;

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;
}

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;
        }
}

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;
}

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;
}

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!
}

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,rad2,shorten;

                dist2 = dist*dist;
                rad2 = sphere_rad*sphere_rad;

                shorten = fl_sqrt(rad2 - 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;
}


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,rad2,shorten;

                dist2 = dist*dist;
                rad2 = sphere_rad*sphere_rad;

                shorten = fl_sqrt(rad2 - 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;
}

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;
}

static int ij_table[3][2] =        {
                                                        {2,1},          //pos x biggest
                                                        {0,2},          //pos y biggest
                                                        {1,0},          //pos z biggest
                                                };

#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 );

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) );
}

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) );
}
        

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;
}

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;
        }
}

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);
}

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;
        }
}

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;
        }
}

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; 
}

// --------------------------------------------------------------------------------------------------------------------
static void accurate_square_root( float A, float B, float C, float discriminant, float *root1, float *root2 )
{
        float root = fl_sqrt(discriminant);

        if (B > 0) {
                *root1 = 2.0f*C / (-B - root);
                *root2 = (-B - root) / (2.0f*A);
        } else {        // B < 0
                *root1 = (-B + root) / (2.0f*A);
                *root2 = 2.0f*C / (-B + root);
        }
}

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;
}