1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44
|
float
Intersect_VF(const vec3f &ta0, const vec3f &tb0, const vec3f &tc0,
const vec3f &ta1, const vec3f &tb1, const vec3f &tc1,
const vec3f &q0, const vec3f &q1,
vec3f &qi, vec3f &baryc)
{
/* Default value returned if no collision occurs */
float collisionTime = -1.0f;
vec3f qd, ad, bd, cd;
/* diff. vectors for linear interpolation */
qd = q1 - q0, ad = ta1 - ta0, bd = tb1 - tb0, cd = tc1 - tc0;
/*
* Compute scalar coefficients by evaluating dot and cross-products.
*/
float a, b, c, d; /* cubic polynomial coefficients */
_equateCubic_VF(ta0, ad, tb0, bd, tc0, cd, q0, qd, a, b, c, d);
if (IsZero(a) && IsZero(b) && IsZero(c) && IsZero(d))
return collisionTime;
NewtonCheckData data;
data.a0 = ta0, data.b0 = tb0;
data.c0 = tc0, data.p0 = q0;
data.ad = ad, data.bd = bd;
data.cd = cd, data.pd = qd;
/*
* iteratively solve the cubic (scalar) equation and test for validity of the solution.
*/
double l = 0;
double r = 1;
double coeffs[4];
coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
if (solveCubicWithIntervalNewton(l, r, baryc, true, data, coeffs)) {
collisionTime = (l+r)*0.5f;
qi = qd*collisionTime + q0; //pont in the time of collision
}
return collisionTime;
}
|