kannonier8/blender
1
0
Fork 0
forked from bartvdbraak/blender
Code Pull Requests Activity

First commit of Brecht's new IK work. This only does the IK module.

Browse Source 
Don't start compiling yet!
This commit is contained in:
Ton Roosendaal 2005-08-27 12:44:41 +00:00
parent 669c4dbecb
commit 665a6b2d95
11 changed files with 1674 additions and 1787 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

264
intern/iksolver/intern/IK_QChain.cpp
View File

@ -1,264 +0,0 @@
/**
* $Id$
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under
* the Blender License. See http://www.blender.org/BL/ for information
* about this.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
/**
* $Id$
* Copyright (C) 2001 NaN Technologies B.V.
*
* @author Laurence
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include "IK_QChain.h"
using namespace std;
IK_QChain::
IK_QChain(
)
{
// nothing to do;
};
const
vector<IK_QSegment> &
IK_QChain::
Segments(
) const {
return m_segments;
};
vector<IK_QSegment> &
IK_QChain::
Segments(
){
return m_segments;
};
void
IK_QChain::
UpdateGlobalTransformations(
){
// now iterate through the segment list
// compute their local transformations if needed
// assign their global transformations
// (relative to chain origin)
vector<IK_QSegment>::const_iterator s_end = m_segments.end();
vector<IK_QSegment>::iterator s_it = m_segments.begin();
MT_Transform global;
global.setIdentity();
for (; s_it != s_end; ++s_it) {
global = s_it->UpdateGlobal(global);
}
// we also need to compute the accumulated local transforms
// for each segment
MT_Transform acc_local;
acc_local.setIdentity();
vector<IK_QSegment>::reverse_iterator s_rit = m_segments.rbegin();
vector<IK_QSegment>::reverse_iterator s_rend = m_segments.rend();
for (; s_rit != s_rend; ++s_rit) {
acc_local = s_rit->UpdateAccumulatedLocal(acc_local);
}
// compute the position of the end effector and it's pose
const MT_Transform &last_t = m_segments.back().GlobalTransform();
m_end_effector = last_t.getOrigin();
#if 0
// The end pose is not currently used.
MT_Matrix3x3 last_basis = last_t.getBasis();
last_basis.transpose();
MT_Vector3 m_end_pose = last_basis[1];
#endif
};
const
TNT::Matrix<MT_Scalar> &
IK_QChain::
Jacobian(
) const {
return m_jacobian;
} ;
const
TNT::Matrix<MT_Scalar> &
IK_QChain::
TransposedJacobian(
) const {
return m_t_jacobian;
};
void
IK_QChain::
ComputeJacobian(
){
// let's assume that the chain's global transfomations
// have already been computed.
int dof = DoF();
int num_segs = m_segments.size();
vector<IK_QSegment>::const_iterator segs = m_segments.begin();
m_t_jacobian.newsize(dof,3);
m_jacobian.newsize(3,dof);
// compute the transposed jacobian first
int n;
for (n= 0; n < num_segs; n++) {
#if 0
// For euler angle computation we can use a slightly quicker method.
const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis();
const MT_Vector3 &origin = segs[n].GlobalSegmentStart();
const MT_Vector3 p = origin-m_end_effector;
const MT_Vector3 x_axis(1,0,0);
const MT_Vector3 y_axis(0,1,0);
const MT_Vector3 z_axis(0,0,1);
MT_Vector3 a = basis * x_axis;
MT_Vector3 pca = p.cross(a);
m_t_jacobian(n*3 + 1,1) = pca.x();
m_t_jacobian(n*3 + 1,2) = pca.y();
m_t_jacobian(n*3 + 1,3) = pca.z();
a = basis * y_axis;
pca = p.cross(a);
m_t_jacobian(n*3 + 2,1) = pca.x();
m_t_jacobian(n*3 + 2,2) = pca.y();
m_t_jacobian(n*3 + 2,3) = pca.z();
a = basis * z_axis;
pca = p.cross(a);
m_t_jacobian(n*3 + 3,1) = pca.x();
m_t_jacobian(n*3 + 3,2) = pca.y();
m_t_jacobian(n*3 + 3,3) = pca.z();
#else
// user slower general jacobian computation method
MT_Vector3 j1 = segs[n].ComputeJacobianColumn(0);
m_t_jacobian(n*3 + 1,1) = j1.x();
m_t_jacobian(n*3 + 1,2) = j1.y();
m_t_jacobian(n*3 + 1,3) = j1.z();
j1 = segs[n].ComputeJacobianColumn(1);
m_t_jacobian(n*3 + 2,1) = j1.x();
m_t_jacobian(n*3 + 2,2) = j1.y();
m_t_jacobian(n*3 + 2,3) = j1.z();
j1 = segs[n].ComputeJacobianColumn(2);
m_t_jacobian(n*3 + 3,1) = j1.x();
m_t_jacobian(n*3 + 3,2) = j1.y();
m_t_jacobian(n*3 + 3,3) = j1.z();
#endif
}
// get the origina1 jacobain
TNT::transpose(m_t_jacobian,m_jacobian);
};
MT_Vector3
IK_QChain::
EndEffector(
) const {
return m_end_effector;
};
MT_Vector3
IK_QChain::
EndPose(
) const {
return m_end_pose;
};
int
IK_QChain::
DoF(
) const {
return 3 * m_segments.size();
}
const
MT_Scalar
IK_QChain::
MaxExtension(
) const {
vector<IK_QSegment>::const_iterator s_end = m_segments.end();
vector<IK_QSegment>::const_iterator s_it = m_segments.begin();
if (m_segments.size() == 0) return MT_Scalar(0);
MT_Scalar output = s_it->Length();
++s_it ;
for (; s_it != s_end; ++s_it) {
output += s_it->MaxExtension();
}
return output;
}

211
intern/iksolver/intern/IK_QChain.h
View File

@ -1,211 +0,0 @@
/**
* $Id$
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under
* the Blender License. See http://www.blender.org/BL/ for information
* about this.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
/**
* $Id$
* Copyright (C) 2001 NaN Technologies B.V.
*
* @author Laurence
*/
#ifndef NAN_INCLUDED_IK_QChain_h
#define NAN_INCLUDED_IK_QChain_h
#include "IK_QSegment.h"
#include <vector>
#include "MT_Scalar.h"
#include "TNT/cmat.h"
/**
* This class is a collection of ordered segments that are used
* in an Inverse Kinematic solving routine. An IK solver operating
* on the chain, will in general manipulate all the segments of the
* chain in order to solve the IK problem.
*
* To build a chain use the default constructor. Once built it's
* then possible to add IK_Segments to the chain by inserting
* them into the vector of IK_Segments. Note that segments will be
* copied into the chain so chains cannot share instances of
* IK_Segments.
*
* You have full control of which segments form the chain via the
* the std::vector routines.
*/
class IK_QChain{
public :
/**
* Construct a IK_QChain with no segments.
*/
IK_QChain(
);
// IK_QChains also have the default copy constructors
// available.
/**
* Const access to the array of segments
* comprising the IK_QChain. Used for rendering
* etc
* @return a const reference to a vector of segments
*/
const
std::vector<IK_QSegment> &
Segments(
) const ;
/**
* Full access to segments used to initialize
* the IK_QChain and manipulate the segments.
* Use the push_back() method of std::vector to add
* segments in order to the chain
* @return a reference to a vector of segments
*/
std::vector<IK_QSegment> &
Segments(
);
/**
* Force the IK_QChain to recompute all the local
* segment transformations and composite them
* to calculate the global transformation for
* each segment. Must be called before
* ComputeJacobian()
*/
void
UpdateGlobalTransformations(
);
/**
* Return the global position of the end
* of the last segment.
*/
MT_Vector3
EndEffector(
) const;
/**
* Return the global pose of the end
* of the last segment.
*/
MT_Vector3
EndPose(
) const;
/**
* Calculate the jacobian matrix for
* the current end effector position.
* A jacobian is the set of column vectors
* of partial derivatives for each active angle.
* This method also computes the transposed jacobian.
* @pre You must have updated the global transformations
* of the chain's segments before a call to this method. Do this
* with UpdateGlobalTransformation()
*/
void
ComputeJacobian(
);
/**
* @return A const reference to the last computed jacobian matrix
*/
const
TNT::Matrix<MT_Scalar> &
Jacobian(
) const ;
/**
* @return A const reference to the last computed transposed jacobian matrix
*/
const
TNT::Matrix<MT_Scalar> &
TransposedJacobian(
) const ;
/**
* Count the degrees of freedom in the IK_QChain
* @warning store this value rather than using this function
* as the termination value of a for loop etc.
*/
int
DoF(
) const;
/**
* Compute the maximum extension of the chain from the
* root segment. This is the length of the root segments
* + the max extensions of all the other segments
*/
const
MT_Scalar
MaxExtension(
) const;
private :
/// The vector of segments comprising the chain
std::vector<IK_QSegment> m_segments;
/// The jacobain of the IK_QChain
TNT::Matrix<MT_Scalar> m_jacobian;
/// It's transpose
TNT::Matrix<MT_Scalar> m_t_jacobian;
MT_Vector3 m_end_effector;
MT_Vector3 m_end_pose;
};
#endif

