Example #1
0
        public static double IndexSegmentsClosestPoint(IList <Vector> segments, Vector point)
        {
            double index = 0;
            double dist2 = (point - segments[0]).Dist2;

            for (int i = 0; i < segments.Count - 1; i++)
            {
                Segment segment = new Segment(segments[i], segments[i + 1]);
                double  lindex  = HMath.Between(0, IndexSegmentClosestPoint(segment, point), 1);
                double  ldist2  = (point - segment[lindex]).Dist2;
                if (ldist2 < dist2)
                {
                    index = i + lindex;
                    dist2 = ldist2;
                }
            }
            return(index);
        }
Example #2
0
//		public static double DistanceLineLine(Line line1, Line line2)
//		{
//			// http://mathworld.wolfram.com/Line-LineDistance.html
//			DoubleVector3 x1 = line1.Base;
//			DoubleVector3 x2 = line1.Normal;
//			DoubleVector3 x3 = line2.Base;
//			DoubleVector3 x4 = line2.Normal;
//			DoubleVector3 a = x2 - x1;
//			DoubleVector3 b = x4 - x3;
//			DoubleVector3 c = x3 - x1;
//			DoubleVector3 axb = DoubleVector3.CrossProduct(a, b);
//			double dist = Math.Abs(DoubleVector3.InnerProduct(c, axb)) / axb.Length;
//			return dist;
//		}
        public static double AngleBetween(Vector left, Vector right)
        {
            double cos_angle = LinAlg.DotProd(left, right) / (left.Dist * right.Dist);

            cos_angle = HMath.Between(-1, cos_angle, 1);
            double angle = Math.Acos(cos_angle);

            while (angle > Math.PI)
            {
                angle -= Math.PI;
            }
            while (angle < -Math.PI)
            {
                angle += Math.PI;
            }
            HDebug.Assert(angle >= 0);
            HDebug.Assert(angle <= Math.PI);
            return(angle);
        }
        public static Tuple <double, Vector, bool> MaxCircleBetweenCircles(double ro, double ra, double rb,
                                                                           double pax, double pbx, double pby)
        {
            ///         b
            ///       /   \
            ///     /      \
            ///   /         \
            /// o ---------- a
            ///
            /// o : (0  ,   0), radii ro
            /// a : (pax,   0), radii ra
            /// b : (pbx, pby), radii rb
            HDebug.Assert(ro > 0, ra > 0, rb > 0);
            HDebug.Assert(pax > 0, pbx > 0, pby > 0);

            double ro2 = ro * ro;
            double ra2 = ra * ra;
            double rb2 = rb * rb;
            Vector po  = new double[] { 0, 0 };
            Vector pa  = new double[] { pax, 0 };
            Vector pb  = new double[] { pbx, pby };

            double maxAngInTriangle = Math.Acos(pbx / pb.Dist);

            double alpha = (ro2 - ra2 + pa.Dist2) * (rb - ro)
                           - (ro2 - rb2 + pb.Dist2) * (ra - ro);
            Vector beta = (pa.Dist2 - (ro - ra) * (ro - ra)) * pb
                          - (pb.Dist2 - (ro - rb) * (ro - rb)) * pa;

            double[] initangs;
            {
                double   aa   = beta.Dist2;
                double   bb   = 2 * alpha * beta[0];
                double   cc   = alpha - beta[1] * beta[1];
                double[] coss = HRoots.GetRootsClosedFormDegree2(aa, bb, cc);

                initangs = new double[0];
                if (coss != null)
                {
                    initangs = initangs.HAddRange
                               (
                        Math.Acos(HMath.Between(-1, coss[0], 1)),
                        Math.Acos(HMath.Between(-1, coss[1], 1))
                               ).ToArray();
                }

                initangs = initangs.HAddRange
                           (
                    0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0,     // Math.PI*2 = 6.2831853071795862
                    0.5, 1.5, 2.5, 3.5, 4.5, 5.5
                           ).ToArray();
            }

            Func <double, double> func = delegate(double lang)
            {
                lang = lang % Math.PI;
                double lcos = Math.Cos(lang);
                double lsin = Math.Sin(lang);
                //double lsin;
                //if(0<lcos && lcos<1) lsin = Math.Sqrt(1- lcos*lcos);
                //else                 lsin = 1-lcos;
                double val = alpha + beta[0] * lcos + beta[1] * lsin;
                return(val);
            };

            foreach (double initang in initangs)
            {
                double?ang = HRoots.GetRootSecant(func, initang, initang + 0.01, 100); /// scant method works better than bisection !!
                if (ang == null)
                {
                    continue;
                }
                ang = ang % Math.PI;

                bool cenerInTriangle = ((0 <= ang) && (ang <= maxAngInTriangle));

                double cos       = Math.Cos(ang.Value);
                double sin       = Math.Sin(ang.Value);
                double radius_oa = (ro2 - ra2 + pa.Dist2 - 2 * ro * (pax * cos)) / (2 * (ra - ro + (pax * cos)));
                double radius_ob = (ro2 - rb2 + pb.Dist2 - 2 * ro * (pbx * cos + pby * sin)) / (2 * (rb - ro + (pbx * cos + pby * sin)));
                //HDebug.AssertTolerance(0.00000001, radius_oa - radius_ob);

                double radius = radius_oa;
                Vector center = (ro + radius) * new Vector(cos, sin);
                double radius2o = (po - center).Dist - ro; double err2o = Math.Abs(radius - radius2o);
                double radius2a = (pa - center).Dist - ra; double err2a = Math.Abs(radius - radius2a);
                double radius2b = (pb - center).Dist - rb; double err2b = Math.Abs(radius - radius2b);

                double toler_err = Math.Abs(radius) * 0.000001;
                if ((err2o < toler_err) && (err2a < toler_err) && (err2b < toler_err))
                {
                    return(new Tuple <double, Vector, bool>(radius, center, cenerInTriangle));
                }
            }

            /// There is a case that three circles are positioned almost in a line, whose circles are all overlap
            /// and the middle circle has the largest radius. (like, oOo)
            /// In that case, the common largest circle cannot be found.
            return(null);

            //double ang;
            //{
            //    double val0 = func(initangs[0]);
            //    double val1 = func(initangs[1]);
            //    ang = (Math.Abs(val0) < Math.Abs(val1)) ? initangs[0] : initangs[1];
            //    ang = HRoots.GetRootSecant(func, ang, ang*1.01, 100); /// scant method works better than bisection !!
            //    //cos = HRoots.GetRootBisection(func, 0, tmax).Value;
            //    ang = ang % Math.PI;
            //}
            //
            //Vector center;
            //{
            //    double cos = Math.Cos(ang);
            //    double sin = Math.Sin(ang);
            //    double radius_oa = (ro2 - ra2 + pa.Dist2 - 2*ro*(pax*cos        ))/(2*(ra - ro + (pax*cos        )));
            //    double radius_ob = (ro2 - rb2 + pb.Dist2 - 2*ro*(pbx*cos+pby*sin))/(2*(rb - ro + (pbx*cos+pby*sin)));
            //    HDebug.AssertTolerance(0.00000001, radius_oa - radius_ob);
            //
            //    center = (ro+radius_oa) * new Vector(cos, sin);
            //}
            //
            //{
            //    double radius2o = (po - center).Dist - ro;
            //    radius = radius2o;
            //
            //    if(HDebug.IsDebuggerAttached)
            //    {
            //        double radius2a = (pa - center).Dist - ra;
            //        double radius2b = (pb - center).Dist - rb;
            //        HDebug.AssertTolerance(0.00000001, radius-radius2o);
            //        HDebug.AssertTolerance(0.00000001, radius-radius2a);
            //        HDebug.AssertTolerance(0.00000001, radius-radius2b);
            //    }
            //}
            //
            //return new Tuple<double, Vector>(radius, center);

            //{
            //    double a = 2;
            //    double b = 3;
            //    double c = 5;
            //    double ro  = 0.5; double ro2 = ro*ro;
            //    double ra  = 0.3; double ra2 = ra*ra;
            //    double rbc = 1.0; double rbc2 = rbc*rbc;
            //    Vector po = new double[] { 0, 0 };
            //    Vector pa = new double[] { a, 0 };
            //    Vector pbc = new double[] { b, c };
            //
            //    double alpha = (ro2 - ra2  + pa.Dist2)*(rbc - ro)
            //                 - (ro2 - rbc2 + pbc.Dist2)*(ra  - ro);
            //    Vector beta = (pa.Dist2 - (ro-ra)*(ro-ra)) * pbc
            //                - (pbc.Dist2 - (ro-rbc)*(ro-rbc)) * pa;
            //    double aa = beta.Dist2;
            //    double bb = 2*alpha*beta[0];
            //    double cc = alpha - beta[1]*beta[1];
            //    double[] ts = HRoots.GetRootsClosedFormDegree2(aa, bb, cc);
            //    double tmax = b/pbc.Dist;
            //    {
            //        double chk0 = aa*ts[0]*ts[0] + bb*ts[0] + cc;
            //        double chk1 = aa*ts[1]*ts[1] + bb*ts[1] + cc;
            //        Vector q0 = new double[] { ts[0], Math.Sqrt(1 - ts[0]*ts[0]) };
            //        Vector q1 = new double[] { ts[1], Math.Sqrt(1 - ts[1]*ts[1]) };
            //        double chkq0 = alpha + LinAlg.VtV(beta, q0);
            //        double chkq1 = alpha + LinAlg.VtV(beta, q1);
            //    }
            //
            //    Func<double,double> func = delegate(double cos)
            //    {
            //        double sin = Math.Sqrt(1- cos*cos);
            //        double val = alpha + beta[0]*cos + beta[1]*sin;
            //        return val;
            //    };
            //    //Func<double,double> dfunc = delegate(double cos)
            //    //{
            //    //    double sin = Math.Sqrt(1- cos*cos);
            //    //    double dval = -1*beta[0]*sin + beta[1]*cos;
            //    //    return dval;
            //    //};
            //    double tb = HRoots.GetRootBisection(func, 0, tmax).Value;
            //    double tc = HRoots.GetRootSecant(func, 0, tmax, 100);
            //    //double td = HRoots.GetRootNewton(func, dfunc, ts[0]).Value;
            //    tb = tc;
            //    {
            //        double cos = tb;
            //        double sin = Math.Sqrt(1 - tb*tb);
            //        Vector q = new double[] { cos, sin };
            //        double chk = alpha + LinAlg.VtV(beta, q);
            //    }
            //
            //    {
            //        double r_a0 = (ro2 - ra2 + pa.Dist2 - 2*ro*a*ts[0])/(2*(ra - ro + a *ts[0]));
            //        double r_a1 = (ro2 - ra2 + pa.Dist2 - 2*ro*a*ts[1])/(2*(ra - ro + a *ts[1]));
            //        double r_a2 = (ro2 - ra2 + pa.Dist2 - 2*ro*a*tb)/(2*(ra - ro + a *tb));
            //
            //        Vector cent0 = (ro+r_a0) * new Vector(ts[0], Math.Sqrt(1-ts[0]*ts[0]));
            //        double radi0o  = (po  - cent0).Dist - ro;
            //        double radi0a  = (pa  - cent0).Dist - ra;
            //        double radi0bc = (pbc - cent0).Dist - rbc;
            //
            //        Vector cent1 = (ro+r_a1) * new Vector(ts[1], Math.Sqrt(1-ts[1]*ts[1]));
            //        double radi1o  = (po  - cent1).Dist - ro;
            //        double radi1a  = (pa  - cent1).Dist - ra;
            //        double radi1bc = (pbc - cent1).Dist - rbc;
            //
            //        Vector cent2 = (ro+r_a2) * new Vector(tb, Math.Sqrt(1-tb*tb));
            //        double radi2o  = (po  - cent2).Dist - ro;
            //        double radi2a  = (pa  - cent2).Dist - ra;
            //        double radi2bc = (pbc - cent2).Dist - rbc;
            //    }
            //}
        }
