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