464
intern/iksolver/intern/IK_QJacobianSolver.cpp
View File

@ -24,308 +24,224 @@
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
* Original Author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include "IK_QJacobianSolver.h"
#include "TNT/svd.h"
//#include "analyze.h"
#include <iostream>
using namespace std;
IK_QJacobianSolver *
IK_QJacobianSolver::
New(
){
return new IK_QJacobianSolver();
void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
{
m_segments.push_back(seg);
IK_QSegment *child;
for (child = seg->Child(); child; child = child->Sibling())
AddSegmentList(child);
}
bool
IK_QJacobianSolver::
Solve(
IK_QChain &chain,
const MT_Vector3 &g_position,
const MT_Vector3 &g_pose,
const MT_Scalar tolerance,
const int max_iterations,
const MT_Scalar max_angle_change
){
bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
{
m_segments.clear();
AddSegmentList(root);
const vector<IK_QSegment> & segs = chain.Segments();
if (segs.size() == 0) return false;
// assing each segment a unique id for the jacobian
std::vector<IK_QSegment*>::iterator seg;
int num_dof = 0;
// compute the goal direction from the base of the chain
// in global coordinates
MT_Vector3 goal_dir = g_position - segs[0].GlobalSegmentStart();
const MT_Scalar chain_max_extension = chain.MaxExtension();
bool do_parallel_check(false);
if (chain_max_extension < goal_dir.length()) {
do_parallel_check = true;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->SetDoFId(num_dof);
num_dof += (*seg)->NumberOfDoF();
}
goal_dir.normalize();
// compute task id's and assing weights to task
int primary_size = 0, primary = 0;
int secondary_size = 0, secondary = 0;
MT_Scalar primary_weight = 0.0, secondary_weight = 0.0;
std::list<IK_QTask*>::iterator task;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
ArmMatrices(chain.DoF());
if (qtask->Primary()) {
qtask->SetId(primary_size);
primary_size += qtask->Size();
primary_weight += qtask->Weight();
primary++;
}
else {
qtask->SetId(secondary_size);
secondary_size += qtask->Size();
secondary_weight += qtask->Weight();
secondary++;
}
}
for (int iterations = 0; iterations < max_iterations; iterations++) {
if (primary_size == 0 || MT_fuzzyZero(primary_weight))
return false;
// check to see if the chain is pointing in the right direction
m_secondary_enabled = (secondary > 0);
if (iterations%32 && do_parallel_check && ParallelCheck(chain,goal_dir)) {
// rescale weights of tasks to sum up to 1
MT_Scalar primary_rescale = 1.0/primary_weight;
MT_Scalar secondary_rescale;
if (MT_fuzzyZero(secondary_weight))
secondary_rescale = 0.0;
else
secondary_rescale = 1.0/secondary_weight;
return false;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
if (qtask->Primary())
qtask->SetWeight(qtask->Weight()*primary_rescale);
else
qtask->SetWeight(qtask->Weight()*secondary_rescale);
}
// set matrix sizes
m_jacobian.ArmMatrices(num_dof, primary_size, primary);
if (secondary > 0)
m_jacobian_sub.ArmMatrices(num_dof, secondary_size, secondary);
// set dof weights
int i;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
for (i = 0; i < (*seg)->NumberOfDoF(); i++)
m_jacobian.SetDoFWeight((*seg)->DoFId()+i, (*seg)->Weight(i));
return true;
}
bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
{
// assing each segment a unique id for the jacobian
std::vector<IK_QSegment*>::iterator seg;
IK_QSegment *qseg, *minseg = NULL;
MT_Scalar minabsdelta = 1e10, absdelta;
MT_Vector3 delta, mindelta;
bool locked = false, clamp[3];
int i, mindof = 0;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
qseg = *seg;
if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
for (i = 0; i < qseg->NumberOfDoF(); i++) {
if (clamp[i] && !qseg->Locked(i)) {
absdelta = MT_abs(delta[i]);
if (absdelta < MT_EPSILON) {
qseg->Lock(i, m_jacobian, delta);
locked = true;
}
else if (absdelta < minabsdelta) {
minabsdelta = absdelta;
mindelta = delta;
minseg = qseg;
mindof = i;
}
}
}
}
}
if (minseg) {
minseg->Lock(mindof, m_jacobian, mindelta);
locked = true;
if (minabsdelta > norm)
norm = minabsdelta;
}
if (locked == false)
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->UnLock();
(*seg)->UpdateAngleApply();
}
MT_Vector3 end_effector = chain.EndEffector();
MT_Vector3 d_pos = g_position - end_effector;
const MT_Scalar x_length = d_pos.length();
return locked;
}
if (x_length < tolerance) {
bool IK_QJacobianSolver::Solve(
IK_QSegment *root,
std::list<IK_QTask*> tasks,
MT_Scalar,
int max_iterations
)
{
//double dt = analyze_time();
if (!Setup(root, tasks))
return false;
// iterate
for (int iterations = 0; iterations < max_iterations; iterations++) {
// update transform
root->UpdateTransform(MT_Transform::Identity());
std::list<IK_QTask*>::iterator task;
//bool done = true;
// compute jacobian
for (task = tasks.begin(); task != tasks.end(); task++) {
if ((*task)->Primary())
(*task)->ComputeJacobian(m_jacobian);
else
(*task)->ComputeJacobian(m_jacobian_sub);
//printf("#> %f\n", (*task)->Distance());
//if ((*task)->Distance() > 1e-4)
// done = false;
}
/*if (done) {
//analyze_add_run(iterations, analyze_time()-dt);
return true;
}*/
MT_Scalar norm = 0.0;
do {
// invert jacobian
try {
m_jacobian.Invert();
if (m_secondary_enabled)
m_jacobian.SubTask(m_jacobian_sub);
}
catch (...) {
printf("IK Exception\n");
return false;
}
// update angles and check limits
} while (UpdateAngles(norm));
// unlock segments again after locking in clamping loop
std::vector<IK_QSegment*>::iterator seg;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
(*seg)->UnLock();
// compute angle update norm
MT_Scalar maxnorm = m_jacobian.AngleUpdateNorm();
if (maxnorm > norm)
norm = maxnorm;
// check for convergence
if (norm < 1e-3) {
//analyze_add_run(iterations, analyze_time()-dt);
return true;
}
chain.ComputeJacobian();
try {
ComputeInverseJacobian(chain,x_length,max_angle_change);
}
catch(...) {
return false;
}
ComputeBetas(chain,d_pos);
UpdateChain(chain);
chain.UpdateGlobalTransformations();
}
//analyze_add_run(max_iterations, analyze_time()-dt);
return false;
};
IK_QJacobianSolver::
~IK_QJacobianSolver(
){
// nothing to do
}
IK_QJacobianSolver::
IK_QJacobianSolver(
){
// nothing to do
};
void
IK_QJacobianSolver::
ComputeBetas(
IK_QChain &chain,
const MT_Vector3 d_pos
){
m_beta = 0;
m_beta[0] = d_pos.x();
m_beta[1] = d_pos.y();
m_beta[2] = d_pos.z();
TNT::matmult(m_d_theta,m_svd_inverse,m_beta);
};
int
IK_QJacobianSolver::
ComputeInverseJacobian(
IK_QChain &chain,
const MT_Scalar x_length,
const MT_Scalar max_angle_change
) {
int dimension = 0;
m_svd_u = MT_Scalar(0);
// copy first 3 rows of jacobian into m_svd_u
int row, column;
for (row = 0; row < 3; row ++) {
for (column = 0; column < chain.Jacobian().num_cols(); column ++) {
m_svd_u[row][column] = chain.Jacobian()[row][column];
}
}
m_svd_w = MT_Scalar(0);
m_svd_v = MT_Scalar(0);
m_svd_work_space = MT_Scalar(0);
TNT::SVD(m_svd_u,m_svd_w,m_svd_v,m_svd_work_space);
// invert the SVD and compute inverse
TNT::transpose(m_svd_v,m_svd_v_t);
TNT::transpose(m_svd_u,m_svd_u_t);
// Compute damped least squares inverse of pseudo inverse
// Compute damping term lambda
// Note when lambda is zero this is equivalent to the
// least squares solution. This is fine when the m_jjt is
// of full rank. When m_jjt is near to singular the least squares
// inverse tries to minimize |J(dtheta) - dX)| and doesn't
// try to minimize dTheta. This results in eratic changes in angle.
// Damped least squares minimizes |dtheta| to try and reduce this
// erratic behaviour.
// We don't want to use the damped solution everywhere so we
// only increase lamda from zero as we approach a singularity.
// find the smallest non-zero m_svd_w value
int i = 0;
MT_Scalar w_min = MT_INFINITY;
// anything below epsilon is treated as zero
MT_Scalar epsilon = MT_Scalar(1e-10);
for ( i = 0; i <m_svd_w.size() ; i++) {
if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) {
w_min = m_svd_w[i];
}
}
MT_Scalar lambda(0);
MT_Scalar d = x_length/max_angle_change;
if (w_min <= d/2) {
lambda = d/2;
} else
if (w_min < d) {
lambda = sqrt(w_min*(d - w_min));
} else {
lambda = MT_Scalar(0);
}
lambda *= lambda;
for (i= 0; i < m_svd_w.size(); i++) {
if (m_svd_w[i] < epsilon) {
m_svd_w[i] = MT_Scalar(0);
} else {
m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
}
}
TNT::matmultdiag(m_svd_temp1,m_svd_w,m_svd_u_t);
TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1);
return dimension;
}
void
IK_QJacobianSolver::
UpdateChain(
IK_QChain &chain
){
// iterate through the set of angles and
// update their values from the d_thetas
vector<IK_QSegment> &segs = chain.Segments();
unsigned int seg_ind = 0;
for (seg_ind = 0;seg_ind < segs.size(); seg_ind++) {
MT_Vector3 dq;
dq[0] = m_d_theta[3*seg_ind];
dq[1] = m_d_theta[3*seg_ind + 1];
dq[2] = m_d_theta[3*seg_ind + 2];
segs[seg_ind].IncrementAngle(dq);
}
};
void
IK_QJacobianSolver::
ArmMatrices(
int dof
){
m_beta.newsize(dof);
m_d_theta.newsize(dof);
m_svd_u.newsize(dof,dof);
m_svd_v.newsize(dof,dof);
m_svd_w.newsize(dof);
m_svd_work_space.newsize(dof);
m_svd_u = MT_Scalar(0);
m_svd_v = MT_Scalar(0);
m_svd_w = MT_Scalar(0);
m_svd_u_t.newsize(dof,dof);
m_svd_v_t.newsize(dof,dof);
m_svd_w_diag.newsize(dof,dof);
m_svd_inverse.newsize(dof,dof);
m_svd_temp1.newsize(dof,dof);
};
bool
IK_QJacobianSolver::
ParallelCheck(
const IK_QChain &chain,
const MT_Vector3 goal_dir
) const {
// compute the start of the chain in global coordinates
const vector<IK_QSegment> &segs = chain.Segments();
int num_segs = segs.size();
if (num_segs == 0) {
return false;
}
MT_Scalar crossp_sum = 0;
int i;
for (i = 0; i < num_segs; i++) {
MT_Vector3 global_seg_direction = segs[i].GlobalSegmentEnd() -
segs[i].GlobalSegmentStart();
global_seg_direction.normalize();
MT_Scalar crossp = (global_seg_direction.cross(goal_dir)).length();
crossp_sum += MT_Scalar(fabs(crossp));
}
if (crossp_sum < MT_Scalar(0.01)) {
return true;
} else {
return false;
}
}

