Update SConscript.
authorBrecht Van Lommel <brechtvanlommel@pandora.be>
Sat, 27 Aug 2005 13:45:19 +0000 (13:45 +0000)
committerBrecht Van Lommel <brechtvanlommel@pandora.be>
Sat, 27 Aug 2005 13:45:19 +0000 (13:45 +0000)
Fix some warnings.
Merge with latest soc code.

What changed in IK lib:

Fully restructured, with components now as follows:
  - IK_Solver: C <=> C++ interface
  - IK_QSegment: base class for bone/segment with 0
    to 3 DOF
  - IK_QTask: base class for a task (currently there's
    a position and a rotation task)
  - IK_QJacobian: the Jacobian matrix, with SVD
    decomposition, damping, etc
  - IK_QJacobianSolver: the iterative solver

The exponential map parametrization is no longer used,
instead we have now:
  - 3DOF and 2DOF XZ segments: directly update matrix
    with Rodrigues' formula
  - Other: Euler angles (no worries about singularities
    here)

Computation of the Jacobian inverse has also changed:
  - The SVD algorithm is now based on LAPACK code,
    instead of NR, to avoid some problems with rounding
    errors.
  - When the problem is underconstrained (as is the case
    most of the time), the SVD is computed for the transpose
    of the Jacobian (faster).
  - A new damping algorithm called the Selectively Damped
    Least Squares is used, result in faster and more
    stable convergence.
  - Stiffness is implemented as if a weighted psuedo-inverse
    was used.

Tree structure support.

Rotation limits:
  - 3DOF and 2DOF XZ segments limits are based on a swing
    (direct axis-angle over XZ) and twist/roll (rotation
    over Y) decomposition. The swing region is an ellipse
    on a sphere.
  - Rotation limits are implemented using an inner clamping
    loop: as long as there is a violation, a violating DOF
    is clamped and removed from the Jacobian, and the solution
    is recomputed.

Convergence checking is based now on the max norm of angle
change, or the maximum number of iterations.

12 files changed:
intern/iksolver/SConscript
intern/iksolver/intern/IK_QJacobian.cpp
intern/iksolver/intern/IK_QJacobian.h
intern/iksolver/intern/IK_QJacobianSolver.cpp
intern/iksolver/intern/IK_QJacobianSolver.h
intern/iksolver/intern/IK_QSegment.cpp
intern/iksolver/intern/IK_QTask.cpp
intern/iksolver/intern/IK_Solver.cpp
intern/iksolver/intern/TNT/svd.h
intern/iksolver/intern/TNT/tntmath.h
intern/moto/include/MT_Point3.h
intern/moto/include/MT_Point3.inl

index aa96002b5f0e6c53dae0a04528733bf1031a13d6..7ef8a9b9d11ee95344c26ad8c1f9ac93ac51401f 100644 (file)
@@ -4,11 +4,11 @@ Import ('library_env')
 
 iksolver_env = library_env.Copy ()
 
-source_files = ['intern/IK_QChain.cpp',
+source_files = ['intern/IK_QTask.cpp',
                 'intern/IK_QJacobianSolver.cpp',
                 'intern/IK_QSegment.cpp',
-                'intern/IK_Solver.cpp',
-                'intern/MT_ExpMap.cpp']
+                'intern/IK_QJacobian.cpp',
+                'intern/IK_Solver.cpp']
 
 iksolver_env.Append (CPPPATH = ['intern',
                                 '../moto/include',
index c9690e68d4c05578ec31f4ce43a88189d4b646f2..cfa764454d494e177be39afdd620818718f81aef 100644 (file)
@@ -33,9 +33,6 @@
 #include "IK_QJacobian.h"
 #include "TNT/svd.h"
 
-#include <iostream>
-using namespace std;
-
 IK_QJacobian::IK_QJacobian()
 : m_sdls(true), m_min_damp(1.0)
 {
@@ -72,8 +69,6 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks)
        m_weight = 1.0;
        m_weight_sqrt = 1.0;
 
-       // m_svd_work_space.newsize(dof); // TNT resizes this?
-
        if (task_size >= dof) {
                m_transpose = false;
 
@@ -81,6 +76,9 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks)
                m_svd_v.newsize(dof, dof);
                m_svd_w.newsize(dof);
 
+               m_work1.newsize(task_size);
+               m_work2.newsize(dof);
+
                m_svd_u_t.newsize(dof, task_size);
                m_svd_u_beta.newsize(dof);
        }
@@ -89,10 +87,15 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks)
                // as the original, and often allows using smaller matrices.
                m_transpose = true;
 
+               m_jacobian_t.newsize(dof, task_size);
+
                m_svd_u.newsize(task_size, task_size);
                m_svd_v.newsize(dof, task_size);
                m_svd_w.newsize(task_size);
 
+               m_work1.newsize(dof);
+               m_work2.newsize(task_size);
+
                m_svd_u_t.newsize(task_size, task_size);
                m_svd_u_beta.newsize(task_size);
        }
@@ -121,51 +124,19 @@ void IK_QJacobian::Invert()
        if (m_transpose) {
                // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
                // so J = U*W*Vt and Jinv = V*Winv*Ut
-               TNT::transpose(m_jacobian, m_svd_v);
-
-               TNT::SVD(m_svd_v, m_svd_w, m_svd_u, m_svd_work_space);
+               TNT::transpose(m_jacobian, m_jacobian_t);
+               TNT::SVD(m_jacobian_t, m_svd_v, m_svd_w, m_svd_u, m_work1, m_work2);
        }
        else {
                // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
                // so Jinv = V*Winv*Ut
-               m_svd_u = m_jacobian;
-
-               TNT::SVD(m_svd_u, m_svd_w, m_svd_v, m_svd_work_space);
+               TNT::SVD(m_jacobian, m_svd_u, m_svd_w, m_svd_v, m_work1, m_work2);
        }
 
        if (m_sdls)
                InvertSDLS();
        else
                InvertDLS();
