Beispiel #1
0
        public static T Distance <T>(T dp, T sd) where T : IVector, new()
        {
            double lp = VecX.Project01(dp, sd);

            lp = MathX.Clamp(lp, 0.0, 1.0);
            return(VecX.Sub(dp, VecX.Mul(sd, lp)));
        }
Beispiel #2
0
 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));
 }
Beispiel #3
0
        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);
        }
Beispiel #4
0
        public void RotateAround(Vec3 target, Quat rotation)
        {
            double dist = VecX.Distance(m_position, target);

            m_rotation *= rotation;
            m_position  = m_rotation * new Vec3(0, 0, -dist) + target;
        }
Beispiel #5
0
        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);
            }
        }
Beispiel #6
0
        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);
            }
        }
Beispiel #7
0
        public static Vec3 Distance(Vec3 dp, Vec3 sd)
        {
            double lp = VecX.Project01(dp, sd);

            lp = MathX.Clamp(lp, 0.0, 1.0);
            return(dp - lp * sd);
        }
Beispiel #8
0
        public static bool InTriangle(Vec2 p, Triangle <Vec2> tri)
        {
            double c0 = VecX.Cross(p - tri.p0, p - tri.p1);
            double c1 = VecX.Cross(p - tri.p1, p - tri.p2);
            double c2 = VecX.Cross(p - tri.p2, p - tri.p0);

            return(c0 * c1 >= 0.0 && c1 * c2 >= 0.0 && c2 * c0 >= 0.0);
        }
Beispiel #9
0
 public static void MinkowskiDiff <T>(IEnumerable <T> s0, IEnumerable <T> s1, HashSet <T> result) where T : IVector, new()
 {
     foreach (T p0 in s0)
     {
         foreach (T p1 in s1)
         {
             result.Add(VecX.Sub(p0, p1));
         }
     }
 }
Beispiel #10
0
        public static double Distance(DirLineSeg <Vec2> u, DirLineSeg <Vec2> v)
        {
            double d1 = VecX.Length(Distance(v.p - u.p, u.d));
            double d2 = VecX.Length(Distance(v.p + v.d - u.p, u.d));
            double d3 = VecX.Length(Distance(u.p - v.p, v.d));
            double d4 = VecX.Length(Distance(u.p + u.d - v.p, v.d));
            double d  = Math.Min(Math.Min(d1, d2), Math.Min(d3, d4));

            return(d);
        }
Beispiel #11
0
        public void CenterRadius(out T center, out double radius)
        {
            center = new T();
            int dim = p.Dimension;

            for (int i = 0; i < dim; i++)
            {
                center[i] = p[i] + d[i] * 0.5;
            }
            radius = VecX.Distance(center, p);
        }
Beispiel #12
0
        public void CenterRadius(out T center, out double radius)
        {
            center = new T();
            int dim = p0.Dimension;

            for (int i = 0; i < dim; i++)
            {
                center[i] = (p0[i] + p1[i]) / 2;
            }
            radius = VecX.Distance(center, p0);
        }
Beispiel #13
0
 private void ProcessBasis()
 {
     for (int i = 0; i < m_basis.Length; i++)
     {
         T v = m_basis[i];
         for (int j = 0; j < i; j++)
         {
             v = VecX.Orthogonalize(v, m_basis[j]);
         }
         m_basis[i] = VecX.Normalize(v);
     }
 }
Beispiel #14
0
        public void CenterRadius(out T center, out double radius)
        {
            center = new T();
            int dim = p0.Dimension;

            for (int i = 0; i < dim; i++)
            {
                center[i] = (p0[i] + p1[i] + p2[i] + p3[i]) / 4;
            }
            radius = VecX.Distance(center, p0);
            radius = Math.Max(radius, VecX.Distance(center, p1));
            radius = Math.Max(radius, VecX.Distance(center, p2));
            radius = Math.Max(radius, VecX.Distance(center, p3));
        }
Beispiel #15
0
        public static Vec2 Distance(Vec2 p, IList <Vec2> poly)
        {
            int  lastI = poly.Count - 1;
            Vec2 dn, n;

            dn = Distance(p - poly[lastI], poly[0] - poly[lastI]);
            for (int i = 0; i < lastI; i++)
            {
                n = Distance(p - poly[i], poly[i + 1] - poly[i]);
                if (VecX.SqrLength(n) < VecX.SqrLength(dn))
                {
                    dn = n;
                }
            }
            return(dn);
        }
Beispiel #16
0
        public static bool InConvexPolygon(Vec2 p, IList <Vec2> poly)
        {
            int    lastI = poly.Count - 1;
            double lc    = VecX.Cross(p - poly[lastI], p - poly[0]);

            for (int i = 0; i < lastI; i++)
            {
                double c = VecX.Cross(p - poly[i], p - poly[i + 1]);
                if (c * lc < 0.0)
                {
                    return(false);
                }
                lc = c;
            }
            return(true);
        }
Beispiel #17
0
        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);
            }
        }
Beispiel #18
0
        public static Vec2 Distance(Vec2 p, Triangle <Vec2> tri)
        {
            Vec2 dn = Distance(p - tri.p0, tri.p1 - tri.p0);
            Vec2 n1 = Distance(p - tri.p1, tri.p2 - tri.p1);
            Vec2 n2 = Distance(p - tri.p2, tri.p0 - tri.p2);

            if (VecX.SqrLength(n1) < VecX.SqrLength(dn))
            {
                dn = n1;
            }
            if (VecX.SqrLength(n2) < VecX.SqrLength(dn))
            {
                dn = n2;
            }
            return(dn);
        }
