Beispiel #1
0
        public static Trans3 GetTransformNoScale(Vector from1, Vector from2, Vector from3, Vector to1, Vector to2, Vector to3)
        {
            //HTLib.DoubleVector3 lfrom1 = new HTLib.DoubleVector3(from1);
            //HTLib.DoubleVector3 lfrom2 = new HTLib.DoubleVector3(from2);
            //HTLib.DoubleVector3 lfrom3 = new HTLib.DoubleVector3(from3);
            //HTLib.DoubleVector3 lto1   = new HTLib.DoubleVector3(to1);
            //HTLib.DoubleVector3 lto2   = new HTLib.DoubleVector3(to2);
            //HTLib.DoubleVector3 lto3   = new HTLib.DoubleVector3(to3);
            //HTLib.Trans3        ltrans = HTLib.Trans3.GetTransformNoScale(lto1, lto2, lto3, lfrom1, lfrom2, lfrom3);
            Trans3 ltrans;

            {
                Vector[] p        = new Vector[] { to1, to2, to3 };
                Vector[] x        = new Vector[] { from1, from2, from3 };
                Trans3   optTrans = ICP3.OptimalTransform(p, x);
                ltrans = optTrans;
            }

            if (HDebug.IsDebuggerAttached)
            {
                var nto1 = ltrans.DoTransform(from1); var err1 = (nto1 - to1).Dist;
                var nto2 = ltrans.DoTransform(from2); var err2 = (nto2 - to2).Dist;
                var nto3 = ltrans.DoTransform(from3); var err3 = (nto3 - to3).Dist;
                HDebug.AssertTolerance(0.00000001, err1, err2, err3);
            }
            return(ltrans);
        }
Beispiel #2
0
        public static Tuple <Vector, Vector, Vector, Vector> Points4TetrahedronEquilateral()
        {
            /// https://en.wikipedia.org/wiki/Tetrahedron
            /// Expressed symmetrically as 4 points on the unit sphere,
            /// centroid at the origin, with lower face level, the vertices are:
            /// v1 = ( sqrt(8 / 9),            0, -1 / 3)
            /// v2 = (-sqrt(2 / 9),  sqrt(2 / 3), -1 / 3)
            /// v3 = (-sqrt(2 / 9), -sqrt(2 / 3), -1 / 3)
            /// v4 = (           0,            0,      1)

            // determine point locations of equilateral tetrahedron
            Vector v1 = new double[3] {
                0, Math.Sqrt(8.0 / 9), -1.0 / 3
            };
            Vector v2 = new double[3] {
                Math.Sqrt(2.0 / 3), -Math.Sqrt(2.0 / 9), -1.0 / 3
            };
            Vector v3 = new double[3] {
                -Math.Sqrt(2.0 / 3), -Math.Sqrt(2.0 / 9), -1.0 / 3
            };
            Vector v4 = new double[3] {
                0, 0, 1
            };
            // scale to make each edge's length as 1
            double d12 = (v1 - v2).Dist;

            v1 = v1 / d12;  v2 = v2 / d12;  v3 = v3 / d12;  v4 = v4 / d12;
            // make v3 as origin
            v1 -= v3;
            v2 -= v3;
            v4 -= v3;
            v3 -= v3;
            // check length
            HDebug.AssertTolerance(0.00000001, 1 - (v1 - v2).Dist);
            HDebug.AssertTolerance(0.00000001, 1 - (v1 - v3).Dist);
            HDebug.AssertTolerance(0.00000001, 1 - (v1 - v4).Dist);
            HDebug.AssertTolerance(0.00000001, 1 - (v2 - v3).Dist);
            HDebug.AssertTolerance(0.00000001, 1 - (v2 - v4).Dist);
            HDebug.AssertTolerance(0.00000001, 1 - (v3 - v4).Dist);

            // check height (v1.z == v2.z == v3.z)
            HDebug.AssertTolerance(0.00000001, v1[2] - v2[2]);
            HDebug.AssertTolerance(0.00000001, v1[2] - v2[2]);
            HDebug.AssertTolerance(0.00000001, v1[2] - v3[2]);
            // check others
            HDebug.AssertTolerance(0.00000001, v1[2] - 0); // v1,v2,v3 are on z-plane
            HDebug.AssertTolerance(0.00000001, v2[2] - 0); // v1,v2,v3 are on z-plane
            HDebug.AssertTolerance(0.00000001, v3[2] - 0); // v1,v2,v3 are on z-plane
            HDebug.AssertTolerance(0.00000001, v2[1] - 0); // v2,v3 are on x-axis
            HDebug.AssertTolerance(0.00000001, v3[1] - 0); // v2,v3 are on x-axis
            HDebug.AssertTolerance(0.00000001, v2[0] - 1); // v3 is on (1,0,0)
            ///       v1
            ///      /  \
            ///     /    \
            ///    /  v4  \
            ///   /        \
            /// v3----------v2
            ///
            return(new Tuple <Vector, Vector, Vector, Vector>(v1, v2, v3, v4));
        }
