Pole Target for IK

==================

This adds an extra target to the IK solver constraint to define the
roll of the IK chain.

http://www.blender.org/development/current-projects/changes-since-244/inverse-kinematics/

Also fixes a crashes using ctrl+I to set an IK constraint on a bone
due to the recent constraints refactor.

9 files changed, 207 insertions, 15 deletions

diff --git a/intern/iksolver/extern/IK_solver.h b/intern/iksolver/extern/IK_solver.h
index 8626ca22beb..bf53a9e3724 100644
--- a/intern/iksolver/extern/IK_solver.h
+++ b/intern/iksolver/extern/IK_solver.h
@@ -158,6 +158,8 @@ void IK_FreeSolver(IK_Solver *solver);
 
 void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight);
 void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight);
+void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle);
+float IK_SolverGetPoleAngle(IK_Solver *solver);
 
 int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations);
 
diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp
index 03c5f9500be..1dd4d086aa8 100644
--- a/intern/iksolver/intern/IK_QJacobian.cpp
+++ b/intern/iksolver/intern/IK_QJacobian.cpp
@@ -42,11 +42,10 @@ IK_QJacobian::~IK_QJacobian()
 {
 }
 
-void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks)
+void IK_QJacobian::ArmMatrices(int dof, int task_size)
 {
 	m_dof = dof;
 	m_task_size = task_size;
-	m_tasks = tasks;
 
 	m_jacobian.newsize(task_size, dof);
 	m_jacobian = 0;
diff --git a/intern/iksolver/intern/IK_QJacobian.h b/intern/iksolver/intern/IK_QJacobian.h
index b80db1d8f53..3e20e4a9fd0 100644
--- a/intern/iksolver/intern/IK_QJacobian.h
+++ b/intern/iksolver/intern/IK_QJacobian.h
@@ -49,7 +49,7 @@ public:
 	~IK_QJacobian();
 
 	// Call once to initialize
-	void ArmMatrices(int dof, int task_size, int tasks);
+	void ArmMatrices(int dof, int task_size);
 	void SetDoFWeight(int dof, MT_Scalar weight);
 
 	// Iteratively called
@@ -75,7 +75,7 @@ private:
 	void InvertSDLS();
 	void InvertDLS();
 
-	int m_dof, m_task_size, m_tasks;
+	int m_dof, m_task_size;
 	bool m_transpose;
 
 	// the jacobian matrix and it's null space projector
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp
index ea18f0b2003..f19b2a162fa 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.cpp
+++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp
@@ -32,6 +32,15 @@
 
 #include <stdio.h>
 #include "IK_QJacobianSolver.h"
+#include "MT_Quaternion.h"
+
+//#include "analyze.h"
+IK_QJacobianSolver::IK_QJacobianSolver()
+{
+	m_poleconstraint = false;
+	m_getpoleangle = false;
+	m_rootmatrix.setIdentity();
+}
 
 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
 {
@@ -47,7 +56,7 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
 	m_segments.clear();
 	AddSegmentList(root);
 
-	// assing each segment a unique id for the jacobian
+	// assign each segment a unique id for the jacobian
 	std::vector<IK_QSegment*>::iterator seg;
 	int num_dof = 0;
 
@@ -105,9 +114,9 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
 	}
 
 	// set matrix sizes
-	m_jacobian.ArmMatrices(num_dof, primary_size, primary);
+	m_jacobian.ArmMatrices(num_dof, primary_size);
 	if (secondary > 0)
-		m_jacobian_sub.ArmMatrices(num_dof, secondary_size, secondary);
+		m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
 
 	// set dof weights
 	int i;
@@ -119,6 +128,109 @@ bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
 	return true;
 }
 
