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);
}
public static Trans3 GetTrans(IList <Vector> source
, IList <Vector> target
)
{
Trans3 trans = ICP3.OptimalTransform(source, target);
return(trans);
}
public static Trans3 GetTrans(IList <Vector> source
, IList <Vector> target
, IList <double> weight
)
{
Trans3 trans = ICP3.OptimalTransformWeighted(source, target, weight);
return(trans);
}
public static Trans3 FromFile(string path)
{
//return new Trans3(HTLib.Trans3.FromFile(path));
{
System.IO.StreamReader reader = new System.IO.StreamReader(path);
string str = reader.ReadLine();
reader.Close();
reader.Dispose();
return(Trans3.Parse(str));
}
}
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 Trans3 GetTransformNoScale(Vector from1, Vector from2, Vector to1, Vector to2)
{
//HTLib.DoubleVector3 lfrom1 = new HTLib.DoubleVector3(from1);
//HTLib.DoubleVector3 lfrom2 = new HTLib.DoubleVector3(from2);
//HTLib.DoubleVector3 lto1 = new HTLib.DoubleVector3(to1 );
//HTLib.DoubleVector3 lto2 = new HTLib.DoubleVector3(to2 );
//HTLib.Trans3 ltrans = HTLib.Trans3.GetTransformNoScale(lfrom1, lfrom2, lto1, lto2);
//return new Trans3(ltrans);
{
double ds = 1;
//double dx = to2.x - from2.x; // dx + from2.x = to2.x
//double dy = to2.y - from2.y; // dx + from2.y = to2.y
//double dz = to2.z - from2.z; // dx + from2.z = to2.z
Vector from = (from2 - from1).UnitVector();
Vector to = (to2 - to1).UnitVector();
Quaternion dr;
double innerprod = LinAlg.DotProd(from, to); // InnerProduct(from,to);
if (innerprod >= 1)
{
HDebug.Assert(innerprod == 1);
dr = Quaternion.UnitRotation;
}
else
{
Vector axis = LinAlg.CrossProd(from, to); // CrossProduct(from, to);
if (axis.Dist2 == 0)
{
// to avoid degenerate cases
double mag = to.Dist / 100000000;
axis = LinAlg.CrossProd(from, to + (new double[] { mag, mag * 2, mag * 3 })); //Vector.CrossProduct(from, to+new DoubleVector3(mag, mag*2, mag*3));
}
double angle = Math.Acos(innerprod);
dr = new Quaternion(axis, angle);
HDebug.Assert(LinAlg.DotProd((dr.RotationMatrix * from), to) > 0.99999);
}
Trans3 trans = Trans3.GetTransform(-from1, 1, Quaternion.UnitRotation);
trans = Trans3.AppendTrans(trans, Trans3.GetTransform(new double[3], ds, dr));
trans = Trans3.AppendTrans(trans, Trans3.GetTransform(to1, 1, Quaternion.UnitRotation));
//new Trans3(dx, dy, dz, ds, dr);
if (HDebug.IsDebuggerAttached)
{
Vector fromto1 = trans.DoTransform(from1);
HDebug.Assert((fromto1 - to1).Dist < 0.000001);
Vector fromto2 = trans.DoTransform(from2);
HDebug.Assert(((fromto2 - fromto1).UnitVector() - (to2 - to1).UnitVector()).Dist < 0.000001);
}
return(trans);
}
}
public static Tuple <double, Vector, bool> MaxCircleBetweenCircles(Vector pt1, double rad1,
Vector pt2, double rad2,
Vector pt3, double rad3)
{
/// b:pt3
/// / \
/// / \
/// / \
/// o:pt1 ------ a:pt2
///
/// o : (0 , 0), radii ro
/// a : (pax, 0), radii ra
/// b : (pbx, pby), radii rb
Vector uvec12 = (pt2 - pt1).UnitVector();
Vector uvec23 = (pt3 - pt2).UnitVector();
Vector uvec31 = (pt1 - pt3).UnitVector();
double cos123 = LinAlg.VtV(-uvec12, uvec23);
double cos231 = LinAlg.VtV(-uvec23, uvec31);
double cos312 = LinAlg.VtV(-uvec31, uvec12);
double cos_aob;
double ro, ra, rb;
Vector po, pa, pb;
if (cos123 > 0)
{
pa = pt1; ra = rad1; po = pt2; ro = rad2; pb = pt3; rb = rad3; cos_aob = cos123;
}
else if (cos231 > 0)
{
pa = pt2; ra = rad2; po = pt3; ro = rad3; pb = pt1; rb = rad1; cos_aob = cos231;
}
else if (cos312 > 0)
{
pa = pt2; ra = rad2; po = pt1; ro = rad1; pb = pt3; rb = rad3; cos_aob = cos312;
}
else
{
throw new Exception("cos123>0 and cos231>0");
}
Vector uvec_oa = (pa - po).UnitVector();
Vector uvec_ob = (pb - po).UnitVector();
Vector normal = LinAlg.CrossProd(uvec_oa, uvec_ob);
double sin_aob = normal.Dist;
double pax = (pa - po).Dist;
double pbx = (pb - po).Dist * cos_aob;
double pby = (pb - po).Dist * sin_aob;
Trans3 trans;
{
Vector npo = new double[] { 0, 0, 0 };
Vector npa = new double[] { 0, pax, 0 };
Vector npb = new double[] { 0, pbx, pby };
trans = Trans3.GetTransformNoScale(npo, npa, npb, po, pa, pb);
if (HDebug.IsDebuggerAttached)
{
Vector pox = trans.DoTransform(npo);
HDebug.AssertTolerance(0.0000001, po - trans.DoTransform(npo));
HDebug.AssertTolerance(0.0000001, pa - trans.DoTransform(npa));
HDebug.AssertTolerance(0.0000001, pb - trans.DoTransform(npb));
}
}
Tuple <double, Vector, bool> rad_cent_in = MaxCircleBetweenCircles(ro, ra, rb, pax, pbx, pby);
if (rad_cent_in == null)
{
return(null);
}
double radius = rad_cent_in.Item1;
bool cenerInTriangle = rad_cent_in.Item3;
Vector center;
{
HDebug.Assert(rad_cent_in.Item2.Size == 2);
center = new double[] { 0, rad_cent_in.Item2[0], rad_cent_in.Item2[1] };
center = trans.DoTransform(center);
}
if (HDebug.IsDebuggerAttached)
{
double radx_1 = (center - pt1).Dist - rad1;
double radx_2 = (center - pt2).Dist - rad2;
double radx_3 = (center - pt3).Dist - rad3;
HDebug.AssertTolerance(0.000001, radx_1 - radx_2, radx_2 - radx_3, radx_3 - radx_1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt1 - pt2));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt2 - pt3));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt3 - pt1));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt1 - center));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt2 - center));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt3 - center));
}
return(new Tuple <double, Vector, bool>(radius, center, cenerInTriangle));
}
public static Trans3 OptimalTransformWeighted(IList <Vector> source, IList <Vector> target, IList <double> weight
, double?rotateRatio = null
)
{
if (OptimalTransformWeighted_selftest)
#region selftest
{
OptimalTransformWeighted_selftest = false;
{
Vector[] _source = new Vector[]
{
new double[] { 0, 0, 0 },
new double[] { 1, 0, 0 },
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 },
};
Vector[] _target = new Vector[]
{
new double[] { 0, 0, 0 },
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 },
new double[] { 1, 0, 0 },
};
double[] _weight = new double[] { 1, 1, 1, 1 };
Trans3 _trans = OptimalTransformWeighted(_source, _target, _weight, null);
Vector[] _moved = _trans.GetTransformed(_source);
double _err2 = 0;
for (int i = 0; i < _source.Length; i++)
{
_err2 = (_target[i] - _moved[i]).Dist2;
}
HDebug.Assert(_err2 < 0.00000001);
}
{
Vector[] _source = new Vector[]
{
new double[] { 1, 0, 0 },
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 },
};
Vector[] _target = new Vector[]
{
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 },
new double[] { 1, 0, 0 },
};
double[] _weight = new double[] { 1, 1, 1 };
Trans3 _trans = OptimalTransformWeighted(_source, _target, _weight, null);
Vector[] _moved = _trans.GetTransformed(_source);
double _err2 = 0;
for (int i = 0; i < _source.Length; i++)
{
_err2 = (_target[i] - _moved[i]).Dist2;
}
HDebug.Assert(_err2 < 0.00000001);
}
}
#endregion
if (HDebug.IsDebuggerAttached)
{
foreach (double _w in weight)
{
HDebug.Assert(_w >= 0);
}
}
Vector[] p = source.ToArray();
Vector[] x = target.ToArray();
double[] w = weight.ToArray();
HDebug.Assert(p.Length == x.Length);
HDebug.Assert(w.Length == x.Length);
Vector up = p.MeanWeighted(w);
Vector ux = x.MeanWeighted(w);
Quaternion quater = OptimalRotationWeighted(p, x, up, ux, w);
if (rotateRatio != null)
{
double r = quater.RotationAngle;
r = r % (2 * Math.PI);
r = (r > Math.PI) ? (r - 2 * Math.PI) : r;
r = r / 2;
quater = new Quaternion(quater.RotationAxis, r);
}
MatrixByArr qR = quater.RotationMatrix;
Vector qT = OptimalTransWeighted(up, ux, qR, w);
return(new Trans3(qT, 1, quater));
}