Beispiel #3
0
 public static IList <Vector> GetOrthonormals(this IList <Vector> vectors)
 {
     Vector[] omvecs = new Vector[vectors.Count];
     omvecs[0] = vectors[0].UnitVector();
     for (int i = 1; i < vectors.Count; i++)
     {
         Vector nomvec = vectors[i].UnitVector();
         foreach (Vector omvec in omvecs)
         {
             if (omvec == null)
             {
                 continue;
             }
             double proj = LinAlg.DotProd(nomvec, omvec);
             nomvec = (nomvec - proj * omvec).UnitVector();
         }
         omvecs[i] = nomvec.UnitVector();
     }
     if (HDebug.IsDebuggerAttached)
     {
         for (int i = 0; i < omvecs.Length - 1; i++)
         {
             for (int j = i + 1; j < omvecs.Length; j++)
             {
                 double proj = LinAlg.DotProd(omvecs[i].UnitVector(), omvecs[j].UnitVector());
                 HDebug.AssertTolerance(0.00000001, proj);
             }
         }
     }
     return(omvecs);
 }
Beispiel #4
0
            public static double GetD2LengthByDAngle2(double a, double b, double c)
            {
                HDebug.ToDo("check !!!");
                /// return d^2_a / d_A^2
                ///
                ///     A = acos(b^2 + c^2 - a^2 / 2 b c)
                ///  cosA = (b^2 + c^2 - a^2 / 2 b c)
                ///   a^2 = b^2 + c^2 - 2 b c cos(A)
                ///   la  = sqrt(b^2 + c^2 - 2 b c cos(A))
                ///       == a
                /// da_dA = (b c Sin[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]]
                ///       = (b c Sin[A])/(                          la)
                /// d2a_dA2 = d_dA (da_dA)
                ///         = (b c Cos[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] - (b^2 c^2 Sin[A]^2)/(b^2 + c^2 - 2 b c Cos[A])^(3/2)
                ///         = (b c Cos[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] - (b^2 c^2 Sin[A]^2)/(Sqrt[b^2 + c^2 - 2 b c Cos[A]]^3)
                ///         = (b c Cos[A])/(                          la) - (b^2 c^2 Sin[A]^2)/(                            la^3)
                ///         = (b c cosA  )/(                          la) - (b2  c2  sinA2   )/(                            la^3)
                ///
                double a2   = a * a;
                double b2   = b * b;
                double c2   = c * c;
                double cosA = (b2 + c2 - a2) / (2 * b * c);
                double la   = Math.Sqrt(b2 + c2 - 2 * b * c * cosA);
                double la3  = la * la * la;

                HDebug.AssertTolerance(0.00000001, la - a);
                double A       = Math.Acos(cosA);
                double sinA    = Math.Sin(A);
                double sinA2   = sinA * sinA;
                double da_dA   = (b * c * sinA) / la;
                double d2a_dA2 = (b * c * cosA) / (la) - (b2 * c2 * sinA2) / (la3);

                return(d2a_dA2);
            }
        public static Vector Point4LinePlaneIntersect(Vector planeNormal, Vector planePoint, Vector lineDire, Vector linePoint)
        {
            if (Point4LinePlaneIntersect_SelfTest)
            {
                Point4LinePlaneIntersect_SelfTest = false;
                Vector _plnm  = new double[] { 1, 0, 0 };
                Vector _plpt  = new double[] { 2, 0, 0 };
                Vector _lndr  = new double[] { 1, 1, 1 };
                Vector _lnpt  = new double[] { 0, 0, 0 };
                Vector _itpt0 = Point4LinePlaneIntersect(_plnm, _plpt, _lndr, _lnpt);
                Vector _itpt1 = new double[] { 2, 2, 2 };
                HDebug.AssertTolerance(0.000001, _itpt0 - _itpt1);
            }

            /// http://en.wikipedia.org/wiki/Line-plane_intersection
            /// Algebraic form
            /// plane : (pt - p0).n = 0
            /// line  :  pt = d l + l0
            ///
            /// point-line intersection: d = ((p0 - l0) . n) / (l . n)
            ///                          pt = d * l + l0
            Vector n  = planeNormal;
            Vector p0 = planePoint;
            Vector l  = lineDire;
            Vector l0 = linePoint;
            double d  = LinAlg.DotProd(p0 - l0, n) / LinAlg.DotProd(l, n);
            Vector pt = d * l + l0;

            HDebug.AssertTolerance(0.00000001, LinAlg.DotProd(pt - p0, n));
            HDebug.AssertTolerance(0.00000001, pt - (d * l + l0));
            return(pt);
        }