Beispiel #19
0
 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);
 }
Beispiel #20
0
        public static bool LineSegsIntersect(LineSeg <Vec2> s0, LineSeg <Vec2> s1)
        {
            Vec2 vp0 = s0.p1 - s0.p0;
            Vec2 vp1 = s1.p0 - s0.p0;
            Vec2 vp2 = s1.p1 - s0.p0;

            if (VecX.Cross(vp0, vp1) * VecX.Cross(vp0, vp2) > 0)
            {
                return(false);
            }
            Vec2 vq0 = s1.p1 - s1.p0;
            Vec2 vq1 = s0.p0 - s1.p0;
            Vec2 vq2 = s0.p1 - s1.p0;

            if (VecX.Cross(vq0, vq1) * VecX.Cross(vq0, vq2) > 0)
            {
                return(false);
            }
            return(true);
        }
Beispiel #21
0
        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);
        }
Beispiel #22
0
        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);
        }
Beispiel #23
0
        public static LineSeg <Vec3> Distance(DirLineSeg <Vec3> u, DirLineSeg <Vec3> v)
        {
            LineSeg <Vec3> r;
            Vec3           dn1 = Distance(v.p - u.p, u.d);
            Vec3           dn2 = Distance(v.p + v.d - u.p, u.d);
            Vec3           dn3 = Distance(u.p - v.p, v.d);
            Vec3           dn4 = Distance(u.p + u.d - v.p, v.d);
            double         d1  = VecX.SqrLength(dn1);
            double         d2  = VecX.SqrLength(dn2);
            double         d3  = VecX.SqrLength(dn3);
            double         d4  = VecX.SqrLength(dn4);

            if (d1 < d2 && d1 < d3 && d1 < d4)
            {
                r.p1 = v.p; r.p0 = r.p1 - dn1;
            }
            else if (d2 < d3 && d2 < d4)
            {
                r.p1 = v.p + v.d; r.p0 = r.p1 - dn2;
            }
            else if (d3 < d4)
            {
                r.p0 = u.p; r.p1 = r.p0 - dn3;
            }
            else
            {
                r.p0 = u.p + u.d; r.p1 = r.p0 - dn4;
            }
            double s, t;

            if (CommonPerpendicular(u, v, out s, out t) &&
                s > 0.0 && s < 1.0 && t > 0.0 && t < 1.0)
            {
                r.p0 = u.p + s * u.d;
                r.p1 = v.p + t * v.d;
            }
            return(r);
        }
Beispiel #24
0
        public static LineSeg <T> Distance <T>(DirLineSeg <T> u, DirLineSeg <T> v) where T : IVector, new()
        {
            LineSeg <T> r;
            T           dn1 = Distance(v.p.Sub(u.p), u.d);
            T           dn2 = Distance(v.p.Add(v.d).Sub(u.p), u.d);
            T           dn3 = Distance(u.p.Sub(v.p), v.d);
            T           dn4 = Distance(u.p.Add(u.d).Sub(v.p), v.d);
            double      d1  = VecX.SqrLength(dn1);
            double      d2  = VecX.SqrLength(dn2);
            double      d3  = VecX.SqrLength(dn3);
            double      d4  = VecX.SqrLength(dn4);

            if (d1 < d2 && d1 < d3 && d1 < d4)
            {
                r.p1 = v.p; r.p0 = r.p1.Sub(dn1);
            }
            else if (d2 < d3 && d2 < d4)
            {
                r.p1 = v.p.Add(v.d); r.p0 = r.p1.Sub(dn2);
            }
            else if (d3 < d4)
            {
                r.p0 = u.p; r.p1 = r.p0.Sub(dn3);
            }
            else
            {
                r.p0 = u.p.Add(u.d); r.p1 = r.p0.Sub(dn4);
            }
            double s, t;

            if (CommonPerpendicular(u, v, out s, out t) &&
                s > 0.0 && s < 1.0 && t > 0.0 && t < 1.0)
            {
                r.p0 = u.d.Mul(s).Add(u.p);
                r.p1 = v.d.Mul(t).Add(v.p);
            }
            return(r);
        }
Beispiel #25
0
        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);
        }
Beispiel #26
0
 public static bool LineSegsIntersection(LineSeg <Vec2> s0, LineSeg <Vec2> s1, out Vec2 cp)
 {
     if (LineSegsIntersect(s0, s1))
     {
         double p0x = s0.p0.x;
         double p0y = s0.p0.y;
         double q0x = s1.p0.x;
         double q0y = s1.p0.y;
         double pvx = s0.p1.x - p0x;
         double pvy = s0.p1.y - p0y;
         double qvx = s1.p1.x - q0x;
         double qvy = s1.p1.y - q0y;
         double det = VecX.Cross(new Vec2(pvx, pvy), new Vec2(qvx, qvy));
         cp.x = -(pvx * qvx * (q0y - p0y) + pvy * qvx * p0x - qvy * pvx * q0x) / det;
         cp.y = (pvy * qvy * (q0x - p0x) + pvx * qvy * p0y - qvx * pvy * q0y) / det;
         return(true);
     }
     else
     {
         cp = default(Vec2);
         return(false);
     }
 }