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