Example #1
0
 public static float CalcError(SMat3 origA, Vector3 x, Vector3 b)
 {
     Mat3 A = new Mat3();
     A.SetSymmetric(origA);
     Vector3 vtmp = A.Vmul(x);
     vtmp = b - vtmp;
     return vtmp.X * vtmp.X + vtmp.Y * vtmp.Y + vtmp.Z * vtmp.Z;
 }
Example #2
0
        public static void Rotate02(ref SMat3 vtav, ref Mat3 v)
        {
            if (vtav.m02 == 0)
                return;

            float c = 0, s = 0;
            vtav.Rot02(c, s);
            v.Rot02_post(c, s);
        }
Example #3
0
        public static void Rotate01(ref SMat3 vtav, ref Mat3 v)
        {
            if (vtav.m01 == 0)
                return;

            float c = 0, s = 0;
            vtav.Rot01(ref c, ref s);
            c = 0; s = 0;
            v.Rot01_post(c, s);
        }
Example #4
0
 public SMat3 Mul_ata(Mat3 a)
 {
     SMat3 m = new SMat3();
     m.SetSymmetric(a.m00 * a.m00 + a.m10 * a.m10 + a.m20 * a.m20,
                      a.m00 * a.m01 + a.m10 * a.m11 + a.m20 * a.m21,
                      a.m00 * a.m02 + a.m10 * a.m12 + a.m20 * a.m22,
                      a.m01 * a.m01 + a.m11 * a.m11 + a.m21 * a.m21,
                      a.m01 * a.m02 + a.m11 * a.m12 + a.m21 * a.m22,
                      a.m02 * a.m02 + a.m12 * a.m12 + a.m22 * a.m22);
     return m;
 }
Example #5
0
 public SMat3 MulATA()
 {
     SMat3 m = new SMat3();
     m.SetSymmetric(m00 * m00 + m10 * m10 + m20 * m20,
                  m00 * m01 + m10 * m11 + m20 * m21,
                  m00 * m02 + m10 * m12 + m20 * m22,
                  m01 * m01 + m11 * m11 + m21 * m21,
                  m01 * m02 + m11 * m12 + m21 * m22,
                  m02 * m02 + m12 * m12 + m22 * m22);
     return m;
 }
Example #6
0
        public static void Rotate12(ref SMat3 vtav, ref Mat3 v)
        {
            if (vtav.m12 == 0)
            {
                return;
            }

            float c = 0, s = 0;

            vtav.Rot12(c, s);
            v.Rot12_post(c, s);
        }
Example #7
0
        public SMat3 Mul_ata(Mat3 a)
        {
            SMat3 m = new SMat3();

            m.SetSymmetric(a.m00 * a.m00 + a.m10 * a.m10 + a.m20 * a.m20,
                           a.m00 * a.m01 + a.m10 * a.m11 + a.m20 * a.m21,
                           a.m00 * a.m02 + a.m10 * a.m12 + a.m20 * a.m22,
                           a.m01 * a.m01 + a.m11 * a.m11 + a.m21 * a.m21,
                           a.m01 * a.m02 + a.m11 * a.m12 + a.m21 * a.m22,
                           a.m02 * a.m02 + a.m12 * a.m12 + a.m22 * a.m22);
            return(m);
        }
Example #8
0
        public SMat3 MulATA()
        {
            SMat3 m = new SMat3();

            m.SetSymmetric(m00 * m00 + m10 * m10 + m20 * m20,
                           m00 * m01 + m10 * m11 + m20 * m21,
                           m00 * m02 + m10 * m12 + m20 * m22,
                           m01 * m01 + m11 * m11 + m21 * m21,
                           m01 * m02 + m11 * m12 + m21 * m22,
                           m02 * m02 + m12 * m12 + m22 * m22);
            return(m);
        }
Example #9
0
        public static void Rotate01(ref SMat3 vtav, ref Mat3 v)
        {
            if (vtav.m01 == 0)
            {
                return;
            }

            float c = 0, s = 0;

            vtav.Rot01(c, s);
            v.Rot01_post(c, s);
        }