147
intern/iksolver/intern/IK_QJacobianSolver.h
View File

@ -24,7 +24,8 @@
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
* Original Author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
@ -38,141 +39,49 @@
* @date 28/6/2001
*/
#include "TNT/cmat.h"
#include <vector>
#include <list>
#include "MT_Vector3.h"
#include "IK_QChain.h"
#include "IK_QJacobian.h"
#include "IK_QSegment.h"
#include "IK_QTask.h"
class IK_QJacobianSolver {
public :
/**
* Create a new IK_QJacobianSolver on the heap
* @return A newly created IK_QJacobianSolver you take ownership of the object
* and responsibility for deleting it
*/
static
IK_QJacobianSolver *
New(
);
class IK_QJacobianSolver
{
public:
IK_QJacobianSolver() {};
~IK_QJacobianSolver() {};
/**
* Compute a solution for a chain.
* @param chain Reference to the chain to modify
* @param g_position Reference to the goal position.
* @param g_pose -not used- Reference to the goal pose.
* @param root Pointer to root segment.
* @param tasks List of tasks.
* @param tolerance The maximum allowed distance between solution
* and goal for termination.
* @param max_iterations should be in the range (50 - 500)
* @param max_angle_change The maximum change in the angle vector
* of the chain (0.1 is a good value)
*
* @return True iff goal position reached.
*/
bool
Solve(
IK_QChain &chain,
const MT_Vector3 &g_position,
const MT_Vector3 &g_pose,
bool Solve(
IK_QSegment *root,
std::list<IK_QTask*> tasks,
const MT_Scalar tolerance,
const int max_iterations,
const MT_Scalar max_angle_change
const int max_iterations
);
~IK_QJacobianSolver(
);
private :
/**
* Private constructor to force use of New()
*/
IK_QJacobianSolver(
);
/**
* Compute the inverse jacobian matrix of the chain.
* Uses a damped least squares solution when the matrix is
* is approaching singularity
*/
int
ComputeInverseJacobian(
IK_QChain &chain,
const MT_Scalar x_length,
const MT_Scalar max_angle_change
);
void
ComputeBetas(
IK_QChain &chain,
const MT_Vector3 d_pos
);
/**
* Updates the angles of the chain with the newly
* computed values
*/
void
UpdateChain(
IK_QChain &chain
);
/**
* Make sure all the matrices are of the correct size
*/
void
ArmMatrices(
int dof
);
/**
* Quick check to see if all the segments are parallel
* to the goal direction.
*/
bool
ParallelCheck(
const IK_QChain &chain,
const MT_Vector3 goal
) const;
private :
/// the vector of intermediate betas
TNT::Vector<MT_Scalar> m_beta;
/// the vector of computed angle changes
TNT::Vector<MT_Scalar> m_d_theta;
/// the constraint gradients for each angle.
TNT::Vector<MT_Scalar> m_dh;
/// Space required for SVD computation
TNT::Vector<MT_Scalar> m_svd_w;
TNT::Vector<MT_Scalar> m_svd_work_space;
TNT::Matrix<MT_Scalar> m_svd_v;
TNT::Matrix<MT_Scalar> m_svd_u;
TNT::Matrix<MT_Scalar> m_svd_w_diag;
TNT::Matrix<MT_Scalar> m_svd_v_t;
TNT::Matrix<MT_Scalar> m_svd_u_t;
TNT::Matrix<MT_Scalar> m_svd_inverse;
TNT::Matrix<MT_Scalar> m_svd_temp1;
private:
void AddSegmentList(IK_QSegment *seg);
bool Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks);
bool UpdateAngles(MT_Scalar& norm);
private:
// the jacobian matrix
IK_QJacobian m_jacobian;
IK_QJacobian m_jacobian_sub;
bool m_secondary_enabled;
std::vector<IK_QSegment*> m_segments;
};
#endif

1090
intern/iksolver/intern/IK_QSegment.cpp
View File

File diff suppressed because it is too large Load Diff

416
intern/iksolver/intern/IK_QSegment.h
View File

@ -24,39 +24,30 @@
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
* Original author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
/**
* $Id$
* Copyright (C) 2001 NaN Technologies B.V.
*
* @author Laurence
*/
#ifndef NAN_INCLUDED_IK_QSegment_h
#define NAN_INCLUDED_IK_QSegment_h
#include "MT_Vector3.h"
#include "MT_Transform.h"
#include "MT_Matrix4x4.h"
#include "MT_ExpMap.h"
#include "IK_QJacobian.h"
#include "MEM_SmartPtr.h"
#include <vector>
/**
* An IK_Qsegment encodes information about a segments
* local coordinate system.
* In these segments exponential maps are used to parameterize
* the 3 DOF joints. Please see the file MT_ExpMap.h for more
* information on this parameterization.
*
* These segments always point along the y-axis.
*
* Here wee define the local coordinates of a joint as
* Here we define the local coordinates of a joint as
* local_transform =
* translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0)
* We use the standard moto column ordered matrices. You can read
@ -71,220 +62,263 @@
* use exactly the same transformations when displaying the segments
*/
class IK_QSegment {
class IK_QSegment
{
public:
virtual ~IK_QSegment();
public :
// start: a user defined translation
// rest_basis: a user defined rotation
// basis: a user defined rotation
// length: length of this segment
/**
* Constructor.
* @param tr1 a user defined translation
* @param a used defined rotation matrix representin
* the rest position of the bone.
* @param the length of the bone.
* @param an exponential map can also be used to
* define the bone rest position.
*/
IK_QSegment(
const MT_Point3 tr1,
const MT_Matrix3x3 A,
const MT_Scalar length,
const MT_ExpMap q
void SetTransform(
const MT_Vector3& start,
const MT_Matrix3x3& rest_basis,
const MT_Matrix3x3& basis,
const MT_Scalar length
);
/**
* Default constructor
* Defines identity local coordinate system,
* with a bone length of 1.
*/
// tree structure access
void SetParent(IK_QSegment *parent);
IK_QSegment *Child() const
{ return m_child; }
IK_QSegment(
);
IK_QSegment *Sibling() const
{ return m_sibling; }
IK_QSegment *Parent() const
{ return m_parent; }
/**
* @return The length of the segment
*/
// number of degrees of freedom
int NumberOfDoF() const
{ return m_num_DoF; }
const
MT_Scalar
Length(
) const ;
// unique id for this segment, for identification in the jacobian
int DoFId() const
{ return m_DoF_id; }
/**
* @return the max distance of the end of this
* bone from the local origin.
*/
void SetDoFId(int dof_id)
{ m_DoF_id = dof_id; }
const
MT_Scalar
MaxExtension(
) const ;
// the max distance of the end of this bone from the local origin.
const MT_Scalar MaxExtension() const
{ return m_max_extension; }
/**
* @return The transform from adjacent
* coordinate systems in the chain.
*/
// the change in rotation and translation w.r.t. the rest pose
MT_Matrix3x3 BasisChange() const;
MT_Vector3 TranslationChange() const;
const
MT_Transform &
LocalTransform(
) const ;
// the start and end of the segment
const MT_Point3 &GlobalStart() const
{ return m_global_start; }
const
MT_ExpMap &
LocalJointParameter(
) const;
const MT_Point3 &GlobalEnd() const
{ return m_global_transform.getOrigin(); }
/**
* Update the global coordinates of this segment.
* @param global the global coordinates of the
* previous bone in the chain
* @return the global coordinates of this segment.
*/
// the global transformation at the end of the segment
const MT_Transform &GlobalTransform() const
{ return m_global_transform; }
MT_Transform
UpdateGlobal(
const MT_Transform & global
);
// is a translational segment?
bool Translational() const
{ return m_translational; }
/**
* @return The global transformation
*/
// locking (during inner clamping loop)
bool Locked(int dof) const
{ return m_locked[dof]; }
MT_Transform
GlobalTransform(
) const;
void UnLock()
{ m_locked[0] = m_locked[1] = m_locked[2] = false; }
// per dof joint weighting
MT_Scalar Weight(int dof) const
{ return m_weight[dof]; }
/**
* Update the accumulated local transform of this segment
* The accumulated local transform is the end effector
* transform in the local coordinates of this segment.
* @param acc_local the accumulated local transform of
* the child of this bone.
* @return the accumulated local transorm of this segment
*/
void ScaleWeight(int dof, MT_Scalar scale)
{ m_weight[dof] *= scale; }
MT_Transform
UpdateAccumulatedLocal(
const MT_Transform & acc_local
);
// recursively update the global coordinates of this segment, 'global'
// is the global transformation from the parent segment
void UpdateTransform(const MT_Transform &global);
/**
* @return A const reference to accumulated local
* transform of this segment.
*/
// get axis from rotation matrix for derivative computation
virtual MT_Vector3 Axis(int dof) const=0;
const
MT_Transform &
AccumulatedLocal(
) const;
// update the angles using the dTheta's computed using the jacobian matrix
virtual bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*)=0;
virtual void Lock(int, IK_QJacobian&, MT_Vector3&) {}
virtual void UpdateAngleApply()=0;
/**
* @return A const Reference to start of segment in global
* coordinates
*/
// set joint limits
virtual void SetLimit(int, MT_Scalar, MT_Scalar) {};
const
MT_Vector3 &
GlobalSegmentStart(
) const;
// set joint weights (per axis)
virtual void SetWeight(int, MT_Scalar) {};
/**
* @return A const Reference to end of segment in global
* coordinates
*/
protected:
const
MT_Vector3 &
GlobalSegmentEnd(
) const;
// num_DoF: number of degrees of freedom
IK_QSegment(int num_DoF, bool translational);
// remove child as a child of this segment
void RemoveChild(IK_QSegment *child);
/**
* @return the partial derivative of the end effector
* with respect to one of the degrees of freedom of this
* segment.
* @param angle the angle parameter you want to compute
* the partial derivatives for. For these segments this
* must be in the range [0,2]
*/
// tree structure variables
IK_QSegment *m_parent;
IK_QSegment *m_child;
IK_QSegment *m_sibling;
MT_Vector3
ComputeJacobianColumn(
int angle
) const ;
// full transform =
// start * rest_basis * basis * translation
MT_Vector3 m_start;
MT_Matrix3x3 m_rest_basis;
MT_Matrix3x3 m_basis;
MT_Vector3 m_translation;
/**
* Explicitly set the angle parameterization value.
*/
void
SetAngle(
const MT_ExpMap &q
);
/**
* Increment the angle parameterization value.
*/
void
IncrementAngle(
const MT_Vector3 &dq
);
/**
* Return the parameterization of this angle
*/
const
MT_ExpMap &
ExpMap(
) const {
return m_q;
};
private :
void
UpdateLocalTransform(
);
private :
/**
* m_transform The user defined transformation, composition of the
* translation and rotation from constructor.
*/
MT_Transform m_transform;
/**
* The exponential map parameterization of this joint.
*/
MT_Scalar m_length;
MT_ExpMap m_q;
/**
* The maximum extension of this segment
* This is the magnitude of the user offset + the length of the
* chain
*/
// original basis
MT_Matrix3x3 m_orig_basis;
MT_Vector3 m_orig_translation;
// maximum extension of this segment
MT_Scalar m_max_extension;
MT_Transform m_local_transform;
// accumulated transformations starting from root
MT_Point3 m_global_start;
MT_Transform m_global_transform;
MT_Transform m_accum_local;
MT_Vector3 m_seg_start;
MT_Vector3 m_seg_end;
// number degrees of freedom, (first) id of this segments DOF's
int m_num_DoF, m_DoF_id;
bool m_locked[3];
bool m_translational;
MT_Scalar m_weight[3];
};
class IK_QSphericalSegment : public IK_QSegment
{
public:
IK_QSphericalSegment();
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
bool ComputeClampRotation(MT_Vector3& clamp);
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
private:
MT_Matrix3x3 m_new_basis;
bool m_limit_x, m_limit_y, m_limit_z;
MT_Scalar m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
MT_Scalar m_locked_ax, m_locked_ay, m_locked_az;
};
class IK_QNullSegment : public IK_QSegment
{
public:
IK_QNullSegment();
bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*) { return false; }
void UpdateAngleApply() {}
MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
};
class IK_QRevoluteSegment : public IK_QSegment
{
public:
// axis: the axis of the DoF, in range 0..2
IK_QRevoluteSegment(int axis);
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
private:
int m_axis;
MT_Scalar m_angle, m_new_angle;
MT_Scalar m_limit;
MT_Scalar m_min, m_max;
};
class IK_QSwingSegment : public IK_QSegment
{
public:
// XZ DOF, uses one direct rotation
IK_QSwingSegment();
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
private:
MT_Matrix3x3 m_new_basis;
bool m_limit_x, m_limit_z;
MT_Scalar m_max_x, m_max_z, m_offset_x, m_offset_z;
};
class IK_QElbowSegment : public IK_QSegment
{
public:
// XY or ZY DOF, uses two sequential rotations: first rotate around
// X or Z, then rotate around Y (twist)
IK_QElbowSegment(int axis);
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
private:
int m_axis;
MT_Scalar m_twist, m_angle, m_new_twist, m_new_angle;
MT_Scalar m_cos_twist, m_sin_twist;
bool m_limit, m_limit_twist;
MT_Scalar m_min, m_max, m_min_twist, m_max_twist;
};
class IK_QTranslateSegment : public IK_QSegment
{
public:
// Revolute, 2DOF or 3DOF translational segments
IK_QTranslateSegment(int axis1);
IK_QTranslateSegment(int axis1, int axis2);
IK_QTranslateSegment();
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int, IK_QJacobian&, MT_Vector3&) {};
void UpdateAngleApply();
void SetWeight(int axis, MT_Scalar weight);
private:
int m_axis[3];
bool m_axis_enabled[3];
MT_Vector3 m_new_translation;
};
#endif

