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())