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