public static Quat FromTo(Vec3 fromNorm, Vec3 toNorm)
{
Vec3 mf = VecX.Normalize(fromNorm + toNorm);
Quat nq = new Quat(VecX.Cross(mf, toNorm), VecX.Dot(mf, toNorm));
return(nq);
}
public SqMat3 Inverse()
{
return(new SqMat3(
this[0, 0], this[0, 1], -VecX.Dot(GetColumn(0).xy, GetColumn(2).xy),
this[1, 0], this[1, 1], -VecX.Dot(GetColumn(1).xy, GetColumn(2).xy),
0, 0, 1));
}
public static int LineSphereIntersection <T>(T lnD, T sC, double r, out double s, out double t) where T : IVector, new()
{
//SqrDistance(sc, lp + ld * t) = sr * sr, solve t
//ld*ld * t*t + 2*(sc-lp)*ld * t + (sc-lp)*(sc-lp) - sr*sr = 0
double a = lnD.SqrLength() * 2;
double b = VecX.Dot(sC, lnD) * 2;
double c = sC.SqrLength() - r * r;
double delta = b * b - 2 * a * c;
if (delta < 0)
{
s = t = 0;
return(0);
}
else if (delta > 0)
{
delta = Math.Sqrt(delta);
s = (b - delta) / a;
t = (b + delta) / a;
return(2);
}
else
{
s = t = b / a;
return(1);
}
}
public static int LineSphereIntersection(Vec3 lnD, Vec3 sC, double r, out double s, out double t)
{
double a = lnD.SqrLength() * 2;
double b = VecX.Dot(sC, lnD) * 2;
double c = sC.SqrLength() - r * r;
double delta = b * b - 2 * a * c;
if (delta < 0)
{
s = t = 0;
return(0);
}
else if (delta > 0)
{
delta = Math.Sqrt(delta);
s = (b - delta) / a;
t = (b + delta) / a;
return(2);
}
else
{
s = t = b / a;
return(1);
}
}
public static bool LinePlaneIntersection(Vec3 lnP, Vec3 lnDir, Vec3 plnP, Vec3 norm, out Vec3 cp)
{
double dot = VecX.Dot(norm, lnDir);
if (dot == 0)
{
cp = default(Vec3);
return(false);
}
else
{
double t = VecX.Dot(norm, plnP - lnP) / dot;
cp = lnP + lnDir * t;
return(true);
}
}
public static bool LineSegTriangleIntersection(LineSeg <Vec3> s, Triangle <Vec3> t, out Vec3 cp)
{
if (LinePlaneIntersection(s.p0, s.p1 - s.p0, t.p0, VecX.Cross(t.p0 - t.p1, t.p0 - t.p2), out cp))
{
if (VecX.Dot(cp - s.p0, cp - s.p1) <= 0)
{
Vec3 c0 = VecX.Cross(cp - t.p0, cp - t.p1);
Vec3 c1 = VecX.Cross(cp - t.p1, cp - t.p2);
Vec3 c2 = VecX.Cross(cp - t.p2, cp - t.p0);
if (VecX.Dot(c0, c1) >= 0 &&
VecX.Dot(c1, c2) >= 0 &&
VecX.Dot(c2, c0) >= 0)
{
return(true);
}
}
}
return(false);
}
public static bool CommonPerpendicular <T>(DirLineSeg <T> u, DirLineSeg <T> v, out double s, out double t) where T : IVector, new()
{
double A = VecX.Dot(u.d, v.d);
double B = VecX.SqrLength(u.d);
double C = VecX.SqrLength(v.d);
double div = A * A - B * C;
if (div == 0.0)
{
s = t = 0;
return(false);
}
T dp = VecX.Sub(v.p, u.p);
double D = VecX.Dot(dp, u.d);
double E = VecX.Dot(dp, v.d);
s = (A * E - C * D) / div;
t = (B * E - A * D) / div;
return(true);
}
public static bool CommonPerpendicular(DirLineSeg <Vec3> u, DirLineSeg <Vec3> v, out double s, out double t)
{
double A = VecX.Dot(u.d, v.d);
double B = VecX.SqrLength(u.d);
double C = VecX.SqrLength(v.d);
double div = A * A - B * C;
if (div == 0.0)
{
s = t = 0;
return(false);
}
Vec3 dp = v.p - u.p;
double D = VecX.Dot(dp, u.d);
double E = VecX.Dot(dp, v.d);
s = (C * D - A * E) / div;
t = (A * D - B * E) / div;
return(true);
}
public T Project(T v)
{
int dim = v.Dimension;
v = VecX.Sub(v, m_origin);
double[] nvArr = new double[dim];
for (int i = 0; i < m_basis.Length; i++)
{
T bv = m_basis[i];
double d = VecX.Dot(v, bv);
for (int j = 0; j < dim; j++)
{
nvArr[j] += d * bv[j];
}
}
T nv = new T();
for (int i = 0; i < dim; i++)
{
nv[i] = nvArr[i] + m_origin[i];
}
return(nv);
}
