Ejemplo n.º 1
0
        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);
                }
Ejemplo n.º 4
0
 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));
     }
 }
Ejemplo n.º 5
0
        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);
        }
Ejemplo n.º 6
0
 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));
        }
Ejemplo n.º 8
0
        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));
        }