public static Tuple <double, Vector> MaxSphereBetweenSpheres(Vector pt1, double rad1,
Vector pt2, double rad2,
Vector pt3, double rad3,
Vector pt4, double rad4)
{
if (MaxSphereBetweenSpheres_SelfTest)
{
MaxSphereBetweenSpheres_SelfTest = false;
Vector tpo = new double[] { 0 + 1, 0 + 2, 0 + 3 }; double tro = 0.10;
Vector tpa = new double[] { 1 + 1, 2 + 2, 3 + 3 }; double tra = 0.20;
Vector tpb = new double[] { 5 + 1, 3 + 2, 2 + 3 }; double trb = 0.30;
Vector tpc = new double[] { 3 + 1, 3 + 2, 3 + 3 }; double trc = 0.15;
var test_rad_cent = MaxSphereBetweenSpheres(tpo, tro,
tpa, tra,
tpb, trb,
tpc, trc
);
double radius = test_rad_cent.Item1;
Vector center = test_rad_cent.Item2;
HDebug.AssertTolerance(0.00000001, radius - ((center - tpo).Dist - tro));
HDebug.AssertTolerance(0.00000001, radius - ((center - tpa).Dist - tra));
HDebug.AssertTolerance(0.00000001, radius - ((center - tpb).Dist - trb));
HDebug.AssertTolerance(0.00000001, radius - ((center - tpc).Dist - trc));
}
Vector po = pt1 - pt1; double po2 = po.Dist2; double ro = rad1; double ro2 = ro * ro;
Vector pa = pt2 - pt1; double pa2 = pa.Dist2; double ra = rad2; double ra2 = ra * ra;
Vector pb = pt3 - pt1; double pb2 = pb.Dist2; double rb = rad3; double rb2 = rb * rb;
Vector pc = pt4 - pt1; double pc2 = pc.Dist2; double rc = rad4; double rc2 = rc * rc;
double alpah_ab = (ro2 - ra2 + pa2) * (rb - ro) - (ro2 - rb2 + pb2) * (ra - ro);
double alpah_bc = (ro2 - rb2 + pb2) * (rc - ro) - (ro2 - rc2 + pc2) * (rb - ro);
Vector beta_ab = (pa2 - (ro - ra) * (ro - ra)) * pb - (pb2 - (ro - rb) * (ro - rb)) * pa;
Vector beta_bc = (pb2 - (ro - rb) * (ro - rb)) * pc - (pc2 - (ro - rc) * (ro - rc)) * pb;
double n_alpah_ab = alpah_ab / beta_ab.Dist; Vector n_beta_ab = beta_ab.UnitVector();
double n_alpah_bc = alpah_bc / beta_bc.Dist; Vector n_beta_bc = beta_bc.UnitVector();
var line_pt_vec = Geometry.LineOnTwoPlanes(n_beta_ab, n_alpah_ab,
n_beta_bc, n_alpah_bc);
Vector gamma = line_pt_vec.Item1;
Vector delta = line_pt_vec.Item2;
double a = delta.Dist2;
double b = 2 * LinAlg.VtV(gamma, delta);
double c = gamma.Dist2 - 1;
double[] roots = HRoots.GetRootsClosedFormDegree2(a, b, c);
foreach (double root in roots)
{
Vector q = gamma + root * delta;
double pa_q = LinAlg.VtV(pa, q);
double radius = (ro2 - ra2 + pa2 - 2 * ro * pa_q) / (2 * (ra - ro + pa_q));
if (radius < 0)
{
continue;
}
Vector center = (ro + radius) * q;
HDebug.AssertTolerance(0.00000001, radius - ((center - po).Dist - ro));
HDebug.AssertTolerance(0.00000001, radius - ((center - pa).Dist - ra));
HDebug.AssertTolerance(0.00000001, radius - ((center - pb).Dist - rb));
HDebug.AssertTolerance(0.00000001, radius - ((center - pc).Dist - rc));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt1).Dist - rad1));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt2).Dist - rad2));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt3).Dist - rad3));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt4).Dist - rad4));
return(new Tuple <double, Vector>(radius, center + pt1));
}
HDebug.Assert(roots == null);
return(null);
}
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;
// }
//}
}