+void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, MT_Vector3& goal, MT_Vector3& polegoal, float poleangle, bool getangle)
+{
+	m_poleconstraint = true;
+	m_poletip = tip;
+	m_goal = goal;
+	m_polegoal = polegoal;
+	m_poleangle = (getangle)? 0.0f: poleangle;
+	m_getpoleangle = getangle;
+}
+
+static MT_Scalar safe_acos(MT_Scalar f)
+{
+	// acos that does not return NaN with rounding errors
+	if (f <= -1.0f) return MT_PI;
+	else if (f >= 1.0f) return 0.0;
+	else return acos(f);
+}
+
+static MT_Vector3 normalize(const MT_Vector3& v)
+{
+	// a sane normalize function that doesn't give (1, 0, 0) in case
+	// of a zero length vector, like MT_Vector3.normalize
+	MT_Scalar len = v.length();
+	return MT_fuzzyZero(len)?  MT_Vector3(0, 0, 0): v/len;
+}
+
+static float angle(const MT_Vector3& v1, const MT_Vector3& v2)
+{
+	return safe_acos(v1.dot(v2));
+}
+
+void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask*>& tasks)
+{
+	// this function will be called before and after solving. calling it before
+	// solving gives predictable solutions by rotating towards the solution,
+	// and calling it afterwards ensures the solution is exact.
+
+	if(!m_poleconstraint)
+		return;
+	
+	// disable pole vector constraint in case of multiple position tasks
+	std::list<IK_QTask*>::iterator task;
+	int positiontasks = 0;
+
+	for (task = tasks.begin(); task != tasks.end(); task++)
+		if((*task)->PositionTask())
+			positiontasks++;
+	
+	if (positiontasks >= 2) {
+		m_poleconstraint = false;
+		return;
+	}
+
+	// get positions and rotations
+	root->UpdateTransform(m_rootmatrix);
+
+	const MT_Vector3 rootpos = root->GlobalStart();
+	const MT_Vector3 endpos = m_poletip->GlobalEnd();
+	const MT_Matrix3x3& rootbasis = root->GlobalTransform().getBasis();
+
+	// construct "lookat" matrices (like gluLookAt), based on a direction and
+	// an up vector, with the direction going from the root to the end effector
+	// and the up vector going from the root to the pole constraint position.
+	MT_Vector3 dir = normalize(endpos - rootpos);
+	MT_Vector3 rootx= rootbasis.getColumn(0);
+	MT_Vector3 rootz= rootbasis.getColumn(2);
+	MT_Vector3 up = rootx*cos(m_poleangle) + rootz*sin(m_poleangle);
+
+	// in post, don't rotate towards the goal but only correct the pole up
+	MT_Vector3 poledir = (m_getpoleangle)? dir: normalize(m_goal - rootpos);
+	MT_Vector3 poleup = normalize(m_polegoal - rootpos);
+
+	MT_Matrix3x3 mat, polemat;
+
+	mat[0] = normalize(MT_cross(dir, up));
+	mat[1] = MT_cross(mat[0], dir);
+	mat[2] = -dir;
+
+	polemat[0] = normalize(MT_cross(poledir, poleup));
+	polemat[1] = MT_cross(polemat[0], poledir);
+	polemat[2] = -poledir;
+
+	if(m_getpoleangle) {
+		// we compute the pole angle that to rotate towards the target
+		m_poleangle = angle(mat[1], polemat[1]);
+
+		if(rootz.dot(mat[1]*cos(m_poleangle) + mat[0]*sin(m_poleangle)) > 0.0f)
+			m_poleangle = -m_poleangle;
+
+		// solve again, with the pole angle we just computed
+		m_getpoleangle = false;
+		ConstrainPoleVector(root, tasks);
+	}
+	else {
+		// now we set as root matrix the difference between the current and
+		// desired rotation based on the pole vector constraint. we use
+		// transpose instead of inverse because we have orthogonal matrices
+		// anyway, and in case of a singular matrix we don't get NaN's.
+		MT_Transform trans(MT_Point3(0, 0, 0), polemat.transposed()*mat);
+		m_rootmatrix = trans*m_rootmatrix;
+	}
+}
+
 bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
 {
 	// assing each segment a unique id for the jacobian
@@ -181,15 +293,17 @@ bool IK_QJacobianSolver::Solve(
 	const int max_iterations
 )
 {
+	bool solved = false;
 	//double dt = analyze_time();
 
-	if (!Setup(root, tasks))
-		return false;
+	ConstrainPoleVector(root, tasks);
+
+	root->UpdateTransform(m_rootmatrix);
 
 	// iterate
 	for (int iterations = 0; iterations < max_iterations; iterations++) {
 		// update transform
-		root->UpdateTransform(MT_Transform::Identity());
+		root->UpdateTransform(m_rootmatrix);
 
 		std::list<IK_QTask*>::iterator task;
 
@@ -211,7 +325,7 @@ bool IK_QJacobianSolver::Solve(
 					m_jacobian.SubTask(m_jacobian_sub);
 			}
 			catch (...) {
-				printf("IK Exception\n");
+				fprintf(stderr, "IK Exception\n");
 				return false;
 			}
 
@@ -230,12 +344,19 @@ bool IK_QJacobianSolver::Solve(
 
 		// check for convergence
 		if (norm < 1e-3) {
+			solved = true;
+			break;
 			//analyze_add_run(iterations, analyze_time()-dt);
+
 			return true;
 		}
 	}
 
+	if(m_poleconstraint)
+		root->PrependBasis(m_rootmatrix.getBasis());
+
 	//analyze_add_run(max_iterations, analyze_time()-dt);
-	return false;
+
+	return solved;
 }
 
diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h
index adf95eb82dc..bc3d1686b59 100644
--- a/intern/iksolver/intern/IK_QJacobianSolver.h
+++ b/intern/iksolver/intern/IK_QJacobianSolver.h
@@ -43,6 +43,7 @@
 #include <list>
 
 #include "MT_Vector3.h"
+#include "MT_Transform.h"
 #include "IK_QJacobian.h"
 #include "IK_QSegment.h"
 #include "IK_QTask.h"
@@ -50,11 +51,18 @@
 class IK_QJacobianSolver
 {
 public:
-	IK_QJacobianSolver() {};
+	IK_QJacobianSolver();
 	~IK_QJacobianSolver() {};
 
-	// returns true if converged, false if max number of iterations was used
+	// setup pole vector constraint
+	void SetPoleVectorConstraint(IK_QSegment *tip, MT_Vector3& goal,
+		MT_Vector3& polegoal, float poleangle, bool getangle);
+	float GetPoleAngle() { return m_poleangle; };
+
+	// call setup once before solving, if it fails don't solve
+	bool Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks);
 
+	// returns true if converged, false if max number of iterations was used
 	bool Solve(
 		IK_QSegment *root,
 		std::list<IK_QTask*> tasks,
@@ -64,8 +72,8 @@ public:
 
 private:
 	void AddSegmentList(IK_QSegment *seg);
-	bool Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks);
 	bool UpdateAngles(MT_Scalar& norm);
+	void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask*>& tasks);
 
 private:
 
@@ -75,6 +83,15 @@ private:
 	bool m_secondary_enabled;
 
 	std::vector<IK_QSegment*> m_segments;
+
+	MT_Transform m_rootmatrix;
+
+	bool m_poleconstraint;
+	bool m_getpoleangle;
+	MT_Vector3 m_goal;
+	MT_Vector3 m_polegoal;
+	float m_poleangle;
+	IK_QSegment *m_poletip;
 };
 
 #endif
diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp
index cf9e1615d8c..1855c1e3db3 100644
--- a/intern/iksolver/intern/IK_QSegment.cpp
+++ b/intern/iksolver/intern/IK_QSegment.cpp
@@ -236,6 +236,18 @@ IK_QSegment::IK_QSegment(int num_DoF, bool translational)
 	m_orig_translation = m_translation;
 }
 
+void IK_QSegment::Reset()
+{
+	m_locked[0] = m_locked[1] = m_locked[2] = false;
+
+	m_basis = m_orig_basis;
+	m_translation = m_orig_translation;
+	SetBasis(m_basis);
+
+	for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling)
+		seg->Reset();
+}
+
 void IK_QSegment::SetTransform(
 	const MT_Vector3& start,
 	const MT_Matrix3x3& rest_basis,
@@ -326,6 +338,11 @@ void IK_QSegment::UpdateTransform(const MT_Transform& global)
 		seg->UpdateTransform(m_global_transform);
 }
 
+void IK_QSegment::PrependBasis(const MT_Matrix3x3& mat)
+{
+	m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
+}
+
 // IK_QSphericalSegment
 
 IK_QSphericalSegment::IK_QSphericalSegment()
diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h
index e406585bc8b..ca0abafb06a 100644
--- a/intern/iksolver/intern/IK_QSegment.h
+++ b/intern/iksolver/intern/IK_QSegment.h
@@ -165,6 +165,10 @@ public:
 
 	virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; }
 
+	// functions needed for pole vector constraint
+	void PrependBasis(const MT_Matrix3x3& mat);
+	void Reset();
+
 protected:
 
 	// num_DoF: number of degrees of freedom
diff --git a/intern/iksolver/intern/IK_QTask.h b/intern/iksolver/intern/IK_QTask.h
index 0e00925d908..26beaa38622 100644
--- a/intern/iksolver/intern/IK_QTask.h
+++ b/intern/iksolver/intern/IK_QTask.h
@@ -75,6 +75,8 @@ public:
 
 	virtual MT_Scalar Distance() const=0;
 
+	virtual bool PositionTask() const { return false; }
+
 protected:
 	int m_id;
 	int m_size;
@@ -97,6 +99,8 @@ public:
 
 	MT_Scalar Distance() const;
 
+	bool PositionTask() const { return true; }
+
 private:
 	MT_Vector3 m_goal;
 	MT_Scalar m_clamp_length;
diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp
index 919eeb739ce..b2d75d783c6 100644
--- a/intern/iksolver/intern/IK_Solver.cpp
+++ b/intern/iksolver/intern/IK_Solver.cpp
@@ -318,6 +318,31 @@ void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[
 	qsolver->tasks.push_back(orient);
 }
 
+void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle)
+{
+	if (solver == NULL || tip == NULL)
+		return;
+
+	IK_QSolver *qsolver = (IK_QSolver*)solver;
+	IK_QSegment *qtip = (IK_QSegment*)tip;
+
+	MT_Vector3 qgoal(goal);
+	MT_Vector3 qpolegoal(polegoal);
+
+	qsolver->solver.SetPoleVectorConstraint(
+		qtip, qgoal, qpolegoal, poleangle, getangle);
+}
+
+float IK_SolverGetPoleAngle(IK_Solver *solver)
+{
+	if (solver == NULL)
+		return 0.0f;
+
+	IK_QSolver *qsolver = (IK_QSolver*)solver;
+
+	return qsolver->solver.GetPoleAngle();
+}
+
 void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
 {
 	if (solver == NULL || root == NULL)
@@ -346,6 +371,9 @@ int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
 	std::list<IK_QTask*>& tasks = qsolver->tasks;
 	MT_Scalar tol = tolerance;
 
+	if(!jacobian.Setup(root, tasks))
+		return 0;
+
 	bool result = jacobian.Solve(root, tasks, tol, max_iterations);
 
 	return ((result)? 1: 0);