int SpherePtFromLine(double p1[3], double p2[3], double center[3], double radius, double roots[2])
{
// given a 3D line defined by point p1 and p2, compute roots such that
// p1+root*(p2-p1) is a point of intersection with a sphere of given center and radius
// whose distance to center (of the sphere) equals radius
double d[3], q[3]; // two points on line, line vector, two roots for t
Sub(p2, p1, d); // set d (line vector)
// vector from point on line, p+t*d, to center is p+td-center, or q+td (q = p-center)
// magnitude-squared of this vector is (q+td)**2 = (q**2)+2tdq+(t**2)(d**2)
// in terms of the quadratic equation, a is d**2, b is 2dq, and c is (q**2)-(radius**2)
Sub(p1, center, q);
double a = MagSq(d);
double b = 2*DotProduct(d, q);
double c = MagSq(q)-radius*radius;
return QuadraticRoots(a, b, c, &roots[0], &roots[1]);
}
double Normalize(double *v, int dim)
{
double mag = sqrt(MagSq(v, dim));
for (int i = 0; i < dim; i++)
v[i] /= mag;
return mag;
}
void TriangleNormal(double *p1, double *p2, double *p3, double *r)
{
// best results if pivot is vertex with greatest angle (ie, opposite longest side)
double dif1[3], dif2[3], dif3[3];
Sub(p3, p2, dif1);
Sub(p1, p3, dif2);
Sub(p2, p1, dif3);
double sqMag1 = MagSq(dif1);
double sqMag2 = MagSq(dif2);
double sqMag3 = MagSq(dif3);
int pivot = sqMag1 > sqMag2? (sqMag1 > sqMag3? 1 : 3) : (sqMag2 > sqMag3? 2 : 3);
switch (pivot) {
case 1: CrossProduct(dif2, dif3, r); break;
case 2: CrossProduct(dif3, dif1, r); break;
case 3: CrossProduct(dif1, dif2, r); break;
}
Normalize(r);
}
//---------------------------------------------------------------------------
PxF32
MVec3::Mag() const
{
return Sqrt( MagSq() );
}
