IK

==

Solving is now done independent of scale, by scaling the chain to have a
size of about 1.0. This solves some issues with small or big chains, and
also makes the IK stretch setting independent of scale. The latter breaks
backwards compatibility somewhat, but is an improvement over what it did
before.

6 files changed, 75 insertions, 7 deletions

diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp
index f19b2a162fa..17750a7195f 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.cpp
+++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp
@@ -42,6 +42,36 @@ IK_QJacobianSolver::IK_QJacobianSolver()
 	m_rootmatrix.setIdentity();
 }
 
+MT_Scalar IK_QJacobianSolver::ComputeScale()
+{
+	std::vector<IK_QSegment*>::iterator seg;
+	float length = 0.0f;
+
+	for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
+		length += (*seg)->MaxExtension();
+	
+	if(length == 0.0f)
+		return 1.0f;
+	else
+		return 1.0f/length;
+}
+
+void IK_QJacobianSolver::Scale(float scale, std::list<IK_QTask*>& tasks)
+{
+	std::list<IK_QTask*>::iterator task;
+	std::vector<IK_QSegment*>::iterator seg;
+
+	for (task = tasks.begin(); task != tasks.end(); task++)
+		(*task)->Scale(scale);
+
+	for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
+		(*seg)->Scale(scale);
+	
+	m_rootmatrix.getOrigin() *= scale;
+	m_goal *= scale;
+	m_polegoal *= scale;
+}
+
 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
 {
 	m_segments.push_back(seg);
@@ -293,9 +323,12 @@ bool IK_QJacobianSolver::Solve(
 	const int max_iterations
 )
 {
+	float scale = ComputeScale();
 	bool solved = false;
 	//double dt = analyze_time();
 
+	Scale(scale, tasks);
+
 	ConstrainPoleVector(root, tasks);
 
 	root->UpdateTransform(m_rootmatrix);
@@ -346,15 +379,14 @@ bool IK_QJacobianSolver::Solve(
 		if (norm < 1e-3) {
 			solved = true;
 			break;
-			//analyze_add_run(iterations, analyze_time()-dt);
-
-			return true;
 		}
 	}
 
 	if(m_poleconstraint)
 		root->PrependBasis(m_rootmatrix.getBasis());
 
+	Scale(1.0f/scale, tasks);
+
 	//analyze_add_run(max_iterations, analyze_time()-dt);
 
 	return solved;
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h
index bc3d1686b59..9ad46cd0aa6 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.h
+++ b/intern/iksolver/intern/IK_QJacobianSolver.h
@@ -75,6 +75,9 @@ private:
 	bool UpdateAngles(MT_Scalar& norm);
 	void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask*>& tasks);
 
+	MT_Scalar ComputeScale();
+	void Scale(float scale, std::list<IK_QTask*>& tasks);
+
 private:
 
 	IK_QJacobian m_jacobian;
diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp
index 1855c1e3db3..a5217ed91d6 100644
--- a/intern/iksolver/intern/IK_QSegment.cpp
+++ b/intern/iksolver/intern/IK_QSegment.cpp
@@ -343,6 +343,16 @@ void IK_QSegment::PrependBasis(const MT_Matrix3x3& mat)
 	m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
 }
 
+void IK_QSegment::Scale(float scale)
+{
+	m_start *= scale;
+	m_translation *= scale;
+	m_orig_translation *= scale;
+	m_global_start *= scale;
+	m_global_transform.getOrigin() *= scale;
+	m_max_extension *= scale;
+}
+
 // IK_QSphericalSegment
 
 IK_QSphericalSegment::IK_QSphericalSegment()
@@ -1026,3 +1036,17 @@ void IK_QTranslateSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
 	m_limit[axis]= true;
 }
 
+void IK_QTranslateSegment::Scale(float scale)
+{
+	int i;
+
+	IK_QSegment::Scale(scale);
+
+	for (i = 0; i < 3; i++) {
+		m_min[0] *= scale;
+		m_max[1] *= scale;
+	}
+
+	m_new_translation *= scale;
+}
+
diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h
index ca0abafb06a..3c5df463468 100644
--- a/intern/iksolver/intern/IK_QSegment.h
+++ b/intern/iksolver/intern/IK_QSegment.h
@@ -169,6 +169,9 @@ public:
 	void PrependBasis(const MT_Matrix3x3& mat);
 	void Reset();
 
+	// scale
+	virtual void Scale(float scale);
+
 protected:
 
 	// num_DoF: number of degrees of freedom
@@ -335,6 +338,8 @@ public:
 	void SetWeight(int axis, MT_Scalar weight);
 	void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
 
+	void Scale(float scale);
+
 private:
 	int m_axis[3];
 	bool m_axis_enabled[3], m_limit[3];
diff --git a/intern/iksolver/intern/IK_QTask.h b/intern/iksolver/intern/IK_QTask.h
index 26beaa38622..f2fd34119a1 100644
--- a/intern/iksolver/intern/IK_QTask.h
+++ b/intern/iksolver/intern/IK_QTask.h
@@ -77,6 +77,8 @@ public:
 
 	virtual bool PositionTask() const { return false; }
 
+	virtual void Scale(float scale) {}
+
 protected:
 	int m_id;
 	int m_size;
@@ -100,6 +102,7 @@ public:
 	MT_Scalar Distance() const;
 
 	bool PositionTask() const { return true; }
+	void Scale(float scale) { m_goal *= scale; m_clamp_length *= scale; }
 
 private:
 	MT_Vector3 m_goal;
@@ -137,6 +140,8 @@ public:
 
 	MT_Scalar Distance() const;
 
+	void Scale(float scale) { m_goal_center *= scale; m_distance *= scale; }
+
 private:
 	MT_Scalar ComputeTotalMass(const IK_QSegment *segment);
 	MT_Vector3 ComputeCenter(const IK_QSegment *segment);
diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp
index b2d75d783c6..140c35c8c46 100644
--- a/intern/iksolver/intern/IK_Solver.cpp
+++ b/intern/iksolver/intern/IK_Solver.cpp
@@ -41,7 +41,7 @@ using namespace std;
 
 class IK_QSolver {
 public:
-	IK_QSolver() {};
+	IK_QSolver() : root(NULL) {};
 
 	IK_QJacobianSolver solver;
 	IK_QSegment *root;
@@ -197,13 +197,12 @@ void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
 	if (stiffness < 0.0)
 		return;
 	
-	if (stiffness > 0.99)
-		stiffness = 0.99;
+	if (stiffness > 0.999)
+		stiffness = 0.999;
 
 	IK_QSegment *qseg = (IK_QSegment*)seg;
 	MT_Scalar weight = 1.0-stiffness;
 
-
 	if (axis >= IK_TRANS_X) {
 		if(!qseg->Translational())
 			if(qseg->Composite() && qseg->Composite()->Translational())