108
intern/iksolver/intern/IK_QSolver_Class.h
View File

@ -1,108 +0,0 @@
/**
* $Id$
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under
* the Blender License. See http://www.blender.org/BL/ for information
* about this.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
/**
* $Id$
* Copyright (C) 2001 NaN Technologies B.V.
*
* @author Laurence
*/
#ifndef NAN_INCLUDED_IK_Solver_Class
#define NAN_INCLUDED_IK_Solver_Class
#include "IK_QChain.h"
#include "IK_QJacobianSolver.h"
#include "IK_QSegment.h"
#include "MEM_SmartPtr.h"
/**
* This class just contains all instances of internal data
* associated with the external chain structure needed for
* an ik solve. A pointer to this class gets hidden in the
* external structure as a void pointer.
*/
class IK_QSolver_Class {
public :
static
IK_QSolver_Class *
New(
){
MEM_SmartPtr<IK_QSolver_Class> output (new IK_QSolver_Class);
MEM_SmartPtr<IK_QJacobianSolver> solver (IK_QJacobianSolver::New());
if (output == NULL ||
solver == NULL
) {
return NULL;
}
output->m_solver = solver.Release();
return output.Release();
};
IK_QChain &
Chain(
) {
return m_chain;
};
IK_QJacobianSolver &
Solver(
) {
return m_solver.Ref();
}
~IK_QSolver_Class(
) {
// nothing to do
}
private :
IK_QSolver_Class(
) {
};
IK_QChain m_chain;
MEM_SmartPtr<IK_QJacobianSolver> m_solver;
};
#endif