Beispiel #6
0
        public static Vector DV(Matrix D, Vector V, bool assertDiag = true)
        {
            if (DV_SelfTest1)
            {
                DV_SelfTest1 = false;
                MatrixByArr tD = new double[3, 3] {
                    { 1, 0, 0 }, { 0, 2, 0 }, { 0, 0, 3 }
                };
                Vector tV = new double[3] {
                    1, 2, 3
                };
                Vector tDV = new double[3] {
                    1, 4, 9
                };
                // [1 0 0]   [1]   [1]
                // [0 2 0] * [2] = [4]
                // [0 0 3]   [3]   [9]
                HDebug.AssertTolerance(double.Epsilon, DV(tD, tV) - tDV);
            }
            // D is the diagonal matrix
            HDebug.Assert(D.ColSize == D.RowSize);
            if (assertDiag) // check diagonal matrix
            {
                HDebug.AssertToleranceMatrix(double.Epsilon, D - Diag(Diag(D)));
            }
            HDebug.Assert(D.ColSize == V.Size);
            Vector diagD  = Diag(D);
            Vector diagDV = DV(diagD, V);

            return(diagDV);
        }
Beispiel #7
0
        public static double AngleBetween(Vector a, Vector b, Vector c)
        {
            if (AngleBetween_selftest2)
            {
                AngleBetween_selftest2 = false;
                Vector ta, tb, tc;
                double tang;
                ta   = new double[] { 1, 0, 0 };
                tb   = new double[] { 0, 0, 0 };
                tc   = new double[] { 0, 1, 0 };
                tang = AngleBetween(ta, tb, tc);
                HDebug.AssertTolerance(0.0001, Math.PI / 2 - tang);
                ta   = new double[] { 0, 1, 0 };
                tb   = new double[] { 0, 0, 0 };
                tc   = new double[] { 0, 1, 1 };
                tang = AngleBetween(ta, tb, tc);
                HDebug.AssertTolerance(0.0001, Math.PI / 4 - tang);
            }
            Vector left  = a - b;
            Vector right = c - b;
            double ang   = AngleBetween(left, right);

            HDebug.Assert(ang >= 0);
            HDebug.Assert(ang <= Math.PI);
            return(ang);
        }
Beispiel #8
0
        //public static double Cov(Vector vec1, Vector vec2)
        //{
        //    return Cov(vec1.ToArray().ToList(), vec2.ToArray().ToList());
        //}
        public static double HCov(double[] vec1, double[] vec2)
        {
            if (HDebug.Selftest())
            {
                // check with mathematica
                double tcov = HCov(new double[] { 1, 2, 3 }, new double[] { 3, 7, 4 });
                double terr = 0.5 - tcov;
                HDebug.AssertTolerance(0.00000001, terr);
            }
            if (vec1.Length != vec2.Length)
            {
                throw new Exception();
            }
            double avg1 = vec1.HAvg();
            double avg2 = vec2.HAvg();
            double cov  = 0;
            int    size = vec1.Length;

            for (int i = 0; i < size; i++)
            {
                cov += (vec1[i] - avg1) * (vec2[i] - avg2);
            }
            cov = cov / (size - 1); /// the unbiased estimate of the covariance, which divide by (n-1)
            return(cov);
        }
Beispiel #9
0
        public static Vector DV(Vector D, Vector V, bool assertDiag = true)
        {
            if (DV_SelfTest2)
            {
                DV_SelfTest2 = false;
                Vector tD = new double[3] {
                    1, 2, 3
                };
                Vector tV = new double[3] {
                    1, 2, 3
                };
                Vector tDV = new double[3] {
                    1, 4, 9
                };
                // [1 0 0]   [1]   [1]
                // [0 2 0] * [2] = [4]
                // [0 0 3]   [3]   [9]
                HDebug.AssertTolerance(double.Epsilon, DV(tD, tV) - tDV);
            }
            // D is the diagonal matrix
            HDebug.Assert(D.Size == V.Size);
            int    size = V.Size;
            Vector dv   = new double[size];

            for (int i = 0; i < size; i++)
            {
                dv[i] = D[i] * V[i];
            }
            return(dv);
        }