-       
-#if 0
-       if (!ComputeNullProjection())
-               return;
-       
-       int i, j;
-       for (i = 0; i < m_weight.size(); i++)
-               m_weight[i] = 1.0 - m_weight[i];
-
-       TNT::matmultdiag(m_null, m_null, m_weight);
-
-       for (i = 0; i < m_null.num_rows(); i++)
-               for (j = 0; j < m_null.num_cols(); j++)
-                       if (i == j)
-                               m_null[i][j] = 1.0 - m_null[i][j];
-                       else
-                               m_null[i][j] = -m_null[i][j];
-       
-       TVector ntheta(m_d_theta);
-       TNT::matmult(ntheta, m_null, m_d_theta);
-
-       cout << "#" << endl;
-       for (i = 0; i < m_d_theta.size(); i++)
-               printf("%f >> %f (%f)\n", m_d_theta[i], ntheta[i], m_weight[i]);
-       m_d_theta = ntheta;
-
-       for (i = 0; i < m_weight.size(); i++)
-               m_weight[i] = 1.0 - m_weight[i];
-#endif
 }
 
 bool IK_QJacobian::ComputeNullProjection()
@@ -209,36 +180,6 @@ void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
 {
        if (!ComputeNullProjection())
                return;
-       
-#if 0
-       int i, j;
-
-       m_null.newsize(m_d_theta.size(), m_d_theta.size());
-
-       for (i = 0; i < m_d_theta.size(); i++)
-               for (j = 0; j < m_d_theta.size(); j++)
-                       if (i == j)
-                               m_null[i][j] = 1.0;
-                       else
-                               m_null[i][j] = 0.0;
-
-       // restrict lower priority jacobian
-       //jacobian.Restrict(m_d_theta, m_null);
-
-       // add angle update from lower priority
-       jacobian.Invert();
-
-       TVector d2(m_d_theta.size());
-       TVector n2(m_d_theta.size());
-
-       for (i = 0; i < m_d_theta.size(); i++)
-               d2[i] = jacobian.AngleUpdate(i);
-       
-       TNT::matmult(n2, m_null, d2);
-
-       m_d_theta = m_d_theta + n2;
-#else
-       int i;
 
        // restrict lower priority jacobian
        jacobian.Restrict(m_d_theta, m_null);
@@ -249,9 +190,9 @@ void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
        // note: now damps secondary angles with minimum damping value from
        // SDLS, to avoid shaking when the primary task is near singularities,
        // doesn't work well at all
+       int i;
        for (i = 0; i < m_d_theta.size(); i++)
                m_d_theta[i] = m_d_theta[i] + /*m_min_damp**/jacobian.AngleUpdate(i);
-#endif
 }
 
 void IK_QJacobian::Restrict(TVector& d_theta, TMatrix& null)
@@ -472,23 +413,6 @@ void IK_QJacobian::Lock(int dof_id, MT_Scalar delta)
        m_d_theta[dof_id] = 0.0;
 }
 
-#if 0
-void IK_QJacobian::SetSecondary(int dof_id, MT_Scalar d)
-{
-       m_alpha[dof_id] = d;
-}
-
-void IK_QJacobian::SolveSecondary()
-{
-       if (!ComputeNullProjection())
-               return;
-       
-       TNT::matmult(m_d_theta, m_null, m_alpha);
-
-       m_alpha = 0;
-}
-#endif
-
 MT_Scalar IK_QJacobian::AngleUpdate(int dof_id) const
 {
        return m_d_theta[dof_id];
index b2d990d548985b515c825a7c41a7b30246848a76..a16d7a7a941ad7210f83c1d6fb32ec30e07f4cc7 100644 (file)
@@ -36,7 +36,6 @@
 #define NAN_INCLUDED_IK_QJacobian_h
 
 #include "TNT/cmat.h"
-#include "TNT/fmat.h"
 #include <vector>
 #include "MT_Vector3.h"
 
@@ -44,7 +43,7 @@ class IK_QJacobian
 {
 public:
        typedef TNT::Matrix<MT_Scalar> TMatrix;
-       typedef TNT::Vector<MT_Scalar>TVector;
+       typedef TNT::Vector<MT_Scalar> TVector;
 
        IK_QJacobian();
        ~IK_QJacobian();
@@ -71,11 +70,6 @@ public:
        void Restrict(TVector& d_theta, TMatrix& null);
        void SubTask(IK_QJacobian& jacobian);
 
-#if 0
-       void SetSecondary(int dof_id, MT_Scalar d);
-       void SolveSecondary();
-#endif
-
 private:
        
        void InvertSDLS();
@@ -85,7 +79,7 @@ private:
        bool m_transpose;
 
        // the jacobian matrix and it's null space projector
-       TMatrix m_jacobian;
+       TMatrix m_jacobian, m_jacobian_t;
        TMatrix m_null;
 
        /// the vector of intermediate betas
@@ -97,9 +91,10 @@ private:
        /// space required for SVD computation
 
        TVector m_svd_w;
-    TVector m_svd_work_space;        
        TMatrix m_svd_v;
        TMatrix m_svd_u;
+    TVector m_work1;
+    TVector m_work2;
 
        TMatrix m_svd_u_t;
        TVector m_svd_u_beta;
index 4bd3d28ce69aef7163ec36b41faf7d0d069f4cde..312b314661947107a118bd243eb1732cb2900da3 100644 (file)
@@ -34,9 +34,6 @@
 
 //#include "analyze.h"
 
-#include <iostream>
-using namespace std;
-
 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
 {
        m_segments.push_back(seg);
@@ -130,6 +127,9 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
        bool locked = false, clamp[3];
        int i, mindof = 0;
 
+       // here we check if any angle limits were violated. angles whose clamped
+       // position is the same as it was before, are locked immediate. of the
+       // other violation angles the most violating angle is rememberd
        for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
                qseg = *seg;
                if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
@@ -152,6 +152,7 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
                }
        }
 
+       // lock most violating angle
        if (minseg) {
                minseg->Lock(mindof, m_jacobian, mindelta);
                locked = true;
@@ -161,11 +162,13 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
        }
 
        if (locked == false)
+               // no locking done, last inner iteration, apply the angles
                for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
                        (*seg)->UnLock();
                        (*seg)->UpdateAngleApply();
                }
        