Example #10
0
        public static void GetSymmetricSvd(ref SMat3 a, ref SMat3 vtav, ref Mat3 v, float tol, int max_sweeps)
        {
            vtav.SetSymmetric(a);
            v.Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
            float delta = tol * vtav.Fnorm();

            for (int i = 0; i < max_sweeps && vtav.Off() > delta; i++)
            {
                Rotate01(ref vtav, ref v);
                Rotate02(ref vtav, ref v);
                Rotate12(ref vtav, ref v);
            }
        }
Example #11
0
        public static void GetSymmetricSvd(ref SMat3 a, ref SMat3 vtav, ref Mat3 v, float tol, int max_sweeps)
        {
            vtav.SetSymmetric(a);
            v.Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
            float delta = tol * vtav.Fnorm();

            for (int i = 0; i < max_sweeps && vtav.Off() > delta; i++)
            {
                Rotate01(ref vtav, ref v);
                Rotate02(ref vtav, ref v);
                Rotate12(ref vtav, ref v);
            }
        }
Example #12
0
        public static void Rotate02(ref SMat3 vtav, ref Mat3 v)
        {
            if (vtav.m02 == 0)
            {
                return;
            }

            float c = 0, s = 0;

            vtav.Rot02(ref c, ref s);
            c = 0; s = 0;
            v.Rot02_post(c, s);
        }
Example #13
0
        public static Mat3 PseudoInverse(SMat3 d, Mat3 v, float tol)
        {
            Mat3 m = new Mat3();
            float d0 = Pinv(d.m00, tol), d1 = Pinv(d.m11, tol), d2 = Pinv(d.m22,
                        tol);
            m.Set(v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02,
                    v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12,
                    v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22,
                    v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02,
                    v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12,
                    v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22,
                    v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02,
                    v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12,
                    v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22);

            return m;
        }
Example #14
0
        public static Mat3 PseudoInverse(SMat3 d, Mat3 v, float tol)
        {
            Mat3  m = new Mat3();
            float d0 = Pinv(d.m00, tol), d1 = Pinv(d.m11, tol), d2 = Pinv(d.m22,
                                                                          tol);

            m.Set(v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02,
                  v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12,
                  v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22,
                  v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02,
                  v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12,
                  v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22,
                  v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02,
                  v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12,
                  v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22);

            return(m);
        }
Example #15
0
 public QEFSolver()
 {
     data = new QEFData();
     ata = new SMat3();
     atb = new Vector3();
     MassPoint = new Vector3();
     x = new Vector3();
     hasSolution = false;
 }
Example #16
0
 public void SetSymmetric(SMat3 rhs)
 {
     SetSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
 }
Example #17
0
 float SolveLeastSquares(Mat3 a, Vector3 b, ref Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
 {
     Mat3 at = new Mat3();
     SMat3 ata = new SMat3();
     Vector3 atb = new Vector3();
     at = a.Transpose();
     ata = a.MulATA();
     atb = at.Vmul(b);
     return SolveSymmetric(ata, atb, ref x, svd_tol, svd_sweeps, pinv_tol);
 }
Example #18
0
 public void SetSymmetric(SMat3 rhs)
 {
     SetSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
 }
Example #19
0
        public static void Rotate12(ref SMat3 vtav, ref Mat3 v)
        {
            if (vtav.m12 == 0)
                return;

            float c = 0, s = 0;
            vtav.Rot12(ref c, ref s);
            c = 0; s = 0;
            v.Rot12_post(c, s);
        }
Example #20
0
 public static float SolveSymmetric(SMat3 A, Vector3 b, ref Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
 {
     Mat3 mtmp = new Mat3(), pinv = new Mat3(), V = new Mat3();
     SMat3 VTAV = new SMat3();
     GetSymmetricSvd(ref A, ref VTAV, ref V, svd_tol, svd_sweeps);
     pinv = PseudoInverse(VTAV, V, pinv_tol);
     x = pinv.Vmul(b);
     return CalcError(A, x, b);
 }