[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [12378] trunk/blender:
Brecht Van Lommel
brechtvanlommel at pandora.be
Wed Oct 24 16:58:31 CEST 2007
Revision: 12378
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=12378
Author: blendix
Date: 2007-10-24 16:58:31 +0200 (Wed, 24 Oct 2007)
Log Message:
-----------
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.
Modified Paths:
--------------
trunk/blender/intern/iksolver/extern/IK_solver.h
trunk/blender/intern/iksolver/intern/IK_QJacobian.cpp
trunk/blender/intern/iksolver/intern/IK_QJacobian.h
trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.cpp
trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.h
trunk/blender/intern/iksolver/intern/IK_QSegment.cpp
trunk/blender/intern/iksolver/intern/IK_QSegment.h
trunk/blender/intern/iksolver/intern/IK_QTask.h
trunk/blender/intern/iksolver/intern/IK_Solver.cpp
trunk/blender/source/blender/blenkernel/BKE_constraint.h
trunk/blender/source/blender/blenkernel/intern/armature.c
trunk/blender/source/blender/blenkernel/intern/constraint.c
trunk/blender/source/blender/blenloader/intern/readfile.c
trunk/blender/source/blender/makesdna/DNA_constraint_types.h
trunk/blender/source/blender/src/buttons_object.c
trunk/blender/source/blender/src/editconstraint.c
trunk/blender/source/blender/src/editobject.c
Modified: trunk/blender/intern/iksolver/extern/IK_solver.h
===================================================================
--- trunk/blender/intern/iksolver/extern/IK_solver.h 2007-10-24 13:12:07 UTC (rev 12377)
+++ trunk/blender/intern/iksolver/extern/IK_solver.h 2007-10-24 14:58:31 UTC (rev 12378)
@@ -158,6 +158,8 @@
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);
Modified: trunk/blender/intern/iksolver/intern/IK_QJacobian.cpp
===================================================================
--- trunk/blender/intern/iksolver/intern/IK_QJacobian.cpp 2007-10-24 13:12:07 UTC (rev 12377)
+++ trunk/blender/intern/iksolver/intern/IK_QJacobian.cpp 2007-10-24 14:58:31 UTC (rev 12378)
@@ -42,11 +42,10 @@
{
}
-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;
Modified: trunk/blender/intern/iksolver/intern/IK_QJacobian.h
===================================================================
--- trunk/blender/intern/iksolver/intern/IK_QJacobian.h 2007-10-24 13:12:07 UTC (rev 12377)
+++ trunk/blender/intern/iksolver/intern/IK_QJacobian.h 2007-10-24 14:58:31 UTC (rev 12378)
@@ -49,7 +49,7 @@
~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 @@
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
Modified: trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.cpp
===================================================================
--- trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.cpp 2007-10-24 13:12:07 UTC (rev 12377)
+++ trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.cpp 2007-10-24 14:58:31 UTC (rev 12378)
@@ -32,7 +32,16 @@
#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)
{
m_segments.push_back(seg);
@@ -47,7 +56,7 @@
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 @@
}
// 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 @@
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 @@
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 @@
m_jacobian.SubTask(m_jacobian_sub);
}
catch (...) {
- printf("IK Exception\n");
+ fprintf(stderr, "IK Exception\n");
return false;
}
@@ -230,12 +344,19 @@
// 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;
}
Modified: trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.h
===================================================================
--- trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.h 2007-10-24 13:12:07 UTC (rev 12377)
+++ trunk/blender/intern/iksolver/intern/IK_QJacobianSolver.h 2007-10-24 14:58:31 UTC (rev 12378)
@@ -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() {};
+ // 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 @@
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 @@
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
Modified: trunk/blender/intern/iksolver/intern/IK_QSegment.cpp
===================================================================
--- trunk/blender/intern/iksolver/intern/IK_QSegment.cpp 2007-10-24 13:12:07 UTC (rev 12377)
+++ trunk/blender/intern/iksolver/intern/IK_QSegment.cpp 2007-10-24 14:58:31 UTC (rev 12378)
@@ -236,6 +236,18 @@
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 @@
seg->UpdateTransform(m_global_transform);
}
+void IK_QSegment::PrependBasis(const MT_Matrix3x3& mat)
+{
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list