Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--intern/iksolver/SConscript6
-rw-r--r--intern/iksolver/intern/IK_QJacobian.cpp100
-rw-r--r--intern/iksolver/intern/IK_QJacobian.h13
-rw-r--r--intern/iksolver/intern/IK_QJacobianSolver.cpp20
-rw-r--r--intern/iksolver/intern/IK_QJacobianSolver.h15
-rw-r--r--intern/iksolver/intern/IK_QSegment.cpp3
-rw-r--r--intern/iksolver/intern/IK_QTask.cpp4
-rw-r--r--intern/iksolver/intern/IK_Solver.cpp3
-rw-r--r--intern/iksolver/intern/TNT/svd.h698
-rw-r--r--intern/iksolver/intern/TNT/tntmath.h7
-rw-r--r--intern/moto/include/MT_Point3.h1
-rw-r--r--intern/moto/include/MT_Point3.inl5
12 files changed, 384 insertions, 491 deletions
diff --git a/intern/iksolver/SConscript b/intern/iksolver/SConscript
index aa96002b5f0..7ef8a9b9d11 100644
--- a/intern/iksolver/SConscript
+++ b/intern/iksolver/SConscript
@@ -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',
diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp
index c9690e68d4c..cfa764454d4 100644
--- a/intern/iksolver/intern/IK_QJacobian.cpp
+++ b/intern/iksolver/intern/IK_QJacobian.cpp
@@ -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];
diff --git a/intern/iksolver/intern/IK_QJacobian.h b/intern/iksolver/intern/IK_QJacobian.h
index b2d990d5489..a16d7a7a941 100644
--- a/intern/iksolver/intern/IK_QJacobian.h
+++ b/intern/iksolver/intern/IK_QJacobian.h
@@ -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;
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp
index 4bd3d28ce69..312b3146619 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.cpp
+++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp
@@ -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 {
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h
index caa47b77539..adf95eb82dc 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.h
+++ b/intern/iksolver/intern/IK_QJacobianSolver.h
@@ -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;
};
diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp
index a380e29e6fa..8d6655b1531 100644
--- a/intern/iksolver/intern/IK_QSegment.cpp
+++ b/intern/iksolver/intern/IK_QSegment.cpp
@@ -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)
diff --git a/intern/iksolver/intern/IK_QTask.cpp b/intern/iksolver/intern/IK_QTask.cpp
index 781a2712885..b0e51aec371 100644
--- a/intern/iksolver/intern/IK_QTask.cpp
+++ b/intern/iksolver/intern/IK_QTask.cpp
@@ -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;
diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp
index a335a946166..b1806a955b5 100644
--- a/intern/iksolver/intern/IK_Solver.cpp
+++ b/intern/iksolver/intern/IK_Solver.cpp
@@ -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);
}
diff --git a/intern/iksolver/intern/TNT/svd.h b/intern/iksolver/intern/TNT/svd.h
index 22cb1d7a088..1b25361811e 100644
--- a/intern/iksolver/intern/TNT/svd.h
+++ b/intern/iksolver/intern/TNT/svd.h
@@ -11,430 +11,414 @@
// ,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;
}
}
}
diff --git a/intern/iksolver/intern/TNT/tntmath.h b/intern/iksolver/intern/TNT/tntmath.h
index 9e79c07fa67..5773900caf9 100644
--- a/intern/iksolver/intern/TNT/tntmath.h
+++ b/intern/iksolver/intern/TNT/tntmath.h
@@ -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)
{
diff --git a/intern/moto/include/MT_Point3.h b/intern/moto/include/MT_Point3.h
index 5e85dc596ab..372718af312 100644
--- a/intern/moto/include/MT_Point3.h
+++ b/intern/moto/include/MT_Point3.h
@@ -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;
diff --git a/intern/moto/include/MT_Point3.inl b/intern/moto/include/MT_Point3.inl
index e6ce4f9d9a3..081a8195694 100644
--- a/intern/moto/include/MT_Point3.inl
+++ b/intern/moto/include/MT_Point3.inl
@@ -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();
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-11 09:37:57 +0300