+       // signal if another inner iteration is needed
        return locked;
 }
 
@@ -188,25 +191,14 @@ bool IK_QJacobianSolver::Solve(
 
                std::list<IK_QTask*>::iterator task;
 
-               //bool done = true;
-
                // compute jacobian
                for (task = tasks.begin(); task != tasks.end(); task++) {
                        if ((*task)->Primary())
                                (*task)->ComputeJacobian(m_jacobian);
                        else
                                (*task)->ComputeJacobian(m_jacobian_sub);
-
-                       //printf("#> %f\n", (*task)->Distance());
-                       //if ((*task)->Distance() > 1e-4)
-                       //      done = false;
                }
 
-               /*if (done) {
-                       //analyze_add_run(iterations, analyze_time()-dt);
-                       return true;
-               }*/
-
                MT_Scalar norm = 0.0;
 
                do {
index caa47b775396b8a7cbc777827123c3962bcab08a..adf95eb82dcb57af01c7e164b98060c9c56c224d 100644 (file)
@@ -53,16 +53,7 @@ public:
        IK_QJacobianSolver() {};
        ~IK_QJacobianSolver() {};
 
-       /**
-        * Compute a solution for a chain.
-     * @param root Pointer to root segment.
-        * @param tasks List of tasks.
-        * @param tolerance The maximum allowed distance between solution
-     * and goal for termination.
-        * @param max_iterations should be in the range (50 - 500) 
-        *
-     * @return True iff goal position reached.
-     */
+       // returns true if converged, false if max number of iterations was used
 
        bool Solve(
                IK_QSegment *root,
@@ -77,10 +68,12 @@ private:
        bool UpdateAngles(MT_Scalar& norm);
 
 private:
-       // the jacobian matrix
+
        IK_QJacobian m_jacobian;
        IK_QJacobian m_jacobian_sub;
+
        bool m_secondary_enabled;
+
        std::vector<IK_QSegment*> m_segments;
 };
 
index a380e29e6fa7d61821cbd7612cac36100543bd57..8d6655b153183dc0ba9fa433b324a0d0edf026c1 100644 (file)
@@ -32,9 +32,6 @@
 
 #include "IK_QSegment.h"
 
-#include <iostream>
-using namespace std;
-
 // Utility functions
 
 static MT_Matrix3x3 RotationMatrix(MT_Scalar sine, MT_Scalar cosine, int axis)
index 781a2712885745b5a626370113b61923e8a78b6a..b0e51aec3711d5c501acbb1f607b0106121df49b 100644 (file)
@@ -34,9 +34,6 @@
 
 // IK_QTask
 
-#include <iostream>
-using namespace std;
-
 IK_QTask::IK_QTask(
        int size,
        bool primary,
@@ -85,6 +82,7 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
        
        jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
 
+
        // compute derivatives
        int i;
        const IK_QSegment *seg;
index a335a94616615a54689634300956142433a4c284..b1806a955b52ac2eb38d1850ccc097fadea434df 100644 (file)
@@ -37,7 +37,6 @@
 #include "IK_QTask.h"
 
 #include <list>
-#include <iostream>
 using namespace std;
 
 typedef struct {
@@ -241,7 +240,7 @@ void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[
                         goal[0][1], goal[1][1], goal[2][1],
                         goal[0][2], goal[1][2], goal[2][2]);
 
-       IK_QTask *orient = new IK_QOrientationTask(false, qtip, rot);
+       IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
        orient->SetWeight(weight);
        qsolver->tasks.push_back(orient);
 }
index 22cb1d7a0882f147bfbb9cbaae3c63dfa76f279f..1b25361811e1627164a3a250e6b4218098ec9b5a 100644 (file)
 // ,W a diagonal matrix and V an orthogonal square matrix s.t. 
 // A = U.W.Vt. From this decomposition it is trivial to compute the 
 // inverse of A as Ainv = V.Winv.tranpose(U).
-// work_space is a temporary vector used by this class to compute
-// intermediate values during the computation of the SVD. This should
-// be of length a.num_cols. This is not checked
+//
+// s = diagonal elements of W
+// work1 = workspace, length must be A.num_rows
+// work2 = workspace, length must be A.num_cols
 
 #include "tntmath.h"
 
 namespace TNT
 {
 
-
 template <class MaTRiX, class VecToR >
-void SVD(MaTRiX &a, VecToR &w,  MaTRiX &v, VecToR &work_space) {
+void SVD(MaTRiX &A, MaTRiX &U, VecToR &s, MaTRiX &V, VecToR &work1, VecToR &work2) {
 
-               int n = a.num_cols();
-       int m = a.num_rows();
+       int m = A.num_rows();
+       int n = A.num_cols();
+       int nu = min(m,n);
 
-       int flag,i,its,j,jj,k,l(0),nm(0);
-       typename MaTRiX::value_type c,f,h,x,y,z;
-       typename MaTRiX::value_type anorm(0),g(0),scale(0);
-    typename MaTRiX::value_type s(0);
+       VecToR& work = work1;
+       VecToR& e = work2;
 
-    work_space.newsize(n);
+       U = 0;
+       s = 0;
 
-       for (i=1;i<=n;i++) {
-               l=i+1;
-               work_space(i)=scale*g;
+       int i=0, j=0, k=0;
 
-               g = (typename MaTRiX::value_type)0;
+       // Reduce A to bidiagonal form, storing the diagonal elements
+       // in s and the super-diagonal elements in e.
 
-               s = (typename  MaTRiX::value_type)0;
-        scale = (typename  MaTRiX::value_type)0;
+       int nct = min(m-1,n);
+       int nrt = max(0,min(n-2,m));
+       for (k = 0; k < max(nct,nrt); k++) {
+               if (k < nct) {
 
-               if (i <= m) {
-                       for (k=i;k<=m;k++) scale += TNT::abs(a(k,i));
-                       if (scale > (typename  MaTRiX::value_type)0) {
-                               for (k=i;k<=m;k++) {
-                                       a(k,i) /= scale;
-                                       s += a(k,i)*a(k,i);
-                               }
-                               f=a(i,i);
-                               g = -TNT::sign(sqrt(s),f);
-                               h=f*g-s;
-                               a(i,i)=f-g;
-                               if (i != n) {
-                                       for (j=l;j<=n;j++) {
-                        s = (typename  MaTRiX::value_type)0;
-                                               for (k=i;k<=m;k++) s += a(k,i)*a(k,j);
-                                               f=s/h;
-                                               for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
-                                       }
-                               }
-                               for (k=i;k<=m;k++) a(k,i) *= scale;
+                       // Compute the transformation for the k-th column and
+                       // place the k-th diagonal in s[k].
+                       // Compute 2-norm of k-th column without under/overflow.
+                       s[k] = 0;
+                       for (i = k; i < m; i++) {
+                               s[k] = hypot(s[k],A[i][k]);
                        }
-               }
-               w(i)=scale*g;
-        g = (typename  MaTRiX::value_type)0;
-        s = (typename  MaTRiX::value_type)0;
-        scale = (typename  MaTRiX::value_type)0;
-               if (i <= m && i != n) {
-                       for (k=l;k<=n;k++) scale += TNT::abs(a(i,k));
-                       if (scale > (typename  MaTRiX::value_type)0) {
-                               for (k=l;k<=n;k++) {
-                                       a(i,k) /= scale;
-                                       s += a(i,k)*a(i,k);
+                       if (s[k] != 0.0) {
+                               if (A[k][k] < 0.0) {
+                                       s[k] = -s[k];
                                }
-                               f=a(i,l);
-                               g = -TNT::sign(sqrt(s),f);
-                               h=f*g-s;
-                               a(i,l)=f-g;
-                               for (k=l;k<=n;k++) work_space(k)=a(i,k)/h;
-                               if (i != m) {
-                                       for (j=l;j<=m;j++) {
-                        s = (typename  MaTRiX::value_type)0;
-                                               for (k=l;k<=n;k++) s += a(j,k)*a(i,k);
-                                               for (k=l;k<=n;k++) a(j,k) += s*work_space(k);
-                                       }
+                               for (i = k; i < m; i++) {
+                                       A[i][k] /= s[k];
                                }
-                               for (k=l;k<=n;k++) a(i,k) *= scale;
+                               A[k][k] += 1.0;
                        }
+                       s[k] = -s[k];
                }
-               anorm=TNT::max(anorm,(TNT::abs(w(i))+TNT::abs(work_space(i))));
-       }
-       for (i=n;i>=1;i--) {
-               if (i < n) {
-                       if (g != (typename  MaTRiX::value_type)0) {
-                               for (j=l;j<=n;j++)
-                                       v(j,i)=(a(i,j)/a(i,l))/g;
-                               for (j=l;j<=n;j++) {
-                    s = (typename  MaTRiX::value_type)0;
-                                       for (k=l;k<=n;k++) s += a(i,k)*v(k,j);
-                                       for (k=l;k<=n;k++) v(k,j) += s*v(k,i);
+               for (j = k+1; j < n; j++) {
+                       if ((k < nct) && (s[k] != 0.0))  {
+
+                       // Apply the transformation.
+
+                               typename MaTRiX::value_type t = 0;
+                               for (i = k; i < m; i++) {
+                                       t += A[i][k]*A[i][j];
                                }
-                       }
-                       for (j=l;j<=n;j++) v(i,j)=v(j,i)= (typename  MaTRiX::value_type)0;
-               }
-               v(i,i)= (typename  MaTRiX::value_type)1;
-               g=work_space(i);
-               l=i;
-       }
-       for (i=n;i>=1;i--) {
-               l=i+1;
-               g=w(i);
-               if (i < n) {
-                       for (j=l;j<=n;j++) a(i,j)= (typename  MaTRiX::value_type)0;
-               }
-               if (g !=  (typename  MaTRiX::value_type)0) {
-                       g= ((typename  MaTRiX::value_type)1)/g;
-                       if (i != n) {
-                               for (j=l;j<=n;j++) {
-                    s =  (typename  MaTRiX::value_type)0;
-                                       for (k=l;k<=m;k++) s += a(k,i)*a(k,j);
-                                       f=(s/a(i,i))*g;
-                                       for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
+                               t = -t/A[k][k];
+                               for (i = k; i < m; i++) {
+                                       A[i][j] += t*A[i][k];
                                }
                        }
-                       for (j=i;j<=m;j++) a(j,i) *= g;
-               } else {
-                       for (j=i;j<=m;j++) a(j,i)= (typename  MaTRiX::value_type)0;
+
+                       // Place the k-th row of A into e for the
+                       // subsequent calculation of the row transformation.
+
+                       e[j] = A[k][j];
                }
-               ++a(i,i);
-       }
-       for (k=n;k>=1;k--) {
-               for (its=1;its<=30;its++) {
-                       flag=1;
-                       for (l=k;l>=1;l--) {
-                               nm=l-1;
-                               if (TNT::abs(work_space(l))+anorm == anorm) {
-                                       flag=0;
-                                       break;
-                               }
-                               if (TNT::abs(w(nm))+anorm == anorm) break;
+               if (k < nct) {
+
+                       // Place the transformation in U for subsequent back
+                       // multiplication.
+
+                       for (i = k; i < m; i++)
+                               U[i][k] = A[i][k];
+               }
+               if (k < nrt) {
+
+                       // Compute the k-th row transformation and place the
+                       // k-th super-diagonal in e[k].
+                       // Compute 2-norm without under/overflow.
+                       e[k] = 0;
+                       for (i = k+1; i < n; i++) {
+                               e[k] = hypot(e[k],e[i]);
                        }
-                       if (flag) {
-                               c= (typename  MaTRiX::value_type)0;
-                               s= (typename  MaTRiX::value_type)1;
-                               for (i=l;i<=k;i++) {
-                                       f=s*work_space(i);
-                                       if (TNT::abs(f)+anorm != anorm) {
-                                               g=w(i);
-                                               h= (typename  MaTRiX::value_type)TNT::pythag(float(f),float(g));
-                                               w(i)=h;
-                                               h= ((typename  MaTRiX::value_type)1)/h;
-                                               c=g*h;
-                                               s=(-f*h);
-                                               for (j=1;j<=m;j++) {
-                                                       y=a(j,nm);
-                                                       z=a(j,i);
-                                                       a(j,nm)=y*c+z*s;
-                                                       a(j,i)=z*c-y*s;
-                                               }
-                                       }
+                       if (e[k] != 0.0) {
+                               if (e[k+1] < 0.0) {
+                                       e[k] = -e[k];
                                }
-                       }
-                       z=w(k);
-                       if (l == k) {
-                               if (z <  (typename  MaTRiX::value_type)0) {
-                                       w(k) = -z;
-                                       for (j=1;j<=n;j++) v(j,k)=(-v(j,k));
+                               for (i = k+1; i < n; i++) {
+                                       e[i] /= e[k];
                                }
-                               break;
+                               e[k+1] += 1.0;
                        }
+                       e[k] = -e[k];
+                       if ((k+1 < m) & (e[k] != 0.0)) {
 
+                       // Apply the transformation.
 
-#if 1
-                       if (its == 30)
-                       {
-                                TNTException an_exception;
-                                an_exception.i = 0;
-                                throw an_exception;
-
-                                return ;
-                               assert(false);
-                       }
-#endif
-                       x=w(l);
-                       nm=k-1;
-                       y=w(nm);
-                       g=work_space(nm);
-                       h=work_space(k);
-                       f=((y-z)*(y+z)+(g-h)*(g+h))/(((typename  MaTRiX::value_type)2)*h*y);
-                       g=(typename  MaTRiX::value_type)TNT::pythag(float(f), float(1));
-                       f=((x-z)*(x+z)+h*((y/(f+TNT::sign(g,f)))-h))/x;
-                        c =  (typename  MaTRiX::value_type)1;
-                        s =  (typename  MaTRiX::value_type)1;
-                       for (j=l;j<=nm;j++) {
-                               i=j+1;
-                               g=work_space(i);
-                               y=w(i);
-                               h=s*g;
-                               g=c*g;
-                               z=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
-                               work_space(j)=z;
-                               c=f/z;
-                               s=h/z;
-                               f=x*c+g*s;
-                               g=g*c-x*s;
-                               h=y*s;
-                               y=y*c;
-                               for (jj=1;jj<=n;jj++) {
-                                       x=v(jj,j);
-                                       z=v(jj,i);
-                                       v(jj,j)=x*c+z*s;
-                                       v(jj,i)=z*c-x*s;
+                               for (i = k+1; i < m; i++) {
+                                       work[i] = 0.0;
                                }
-                               z=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
-                               w(j)=z;
-                               if (z !=  (typename  MaTRiX::value_type)0) {
-                                       z= ((typename  MaTRiX::value_type)1)/z;
-                                       c=f*z;
-                                       s=h*z;
+                               for (j = k+1; j < n; j++) {
+                                       for (i = k+1; i < m; i++) {
+                                               work[i] += e[j]*A[i][j];
+                                       }
                                }
-                               f=(c*g)+(s*y);
-                               x=(c*y)-(s*g);
-                               for (jj=1;jj<=m;jj++) {
-                                       y=a(jj,j);
-                                       z=a(jj,i);
-                                       a(jj,j)=y*c+z*s;
-                                       a(jj,i)=z*c-y*s;
+                               for (j = k+1; j < n; j++) {
+                                       typename MaTRiX::value_type t = -e[j]/e[k+1];
+                                       for (i = k+1; i < m; i++) {
+                                               A[i][j] += t*work[i];
+                                       }
                                }
                        }
-                       work_space(l)= (typename  MaTRiX::value_type)0;
-                       work_space(k)=f;
-                       w(k)=x;
-               }
-       }
-}
-
-// A is replaced by the column orthogonal matrix U 
 
-template <class MaTRiX, class VecToR >
-void SVD_a( MaTRiX &a, VecToR &w,  MaTRiX &v) {
-
-       int n = a.num_cols();
-       int m = a.num_rows();
+                       // Place the transformation in V for subsequent
+                       // back multiplication.
 
-       int flag,i,its,j,jj,k,l,nm;
-       typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;
+                       for (i = k+1; i < n; i++)
+                               V[i][k] = e[i];
+               }
+       }
 
-       VecToR work_space;
-       work_space.newsize(n);
+       // Set up the final bidiagonal matrix or order p.
 
-       g = scale = anorm = 0.0;
-       
-       for (i=1;i <=n;i++) {
-               l = i+1;
-               work_space(i) = scale*g;
-               g = s=scale=0.0;
+       int p = min(n,m+1);
+       if (nct < n) {
+               s[nct] = A[nct][nct];
+       }
+       if (m < p) {
+               s[p-1] = 0.0;
+       }
+       if (nrt+1 < p) {
+               e[nrt] = A[nrt][p-1];
+       }
+       e[p-1] = 0.0;
 
-               if (i <= m) {
-                       for(k=i; k<=m; k++) scale += abs(a(k,i));
+       // If required, generate U.
 
-                       if (scale) {
-                               for (k = i; k <=m ; k++) {
-                                       a(k,i) /= scale;
-                                       s += a(k,i)*a(k,i);
+       for (j = nct; j < nu; j++) {
+               for (i = 0; i < m; i++) {
+                       U[i][j] = 0.0;
+               }
+               U[j][j] = 1.0;
+       }
+       for (k = nct-1; k >= 0; k--) {
+               if (s[k] != 0.0) {
+                       for (j = k+1; j < nu; j++) {
+                               typename MaTRiX::value_type t = 0;
+                               for (i = k; i < m; i++) {
+                                       t += U[i][k]*U[i][j];
                                }
-                               f = a(i,i);
-                               g = -sign(sqrt(s),f);
-                               h = f*g -s;
-                               a(i,i) = f-g;
-       
-                               for (j = l; j <=n; j++) {
-                                       for (s = 0.0,k =i;k<=m;k++) s += a(k,i)*a(k,j);
-                                       f = s/h;
-                                       for (k = i; k <= m; k++) a(k,j) += f*a(k,i);
+                               t = -t/U[k][k];
+                               for (i = k; i < m; i++) {
+                                       U[i][j] += t*U[i][k];
                                }
-                               for (k = i; k <=m;k++) a(k,i) *= scale;
                        }
+                       for (i = k; i < m; i++ ) {
+                               U[i][k] = -U[i][k];
+                       }
+                       U[k][k] = 1.0 + U[k][k];
+                       for (i = 0; i < k-1; i++) {
+                               U[i][k] = 0.0;
+                       }
+               } else {
+                       for (i = 0; i < m; i++) {
+                               U[i][k] = 0.0;
+                       }
+                       U[k][k] = 1.0;
                }
+       }
 
-               w(i) = scale*g;
-               g = s = scale = 0.0;
+       // If required, generate V.
 
-               if (i <=m && i != n) {
-                       for (k = l; k <=n;k++) scale += abs(a(i,k));
-                       if (scale) {
-                               for(k = l;k <=n;k++) {
-                                       a(i,k) /= scale;
-                                       s += a(i,k) * a(i,k);
+       for (k = n-1; k >= 0; k--) {
+               if ((k < nrt) & (e[k] != 0.0)) {
+                       for (j = k+1; j < nu; j++) {
+                               typename MaTRiX::value_type t = 0;
+                               for (i = k+1; i < n; i++) {
+                                       t += V[i][k]*V[i][j];
                                }
-
-                               f = a(i,l);
-                               g = -sign(sqrt(s),f);
-                               h= f*g -s;
-                               a(i,l) = f-g;
-                               for(k=l;k<=n;k++) work_space(k) = a(i,k)/h;
-                               for (j=l;j<=m;j++) {
-                                       for(s = 0.0,k=l;k<=n;k++) s+= a(j,k)*a(i,k);
-                                       for(k=l;k<=n;k++) a(j,k) += s*work_space(k);
+                               t = -t/V[k+1][k];
+                               for (i = k+1; i < n; i++) {
+                                       V[i][j] += t*V[i][k];
                                }
-                               for(k=l;k<=n;k++) a(i,k)*=scale;
                        }
                }
-               anorm = max(anorm,(abs(w(i)) + abs(work_space(i))));
-       }
-       for (i=n;i>=1;i--) {
-               if (i <n) {
-                       if (g) {
-                               for(j=l;j<=n;j++) v(j,i) = (a(i,j)/a(i,l))/g;
-                               for(j=l;j<=n;j++) {
-                                       for(s=0.0,k=l;k<=n;k++) s += a(i,k)*v(k,j);
-                                       for(k=l; k<=n;k++) v(k,j) +=s*v(k,i);
-                               }
-                       }
-                       for(j=l;j <=n;j++) v(i,j) = v(j,i) = 0.0;
+               for (i = 0; i < n; i++) {
+                       V[i][k] = 0.0;
                }
-               v(i,i) = 1.0;
-               g = work_space(i);
-               l = i;
+               V[k][k] = 1.0;
        }
 
-       for (i = min(m,n);i>=1;i--) {
-               l = i+1;
-               g = w(i);
-               for (j=l;j <=n;j++) a(i,j) = 0.0;
-               if (g) {
-                       g = 1.0/g;
-                       for (j=l;j<=n;j++) {
-                               for (s = 0.0,k=l;k<=m;k++) s += a(k,i)*a(k,j);
-                               f = (s/a(i,i))*g;
-                               for (k=i;k<=m;k++) a(k,j) += f*a(k,i);  
+       // Main iteration loop for the singular values.
+
+       int pp = p-1;
+       int iter = 0;
+       typename MaTRiX::value_type eps = pow(2.0,-52.0);
+       while (p > 0) {
+               int kase=0;
+               k=0;
+
+               // Here is where a test for too many iterations would go.
+
+               // This section of the program inspects for
+               // negligible elements in the s and e arrays.  On
+               // completion the variables kase and k are set as follows.
+
+               // kase = 1       if s(p) and e[k-1] are negligible and k<p
+               // kase = 2       if s(k) is negligible and k<p
+               // kase = 3       if e[k-1] is negligible, k<p, and
+               //                                s(k), ..., s(p) are not negligible (qr step).
+               // kase = 4       if e(p-1) is negligible (convergence).
+
+               for (k = p-2; k >= -1; k--) {
+                       if (k == -1) {
+                               break;
+                       }
+                       if (abs(e[k]) <= eps*(abs(s[k]) + abs(s[k+1]))) {
+                               e[k] = 0.0;
+                               break;
                        }
-                       for (j=i;j<=m;j++) a(j,i)*=g;
-               } else {
-                       for (j=i;j<=m;j++) a(j,i) = 0.0;
                }
-               ++a(i,i);
-       }
-
-       for (k=n;k>=1;k--) {
-               for (its=1;its<=30;its++) {
-                       flag=1;
-                       for(l=k;l>=1;l--) {
-                               nm = l-1;
-                               if (abs(work_space(l)) + anorm == anorm) {
-                                       flag = 0;
+               if (k == p-2) {
+                       kase = 4;
+               } else {
+                       int ks;
+                       for (ks = p-1; ks >= k; ks--) {
+                               if (ks == k) {
+                                       break;
+                               }
+                               typename MaTRiX::value_type t = (ks != p ? abs(e[ks]) : 0.) + 
+                                                         (ks != k+1 ? abs(e[ks-1]) : 0.);
+                               if (abs(s[ks]) <= eps*t)  {
+                                       s[ks] = 0.0;
                                        break;
                                }
-                               if (abs(w(nm)) + anorm == anorm) break;
                        }
-                       if (flag) {
-                               c = 0.0;
-                               s = 1.0;
-                               for (i=l;i<=k;i++) {
-                                       f = s*work_space(i);
-                                       work_space(i) = c*work_space(i);
-                                       if (abs(f) +anorm == anorm) break;
-                                       g = w(i);
-                                       h  = pythag(f,g);
-                                       w(i) = h;
-                                       h = 1.0/h;
-                                       c = g*h;
-                                       s = -f*h;
-                                       for (j=1;j<=m;j++) {
-                                               y= a(j,nm);
-                                               z=a(j,i);
-                                               a(j,nm) = y*c + z*s;
-                                               a(j,i) = z*c - y*s;
+                       if (ks == k) {
+                               kase = 3;
+                       } else if (ks == p-1) {
+                               kase = 1;
+                       } else {
+                               kase = 2;
+                               k = ks;
+                       }
+               }
+               k++;
+
+               // Perform the task indicated by kase.
+
+               switch (kase) {
+
+                       // Deflate negligible s(p).
+
+                       case 1: {
+                               typename MaTRiX::value_type f = e[p-2];
+                               e[p-2] = 0.0;
+                               for (j = p-2; j >= k; j--) {
+                                       typename MaTRiX::value_type t = hypot(s[j],f);
+                                       typename MaTRiX::value_type cs = s[j]/t;
+                                       typename MaTRiX::value_type sn = f/t;
+                                       s[j] = t;
+                                       if (j != k) {
+                                               f = -sn*e[j-1];
+                                               e[j-1] = cs*e[j-1];
+                                       }
+
+                                       for (i = 0; i < n; i++) {
+                                               t = cs*V[i][j] + sn*V[i][p-1];
+                                               V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1];
+                                               V[i][j] = t;
                                        }
                                }
                        }
-                       z=w(k);
-                       if (l==k) {
-                               if (z <0.0) {
-                                       w(k) = -z;
-                                       for (j=1;j<=n;j++) v(j,k) = -v(j,k);
+                       break;
+
+                       // Split at negligible s(k).
+
+                       case 2: {
+                               typename MaTRiX::value_type f = e[k-1];
+                               e[k-1] = 0.0;
+                               for (j = k; j < p; j++) {
+                                       typename MaTRiX::value_type t = hypot(s[j],f);
+                                       typename MaTRiX::value_type cs = s[j]/t;
+                                       typename MaTRiX::value_type sn = f/t;
+                                       s[j] = t;
+                                       f = -sn*e[j];
+                                       e[j] = cs*e[j];
+
+                                       for (i = 0; i < m; i++) {
+                                               t = cs*U[i][j] + sn*U[i][k-1];
+                                               U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1];
+                                               U[i][j] = t;
+                                       }
                                }
-                               break;
                        }
+                       break;
+
+                       // Perform one qr step.
+
+                       case 3: {
+
+                               // Calculate the shift.
+
+                               typename MaTRiX::value_type scale = max(max(max(max(
+                                                 abs(s[p-1]),abs(s[p-2])),abs(e[p-2])), 
+                                                 abs(s[k])),abs(e[k]));
+                               typename MaTRiX::value_type sp = s[p-1]/scale;
+                               typename MaTRiX::value_type spm1 = s[p-2]/scale;
+                               typename MaTRiX::value_type epm1 = e[p-2]/scale;
+                               typename MaTRiX::value_type sk = s[k]/scale;
+                               typename MaTRiX::value_type ek = e[k]/scale;
+                               typename MaTRiX::value_type b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0;
+                               typename MaTRiX::value_type c = (sp*epm1)*(sp*epm1);
+                               typename MaTRiX::value_type shift = 0.0;
+                               if ((b != 0.0) || (c != 0.0)) {
+                                       shift = sqrt(b*b + c);
+                                       if (b < 0.0) {
+                                               shift = -shift;
+                                       }
+                                       shift = c/(b + shift);
+                               }
+                               typename MaTRiX::value_type f = (sk + sp)*(sk - sp) + shift;
+                               typename MaTRiX::value_type g = sk*ek;
+
+                               // Chase zeros.
+
+                               for (j = k; j < p-1; j++) {
+                                       typename MaTRiX::value_type t = hypot(f,g);
+                                       typename MaTRiX::value_type cs = f/t;
+                                       typename MaTRiX::value_type sn = g/t;
+                                       if (j != k) {
+                                               e[j-1] = t;
+                                       }
+                                       f = cs*s[j] + sn*e[j];
+                                       e[j] = cs*e[j] - sn*s[j];
+                                       g = sn*s[j+1];
+                                       s[j+1] = cs*s[j+1];
+
+                                       for (i = 0; i < n; i++) {
+                                               t = cs*V[i][j] + sn*V[i][j+1];
+                                               V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1];
+                                               V[i][j] = t;
+                                       }
 
-                       if (its == 30) assert(false);
-
-                       x=w(l);
-                       nm=k-1;
-                       y=w(nm);
-                       g=work_space(nm);
-                       h=work_space(k);
-                       
-                       f= ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y);
-                       g = pythag(f,1.0);
-                       f= ((x-z)*(x+z) + h*((y/(f + sign(g,f)))-h))/x;
-                       c=s=1.0;
-
-                       for (j=l;j<=nm;j++) {
-                               i=j+1;
-                               g = work_space(i);
-                               y=w(i);
-                               h=s*g;
-                               g=c*g;
-                               z=pythag(f,h);
-                               work_space(j) = z;
-                               c=f/z;
-                               s=h/z;
-                               f=x*c + g*s;
-                               g= g*c - x*s;
-                               h=y*s;
-                               y*=c;
-                               for(jj=1;jj<=n;jj++) {
-                                       x=v(jj,j);
-                                       z=v(jj,i);
-                                       v(jj,j) = x*c + z*s;
-                                       v(jj,i) = z*c- x*s;
+                                       t = hypot(f,g);
+                                       cs = f/t;
+                                       sn = g/t;
+                                       s[j] = t;
+                                       f = cs*e[j] + sn*s[j+1];
+                                       s[j+1] = -sn*e[j] + cs*s[j+1];
+                                       g = sn*e[j+1];
+                                       e[j+1] = cs*e[j+1];
+                                       if (j < m-1) {
+                                               for (i = 0; i < m; i++) {
+                                                       t = cs*U[i][j] + sn*U[i][j+1];
+                                                       U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1];
+                                                       U[i][j] = t;
+                                               }
+                                       }
                                }
-                               z=pythag(f,h);
-                               w(j)=z;
-                               if(z) {
-                                       z = 1.0/z;
-                                       c=f*z;
-                                       s=h*z;
+                               e[p-2] = f;
+                               iter = iter + 1;
+                       }
+                       break;
+
+                       // Convergence.
+
+                       case 4: {
+
+                               // Make the singular values positive.
+
+                               if (s[k] <= 0.0) {
+                                       s[k] = (s[k] < 0.0 ? -s[k] : 0.0);
+
+                                       for (i = 0; i <= pp; i++)
+                                               V[i][k] = -V[i][k];
                                }
-                               f=c*g + s*y;
-                               x= c*y-s*g;
-                       
-                               for(jj=1;jj<=m;jj++) {
-                                       y=a(jj,j);
-                                       z=a(jj,i);
-                                       a(jj,j) = y*c+z*s;
-                                       a(jj,i) = z*c - y*s;
+
+                               // Order the singular values.
+
+                               while (k < pp) {
+                                       if (s[k] >= s[k+1]) {
+                                               break;
+                                       }
+                                       typename MaTRiX::value_type t = s[k];
+                                       s[k] = s[k+1];
+                                       s[k+1] = t;
+                                       if (k < n-1) {
+                                               for (i = 0; i < n; i++) {
+                                                       t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t;
+                                               }
+                                       }
+                                       if (k < m-1) {
+                                               for (i = 0; i < m; i++) {
+                                                       t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t;
+                                               }
+                                       }
+                                       k++;
                                }
+                               iter = 0;
+                               p--;
                        }
-
-                       work_space(l) = 0.0;
-                       work_space(k) = f;
-                       w(k) = x;
+                       break;
                }
        }
 }
index 9e79c07fa673ac50f935240dccc0b9b7dd672b7f..5773900caf98eadd7ba901fd53e5eb99ad06abd4 100644 (file)
@@ -70,10 +70,15 @@ inline float min(float a, float b)
     return (a < b ? a : b);
 }
 
-inline int min(int a,int b) {
+inline int min(int a, int b)
+{
     return (a < b ? a : b);
 }
 
+inline int max(int a, int b)
+{
+    return (a > b ? a : b);
+}
 
 inline float max(float a, float b)
 {
index 5e85dc596ab3df63e8dd8afe769269fbd2035818..372718af3121851e4b2fc371e7a17f67e2742d2c 100644 (file)
@@ -58,6 +58,7 @@ public:
     MT_Point3& operator+=(const MT_Vector3& v);
     MT_Point3& operator-=(const MT_Vector3& v);
     MT_Point3& operator=(const MT_Vector3& v);
+    MT_Point3& operator=(const MT_Point3& v);
 
     MT_Scalar  distance(const MT_Point3& p) const;
     MT_Scalar  distance2(const MT_Point3& p) const;
index e6ce4f9d9a3db34fda933870039eefa404e20117..081a819569454ea048aa7e71df8330087b764824 100644 (file)
@@ -15,6 +15,11 @@ GEN_INLINE MT_Point3& MT_Point3::operator=(const MT_Vector3& v) {
     return *this;
 }
 
+GEN_INLINE MT_Point3& MT_Point3::operator=(const MT_Point3& v) {
+    m_co[0] = v[0]; m_co[1] = v[1]; m_co[2] = v[2];
+    return *this;
+}
+
 GEN_INLINE MT_Scalar MT_Point3::distance(const MT_Point3& p) const {
     return (p - *this).length();
 }