Beispiel #10
0
        public static double V1tD2V3(Vector V1, Matrix D2, Vector V3, bool assertDiag = true)
        {
            if (V1tD2V3_SelfTest)
            {
                V1tD2V3_SelfTest = false;
                Vector tV1 = new double[3] {
                    1, 2, 3
                };
                MatrixByArr tD2 = new double[3, 3] {
                    { 2, 0, 0 }, { 0, 3, 0 }, { 0, 0, 4 }
                };
                Vector tV3 = new double[3] {
                    3, 4, 5
                };
                //           [2    ]   [3]             [ 6]
                // [1 2 3] * [  3  ] * [4] = [1 2 3] * [12] = 6+24+60 = 90
                //           [    4]   [5]             [20]
                double tV1tD2V3 = 90;
                HDebug.AssertTolerance(double.Epsilon, tV1tD2V3 - V1tD2V3(tV1, tD2, tV3));
            }
            if (assertDiag) // check diagonal matrix
            {
                HDebug.AssertToleranceMatrix(double.Epsilon, D2 - Diag(Diag(D2)));
            }
            HDebug.Assert(V1.Size == D2.ColSize);
            HDebug.Assert(D2.RowSize == V3.Size);

            Vector lD2V3    = DV(D2, V3, assertDiag);
            double lV1tD2V3 = VtV(V1, lD2V3);

            return(lV1tD2V3);
        }
        public static Vector ClosestPointOnLine(Vector A, Vector B, Vector P, bool segmentClamp)
        {
            // http://www.gamedev.net/community/forums/topic.asp?topic_id=444154
            HDebug.AssertAnd(A.Size == B.Size, B.Size == P.Size);
            Vector AP  = P - A;
            Vector AB  = B - A;
            double ab2 = LinAlg.VtV(AB, AB);// AB.x*AB.x + AB.y*AB.y;

            if (ab2 == 0)
            {
                return(A.Clone());
            }
            double ap_ab = LinAlg.VtV(AP, AB); //AP.x*AB.x + AP.y*AB.y;
            double t0    = ap_ab / ab2;
            double t     = t0;

            if (segmentClamp)
            {
                if (t < 0.0)
                {
                    t = 0.0;
                }
                else if (t > 1.0)
                {
                    t = 1.0;
                }
            }
            Vector Closest = A + AB * t;

            HDebug.AssertTolerance(0.000000001, LinAlg.VtV(B - A, P - (A + AB * t0)));
            return(Closest);
        }
        public static PClosestPointOnSegment ClosestPointOnSegment(IList <Vector> ABs, Vector Pt)
        {
            HDebug.Assert(ABs.Count >= 2);
            Vector closest = ABs[0];
            //Tuple<Vector,Vector> closest_segment = new Tuple<Vector,Vector>(ABs[0], ABs[1]);
            double closest_dist2 = (closest - Pt).Dist2;
            double closest_dist  = Math.Sqrt(closest_dist2);
            int    closest_iAB   = 0;

            for (int i = 1; i < ABs.Count; i++)
            {
                int    iAB     = i - 1;
                Vector A       = ABs[iAB];
                Vector B       = ABs[iAB + 1];
                Vector C       = ClosestPointOnLine(A, B, Pt, true);
                double C_dist2 = (C - Pt).Dist2;
                if (C_dist2 < closest_dist2)
                {
                    closest       = C;
                    closest_dist2 = C_dist2;
                    closest_dist  = Math.Sqrt(C_dist2);
                    closest_iAB   = iAB;
                    //closest_segment = new Tuple<Vector, Vector>(A, B);
                }
            }
            HDebug.AssertTolerance(0.00000001, closest_dist - DistPointSegment(Pt, ABs));
            return(new PClosestPointOnSegment
            {
                point = closest,
                dist = closest_dist,
                iAB = closest_iAB,
                //segment = closest_segment,
            });
        }