365
intern/iksolver/intern/IK_Solver.cpp
View File

@ -24,177 +24,258 @@
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
* Original Author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#define IK_USE_EXPMAP
#ifdef IK_USE_EXPMAP
# include "IK_QSolver_Class.h"
#else
# include "IK_Solver_Class.h"
#endif
#include "../extern/IK_solver.h"
#include "IK_QJacobianSolver.h"
#include "IK_QSegment.h"
#include "IK_QTask.h"
#include <list>
#include <iostream>
using namespace std;
IK_Chain_ExternPtr
IK_CreateChain(
void
) {
typedef struct {
IK_QJacobianSolver solver;
IK_QSegment *root;
std::list<IK_QTask*> tasks;
} IK_QSolver;
MEM_SmartPtr<IK_Chain_Extern> output (new IK_Chain_Extern);
MEM_SmartPtr<IK_QSolver_Class> output_void (IK_QSolver_Class::New());
IK_Segment *IK_CreateSegment(int flag)
{
int ndof = 0;
ndof += (flag & IK_XDOF)? 1: 0;
ndof += (flag & IK_YDOF)? 1: 0;
ndof += (flag & IK_ZDOF)? 1: 0;
IK_QSegment *seg;
if (output == NULL || output_void == NULL) {
if (ndof == 0)
seg = new IK_QNullSegment();
else if (ndof == 1) {
int axis;
if (flag & IK_XDOF) axis = 0;
else if (flag & IK_YDOF) axis = 1;
else axis = 2;
if (flag & IK_TRANSLATIONAL)
seg = new IK_QTranslateSegment(axis);
else
seg = new IK_QRevoluteSegment(axis);
}
else if (ndof == 2) {
int axis1, axis2;
if (flag & IK_XDOF) {
axis1 = 0;
axis2 = (flag & IK_YDOF)? 1: 2;
}
else {
axis1 = 1;
axis2 = 2;
}
if (flag & IK_TRANSLATIONAL)
seg = new IK_QTranslateSegment(axis1, axis2);
else {
if (axis1 + axis2 == 2)
seg = new IK_QSwingSegment();
else
seg = new IK_QElbowSegment((axis1 == 0)? 0: 2);
}
}
else {
if (flag & IK_TRANSLATIONAL)
seg = new IK_QTranslateSegment();
else
seg = new IK_QSphericalSegment();
}
return (IK_Segment*)seg;
}
void IK_FreeSegment(IK_Segment *seg)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
delete qseg;
}
void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
qseg->SetParent((IK_QSegment*)parent);
}
void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
MT_Vector3 mstart(start);
// convert from blender column major to moto row major
MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
basis[0][1], basis[1][1], basis[2][1],
basis[0][2], basis[1][2], basis[2][2]);
MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
rest[0][1], rest[1][1], rest[2][1],
rest[0][2], rest[1][2], rest[2][2]);
MT_Scalar mlength(length);
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
if (axis == IK_X)
qseg->SetLimit(0, lmin, lmax);
else if (axis == IK_Y)
qseg->SetLimit(1, lmin, lmax);
else if (axis == IK_Z)
qseg->SetLimit(2, lmin, lmax);
}
void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
{
if (stiffness < 1.0)
return;
IK_QSegment *qseg = (IK_QSegment*)seg;
MT_Scalar weight = 1.0/stiffness;
if (axis == IK_X)
qseg->SetWeight(0, weight);
else if (axis == IK_Y)
qseg->SetWeight(1, weight);
else if (axis == IK_Z)
qseg->SetWeight(2, weight);
}
void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
{
IK_QSegment *qseg = (IK_QSegment*)seg;
const MT_Matrix3x3& change = qseg->BasisChange();
// convert from moto row major to blender column major
basis_change[0][0] = (float)change[0][0];
basis_change[1][0] = (float)change[0][1];
basis_change[2][0] = (float)change[0][2];
basis_change[0][1] = (float)change[1][0];
basis_change[1][1] = (float)change[1][1];
basis_change[2][1] = (float)change[1][2];
basis_change[0][2] = (float)change[2][0];
basis_change[1][2] = (float)change[2][1];
basis_change[2][2] = (float)change[2][2];
}
void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
const MT_Vector3& change = qseg->TranslationChange();
translation_change[0] = (float)change[0];
translation_change[1] = (float)change[1];
translation_change[2] = (float)change[2];
}
IK_Solver *IK_CreateSolver(IK_Segment *root)
{
if (root == NULL)
return NULL;
}
output->chain_dof = 0;
output->num_segments = 0;
output->segments = NULL;
output->intern = output_void.Release();
return output.Release();
};
IK_QSolver *solver = new IK_QSolver();
solver->root = (IK_QSegment*)root;
int
IK_LoadChain(
IK_Chain_ExternPtr chain,
IK_Segment_ExternPtr segments,
int num_segs
) {
if (chain == NULL || segments == NULL) return 0;
IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
if (intern_cpp == NULL) return 0;
std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments();
if (segs.size() != (unsigned int) num_segs) {
segs = std::vector<IK_QSegment>(num_segs);
}
std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
std::vector<IK_QSegment>::iterator seg_begin= segs.begin();
IK_Segment_ExternPtr extern_seg_it = segments;
for (;seg_begin != seg_end; ++seg_begin,++extern_seg_it) {
MT_Point3 tr1(extern_seg_it->seg_start);
MT_Matrix3x3 A(
extern_seg_it->basis[0],extern_seg_it->basis[1],extern_seg_it->basis[2],
extern_seg_it->basis[3],extern_seg_it->basis[4],extern_seg_it->basis[5],
extern_seg_it->basis[6],extern_seg_it->basis[7],extern_seg_it->basis[8]
);
MT_Scalar length(extern_seg_it->length);
*seg_begin = IK_QSegment(
tr1,A,length,MT_Vector3(0,0,0)
);
}
intern_cpp->Chain().UpdateGlobalTransformations();
intern_cpp->Chain().ComputeJacobian();
chain->num_segments = num_segs;
chain->chain_dof = intern_cpp->Chain().DoF();
chain->segments = segments;
return 1;
};
int
IK_SolveChain(
IK_Chain_ExternPtr chain,
float goal[3],
float tolerance,
int max_iterations,
float max_angle_change,
IK_Segment_ExternPtr output
){
if (chain == NULL || output == NULL) return 0;
if (chain->intern == NULL) return 0;
IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
IK_QJacobianSolver & solver = intern_cpp->Solver();
bool solve_result = solver.Solve(
intern_cpp->Chain(),
MT_Vector3(goal),
MT_Vector3(0,0,0),
MT_Scalar(tolerance),
max_iterations,
MT_Scalar(max_angle_change)
);
// turn the computed role->pitch->yaw into a quaternion and
// return the result in output
std::vector<IK_QSegment> & segs = intern_cpp->Chain().Segments();
std::vector<IK_QSegment>::const_iterator seg_end= segs.end();
std::vector<IK_QSegment>::iterator seg_begin= segs.begin();
for (;seg_begin != seg_end; ++seg_begin, ++output) {
MT_Matrix3x3 qrot = seg_begin->ExpMap().getMatrix();
// don't forget to transpose this qrot for use by blender!
qrot.transpose(); // blender uses transpose here ????
output->basis_change[0] = float(qrot[0][0]);
output->basis_change[1] = float(qrot[0][1]);
output->basis_change[2] = float(qrot[0][2]);
output->basis_change[3] = float(qrot[1][0]);
output->basis_change[4] = float(qrot[1][1]);
output->basis_change[5] = float(qrot[1][2]);
output->basis_change[6] = float(qrot[2][0]);
output->basis_change[7] = float(qrot[2][1]);
output->basis_change[8] = float(qrot[2][2]);
}
return solve_result ? 1 : 0;
return (IK_Solver*)solver;
}
void
IK_FreeChain(
IK_Chain_ExternPtr chain
){
IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern;
void IK_FreeSolver(IK_Solver *solver)
{
if (solver == NULL)
return;
delete(intern_cpp);
delete(chain);
IK_QSolver *qsolver = (IK_QSolver*)solver;
std::list<IK_QTask*>& tasks = qsolver->tasks;
std::list<IK_QTask*>::iterator task;
for (task = tasks.begin(); task != tasks.end(); task++)
delete (*task);
delete qsolver;
}
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
{
if (solver == NULL || tip == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver*)solver;
IK_QSegment *qtip = (IK_QSegment*)tip;
MT_Vector3 pos(goal);
// qsolver->tasks.empty()
IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
ee->SetWeight(weight);
qsolver->tasks.push_back(ee);
}
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
{
if (solver == NULL || tip == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver*)solver;
IK_QSegment *qtip = (IK_QSegment*)tip;
// convert from blender column major to moto row major
MT_Matrix3x3 rot(goal[0][0], goal[1][0], goal[2][0],
goal[0][1], goal[1][1], goal[2][1],
goal[0][2], goal[1][2], goal[2][2]);
IK_QTask *orient = new IK_QOrientationTask(false, qtip, rot);
orient->SetWeight(weight);
qsolver->tasks.push_back(orient);
}
void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
{
if (solver == NULL || root == NULL)
return;
IK_QSolver *qsolver = (IK_QSolver*)solver;
IK_QSegment *qroot = (IK_QSegment*)root;
// convert from blender column major to moto row major
MT_Vector3 center(goal);
IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
com->SetWeight(weight);
qsolver->tasks.push_back(com);
}
int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
{
if (solver == NULL)
return 0;
IK_QSolver *qsolver = (IK_QSolver*)solver;
IK_QSegment *root = qsolver->root;
IK_QJacobianSolver& jacobian = qsolver->solver;
std::list<IK_QTask*>& tasks = qsolver->tasks;
MT_Scalar tol = tolerance;
bool result = jacobian.Solve(root, tasks, tol, max_iterations);
return ((result)? 1: 0);
}

276
intern/iksolver/intern/MT_ExpMap.cpp
View File

@ -24,23 +24,12 @@
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
* Original Author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
/**
* $Id$
* Copyright (C) 2001 NaN Technologies B.V.
*
* @author Laurence
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include "MT_ExpMap.h"
/**
@ -52,21 +41,19 @@ MT_ExpMap::
setRotation(
const MT_Quaternion &q
) {
// ok first normailize the quaternion
// ok first normalize the quaternion
// then compute theta the axis-angle and the normalized axis v
// scale v by theta and that's it hopefully!
MT_Quaternion qt = q.normalized();
m_q = q.normalized();
m_v = MT_Vector3(m_q.x(), m_q.y(), m_q.z());
MT_Vector3 axis(qt.x(),qt.y(),qt.z());
MT_Scalar cosp = qt.w();
MT_Scalar sinp = axis.length();
axis /= sinp;
MT_Scalar cosp = m_q.w();
m_sinp = m_v.length();
m_v /= m_sinp;
MT_Scalar theta = atan2(double(sinp),double(cosp));
axis *= theta;
m_v = axis;
m_theta = atan2(double(m_sinp),double(cosp));
m_v *= m_theta;
}
/**
@ -74,35 +61,11 @@ setRotation(
* representation
*/
MT_Quaternion
const MT_Quaternion&
MT_ExpMap::
getRotation(
) const {
MT_Scalar cosp, sinp, theta;
MT_Quaternion q;
theta = m_v.length();
cosp = MT_Scalar(cos(.5*theta));
sinp = MT_Scalar(sin(.5*theta));
q.w() = cosp;
if (theta < MT_EXPMAP_MINANGLE) {
MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - theta*theta/MT_Scalar(48.0)); /* Taylor Series for sinc */
q.x() = temp.x();
q.y() = temp.y();
q.z() = temp.z();
} else {
MT_Vector3 temp = m_v * (sinp/theta); /* Taylor Series for sinc */
q.x() = temp.x();
q.y() = temp.y();
q.z() = temp.z();
}
return q;
return m_q;
}
/**
@ -113,114 +76,82 @@ getRotation(
MT_ExpMap::
getMatrix(
) const {
MT_Quaternion q = getRotation();
return MT_Matrix3x3(q);
return MT_Matrix3x3(m_q);
}
/**
* Force a reparameterization of the exponential
* map.
* Update & reparameterizate the exponential map
*/
bool
void
MT_ExpMap::
reParameterize(
MT_Scalar &theta
update(
const MT_Vector3& dv
){
bool rep(false);
theta = m_v.length();
if (theta > MT_PI){
MT_Scalar scl = theta;
if (theta > MT_2_PI){ /* first get theta into range 0..2PI */
theta = MT_Scalar(fmod(theta, MT_2_PI));
scl = theta/scl;
m_v *= scl;
rep = true;
}
if (theta > MT_PI){
scl = theta;
theta = MT_2_PI - theta;
scl = MT_Scalar(1.0) - MT_2_PI/scl;
m_v *= scl;
rep = true;
}
}
return rep;
m_v += dv;
angleUpdated();
}
/**
* Compute the partial derivatives of the exponential
* map (dR/de - where R is a 4x4 rotation matrix formed
* from the map) and return them as a 4x4 matrix
* map (dR/de - where R is a 3x3 rotation matrix formed
* from the map) and return them as a 3x3 matrix
*/
MT_Matrix4x4
void
MT_ExpMap::
partialDerivatives(
const int i
MT_Matrix3x3& dRdx,
MT_Matrix3x3& dRdy,
MT_Matrix3x3& dRdz
) const {
MT_Quaternion q = getRotation();
MT_Quaternion dQdx;
MT_Quaternion dQdx[3];
MT_Matrix4x4 output;
compute_dQdVi(dQdx);
compute_dQdVi(i,dQdx);
compute_dRdVi(q,dQdx,output);
return output;
compute_dRdVi(dQdx[0], dRdx);
compute_dRdVi(dQdx[1], dRdy);
compute_dRdVi(dQdx[2], dRdz);
}
void
MT_ExpMap::
compute_dRdVi(
const MT_Quaternion &q,
const MT_Quaternion &dQdvi,
MT_Matrix4x4 & dRdvi
MT_Matrix3x3 & dRdvi
) const {
MT_Scalar prod[9];
MT_Scalar prod[9];
/* This efficient formulation is arrived at by writing out the
* entire chain rule product dRdq * dqdv in terms of 'q' and
* noticing that all the entries are formed from sums of just
* nine products of 'q' and 'dqdv' */
/* This efficient formulation is arrived at by writing out the
* entire chain rule product dRdq * dqdv in terms of 'q' and
* noticing that all the entries are formed from sums of just
* nine products of 'q' and 'dqdv' */
prod[0] = -MT_Scalar(4)*q.x()*dQdvi.x();
prod[1] = -MT_Scalar(4)*q.y()*dQdvi.y();
prod[2] = -MT_Scalar(4)*q.z()*dQdvi.z();
prod[3] = MT_Scalar(2)*(q.y()*dQdvi.x() + q.x()*dQdvi.y());
prod[4] = MT_Scalar(2)*(q.w()*dQdvi.z() + q.z()*dQdvi.w());
prod[5] = MT_Scalar(2)*(q.z()*dQdvi.x() + q.x()*dQdvi.z());
prod[6] = MT_Scalar(2)*(q.w()*dQdvi.y() + q.y()*dQdvi.w());
prod[7] = MT_Scalar(2)*(q.z()*dQdvi.y() + q.y()*dQdvi.z());
prod[8] = MT_Scalar(2)*(q.w()*dQdvi.x() + q.x()*dQdvi.w());
prod[0] = -MT_Scalar(4)*m_q.x()*dQdvi.x();
prod[1] = -MT_Scalar(4)*m_q.y()*dQdvi.y();
prod[2] = -MT_Scalar(4)*m_q.z()*dQdvi.z();
prod[3] = MT_Scalar(2)*(m_q.y()*dQdvi.x() + m_q.x()*dQdvi.y());
prod[4] = MT_Scalar(2)*(m_q.w()*dQdvi.z() + m_q.z()*dQdvi.w());
prod[5] = MT_Scalar(2)*(m_q.z()*dQdvi.x() + m_q.x()*dQdvi.z());
prod[6] = MT_Scalar(2)*(m_q.w()*dQdvi.y() + m_q.y()*dQdvi.w());
prod[7] = MT_Scalar(2)*(m_q.z()*dQdvi.y() + m_q.y()*dQdvi.z());
prod[8] = MT_Scalar(2)*(m_q.w()*dQdvi.x() + m_q.x()*dQdvi.w());
/* first row, followed by second and third */
dRdvi[0][0] = prod[1] + prod[2];
dRdvi[0][1] = prod[3] - prod[4];
dRdvi[0][2] = prod[5] + prod[6];
/* first row, followed by second and third */
dRdvi[0][0] = prod[1] + prod[2];
dRdvi[0][1] = prod[3] - prod[4];
dRdvi[0][2] = prod[5] + prod[6];
dRdvi[1][0] = prod[3] + prod[4];
dRdvi[1][1] = prod[0] + prod[2];
dRdvi[1][2] = prod[7] - prod[8];
dRdvi[1][0] = prod[3] + prod[4];
dRdvi[1][1] = prod[0] + prod[2];
dRdvi[1][2] = prod[7] - prod[8];
dRdvi[2][0] = prod[5] - prod[6];
dRdvi[2][1] = prod[7] + prod[8];
dRdvi[2][2] = prod[0] + prod[1];
/* the 4th row and column are all zero */
int i;
for (i=0; i<3; i++)
dRdvi[3][i] = dRdvi[i][3] = MT_Scalar(0);
dRdvi[3][3] = 0;
dRdvi[2][0] = prod[5] - prod[6];
dRdvi[2][1] = prod[7] + prod[8];
dRdvi[2][2] = prod[0] + prod[1];
}
// compute partial derivatives dQ/dVi
@ -228,44 +159,93 @@ compute_dRdVi(
void
MT_ExpMap::
compute_dQdVi(
const int i,
MT_Quaternion & dQdX
MT_Quaternion *dQdX
) const {
MT_Scalar theta = m_v.length();
MT_Scalar cosp(cos(MT_Scalar(.5)*theta)), sinp(sin(MT_Scalar(.5)*theta));
/* This is an efficient implementation of the derivatives given
* in Appendix A of the paper with common subexpressions factored out */
MT_assert(i>=0 && i<3);
MT_Scalar sinc, termCoeff;
/* This is an efficient implementation of the derivatives given
* in Appendix A of the paper with common subexpressions factored out */
if (theta < MT_EXPMAP_MINANGLE){
const int i2 = (i+1)%3, i3 = (i+2)%3;
MT_Scalar Tsinc = MT_Scalar(0.5) - theta*theta/MT_Scalar(48.0);
MT_Scalar vTerm = m_v[i] * (theta*theta/MT_Scalar(40.0) - MT_Scalar(1.0)) / MT_Scalar(24.0);
if (m_theta < MT_EXPMAP_MINANGLE) {
sinc = 0.5 - m_theta*m_theta/48.0;
termCoeff = (m_theta*m_theta/40.0 - 1.0)/24.0;
}
else {
MT_Scalar cosp = m_q.w();
MT_Scalar ang = 1.0/m_theta;
dQdX.w() = -.5*m_v[i]*Tsinc;
dQdX[i] = m_v[i]* vTerm + Tsinc;
dQdX[i2] = m_v[i2]*vTerm;
dQdX[i3] = m_v[i3]*vTerm;
} else {
const int i2 = (i+1)%3, i3 = (i+2)%3;
const MT_Scalar ang = 1.0/theta, ang2 = ang*ang*m_v[i], sang = sinp*ang;
const MT_Scalar cterm = ang2*(.5*cosp - sang);
sinc = m_sinp*ang;
termCoeff = ang*ang*(0.5*cosp - sinc);
}
dQdX[i] = cterm*m_v[i] + sang;
dQdX[i2] = cterm*m_v[i2];
dQdX[i3] = cterm*m_v[i3];
dQdX.w() = MT_Scalar(-.5)*m_v[i]*sang;
}
for (int i = 0; i < 3; i++) {
MT_Quaternion& dQdx = dQdX[i];
int i2 = (i+1)%3;
int i3 = (i+2)%3;
MT_Scalar term = m_v[i]*termCoeff;
dQdx[i] = term*m_v[i] + sinc;
dQdx[i2] = term*m_v[i2];
dQdx[i3] = term*m_v[i3];
dQdx.w() = -0.5*m_v[i]*sinc;
}
}
// reParametize away from singularity, updating
// m_v and m_theta
void
MT_ExpMap::
reParametrize(
){
if (m_theta > MT_PI) {
MT_Scalar scl = m_theta;
if (m_theta > MT_2_PI){ /* first get theta into range 0..2PI */
m_theta = MT_Scalar(fmod(m_theta, MT_2_PI));
scl = m_theta/scl;
m_v *= scl;
}
if (m_theta > MT_PI){
scl = m_theta;
m_theta = MT_2_PI - m_theta;
scl = MT_Scalar(1.0) - MT_2_PI/scl;
m_v *= scl;
}
}
}
// compute cached variables
void
MT_ExpMap::
angleUpdated(
){
m_theta = m_v.length();
reParametrize();
// compute quaternion, sinp and cosp
if (m_theta < MT_EXPMAP_MINANGLE) {
m_sinp = MT_Scalar(0.0);
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - m_theta*m_theta/MT_Scalar(48.0));
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(1.0);
} else {
m_sinp = MT_Scalar(sin(.5*m_theta));
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * (m_sinp/m_theta);
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(cos(.5*m_theta));
}
}

69
intern/iksolver/intern/MT_ExpMap.h
View File

@ -24,19 +24,12 @@
*
* The Original Code is: all of this file.
*
* Contributor(s): none yet.
* Original author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
/**
* $Id$
* Copyright (C) 2001 NaN Technologies B.V.
*
* @author Laurence
*/
#ifndef MT_ExpMap_H
#define MT_ExpMap_H
@ -93,13 +86,13 @@ public:
*/
MT_ExpMap() {}
MT_ExpMap(const MT_Vector3& v) : m_v(v) {}
MT_ExpMap(const MT_Vector3& v) : m_v(v) { angleUpdated(); }
MT_ExpMap(const float v[3]) : m_v(v) {}
MT_ExpMap(const double v[3]) : m_v(v) {}
MT_ExpMap(const float v[3]) : m_v(v) { angleUpdated(); }
MT_ExpMap(const double v[3]) : m_v(v) { angleUpdated(); }
MT_ExpMap(MT_Scalar x, MT_Scalar y, MT_Scalar z) :
m_v(x, y, z) {}
m_v(x, y, z) { angleUpdated(); }
/**
* Construct an exponential map from a quaternion
@ -119,12 +112,6 @@ public:
* on this class and some of them have no direct meaning.
*/
MT_Vector3 &
vector(
) {
return m_v;
};
const
MT_Vector3 &
vector(
@ -146,7 +133,7 @@ public:
* representation
*/
MT_Quaternion
const MT_Quaternion&
getRotation(
) const;
@ -159,17 +146,13 @@ public:
) const;
/**
* Force a reparameterization check of the exponential
* map.
* @param theta returns the new axis-angle.
* @return true iff a reParameterization took place.
* Use this function whenever you adjust the vector
* representing the exponential map.
* Update (and reparameterize) the expontial map
* @param dv delta update values.
*/
bool
reParameterize(
MT_Scalar &theta
void
update(
const MT_Vector3& dv
);
/**
@ -178,37 +161,51 @@ public:
* from the map) and return them as a 4x4 matrix
*/
MT_Matrix4x4
void
partialDerivatives(
const int i
MT_Matrix3x3& dRdx,
MT_Matrix3x3& dRdy,
MT_Matrix3x3& dRdz
) const ;
private :
// m_v contains the exponential map, the other variables are
// cached for efficiency
MT_Vector3 m_v;
MT_Scalar m_theta, m_sinp;
MT_Quaternion m_q;
// private methods
// Compute partial derivatives dR (3x3 rotation matrix) / dVi (EM vector)
// given the partial derivative dQ (Quaternion) / dVi (ith element of EM vector)
void
compute_dRdVi(
const MT_Quaternion &q,
const MT_Quaternion &dQdV,
MT_Matrix4x4 & dRdVi
MT_Matrix3x3 & dRdVi
) const;
// compute partial derivatives dQ/dVi
void
compute_dQdVi(
int i,
MT_Quaternion & dQdX
MT_Quaternion *dQdX
) const ;
// reparametrize away from singularity
void
reParametrize(
);
// (re-)compute cached variables
void
angleUpdated(
);
};
#endif

3
intern/iksolver/intern/Makefile
View File

@ -33,7 +33,8 @@
LIBNAME = iksolver
DIR = $(OCGDIR)/intern/$(LIBNAME)
CCSRCS = IK_QChain.cpp IK_QJacobianSolver.cpp IK_QSegment.cpp MT_ExpMap.cpp IK_Solver.cpp
CCSRCS = IK_QJacobianSolver.cpp IK_QSegment.cpp IK_Solver.cpp IK_QJacobian.cpp
CCSRCS += IK_QTask.cpp
include nan_compile.mk