Example #4
0
        public static object LeastSquare
            (double[,] As, double[] bs
            , bool opt_get_stat = false
            )
        {
            if (HDebug.Selftest())
            {
                /// >> A = [ 1,3,2, 1 ; 4,5,6, 1 ; 7,9,9, 1 ; 11,11,12, 1 ; 13,16,15, 1 ]
                /// >> b = [1, 4, 6, 9, 12]'
                /// >> x = inv(A' * A) * (A' * b)
                ///     0.2171
                ///     0.2125
                ///     0.4205
                ///    -0.7339
                /// >> esti = A * x
                ///     0.9619
                ///     3.7203
                ///     6.4832
                ///     9.0381
                ///    11.7965
                /// >> corr(esti,b)
                ///     0.9976
                /// >> mean( (b-esti).^2 )
                ///     0.0712
                double[,] _A = new double[5, 3] {
                    { 1, 3, 2 }, { 4, 5, 6 }, { 7, 9, 9 }, { 11, 11, 12 }, { 13, 16, 15 }
                };
                double[] _b = new double[5] {
                    1, 4, 6, 9, 12
                };
                dynamic _out = LeastSquare(_A, _b, true);

                double   _matlab_corr = 0.9976;
                double   _matlab_mse  = 0.0712;
                double[] _matlab_x    = new double[] { 0.2171, 0.2125, 0.4205, -0.7339 };
                double[] _matlab_esti = new double[] { 0.9619, 3.7203, 6.4832, 9.0381, 11.7965 };

                double err1 = Math.Abs(_matlab_corr - _out.opt_estimation_corr);
                double err2 = Math.Abs(_matlab_mse - _out.opt_mean_square_err);
                double err3 = (_matlab_x - (Vector)_out.x).ToArray().MaxAbs();
                double err4 = (_matlab_esti - (Vector)_out.opt_estimation).ToArray().MaxAbs();

                HDebug.Assert(err1 < 0.0001);
                HDebug.Assert(err2 < 0.0001);
                HDebug.Assert(err3 < 0.0001);
                HDebug.Assert(err4 < 0.0001);
            }
            /// => A x = b
            ///
            /// => At A x = At b
            ///
            /// => AA * x = Ab
            /// => x = inv(AA) * Ab
            HDebug.Assert(As.GetLength(0) == bs.Length);
            int n = As.GetLength(0);
            int k = As.GetLength(1);

            Matrix A = Matrix.Zeros(n, k + 1);

            for (int c = 0; c < n; c++)
            {
                for (int r = 0; r < k; r++)
                {
                    A[c, r] = As[c, r];
                }
                A[c, k] = 1;
            }

            Matrix AA = LinAlg.MtM(A, A);
            Vector Ab = LinAlg.MtV(A, bs);

            Vector x;

            switch (k + 1)
            {
            case 2: { Matrix invAA = LinAlg.Inv2x2(AA.ToArray()); x = LinAlg.MV(invAA, Ab); } break;

            case 3: { Matrix invAA = LinAlg.Inv3x3(AA.ToArray()); x = LinAlg.MV(invAA, Ab); } break;

            case 4: { Matrix invAA = LinAlg.Inv4x4(AA.ToArray()); x = LinAlg.MV(invAA, Ab); } break;

            default:
                Matlab.PutMatrix("LinAlg_LeastSquare.AA", AA);
                Matlab.PutVector("LinAlg_LeastSquare.Ab", Ab);
                Matlab.Execute("LinAlg_LeastSquare.AA = inv(LinAlg_LeastSquare.AA);");
                Matlab.Execute("LinAlg_LeastSquare.x = LinAlg_LeastSquare.AA * LinAlg_LeastSquare.Ab;");
                x = Matlab.GetVector("LinAlg_LeastSquare.x");
                Matlab.Execute("clear LinAlg_LeastSquare;");
                break;
            }

            double?opt_mean_square_err = null;
            double?opt_estimation_corr = null;
            Vector opt_estimation      = null;

            if (opt_get_stat)
            {
                opt_estimation = new double[n];
                double avg_err2 = 0;
                for (int i = 0; i < n; i++)
                {
                    double esti = 0;
                    for (int j = 0; j < k; j++)
                    {
                        esti += As[i, j] * x[j];
                    }
                    esti += x[k];

                    opt_estimation[i] = esti;
                    avg_err2         += (esti - bs[i]) * (esti - bs[i]);
                }
                avg_err2 /= n;

                opt_mean_square_err = avg_err2;
                opt_estimation_corr = HMath.HCorr(opt_estimation, bs);
            }

            return(new
            {
                x = x,
                /// optional outputs
                opt_mean_square_err = opt_mean_square_err,
                opt_estimation_corr = opt_estimation_corr,
                opt_estimation = opt_estimation,
            });
        }