Beispiel #13
0
        public static Vector CrossProd3(Vector v1, Vector v2)
        {
            // http://en.wikipedia.org/wiki/Cross_product
            HDebug.Assert(v1.Size == 3);
            HDebug.Assert(v2.Size == 3);
            if (CrossProd3_SelfTest)
            {
                CrossProd3_SelfTest = false;
                Vector tv1 = new double[] { 1, 2, 3 };
                Vector tv2 = new double[] { 4, 5, 6 };
                Vector tv  = CrossProd3(tv1, tv2);
                HDebug.AssertTolerance(0, tv - new Vector(-3, 6, -3));
            }
            Vector cross = new double[3];

            cross[0] = v1[1] * v2[2] - v1[2] * v2[1];
            cross[1] = v1[2] * v2[0] - v1[0] * v2[2];
            cross[2] = v1[0] * v2[1] - v1[1] * v2[0];
            return(cross);
            //HDebug.Assert(left.Size == 3, right.Size == 3);
            //return new double[]{
            //                left[1]*right[2] - left[2]*right[1],
            //                left[2]*right[0] - left[0]*right[2],
            //                left[0]*right[1] - left[1]*right[0]
            //                };
        }
Beispiel #14
0
        public static Trans3 AppendTrans(Trans3 _trans, Trans3 dtrans)
        {
            //Trans3 newtrans = new Trans3(HTLib.Trans3.GetTransform(dtrans.trans, trans.trans));
            Trans3 newtrans;

            {   //public static Trans3 GetTransform(Trans3 trans2, Trans3 trans1)
                Trans3 trans2 = dtrans;
                Trans3 trans1 = _trans;
                // trans2 * trans2
                //          DoubleMatrix4 mat2 = trans2.TransformMatrix;
                //          DoubleMatrix4 mat1 = trans1.TransformMatrix;
                //          DoubleMatrix4 trans_mat = mat2*mat1;
                //          DoubleVector3 dt = new DoubleVector3(trans_mat.m03, trans_mat.m13, trans_mat.m23);
                Vector     dt = trans2.DoTransform(trans1.dt);
                double     ds = trans2.ds * trans1.ds;
                Quaternion dr = trans2.dr * trans1.dr;    //Quaternion.GetQuaternion(trans_mat, ds);

                Trans3 trans = new Trans3(dt, ds, dr);

                if (HDebug.IsDebuggerAttached)
                {
                    Vector pt1, pt2, pt12;
                    pt1  = new double[] { 10, 0, 0 };
                    pt2  = trans2.DoTransform(trans1.DoTransform(pt1));
                    pt12 = trans.DoTransform(pt1);
                    HDebug.Assert((pt12 - pt2).Dist < 0.001);
                    pt1  = new double[] { 0, 10, 0 };
                    pt2  = trans2.DoTransform(trans1.DoTransform(pt1));
                    pt12 = trans.DoTransform(pt1);
                    HDebug.Assert((pt12 - pt2).Dist < 0.001);
                    pt1  = new double[] { 0, 0, 10 };
                    pt2  = trans2.DoTransform(trans1.DoTransform(pt1));
                    pt12 = trans.DoTransform(pt1);
                    HDebug.Assert((pt12 - pt2).Dist < 0.001);
                }

                //return trans;
                newtrans = trans;
            }

            if (HDebug.IsDebuggerAttached)
            {
                MatrixByArr mat    = _trans.TransformMatrix;
                MatrixByArr dmat   = dtrans.TransformMatrix;
                MatrixByArr newmat = newtrans.TransformMatrix;
                MatrixByArr diff   = newmat - dmat * mat;
                HDebug.AssertTolerance(0.00001, diff);
            }
            return(newtrans);
        }
        public static Vector RotateTangentUnit(Vector pt, Vector axis1, Vector axis2)
        {
            Vector pt_on_axis   = ClosestPointOnLine(axis1, axis2, pt, false);
            Vector direct_pt    = (pt - pt_on_axis).UnitVector();
            Vector direct_axis  = (axis2 - axis1).UnitVector();
            Vector direct_tanpt = LinAlg.CrossProd(direct_axis, direct_pt).UnitVector();

            if (HDebug.IsDebuggerAttached)
            {
                Vector dbg_axis = LinAlg.CrossProd(direct_pt, direct_tanpt).UnitVector();
                HDebug.AssertTolerance(0.00000001, (direct_axis - dbg_axis).Dist);
            }
            return(direct_tanpt);
        }
Beispiel #16
0
            public static PlatonicSolid CloneByRadius(PlatonicSolid obj, double radius)
            {
                Vector[] verts = obj.verts.HCloneVectors().ToArray();
                double   rad   = verts[0].Dist;

                for (int i = 0; i < verts.Length; i++)
                {
                    HDebug.AssertTolerance(0.00000001, rad - verts[i].Dist);
                    verts[i] = verts[i] * (radius / rad);
                }
                return(new PlatonicSolid
                {
                    verts = verts
                });
            }
