sandey/blender
1
0
Fork 0
forked from bartvdbraak/blender
Code Pull Requests Activity

Update SConscript.

Browse Source 
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.
This commit is contained in:
Brecht Van Lommel 2005-08-27 13:45:19 +00:00
parent fa0bbaf380
commit ae9dcb3dc2
12 changed files with 405 additions and 512 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
intern/iksolver/SConscript
View File

@ -4,11 +4,11 @@ Import ('library_env')
iksolver_env = library_env.Copy () iksolver_env = library_env.Copy ()
source_files = ['intern/IK_QChain.cpp', source_files = ['intern/IK_QTask.cpp',
'intern/IK_QJacobianSolver.cpp', 'intern/IK_QJacobianSolver.cpp',
'intern/IK_QSegment.cpp', 'intern/IK_QSegment.cpp',
'intern/IK_Solver.cpp', 'intern/IK_QJacobian.cpp',
'intern/MT_ExpMap.cpp'] 'intern/IK_Solver.cpp']
iksolver_env.Append (CPPPATH = ['intern', iksolver_env.Append (CPPPATH = ['intern',
'../moto/include', '../moto/include',

100
intern/iksolver/intern/IK_QJacobian.cpp
View File

@ -33,9 +33,6 @@
#include "IK_QJacobian.h" #include "IK_QJacobian.h"
#include "TNT/svd.h" #include "TNT/svd.h"
#include <iostream>
using namespace std;
IK_QJacobian::IK_QJacobian() IK_QJacobian::IK_QJacobian()
: m_sdls(true), m_min_damp(1.0) : 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 = 1.0;
m_weight_sqrt = 1.0; m_weight_sqrt = 1.0;
// m_svd_work_space.newsize(dof); // TNT resizes this?
if (task_size >= dof) { if (task_size >= dof) {
m_transpose = false; 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_v.newsize(dof, dof);
m_svd_w.newsize(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_t.newsize(dof, task_size);
m_svd_u_beta.newsize(dof); 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. // as the original, and often allows using smaller matrices.
m_transpose = true; m_transpose = true;
m_jacobian_t.newsize(dof, task_size);
m_svd_u.newsize(task_size, task_size); m_svd_u.newsize(task_size, task_size);
m_svd_v.newsize(dof, task_size); m_svd_v.newsize(dof, task_size);
m_svd_w.newsize(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_t.newsize(task_size, task_size);
m_svd_u_beta.newsize(task_size); m_svd_u_beta.newsize(task_size);
} }
@ -121,51 +124,19 @@ void IK_QJacobian::Invert()
if (m_transpose) { if (m_transpose) {
// SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal, // 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 // so J = U*W*Vt and Jinv = V*Winv*Ut
TNT::transpose(m_jacobian, m_svd_v); 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);
TNT::SVD(m_svd_v, m_svd_w, m_svd_u, m_svd_work_space);
} }
else { else {
// SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal, // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
// so Jinv = V*Winv*Ut // so Jinv = V*Winv*Ut
m_svd_u = m_jacobian; TNT::SVD(m_jacobian, m_svd_u, m_svd_w, m_svd_v, m_work1, m_work2);
TNT::SVD(m_svd_u, m_svd_w, m_svd_v, m_svd_work_space);
} }
if (m_sdls) if (m_sdls)
InvertSDLS(); InvertSDLS();
else else
InvertDLS(); 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() bool IK_QJacobian::ComputeNullProjection()
@ -209,36 +180,6 @@ void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
{ {
if (!ComputeNullProjection()) if (!ComputeNullProjection())
return; 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 // restrict lower priority jacobian
jacobian.Restrict(m_d_theta, m_null); 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 // note: now damps secondary angles with minimum damping value from
// SDLS, to avoid shaking when the primary task is near singularities, // SDLS, to avoid shaking when the primary task is near singularities,
// doesn't work well at all // doesn't work well at all
int i;
for (i = 0; i < m_d_theta.size(); i++) for (i = 0; i < m_d_theta.size(); i++)
m_d_theta[i] = m_d_theta[i] + /*m_min_damp**/jacobian.AngleUpdate(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) 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; 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 MT_Scalar IK_QJacobian::AngleUpdate(int dof_id) const
{ {
return m_d_theta[dof_id]; return m_d_theta[dof_id];

13
intern/iksolver/intern/IK_QJacobian.h
View File

@ -36,7 +36,6 @@
#define NAN_INCLUDED_IK_QJacobian_h #define NAN_INCLUDED_IK_QJacobian_h
#include "TNT/cmat.h" #include "TNT/cmat.h"
#include "TNT/fmat.h"
#include <vector> #include <vector>
#include "MT_Vector3.h" #include "MT_Vector3.h"
@ -44,7 +43,7 @@ class IK_QJacobian
{ {
public: public:
typedef TNT::Matrix<MT_Scalar> TMatrix; typedef TNT::Matrix<MT_Scalar> TMatrix;
typedef TNT::Vector<MT_Scalar>TVector; typedef TNT::Vector<MT_Scalar> TVector;
IK_QJacobian(); IK_QJacobian();
~IK_QJacobian(); ~IK_QJacobian();
@ -71,11 +70,6 @@ public:
void Restrict(TVector& d_theta, TMatrix& null); void Restrict(TVector& d_theta, TMatrix& null);
void SubTask(IK_QJacobian& jacobian); void SubTask(IK_QJacobian& jacobian);
#if 0
void SetSecondary(int dof_id, MT_Scalar d);
void SolveSecondary();
#endif
private: private:
void InvertSDLS(); void InvertSDLS();
@ -85,7 +79,7 @@ private:
bool m_transpose; bool m_transpose;
// the jacobian matrix and it's null space projector // the jacobian matrix and it's null space projector
TMatrix m_jacobian; TMatrix m_jacobian, m_jacobian_t;
TMatrix m_null; TMatrix m_null;
/// the vector of intermediate betas /// the vector of intermediate betas
@ -97,9 +91,10 @@ private:
/// space required for SVD computation /// space required for SVD computation
TVector m_svd_w; TVector m_svd_w;
TVector m_svd_work_space;
TMatrix m_svd_v; TMatrix m_svd_v;
TMatrix m_svd_u; TMatrix m_svd_u;
TVector m_work1;
TVector m_work2;
TMatrix m_svd_u_t; TMatrix m_svd_u_t;
TVector m_svd_u_beta; TVector m_svd_u_beta;

20
intern/iksolver/intern/IK_QJacobianSolver.cpp
View File

@ -34,9 +34,6 @@
//#include "analyze.h" //#include "analyze.h"
#include <iostream>
using namespace std;
void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg) void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
{ {
m_segments.push_back(seg); m_segments.push_back(seg);
@ -130,6 +127,9 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
bool locked = false, clamp[3]; bool locked = false, clamp[3];
int i, mindof = 0; 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++) { for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
qseg = *seg; qseg = *seg;
if (qseg->UpdateAngle(m_jacobian, delta, clamp)) { if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
@ -152,6 +152,7 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
} }
} }
// lock most violating angle
if (minseg) { if (minseg) {
minseg->Lock(mindof, m_jacobian, mindelta); minseg->Lock(mindof, m_jacobian, mindelta);
locked = true; locked = true;
@ -161,11 +162,13 @@ bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
} }
if (locked == false) if (locked == false)
// no locking done, last inner iteration, apply the angles
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) { for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->UnLock(); (*seg)->UnLock();
(*seg)->UpdateAngleApply(); (*seg)->UpdateAngleApply();
} }
// signal if another inner iteration is needed
return locked; return locked;
} }
@ -188,25 +191,14 @@ bool IK_QJacobianSolver::Solve(
std::list<IK_QTask*>::iterator task; std::list<IK_QTask*>::iterator task;
//bool done = true;
// compute jacobian // compute jacobian
for (task = tasks.begin(); task != tasks.end(); task++) { for (task = tasks.begin(); task != tasks.end(); task++) {
if ((*task)->Primary()) if ((*task)->Primary())
(*task)->ComputeJacobian(m_jacobian); (*task)->ComputeJacobian(m_jacobian);
else else
(*task)->ComputeJacobian(m_jacobian_sub); (*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; MT_Scalar norm = 0.0;
do { do {

15
intern/iksolver/intern/IK_QJacobianSolver.h
View File

@ -53,16 +53,7 @@ public:
IK_QJacobianSolver() {}; IK_QJacobianSolver() {};
~IK_QJacobianSolver() {}; ~IK_QJacobianSolver() {};
/** // returns true if converged, false if max number of iterations was used
* 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.
*/
bool Solve( bool Solve(
IK_QSegment *root, IK_QSegment *root,
@ -77,10 +68,12 @@ private:
bool UpdateAngles(MT_Scalar& norm); bool UpdateAngles(MT_Scalar& norm);
private: private:
// the jacobian matrix
IK_QJacobian m_jacobian; IK_QJacobian m_jacobian;
IK_QJacobian m_jacobian_sub; IK_QJacobian m_jacobian_sub;
bool m_secondary_enabled; bool m_secondary_enabled;
std::vector<IK_QSegment*> m_segments; std::vector<IK_QSegment*> m_segments;
}; };

3
intern/iksolver/intern/IK_QSegment.cpp
View File

@ -32,9 +32,6 @@
#include "IK_QSegment.h" #include "IK_QSegment.h"
#include <iostream>
using namespace std;
// Utility functions // Utility functions
static MT_Matrix3x3 RotationMatrix(MT_Scalar sine, MT_Scalar cosine, int axis) static MT_Matrix3x3 RotationMatrix(MT_Scalar sine, MT_Scalar cosine, int axis)

4
intern/iksolver/intern/IK_QTask.cpp
View File

@ -34,9 +34,6 @@
// IK_QTask // IK_QTask
#include <iostream>
using namespace std;
IK_QTask::IK_QTask( IK_QTask::IK_QTask(
int size, int size,
bool primary, bool primary,
@ -85,6 +82,7 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
jacobian.SetBetas(m_id, m_size, m_weight*d_pos); jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
// compute derivatives // compute derivatives
int i; int i;
const IK_QSegment *seg; const IK_QSegment *seg;

3
intern/iksolver/intern/IK_Solver.cpp
View File

@ -37,7 +37,6 @@
#include "IK_QTask.h" #include "IK_QTask.h"
#include <list> #include <list>
#include <iostream>
using namespace std; using namespace std;
typedef struct { 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][1], goal[1][1], goal[2][1],
goal[0][2], goal[1][2], goal[2][2]); 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); orient->SetWeight(weight);
qsolver->tasks.push_back(orient); qsolver->tasks.push_back(orient);
} }

740
intern/iksolver/intern/TNT/svd.h
View File

@ -11,430 +11,414 @@
// ,W a diagonal matrix and V an orthogonal square matrix s.t. // ,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 // A = U.W.Vt. From this decomposition it is trivial to compute the
// inverse of A as Ainv = V.Winv.tranpose(U). // 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 // s = diagonal elements of W
// be of length a.num_cols. This is not checked // work1 = workspace, length must be A.num_rows
// work2 = workspace, length must be A.num_cols
#include "tntmath.h" #include "tntmath.h"
namespace TNT namespace TNT
{ {
template <class MaTRiX, class VecToR > 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); VecToR& work = work1;
typename MaTRiX::value_type c,f,h,x,y,z; VecToR& e = work2;
typename MaTRiX::value_type anorm(0),g(0),scale(0);
typename MaTRiX::value_type s(0);
work_space.newsize(n); U = 0;
s = 0;
for (i=1;i<=n;i++) { int i=0, j=0, k=0;
l=i+1;
work_space(i)=scale*g;
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; int nct = min(m-1,n);
scale = (typename MaTRiX::value_type)0; int nrt = max(0,min(n-2,m));
for (k = 0; k < max(nct,nrt); k++) {
if (k < nct) {
if (i <= m) { // Compute the transformation for the k-th column and
for (k=i;k<=m;k++) scale += TNT::abs(a(k,i)); // place the k-th diagonal in s[k].
if (scale > (typename MaTRiX::value_type)0) { // Compute 2-norm of k-th column without under/overflow.
for (k=i;k<=m;k++) { s[k] = 0;
a(k,i) /= scale; for (i = k; i < m; i++) {
s += a(k,i)*a(k,i); s[k] = hypot(s[k],A[i][k]);
}
if (s[k] != 0.0) {
if (A[k][k] < 0.0) {
s[k] = -s[k];
} }
f=a(i,i); for (i = k; i < m; i++) {
g = -TNT::sign(sqrt(s),f); A[i][k] /= s[k];
h=f*g-s; }
a(i,i)=f-g; A[k][k] += 1.0;
if (i != n) { }
for (j=l;j<=n;j++) { s[k] = -s[k];
s = (typename MaTRiX::value_type)0; }
for (k=i;k<=m;k++) s += a(k,i)*a(k,j); for (j = k+1; j < n; j++) {
f=s/h; if ((k < nct) && (s[k] != 0.0)) {
for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
// Apply the transformation.
typename MaTRiX::value_type t = 0;
for (i = k; i < m; i++) {
t += A[i][k]*A[i][j];
}
t = -t/A[k][k];
for (i = k; i < m; i++) {
A[i][j] += t*A[i][k];
}
}
// Place the k-th row of A into e for the
// subsequent calculation of the row transformation.
e[j] = A[k][j];
}
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 (e[k] != 0.0) {
if (e[k+1] < 0.0) {
e[k] = -e[k];
}
for (i = k+1; i < n; i++) {
e[i] /= e[k];
}
e[k+1] += 1.0;
}
e[k] = -e[k];
if ((k+1 < m) & (e[k] != 0.0)) {
// Apply the transformation.
for (i = k+1; i < m; i++) {
work[i] = 0.0;
}
for (j = k+1; j < n; j++) {
for (i = k+1; i < m; i++) {
work[i] += e[j]*A[i][j];
} }
} }
for (k=i;k<=m;k++) a(k,i) *= scale; for (j = k+1; j < n; j++) {
} typename MaTRiX::value_type t = -e[j]/e[k+1];
} for (i = k+1; i < m; i++) {
w(i)=scale*g; A[i][j] += t*work[i];
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);
}
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 (k=l;k<=n;k++) a(i,k) *= scale;
} }
// Place the transformation in V for subsequent
// back multiplication.
for (i = k+1; i < n; i++)
V[i][k] = e[i];
} }
anorm=TNT::max(anorm,(TNT::abs(w(i))+TNT::abs(work_space(i))));
} }
for (i=n;i>=1;i--) {
if (i < n) { // Set up the final bidiagonal matrix or order p.
if (g != (typename MaTRiX::value_type)0) {
for (j=l;j<=n;j++) int p = min(n,m+1);
v(j,i)=(a(i,j)/a(i,l))/g; if (nct < n) {
for (j=l;j<=n;j++) { s[nct] = A[nct][nct];
s = (typename MaTRiX::value_type)0; }
for (k=l;k<=n;k++) s += a(i,k)*v(k,j); if (m < p) {
for (k=l;k<=n;k++) v(k,j) += s*v(k,i); s[p-1] = 0.0;
}
if (nrt+1 < p) {
e[nrt] = A[nrt][p-1];
}
e[p-1] = 0.0;
// If required, generate U.
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];
}
t = -t/U[k][k];
for (i = k; i < m; i++) {
U[i][j] += t*U[i][k];
} }
} }
for (j=l;j<=n;j++) v(i,j)=v(j,i)= (typename MaTRiX::value_type)0; for (i = k; i < m; i++ ) {
} U[i][k] = -U[i][k];
v(i,i)= (typename MaTRiX::value_type)1; }
g=work_space(i); U[k][k] = 1.0 + U[k][k];
l=i; for (i = 0; i < k-1; i++) {
} U[i][k] = 0.0;
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);
}
} }
for (j=i;j<=m;j++) a(j,i) *= g;
} else { } else {
for (j=i;j<=m;j++) a(j,i)= (typename MaTRiX::value_type)0; for (i = 0; i < m; i++) {
U[i][k] = 0.0;
}
U[k][k] = 1.0;
} }
++a(i,i);
} }
for (k=n;k>=1;k--) {
for (its=1;its<=30;its++) { // If required, generate V.
flag=1;
for (l=k;l>=1;l--) { for (k = n-1; k >= 0; k--) {
nm=l-1; if ((k < nrt) & (e[k] != 0.0)) {
if (TNT::abs(work_space(l))+anorm == anorm) { for (j = k+1; j < nu; j++) {
flag=0; typename MaTRiX::value_type t = 0;
for (i = k+1; i < n; i++) {
t += V[i][k]*V[i][j];
}
t = -t/V[k+1][k];
for (i = k+1; i < n; i++) {
V[i][j] += t*V[i][k];
}
}
}
for (i = 0; i < n; i++) {
V[i][k] = 0.0;
}
V[k][k] = 1.0;
}
// 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;
}
}
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; break;
} }
if (TNT::abs(w(nm))+anorm == anorm) break;
} }
if (flag) { if (ks == k) {
c= (typename MaTRiX::value_type)0; kase = 3;
s= (typename MaTRiX::value_type)1; } else if (ks == p-1) {
for (i=l;i<=k;i++) { kase = 1;
f=s*work_space(i); } else {
if (TNT::abs(f)+anorm != anorm) { kase = 2;
g=w(i); k = ks;
h= (typename MaTRiX::value_type)TNT::pythag(float(f),float(g)); }
w(i)=h; }
h= ((typename MaTRiX::value_type)1)/h; k++;
c=g*h;
s=(-f*h); // Perform the task indicated by kase.
for (j=1;j<=m;j++) {
y=a(j,nm); switch (kase) {
z=a(j,i);
a(j,nm)=y*c+z*s; // Deflate negligible s(p).
a(j,i)=z*c-y*s;
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;
}
}
}
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;
// 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;
}
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;
} }
} }
} }
e[p-2] = f;
iter = iter + 1;
} }
z=w(k); break;
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));
}
break;
}
// Convergence.
#if 1 case 4: {
if (its == 30)
{
TNTException an_exception;
an_exception.i = 0;
throw an_exception;
return ; // Make the singular values positive.
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;
}
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;
}
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;
}
}
work_space(l)= (typename MaTRiX::value_type)0;
work_space(k)=f;
w(k)=x;
}
}
}
// A is replaced by the column orthogonal matrix U if (s[k] <= 0.0) {
s[k] = (s[k] < 0.0 ? -s[k] : 0.0);
template <class MaTRiX, class VecToR > for (i = 0; i <= pp; i++)
void SVD_a( MaTRiX &a, VecToR &w, MaTRiX &v) { V[i][k] = -V[i][k];
int n = a.num_cols();
int m = a.num_rows();
int flag,i,its,j,jj,k,l,nm;
typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;
VecToR work_space;
work_space.newsize(n);
g = scale = anorm = 0.0;
for (i=1;i <=n;i++) {
l = i+1;
work_space(i) = scale*g;
g = s=scale=0.0;
if (i <= m) {
for(k=i; k<=m; k++) scale += abs(a(k,i));
if (scale) {
for (k = i; k <=m ; k++) {
a(k,i) /= scale;
s += a(k,i)*a(k,i);
}
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);
}
for (k = i; k <=m;k++) a(k,i) *= scale;
}
}
w(i) = scale*g;
g = s = scale = 0.0;
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);
} }
f = a(i,l); // Order the singular values.
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);
}
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;
}
v(i,i) = 1.0;
g = work_space(i);
l = i;
}
for (i = min(m,n);i>=1;i--) { while (k < pp) {
l = i+1; if (s[k] >= s[k+1]) {
g = w(i); break;
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);
}
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;
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;
} }
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--;
} }
z=w(k); break;
if (l==k) {
if (z <0.0) {
w(k) = -z;
for (j=1;j<=n;j++) v(j,k) = -v(j,k);
}
break;
}
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;
}
z=pythag(f,h);
w(j)=z;
if(z) {
z = 1.0/z;
c=f*z;
s=h*z;
}
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;
}
}
work_space(l) = 0.0;
work_space(k) = f;
w(k) = x;
} }
} }
} }

7
intern/iksolver/intern/TNT/tntmath.h
View File

@ -70,10 +70,15 @@ inline float min(float a, float b)
return (a < b ? a : 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); return (a < b ? a : b);
} }
inline int max(int a, int b)
{
return (a > b ? a : b);
}
inline float max(float a, float b) inline float max(float a, float b)
{ {

1
intern/moto/include/MT_Point3.h
View 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_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 distance(const MT_Point3& p) const;
MT_Scalar distance2(const MT_Point3& p) const; MT_Scalar distance2(const MT_Point3& p) const;

5
intern/moto/include/MT_Point3.inl
View File

@ -15,6 +15,11 @@ GEN_INLINE MT_Point3& MT_Point3::operator=(const MT_Vector3& v) {
return *this; 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 { GEN_INLINE MT_Scalar MT_Point3::distance(const MT_Point3& p) const {
return (p - *this).length(); return (p - *this).length();
} }