forked from bartvdbraak/blender
Got rid of unused files in intern/iksolver
(removed them from cvs and from the Makefile.am) Kent -- mein@cs.umn.edu
This commit is contained in:
parent
5b88406c3f
commit
671a355e9f
@ -1,303 +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 *****
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifdef HAVE_CONFIG_H
|
|
||||||
#include <config.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "IK_CGChainSolver.h"
|
|
||||||
#include "IK_Segment.h"
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
ChainPotential *
|
|
||||||
ChainPotential::
|
|
||||||
New(
|
|
||||||
IK_Chain &chain
|
|
||||||
){
|
|
||||||
return new ChainPotential(chain);
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// End effector goal
|
|
||||||
|
|
||||||
void
|
|
||||||
ChainPotential::
|
|
||||||
SetGoal(
|
|
||||||
const MT_Vector3 goal
|
|
||||||
){
|
|
||||||
m_goal = goal;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Inherited from DifferentiablePotenialFunctionNd
|
|
||||||
//////////////////////////////////////////////////
|
|
||||||
|
|
||||||
MT_Scalar
|
|
||||||
ChainPotential::
|
|
||||||
Evaluate(
|
|
||||||
MT_Scalar x
|
|
||||||
){
|
|
||||||
|
|
||||||
// evaluate the function at postiion x along the line specified
|
|
||||||
// by the vector pair (m_line_pos,m_line_dir)
|
|
||||||
|
|
||||||
m_temp_pos.newsize(m_line_dir.size());
|
|
||||||
|
|
||||||
TNT::vectorscale(m_temp_pos,m_line_dir,x);
|
|
||||||
TNT::vectoradd(m_temp_pos,m_line_pos);
|
|
||||||
|
|
||||||
return Evaluate(m_temp_pos);
|
|
||||||
}
|
|
||||||
|
|
||||||
MT_Scalar
|
|
||||||
ChainPotential::
|
|
||||||
Derivative(
|
|
||||||
MT_Scalar x
|
|
||||||
){
|
|
||||||
m_temp_pos.newsize(m_line_dir.size());
|
|
||||||
m_temp_grad.newsize(m_line_dir.size());
|
|
||||||
|
|
||||||
TNT::vectorscale(m_temp_pos,m_line_dir,x);
|
|
||||||
TNT::vectoradd(m_temp_pos,m_line_pos);
|
|
||||||
|
|
||||||
Derivative(m_temp_pos,m_temp_grad);
|
|
||||||
|
|
||||||
return TNT::dot_prod(m_temp_grad,m_line_dir);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
MT_Scalar
|
|
||||||
ChainPotential::
|
|
||||||
Evaluate(
|
|
||||||
const TNT::Vector<MT_Scalar> &x
|
|
||||||
){
|
|
||||||
|
|
||||||
|
|
||||||
// set the vector of angles in the backup chain
|
|
||||||
|
|
||||||
vector<IK_Segment> &segs = m_t_chain.Segments();
|
|
||||||
vector<IK_Segment>::iterator seg_it = segs.begin();
|
|
||||||
TNT::Vector<MT_Scalar>::const_iterator a_it = x.begin();
|
|
||||||
#if 0
|
|
||||||
TNT::Vector<MT_Scalar>::const_iterator a_end = x.end();
|
|
||||||
#endif
|
|
||||||
// while we are iterating through the angles and segments
|
|
||||||
// we compute the current angle potenial
|
|
||||||
MT_Scalar angle_potential = 0;
|
|
||||||
#if 0
|
|
||||||
|
|
||||||
for (; a_it != a_end; ++a_it, ++seg_it) {
|
|
||||||
|
|
||||||
MT_Scalar dif = (*a_it - seg_it->CentralAngle());
|
|
||||||
dif *= dif * seg_it->Weight();
|
|
||||||
angle_potential += dif;
|
|
||||||
seg_it->SetTheta(*a_it);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
int chain_dof = m_t_chain.DoF();
|
|
||||||
int n;
|
|
||||||
|
|
||||||
for (n=0; n < chain_dof;seg_it ++) {
|
|
||||||
n += seg_it->SetAngles(a_it + n);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
m_t_chain.UpdateGlobalTransformations();
|
|
||||||
|
|
||||||
MT_Scalar output = (DistancePotential(m_t_chain.EndEffector(),m_goal) + angle_potential);
|
|
||||||
|
|
||||||
return output;
|
|
||||||
};
|
|
||||||
|
|
||||||
void
|
|
||||||
ChainPotential::
|
|
||||||
Derivative(
|
|
||||||
const TNT::Vector<MT_Scalar> &x,
|
|
||||||
TNT::Vector<MT_Scalar> &dy
|
|
||||||
){
|
|
||||||
|
|
||||||
m_distance_grad.newsize(3);
|
|
||||||
// set the vector of angles in the backup chain
|
|
||||||
|
|
||||||
vector<IK_Segment> & segs = m_t_chain.Segments();
|
|
||||||
vector<IK_Segment>::iterator seg_it = segs.begin();
|
|
||||||
TNT::Vector<MT_Scalar>::const_iterator a_it = x.begin();
|
|
||||||
TNT::Vector<MT_Scalar>::const_iterator a_end = x.end();
|
|
||||||
|
|
||||||
m_angle_grad.newsize(segs.size());
|
|
||||||
m_angle_grad = 0;
|
|
||||||
#if 0
|
|
||||||
// FIXME compute angle gradients
|
|
||||||
TNT::Vector<MT_Scalar>::iterator ag_it = m_angle_grad.begin();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
const int chain_dof = m_t_chain.DoF();
|
|
||||||
for (int n=0; n < chain_dof;seg_it ++) {
|
|
||||||
n += seg_it->SetAngles(a_it + n);
|
|
||||||
}
|
|
||||||
|
|
||||||
m_t_chain.UpdateGlobalTransformations();
|
|
||||||
m_t_chain.ComputeJacobian();
|
|
||||||
|
|
||||||
DistanceGradient(m_t_chain.EndEffector(),m_goal);
|
|
||||||
|
|
||||||
// use chain rule for calculating derivative
|
|
||||||
// of potenial function
|
|
||||||
TNT::matmult(dy,m_t_chain.TransposedJacobian(),m_distance_grad);
|
|
||||||
#if 0
|
|
||||||
TNT::vectoradd(dy,m_angle_grad);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
MT_Scalar
|
|
||||||
ChainPotential::
|
|
||||||
DistancePotential(
|
|
||||||
MT_Vector3 pos,
|
|
||||||
MT_Vector3 goal
|
|
||||||
) const {
|
|
||||||
return (pos - goal).length2();
|
|
||||||
}
|
|
||||||
|
|
||||||
// update m_distance_gradient
|
|
||||||
|
|
||||||
void
|
|
||||||
ChainPotential::
|
|
||||||
DistanceGradient(
|
|
||||||
MT_Vector3 pos,
|
|
||||||
MT_Vector3 goal
|
|
||||||
){
|
|
||||||
|
|
||||||
MT_Vector3 output = 2*(pos - goal);
|
|
||||||
m_distance_grad.newsize(3);
|
|
||||||
|
|
||||||
m_distance_grad[0] = output.x();
|
|
||||||
m_distance_grad[1] = output.y();
|
|
||||||
m_distance_grad[2] = output.z();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
IK_CGChainSolver *
|
|
||||||
IK_CGChainSolver::
|
|
||||||
New(
|
|
||||||
){
|
|
||||||
|
|
||||||
MEM_SmartPtr<IK_CGChainSolver> output (new IK_CGChainSolver());
|
|
||||||
MEM_SmartPtr<IK_ConjugateGradientSolver>solver (IK_ConjugateGradientSolver::New());
|
|
||||||
|
|
||||||
if (output == NULL || solver == NULL) return NULL;
|
|
||||||
|
|
||||||
output->m_grad_solver = solver.Release();
|
|
||||||
return output.Release();
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
bool
|
|
||||||
IK_CGChainSolver::
|
|
||||||
Solve(
|
|
||||||
IK_Chain & chain,
|
|
||||||
MT_Vector3 new_position,
|
|
||||||
MT_Scalar tolerance
|
|
||||||
){
|
|
||||||
|
|
||||||
// first build a potenial function for the chain
|
|
||||||
|
|
||||||
m_potential = ChainPotential::New(chain);
|
|
||||||
if (m_potential == NULL) return false;
|
|
||||||
|
|
||||||
m_potential->SetGoal(new_position);
|
|
||||||
|
|
||||||
// make a TNT::vector to describe the current
|
|
||||||
// chain state
|
|
||||||
|
|
||||||
TNT::Vector<MT_Scalar> p;
|
|
||||||
p.newsize(chain.DoF());
|
|
||||||
|
|
||||||
TNT::Vector<MT_Scalar>::iterator p_it = p.begin();
|
|
||||||
vector<IK_Segment>::const_iterator seg_it = chain.Segments().begin();
|
|
||||||
vector<IK_Segment>::const_iterator seg_end = chain.Segments().end();
|
|
||||||
|
|
||||||
for (; seg_it != seg_end; seg_it++) {
|
|
||||||
|
|
||||||
int i;
|
|
||||||
int seg_dof = seg_it->DoF();
|
|
||||||
for (i=0; i < seg_dof; ++i,++p_it) {
|
|
||||||
*p_it = seg_it->ActiveAngle(i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// solve the thing
|
|
||||||
int iterations(0);
|
|
||||||
MT_Scalar return_value(0);
|
|
||||||
|
|
||||||
m_grad_solver->Solve(
|
|
||||||
p,
|
|
||||||
tolerance,
|
|
||||||
iterations,
|
|
||||||
return_value,
|
|
||||||
m_potential.Ref(),
|
|
||||||
100
|
|
||||||
);
|
|
||||||
|
|
||||||
// update this chain
|
|
||||||
|
|
||||||
vector<IK_Segment>::iterator out_seg_it = chain.Segments().begin();
|
|
||||||
TNT::Vector<MT_Scalar>::const_iterator p_cit = p.begin();
|
|
||||||
|
|
||||||
const int chain_dof = chain.DoF();
|
|
||||||
int n;
|
|
||||||
|
|
||||||
for (n=0; n < chain_dof;out_seg_it ++) {
|
|
||||||
n += out_seg_it->SetAngles(p_cit + n);
|
|
||||||
}
|
|
||||||
|
|
||||||
chain.UpdateGlobalTransformations();
|
|
||||||
chain.ComputeJacobian();
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
IK_CGChainSolver::
|
|
||||||
~IK_CGChainSolver(
|
|
||||||
){
|
|
||||||
//nothing to do
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
IK_CGChainSolver::
|
|
||||||
IK_CGChainSolver(
|
|
||||||
){
|
|
||||||
//nothing to do;
|
|
||||||
};
|
|
||||||
|
|
@ -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 *****
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifdef HAVE_CONFIG_H
|
|
||||||
#include <config.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "IK_Chain.h"
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
IK_Chain::
|
|
||||||
IK_Chain(
|
|
||||||
)
|
|
||||||
{
|
|
||||||
// nothing to do;
|
|
||||||
};
|
|
||||||
|
|
||||||
const
|
|
||||||
vector<IK_Segment> &
|
|
||||||
IK_Chain::
|
|
||||||
Segments(
|
|
||||||
) const {
|
|
||||||
return m_segments;
|
|
||||||
};
|
|
||||||
|
|
||||||
vector<IK_Segment> &
|
|
||||||
IK_Chain::
|
|
||||||
Segments(
|
|
||||||
){
|
|
||||||
return m_segments;
|
|
||||||
};
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_Chain::
|
|
||||||
UpdateGlobalTransformations(
|
|
||||||
){
|
|
||||||
|
|
||||||
// now iterate through the segment list
|
|
||||||
// compute their local transformations if needed
|
|
||||||
|
|
||||||
// assign their global transformations
|
|
||||||
// (relative to chain origin)
|
|
||||||
|
|
||||||
vector<IK_Segment>::const_iterator s_end = m_segments.end();
|
|
||||||
vector<IK_Segment>::iterator s_it = m_segments.begin();
|
|
||||||
|
|
||||||
MT_Transform global;
|
|
||||||
|
|
||||||
global.setIdentity();
|
|
||||||
|
|
||||||
for (; s_it != s_end; ++s_it) {
|
|
||||||
s_it->UpdateGlobal(global);
|
|
||||||
global = s_it->GlobalTransform();
|
|
||||||
}
|
|
||||||
// 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();
|
|
||||||
|
|
||||||
MT_Matrix3x3 last_basis = last_t.getBasis();
|
|
||||||
last_basis.transpose();
|
|
||||||
MT_Vector3 m_end_pose = last_basis[2];
|
|
||||||
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
const
|
|
||||||
TNT::Matrix<MT_Scalar> &
|
|
||||||
IK_Chain::
|
|
||||||
Jacobian(
|
|
||||||
) const {
|
|
||||||
return m_jacobian;
|
|
||||||
} ;
|
|
||||||
|
|
||||||
|
|
||||||
const
|
|
||||||
TNT::Matrix<MT_Scalar> &
|
|
||||||
IK_Chain::
|
|
||||||
TransposedJacobian(
|
|
||||||
) const {
|
|
||||||
return m_t_jacobian;
|
|
||||||
};
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_Chain::
|
|
||||||
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_Segment>::const_iterator segs = m_segments.begin();
|
|
||||||
|
|
||||||
m_t_jacobian.newsize(dof,3);
|
|
||||||
m_jacobian.newsize(3,dof);
|
|
||||||
|
|
||||||
// compute the transposed jacobian first
|
|
||||||
|
|
||||||
int n;
|
|
||||||
int i = 0;
|
|
||||||
|
|
||||||
|
|
||||||
for (n= 0; n < num_segs; n++) {
|
|
||||||
|
|
||||||
const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis();
|
|
||||||
const MT_Vector3 &origin = segs[n].GlobalSegmentStart();
|
|
||||||
|
|
||||||
MT_Vector3 p = origin-m_end_effector;
|
|
||||||
|
|
||||||
// iterate through the active angle vectors of this segment
|
|
||||||
|
|
||||||
int angle_ind =0;
|
|
||||||
int seg_dof = segs[n].DoF();
|
|
||||||
|
|
||||||
const std::vector<MT_Vector3> & angle_vectors = segs[n].AngleVectors();
|
|
||||||
|
|
||||||
for (angle_ind = 0;angle_ind <seg_dof; angle_ind++,i++) {
|
|
||||||
|
|
||||||
MT_Vector3 angle_axis = angle_vectors[angle_ind];
|
|
||||||
|
|
||||||
MT_Vector3 a = basis * angle_axis;
|
|
||||||
MT_Vector3 pca = p.cross(a);
|
|
||||||
|
|
||||||
m_t_jacobian(i + 1,1) = pca.x();
|
|
||||||
m_t_jacobian(i + 1,2) = pca.y();
|
|
||||||
m_t_jacobian(i + 1,3) = pca.z();
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// get the origina1 jacobain
|
|
||||||
|
|
||||||
TNT::transpose(m_t_jacobian,m_jacobian);
|
|
||||||
};
|
|
||||||
|
|
||||||
MT_Vector3
|
|
||||||
IK_Chain::
|
|
||||||
EndEffector(
|
|
||||||
) const {
|
|
||||||
return m_end_effector;
|
|
||||||
};
|
|
||||||
|
|
||||||
MT_Vector3
|
|
||||||
IK_Chain::
|
|
||||||
EndPose(
|
|
||||||
) const {
|
|
||||||
return m_end_pose;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
int
|
|
||||||
IK_Chain::
|
|
||||||
DoF(
|
|
||||||
) const {
|
|
||||||
|
|
||||||
// iterate through the segs and compute the DOF
|
|
||||||
|
|
||||||
vector<IK_Segment>::const_iterator s_end = m_segments.end();
|
|
||||||
vector<IK_Segment>::const_iterator s_it = m_segments.begin();
|
|
||||||
|
|
||||||
int dof = 0;
|
|
||||||
|
|
||||||
for (;s_it != s_end; ++s_it) {
|
|
||||||
dof += s_it->DoF();
|
|
||||||
}
|
|
||||||
|
|
||||||
return dof;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,165 +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 *****
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifdef HAVE_CONFIG_H
|
|
||||||
#include <config.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "IK_ConjugateGradientSolver.h"
|
|
||||||
|
|
||||||
#define EPS 1.0e-10
|
|
||||||
|
|
||||||
IK_ConjugateGradientSolver *
|
|
||||||
IK_ConjugateGradientSolver::
|
|
||||||
New(
|
|
||||||
){
|
|
||||||
return new IK_ConjugateGradientSolver();
|
|
||||||
};
|
|
||||||
|
|
||||||
IK_ConjugateGradientSolver::
|
|
||||||
~IK_ConjugateGradientSolver(
|
|
||||||
){
|
|
||||||
//nothing to do
|
|
||||||
};
|
|
||||||
|
|
||||||
// Compute the minimum of the potenial function
|
|
||||||
// starting at point p. On return p contains the
|
|
||||||
// computed minima, iter the number of iterations performed,
|
|
||||||
// fret the potenial value at the minima
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_ConjugateGradientSolver::
|
|
||||||
Solve(
|
|
||||||
TNT::Vector<MT_Scalar> &p,
|
|
||||||
MT_Scalar ftol,
|
|
||||||
int &iter,
|
|
||||||
MT_Scalar &fret,
|
|
||||||
DifferentiablePotenialFunctionNd &potenial,
|
|
||||||
int max_its
|
|
||||||
){
|
|
||||||
|
|
||||||
int j;
|
|
||||||
MT_Scalar gg,gam,fp,dgg;
|
|
||||||
|
|
||||||
int n = potenial.Dimension();
|
|
||||||
ArmVectors(n);
|
|
||||||
|
|
||||||
// initialize --- FIXME we probably have these
|
|
||||||
// values to hand already.
|
|
||||||
|
|
||||||
fp = potenial.Evaluate(p);
|
|
||||||
potenial.Derivative(p,m_xi);
|
|
||||||
|
|
||||||
for (j = 1; j<=n; j++) {
|
|
||||||
m_g(j) = -m_xi(j);
|
|
||||||
m_xi(j) = m_h(j) = m_g(j);
|
|
||||||
}
|
|
||||||
for (iter =1;iter <= max_its; iter++) {
|
|
||||||
LineMinimize(p,m_xi,fret,potenial);
|
|
||||||
|
|
||||||
if (2 *TNT::abs(fret-fp) <= ftol*(TNT::abs(fret) + TNT::abs(fp) + EPS)) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
fp = fret;
|
|
||||||
potenial.Derivative(p,m_xi);
|
|
||||||
dgg = gg = 0.0;
|
|
||||||
|
|
||||||
for (j = 1; j<=n;j++) {
|
|
||||||
gg += m_g(j)*m_g(j);
|
|
||||||
//dgg+= xi(j)*xi(j);
|
|
||||||
dgg += (m_xi(j) + m_g(j))*m_xi(j);
|
|
||||||
}
|
|
||||||
if (gg == 0.0) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
gam = dgg/gg;
|
|
||||||
|
|
||||||
for (j = 1; j<=n; j++) {
|
|
||||||
m_g(j) = -m_xi(j);
|
|
||||||
m_xi(j) = m_h(j) = m_g(j) + gam*m_h(j);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// FIXME throw exception
|
|
||||||
//assert(false);
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
IK_ConjugateGradientSolver::
|
|
||||||
IK_ConjugateGradientSolver(
|
|
||||||
){
|
|
||||||
//nothing to do
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_ConjugateGradientSolver::
|
|
||||||
ArmVectors(
|
|
||||||
int dimension
|
|
||||||
){
|
|
||||||
m_g.newsize(dimension);
|
|
||||||
m_h.newsize(dimension);
|
|
||||||
m_xi.newsize(dimension);
|
|
||||||
m_xi_temp.newsize(dimension);
|
|
||||||
};
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_ConjugateGradientSolver::
|
|
||||||
LineMinimize(
|
|
||||||
TNT::Vector<MT_Scalar> & p,
|
|
||||||
const TNT::Vector<MT_Scalar> & xi,
|
|
||||||
MT_Scalar &fret,
|
|
||||||
DifferentiablePotenialFunctionNd &potenial
|
|
||||||
){
|
|
||||||
MT_Scalar ax(0),bx(1); // initial bracket guess
|
|
||||||
MT_Scalar cx,fa,fb,fc;
|
|
||||||
|
|
||||||
MT_Scalar xmin(0); // the 1d function minima
|
|
||||||
|
|
||||||
potenial.SetLineVector(p,xi);
|
|
||||||
IK_LineMinimizer::InitialBracket1d(ax,bx,cx,fa,fb,fc,potenial);
|
|
||||||
fret = IK_LineMinimizer::DerivativeBrent1d(ax,bx,cx,potenial,xmin,0.001);
|
|
||||||
|
|
||||||
// x_min in minimum along line and corresponds with
|
|
||||||
// p[] + x_min *xi[]
|
|
||||||
|
|
||||||
TNT::vectorscale(m_xi_temp,xi,xmin);
|
|
||||||
TNT::vectoradd(p,m_xi_temp);
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
#undef EPS
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,270 +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 *****
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifdef HAVE_CONFIG_H
|
|
||||||
#include <config.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "IK_JacobianSolver.h"
|
|
||||||
#include "TNT/svd.h"
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
IK_JacobianSolver *
|
|
||||||
IK_JacobianSolver::
|
|
||||||
New(
|
|
||||||
){
|
|
||||||
return new IK_JacobianSolver();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool
|
|
||||||
IK_JacobianSolver::
|
|
||||||
Solve(
|
|
||||||
IK_Chain &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
|
|
||||||
){
|
|
||||||
|
|
||||||
ArmMatrices(chain.DoF());
|
|
||||||
|
|
||||||
for (int iterations = 0; iterations < max_iterations; iterations++) {
|
|
||||||
|
|
||||||
|
|
||||||
MT_Vector3 end_effector = chain.EndEffector();
|
|
||||||
|
|
||||||
MT_Vector3 d_pos = g_position - end_effector;
|
|
||||||
const MT_Scalar x_length = d_pos.length();
|
|
||||||
|
|
||||||
if (x_length < tolerance) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
chain.ComputeJacobian();
|
|
||||||
ComputeInverseJacobian(chain,x_length,max_angle_change);
|
|
||||||
|
|
||||||
ComputeBetas(chain,d_pos);
|
|
||||||
UpdateChain(chain);
|
|
||||||
chain.UpdateGlobalTransformations();
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
|
||||||
};
|
|
||||||
|
|
||||||
IK_JacobianSolver::
|
|
||||||
~IK_JacobianSolver(
|
|
||||||
){
|
|
||||||
// nothing to do
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
IK_JacobianSolver::
|
|
||||||
IK_JacobianSolver(
|
|
||||||
){
|
|
||||||
// nothing to do
|
|
||||||
};
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_JacobianSolver::
|
|
||||||
ComputeBetas(
|
|
||||||
IK_Chain &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_JacobianSolver::
|
|
||||||
ComputeInverseJacobian(
|
|
||||||
IK_Chain &chain,
|
|
||||||
const MT_Scalar x_length,
|
|
||||||
const MT_Scalar max_angle_change
|
|
||||||
) {
|
|
||||||
|
|
||||||
int dimension = 0;
|
|
||||||
|
|
||||||
m_svd_u = 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 = 0;
|
|
||||||
m_svd_v = 0;
|
|
||||||
|
|
||||||
TNT::SVD_a(m_svd_u,m_svd_w,m_svd_v);
|
|
||||||
|
|
||||||
// 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 = 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 = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
lambda *= lambda;
|
|
||||||
|
|
||||||
for (i= 0; i < m_svd_w.size(); i++) {
|
|
||||||
if (m_svd_w[i] < epsilon) {
|
|
||||||
m_svd_w[i] = 0;
|
|
||||||
} else {
|
|
||||||
m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
m_svd_w_diag.diagonal(m_svd_w);
|
|
||||||
|
|
||||||
// FIXME optimize these matrix multiplications
|
|
||||||
// using the fact that m_svd_w_diag is diagonal!
|
|
||||||
|
|
||||||
TNT::matmult(m_svd_temp1,m_svd_w_diag,m_svd_u_t);
|
|
||||||
TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1);
|
|
||||||
return dimension;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_JacobianSolver::
|
|
||||||
UpdateChain(
|
|
||||||
IK_Chain &chain
|
|
||||||
){
|
|
||||||
|
|
||||||
// iterate through the set of angles and
|
|
||||||
// update their values from the d_thetas
|
|
||||||
|
|
||||||
int n;
|
|
||||||
vector<IK_Segment> &segs = chain.Segments();
|
|
||||||
|
|
||||||
int chain_dof = chain.DoF();
|
|
||||||
int seg_ind = 0;
|
|
||||||
for (n=0; n < chain_dof;seg_ind ++) {
|
|
||||||
n += segs[seg_ind].IncrementAngles(m_d_theta.begin() + n);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_JacobianSolver::
|
|
||||||
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_u = 0;
|
|
||||||
m_svd_v = 0;
|
|
||||||
m_svd_w = 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);
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,281 +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 *****
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifdef HAVE_CONFIG_H
|
|
||||||
#include <config.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "IK_Segment.h"
|
|
||||||
|
|
||||||
|
|
||||||
IK_Segment::
|
|
||||||
IK_Segment (
|
|
||||||
const MT_Point3 tr1,
|
|
||||||
const MT_Matrix3x3 A,
|
|
||||||
const MT_Scalar length,
|
|
||||||
const bool pitch_on,
|
|
||||||
const bool yaw_on,
|
|
||||||
const bool role_on
|
|
||||||
){
|
|
||||||
m_transform.setOrigin(tr1);
|
|
||||||
m_transform.setBasis(A);
|
|
||||||
m_angles[0] =MT_Scalar(0);
|
|
||||||
m_angles[1] =MT_Scalar(0);
|
|
||||||
m_angles[2] =MT_Scalar(0);
|
|
||||||
|
|
||||||
m_active_angles[0] = role_on;
|
|
||||||
m_active_angles[1] = yaw_on;
|
|
||||||
m_active_angles[2] = pitch_on;
|
|
||||||
m_length = length;
|
|
||||||
|
|
||||||
if (role_on) {
|
|
||||||
m_angle_vectors.push_back(MT_Vector3(1,0,0));
|
|
||||||
}
|
|
||||||
if (yaw_on) {
|
|
||||||
m_angle_vectors.push_back(MT_Vector3(0,1,0));
|
|
||||||
}
|
|
||||||
if (pitch_on) {
|
|
||||||
m_angle_vectors.push_back(MT_Vector3(0,0,1));
|
|
||||||
}
|
|
||||||
UpdateLocalTransform();
|
|
||||||
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
IK_Segment::
|
|
||||||
IK_Segment (
|
|
||||||
) {
|
|
||||||
m_transform.setIdentity();
|
|
||||||
|
|
||||||
m_angles[0] =MT_Scalar(0);
|
|
||||||
m_angles[1] =MT_Scalar(0);
|
|
||||||
m_angles[2] =MT_Scalar(0);
|
|
||||||
|
|
||||||
m_active_angles[0] = false;
|
|
||||||
m_active_angles[1] = false;
|
|
||||||
m_active_angles[2] = false;
|
|
||||||
m_length = MT_Scalar(1);
|
|
||||||
|
|
||||||
UpdateLocalTransform();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// accessors
|
|
||||||
////////////
|
|
||||||
|
|
||||||
// The length of the segment
|
|
||||||
|
|
||||||
const
|
|
||||||
MT_Scalar
|
|
||||||
IK_Segment::
|
|
||||||
Length(
|
|
||||||
) const {
|
|
||||||
return m_length;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// This is the transform from adjacent
|
|
||||||
// coordinate systems in the chain.
|
|
||||||
|
|
||||||
const
|
|
||||||
MT_Transform &
|
|
||||||
IK_Segment::
|
|
||||||
LocalTransform(
|
|
||||||
) const {
|
|
||||||
return m_local_transform;
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_Segment::
|
|
||||||
UpdateGlobal(
|
|
||||||
const MT_Transform & global
|
|
||||||
){
|
|
||||||
|
|
||||||
// compute the global transform
|
|
||||||
// and the start of the segment in global coordinates.
|
|
||||||
|
|
||||||
m_seg_start = global * m_transform.getOrigin();
|
|
||||||
m_global_transform = global * m_local_transform;
|
|
||||||
}
|
|
||||||
|
|
||||||
const
|
|
||||||
MT_Transform &
|
|
||||||
IK_Segment::
|
|
||||||
GlobalTransform(
|
|
||||||
) const {
|
|
||||||
return m_global_transform;
|
|
||||||
}
|
|
||||||
|
|
||||||
const
|
|
||||||
MT_Vector3 &
|
|
||||||
IK_Segment::
|
|
||||||
GlobalSegmentStart(
|
|
||||||
) const{
|
|
||||||
return m_seg_start;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Return the number of Degrees of Freedom
|
|
||||||
// for this segment
|
|
||||||
|
|
||||||
int
|
|
||||||
IK_Segment::
|
|
||||||
DoF(
|
|
||||||
) const {
|
|
||||||
return
|
|
||||||
(m_active_angles[0] == true) +
|
|
||||||
(m_active_angles[1] == true) +
|
|
||||||
(m_active_angles[2] == true);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// suspect interface...
|
|
||||||
// Increment the active angles (at most 3) by
|
|
||||||
// d_theta. Which angles are incremented depends
|
|
||||||
// on which are active. It returns DoF
|
|
||||||
|
|
||||||
int
|
|
||||||
IK_Segment::
|
|
||||||
IncrementAngles(
|
|
||||||
MT_Scalar *d_theta
|
|
||||||
){
|
|
||||||
int i =0;
|
|
||||||
if (m_active_angles[0]) {
|
|
||||||
m_angles[0] += d_theta[i];
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
if (m_active_angles[1]) {
|
|
||||||
m_angles[1] += d_theta[i];
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
if (m_active_angles[2]) {
|
|
||||||
m_angles[2] += d_theta[i];
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
UpdateLocalTransform();
|
|
||||||
|
|
||||||
return i;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int
|
|
||||||
IK_Segment::
|
|
||||||
SetAngles(
|
|
||||||
const MT_Scalar *angles
|
|
||||||
){
|
|
||||||
int i =0;
|
|
||||||
if (m_active_angles[0]) {
|
|
||||||
m_angles[0] = angles[i];
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
if (m_active_angles[1]) {
|
|
||||||
m_angles[1] = angles[i];
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
if (m_active_angles[2]) {
|
|
||||||
m_angles[2] = angles[i];
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
UpdateLocalTransform();
|
|
||||||
|
|
||||||
return i;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void
|
|
||||||
IK_Segment::
|
|
||||||
UpdateLocalTransform(
|
|
||||||
){
|
|
||||||
// The local transformation is defined by
|
|
||||||
// a user defined translation and rotation followed by
|
|
||||||
// rotation by (roll,pitch,yaw) followed by
|
|
||||||
// a translation in x of m_length
|
|
||||||
|
|
||||||
MT_Quaternion rotx,roty,rotz;
|
|
||||||
|
|
||||||
rotx.setRotation(MT_Vector3(1,0,0),m_angles[0]);
|
|
||||||
roty.setRotation(MT_Vector3(0,1,0),m_angles[1]);
|
|
||||||
rotz.setRotation(MT_Vector3(0,0,1),m_angles[2]);
|
|
||||||
|
|
||||||
MT_Quaternion rot = rotx * roty *rotz;
|
|
||||||
|
|
||||||
MT_Transform rx(MT_Point3(0,0,0),rot);
|
|
||||||
|
|
||||||
MT_Transform translation;
|
|
||||||
translation.setIdentity();
|
|
||||||
translation.translate(MT_Vector3(0,m_length,0));
|
|
||||||
|
|
||||||
m_local_transform = m_transform * rx * translation;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
const
|
|
||||||
std::vector<MT_Vector3> &
|
|
||||||
IK_Segment::
|
|
||||||
AngleVectors(
|
|
||||||
) const{
|
|
||||||
return m_angle_vectors;
|
|
||||||
};
|
|
||||||
|
|
||||||
MT_Scalar
|
|
||||||
IK_Segment::
|
|
||||||
ActiveAngle(
|
|
||||||
int i
|
|
||||||
) const {
|
|
||||||
MT_assert((i >=0) && (i < DoF()));
|
|
||||||
|
|
||||||
// umm want to return the ith active angle
|
|
||||||
// and not the ith angle
|
|
||||||
|
|
||||||
int j;
|
|
||||||
int angles = -1;
|
|
||||||
for (j=0;j < 3;j++) {
|
|
||||||
if (m_active_angles[j]) angles ++;
|
|
||||||
if (i == angles) return m_angles[j];
|
|
||||||
}
|
|
||||||
return m_angles[0];
|
|
||||||
}
|
|
||||||
|
|
||||||
MT_Scalar
|
|
||||||
IK_Segment::
|
|
||||||
Angle(
|
|
||||||
int i
|
|
||||||
) const {
|
|
||||||
MT_assert((i >=0) && (i < 3));
|
|
||||||
return m_angles[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue
Block a user