Beispiel #17
0
        public static Vector[] ExtendSegmentBegin(IList <Vector> segment, double extlength)
        {
            List <Vector> extsegment = segment.HCloneVectors().ToList();

            Vector vec10 = (segment[0] - segment[1]).UnitVector();

            extsegment.Insert(0, extsegment[0] + vec10 * extlength);

            if (HDebug.IsDebuggerAttached)
            {
                Vector vec01 = (extsegment[0] - extsegment[1]).UnitVector();
                Vector vec12 = (extsegment[1] - extsegment[2]).UnitVector();
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(vec01, vec12) - 1);
            }
            return(extsegment.ToArray());
        }
Beispiel #18
0
        public static Tuple <Vector, Vector> LineOnTwoPlanes(Vector norm1, double val1, Vector norm2, double val2)
        {
            /// http://intumath.org/Math/Geometry/Analytic%20Geometry/plane-planeinter.html
            ///
            if (LineOnTwoPlanes_SelfTest)
            {
                LineOnTwoPlanes_SelfTest = false;
                Vector tn1 = new double[] { 1, 2, 3 };
                Vector tn2 = new double[] { 2, 5, 3 };
                double td1 = 10;
                double td2 = 2;
                LineOnTwoPlanes(tn1, td1, tn2, td2);
            }

            Vector norm = LinAlg.CrossProd(norm1, norm2);
            /// norm1' * pt = -val1      [n1x n1y n1z]   [ptx]   [-val1]      [n1x n1y 0]   [ptx]   [-val1]
            /// norm2' * pt = -val2  =>  [n2x n2y n2z] * [pty] = [-val2]  =>  [n2x n2y 0] * [pty] = [-val2]
            ///                                          [ptz]                              [0  ]
            ///                      =>  [n1x n1y] * [ptx] = [-val1]  =>  [n1x n1y] * [ptx] = [-val1]
            ///                          [n2x n2y]   [pty]   [-val2]      [n2x n2y]   [pty]   [-val2]
            ///                      =>  [ptx] = [n1x n1y]-1  [-val1] = [ n2y -n1y] * [-val1] / (n1x*n2y - n1y*n2x)
            ///                          [pty]   [n2x n2y]  * [-val2]   [-n2x  n1x]   [-val2]
            ///                                = [ n2y*-val1 + -n1y*-val2 ] / (n1x*n2y - n1y*n2x)
            ///                                  [-n2x*-val1 +  n1x*-val2 ]
            double n1x = norm1[0], n1y = norm1[1];
            double n2x = norm2[0], n2y = norm2[1];
            double div = n1x * n2y - n1y * n2x;
            Vector pt  = new double[] { (n2y * -val1 + -n1y * -val2) / div,
                                        (-n2x * -val1 + n1x * -val2) / div,
                                        0 };

            if (HDebug.IsDebuggerAttached)
            {
                Vector dbg_pt;
                dbg_pt = pt + 1 * norm;
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
                dbg_pt = pt + 2 * norm;
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
                dbg_pt = pt - 2 * norm;
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
            }
            return(new Tuple <Vector, Vector>(pt, norm));
        }
Beispiel #19
0
        //public static double Corr(Vector vec1, Vector vec2)
        //{
        //    return Corr(vec1.ToArray().ToList(), vec2.ToArray().ToList());
        //}
        public static double HCorr(double[] vec1, double[] vec2)
        {
            if (HDebug.Selftest())
            {
                // check with mathematica
                double tcorr = HCorr(new double[] { 1, 2, 3 }, new double[] { 3, 7, 4 });
                double terr  = 0.2401922307076307 - tcorr;
                HDebug.AssertTolerance(0.00000001, terr);
            }
            if (vec1.Length != vec2.Length)
            {
                throw new Exception();
            }
            double corr = HCov(vec1, vec2) / Math.Sqrt(vec1.HVar() * vec2.HVar());

            return(corr);
        }
Beispiel #20
0
        public static double HVar(this IList <double> values)
        {
            if (HDebug.Selftest())
            {
                double tvar = (new double[] { 1, 2, 3, 4, 5 }).HVar();
                double terr = 2.5 - tvar;
                HDebug.AssertTolerance(0.0000000001, terr);
            }
            double avg = values.HAvg();
            double var = 0;

            foreach (double value in values)
            {
                var += (avg - value) * (avg - value);
            }
            var /= (values.Count - 1); /// the unbiased estimate of variance, which divide by (n-1)
            return(var);
        }
