diff options
author | Brecht Van Lommel <brechtvanlommel@pandora.be> | 2007-10-24 18:58:31 +0400 |
---|---|---|
committer | Brecht Van Lommel <brechtvanlommel@pandora.be> | 2007-10-24 18:58:31 +0400 |
commit | 30be716fc8e0ada286a94a53bf64dc5d16402c24 (patch) | |
tree | 6d5bbd71a1eea495afabfe7b93e8556b50dbab58 /intern/iksolver | |
parent | 79224961836db454b454f20479a76b83e3eed3bc (diff) |
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.
Diffstat (limited to 'intern/iksolver')
-rw-r--r-- | intern/iksolver/extern/IK_solver.h | 2 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobian.cpp | 3 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobian.h | 4 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobianSolver.cpp | 137 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QJacobianSolver.h | 23 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QSegment.cpp | 17 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QSegment.h | 4 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_QTask.h | 4 | ||||
-rw-r--r-- | intern/iksolver/intern/IK_Solver.cpp | 28 |
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); |