Beispiel #21
0
        public static Vector[] ExtendSegmentEnd(IList <Vector> segment, double extlength)
        {
            List <Vector> extsegment = segment.HCloneVectors().ToList();

            int    cnt   = segment.Count;
            Vector vec89 = (segment[cnt - 1] - segment[cnt - 2]).UnitVector();

            extsegment.Add(extsegment[cnt - 1] + vec89 * extlength);

            if (HDebug.IsDebuggerAttached)
            {
                int    extcnt   = extsegment.Count;
                Vector extvec78 = (extsegment[extcnt - 3] - extsegment[extcnt - 2]).UnitVector();
                Vector extvec89 = (extsegment[extcnt - 2] - extsegment[extcnt - 1]).UnitVector();
                HDebug.AssertTolerance(0.00000001, LinAlg.VtV(extvec78, extvec89) - 1);
            }
            return(extsegment.ToArray());
        }
Beispiel #22
0
        public static MatrixByArr Eye(int size, double diagval = 1)
        {
            if (Eye_SelfTest)
            {
                Eye_SelfTest = false;
                MatrixByArr tT0 = new double[3, 3] {
                    { 2, 0, 0 }, { 0, 2, 0 }, { 0, 0, 2 },
                };
                MatrixByArr tT1 = Eye(3, 2);
                HDebug.AssertTolerance(double.Epsilon, tT0 - tT1);
            }
            MatrixByArr mat = new MatrixByArr(size, size);

            for (int i = 0; i < size; i++)
            {
                mat[i, i] = diagval;
            }
            return(mat);
        }
Beispiel #23
0
 public static T[] Differences <T>(this IList <T> values)
 {
     if (Differences_SelfTest)
     {
         Differences_SelfTest = false;
         double[] tvals = new double[] { 0, 1, 3, 6, 10, 15, 21 };
         Vector   tacs0 = new double[] { 1, 2, 3, 4, 5, 6 };
         Vector   tacs1 = Differences(tvals);
         HDebug.AssertTolerance(double.Epsilon, tacs0 - tacs1);
     }
     T[] diffs = new T[values.Count - 1];
     for (int i = 0; i < diffs.Length; i++)
     {
         dynamic val0 = values[i];
         dynamic val1 = values[i + 1];
         diffs[i] = (val1 - val0);
     }
     return(diffs);
 }
Beispiel #24
0
 public static T[] HAccumulate <T>(this IList <T> values)
 {
     if (HAccumulate_SelfTest)
     {
         HAccumulate_SelfTest = false;
         double[] tvals = new double[] { 0, 1, 2, 4, 6, 9, 12 };
         Vector   tacs0 = new double[] { 0, 1, 3, 7, 13, 22, 34 };
         Vector   tacs1 = HAccumulate(tvals);
         HDebug.AssertTolerance(double.Epsilon, tacs0 - tacs1);
     }
     T[] accs = new T[values.Count];
     accs[0] = values[0];
     for (int i = 1; i < accs.Length; i++)
     {
         dynamic val = values[i];
         accs[i] = (val + accs[i - 1]);
     }
     return(accs);
 }
Beispiel #25
0
        public static MatrixByArr AlterDotProd(Vector v1, Vector v2)
        {
            HDebug.ToDo("depreciated. Call VVt(v1,v2)");
            if (HDebug.IsDebuggerAttached && HDebug.Selftest())
            {
                MatrixByArr M0 = AlterDotProd(v1, v2);
                MatrixByArr M1 = VVt(v1, v2);
                HDebug.AssertTolerance(0, M0 - M1);
            }

            MatrixByArr mat = new MatrixByArr(v1.Size, v2.Size);

            for (int c = 0; c < mat.ColSize; c++)
            {
                for (int r = 0; r < mat.RowSize; r++)
                {
                    mat[c, r] = v1[c] * v2[r];
                }
            }
            return(mat);
        }
Beispiel #26
0
 public override Tuple <ILinAlgMat, double[]> _EigSymm(ILinAlgMat A)
 {
     using (new Matlab.NamedLock("LA"))
     {
         Matlab.PutMatrix("LA.A", A.ToArray());
         Matlab.Execute("LA.A = (LA.A + LA.A)/2;");
         Matlab.Execute("[LA.V, LA.D] = eig(LA.A);");
         Matlab.Execute("LA.D = diag(LA.D);");
         CMatrix  V = Matlab.GetMatrix("LA.V");
         double[] D = Matlab.GetVector("LA.D");
         if (HDebug.IsDebuggerAttached)
         {
             Matlab.Execute("LA.ERR = LA.A - (LA.V * diag(LA.D) * LA.V');");
             Matlab.Execute("LA.ERR = max(max(abs(LA.ERR)));");
             double err = Matlab.GetValue("LA.ERR");
             HDebug.AssertTolerance(0.00000001, err);
         }
         Matlab.Clear();
         return(new Tuple <ILinAlgMat, double[]>(V, D));
     }
 }
Beispiel #27
0
        public static Vector MV <MATRIX>(MATRIX lmat, Vector rvec, string options = "")
            where MATRIX : IMatrix <double>
        {
            Vector result = new Vector(lmat.ColSize);

            MV(lmat, rvec, result, options);
            if (MV_SelfTest_lmat_rvec)
            {
                MV_SelfTest_lmat_rvec = false;
                HDebug.Assert(lmat.RowSize == rvec.Size);
                Vector lresult = new Vector(lmat.ColSize);
                for (int c = 0; c < lmat.ColSize; c++)
                {
                    for (int r = 0; r < lmat.RowSize; r++)
                    {
                        lresult[c] += lmat[c, r] * rvec[r];
                    }
                }
                HDebug.AssertTolerance(double.Epsilon, lresult - result);
            }
            return(result);
        }
Beispiel #28
0
        public static MatrixByArr Diag(this Vector d)
        {
            if (Diagd_SelfTest)
            {
                Diagd_SelfTest = false;
                MatrixByArr tD1 = new double[3, 3] {
                    { 1, 0, 0 }, { 0, 2, 0 }, { 0, 0, 3 }
                };
                Vector td1 = new double[3] {
                    1, 2, 3
                };
                MatrixByArr tD = Diag(td1);
                HDebug.AssertTolerance(double.Epsilon, tD - tD1);
            }
            int         size = d.Size;
            MatrixByArr D    = new MatrixByArr(size, size);

            for (int i = 0; i < size; i++)
            {
                D[i, i] = d[i];
            }
            return(D);
        }
Beispiel #29
0
        public static double DistancePointLine(Vector point, Vector line1, Vector line2)
        {
            // http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
            if (DistancePointLine_selftest)
            {
                DistancePointLine_selftest = false;
                Vector tp     = new Vector(2, 2, 0);
                Vector tl1    = new Vector(0, 0, -20);
                Vector tl2    = new Vector(0, 0, -10);
                double tdist0 = tp.Dist;
                double tdist1 = DistancePointLine(tp, tl1, tl2);
                double tdist2 = (ClosestPointOnLine(tl1, tl2, tp, false) - tp).Dist;
                HDebug.AssertTolerance(0.00000001, tdist0 - tdist1);
                HDebug.AssertTolerance(0.00000001, tdist0 - tdist2);
            }
            Vector x21 = line2 - line1;
            Vector x10 = line1 - point;
            double dist
                = LinAlg.CrossProd(x21, x10).Dist
                  / x21.Dist;

            return(dist);
        }
Beispiel #30
0
        public static MatrixByArr InvSymm(MatrixByArr A)
        {
            if (HDebug.Selftest())
            {
                MatrixByArr tA = new double[, ] {
                    { 1, 2, 3 },
                    { 2, 9, 5 },
                    { 3, 5, 6 }
                };
                MatrixByArr tB0 = new double[, ] {
                    { -1.8125, -0.1875, 1.0625 },
                    { -0.1875, 0.1875, -0.0625 },
                    { 1.0625, -0.0625, -0.3125 }
                };
                MatrixByArr tI = LinAlg.Eye(3);

                MatrixByArr tB1 = InvSymm(tA);

                HDebug.AssertTolerance(0.0001, tB0 - tB1);
                HDebug.AssertTolerance(0.0001, tI - tA * tB1);
                HDebug.AssertTolerance(0.0001, tI - tB1 * tA);
            }

            HDebug.Assert(A.ColSize == A.RowSize);
            //double[] eigval;
            //double[,] eigvec;

            bool success = false;//alglib.smatrixevd(A, A.ColSize, 1, false, out eigval, out eigvec);

            if (success == false)
            {
                HDebug.Assert(false);
                return(null);
            }
            HDebug.Assert(false);
            return(null);
        }