forked from bartvdbraak/blender
Finished cleaning up the ik directory removing unused files.
Kent -- mein@cs.umn.edu
This commit is contained in:
parent
26aa841a1f
commit
077fd2d1de
@ -1,178 +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 *****
|
||||
*/
|
||||
|
||||
#ifndef CGChainSolver_h
|
||||
|
||||
#define CGChainSolver_h
|
||||
|
||||
/**
|
||||
* @author Laurence Bourn
|
||||
* @date 28/6/2001
|
||||
*/
|
||||
|
||||
#include "IK_ConjugateGradientSolver.h"
|
||||
#include "IK_Chain.h"
|
||||
#include "MT_Scalar.h"
|
||||
#include "TNT/vec.h"
|
||||
#include "MEM_SmartPtr.h"
|
||||
|
||||
|
||||
/**
|
||||
* This class is a concrete differentiable potenial function for
|
||||
* an IK_Chain representing the distance to the goal.
|
||||
* @warning This method of solving IK problems is not as good
|
||||
* as IK_JacobianSolver. I advise you to use that class instead.
|
||||
*/
|
||||
|
||||
class ChainPotential :
|
||||
public DifferentiablePotenialFunctionNd
|
||||
{
|
||||
public :
|
||||
|
||||
static
|
||||
ChainPotential *
|
||||
New(
|
||||
IK_Chain &chain
|
||||
);
|
||||
|
||||
// End effector goal
|
||||
|
||||
void
|
||||
SetGoal(
|
||||
const MT_Vector3 goal
|
||||
);
|
||||
|
||||
// Inherited from DifferentiablePotenialFunctionNd
|
||||
//////////////////////////////////////////////////
|
||||
|
||||
MT_Scalar
|
||||
Evaluate(
|
||||
const MT_Scalar x
|
||||
);
|
||||
|
||||
MT_Scalar
|
||||
Derivative(
|
||||
const MT_Scalar x
|
||||
);
|
||||
|
||||
MT_Scalar
|
||||
Evaluate(
|
||||
const TNT::Vector<MT_Scalar> &x
|
||||
);
|
||||
|
||||
void
|
||||
Derivative(
|
||||
const TNT::Vector<MT_Scalar> &x,
|
||||
TNT::Vector<MT_Scalar> &dy
|
||||
);
|
||||
|
||||
// return the dimension of the domain of the potenial
|
||||
// function
|
||||
|
||||
int
|
||||
Dimension(
|
||||
) const {
|
||||
return m_dimension;
|
||||
}
|
||||
|
||||
~ChainPotential(
|
||||
){
|
||||
};
|
||||
|
||||
private :
|
||||
|
||||
MT_Scalar
|
||||
DistancePotential(
|
||||
MT_Vector3 pos,
|
||||
MT_Vector3 goal
|
||||
) const;
|
||||
|
||||
void
|
||||
DistanceGradient(
|
||||
MT_Vector3 pos,
|
||||
MT_Vector3 goal
|
||||
);
|
||||
|
||||
ChainPotential(
|
||||
IK_Chain & chain
|
||||
) :
|
||||
DifferentiablePotenialFunctionNd(),
|
||||
m_chain(chain),
|
||||
m_t_chain(chain),
|
||||
m_dimension (chain.Segments().size())
|
||||
{
|
||||
};
|
||||
|
||||
MT_Vector3 m_goal;
|
||||
TNT::Vector<MT_Scalar> m_distance_grad;
|
||||
TNT::Vector<MT_Scalar> m_angle_grad;
|
||||
TNT::Vector<MT_Scalar> m_temp_pos;
|
||||
TNT::Vector<MT_Scalar> m_temp_grad;
|
||||
|
||||
TNT::Vector<MT_Scalar> m_original_pos;
|
||||
int m_dimension;
|
||||
|
||||
IK_Chain &m_chain;
|
||||
IK_Chain m_t_chain; // deep copy
|
||||
|
||||
};
|
||||
|
||||
|
||||
class IK_CGChainSolver : public MEM_NonCopyable
|
||||
{
|
||||
public :
|
||||
|
||||
|
||||
static
|
||||
IK_CGChainSolver *
|
||||
New(
|
||||
);
|
||||
|
||||
bool
|
||||
Solve(
|
||||
IK_Chain & chain,
|
||||
MT_Vector3 new_position,
|
||||
MT_Scalar tolerance
|
||||
);
|
||||
|
||||
~IK_CGChainSolver();
|
||||
|
||||
private :
|
||||
|
||||
IK_CGChainSolver(
|
||||
);
|
||||
|
||||
MEM_SmartPtr<ChainPotential> m_potential;
|
||||
MEM_SmartPtr<IK_ConjugateGradientSolver> m_grad_solver;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -1,194 +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 *****
|
||||
*/
|
||||
|
||||
#ifndef NAN_INCLUDED_IK_Chain_h
|
||||
|
||||
#define NAN_INCLUDED_IK_Chain_h
|
||||
|
||||
/**
|
||||
* @author Laurence Bourn
|
||||
* @date 28/6/2001
|
||||
*/
|
||||
|
||||
#include "IK_Segment.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 chain's cannot share instances of
|
||||
* IK_Segments.
|
||||
*
|
||||
* You have full control of which segments form the chain via the
|
||||
* the std::vector routines.
|
||||
*/
|
||||
class IK_Chain{
|
||||
|
||||
public :
|
||||
|
||||
/**
|
||||
* Construct a IK_Chain with no segments.
|
||||
*/
|
||||
|
||||
IK_Chain(
|
||||
);
|
||||
|
||||
// IK_Chains also have the default copy constructors
|
||||
// available.
|
||||
|
||||
/**
|
||||
* Const access to the array of segments
|
||||
* comprising the IK_Chain. Used for rendering
|
||||
* etc
|
||||
* @return a vector of segments
|
||||
*/
|
||||
|
||||
const
|
||||
std::vector<IK_Segment> &
|
||||
Segments(
|
||||
) const ;
|
||||
|
||||
|
||||
/**
|
||||
* Full access to segments used to initialize
|
||||
* the IK_Chain and manipulate the segments.
|
||||
* Use the push_back() method of std::vector to add
|
||||
* segments in order to the chain
|
||||
*/
|
||||
|
||||
std::vector<IK_Segment> &
|
||||
Segments(
|
||||
);
|
||||
|
||||
|
||||
/**
|
||||
* Force the IK_Chain 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 reference to the last computed jacobian matrix
|
||||
*/
|
||||
|
||||
const
|
||||
TNT::Matrix<MT_Scalar> &
|
||||
Jacobian(
|
||||
) const ;
|
||||
|
||||
/**
|
||||
* @return A reference to the last computed transposed jacobian matrix
|
||||
*/
|
||||
|
||||
const
|
||||
TNT::Matrix<MT_Scalar> &
|
||||
TransposedJacobian(
|
||||
) const ;
|
||||
|
||||
/**
|
||||
* Count the degrees of freedom in the IK_Chain
|
||||
* @warning store this value rather than using this function
|
||||
* as the termination value of a for loop etc.
|
||||
*/
|
||||
|
||||
int
|
||||
DoF(
|
||||
) const;
|
||||
|
||||
|
||||
private :
|
||||
|
||||
/// The vector of segments comprising the chain
|
||||
std::vector<IK_Segment> m_segments;
|
||||
|
||||
/// The jacobain of the IK_Chain
|
||||
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
|
||||
|
@ -1,194 +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 *****
|
||||
*/
|
||||
|
||||
#ifndef NAN_INCLUDED_IK_ConjugateGradientSolver_h
|
||||
|
||||
#define NAN_INCLUDED_IK_ConjugateGradientSolver_h
|
||||
|
||||
/**
|
||||
* @author Laurence Bourn
|
||||
* @date 28/6/2001
|
||||
*/
|
||||
|
||||
#include "TNT/cmat.h"
|
||||
#include "MT_Scalar.h"
|
||||
#include "IK_LineMinimizer.h"
|
||||
|
||||
/**
|
||||
* These classes locally minimize n dimensional potenial functions.
|
||||
* See Numerical Recipes in C www.nr.com for more details.
|
||||
* If a n dimensionable potenial function is
|
||||
* differentiable, then it is diferentiable along
|
||||
* any vector x. This can be found by the dot product
|
||||
* of the gradient operator with x.
|
||||
* The conjugate gradient solver boils down to
|
||||
* a collection of line minimizations along various lines
|
||||
* defined by position,direction pairs. There are
|
||||
* methods in this class to set the lines along which
|
||||
* minimizations via the DiffentiablePotenialFunction1d interface
|
||||
* are to be performed.
|
||||
*
|
||||
* @warning I don't like data inheritance but it is the most efficient
|
||||
* wasy to do this here.
|
||||
*/
|
||||
|
||||
|
||||
class DifferentiablePotenialFunctionNd
|
||||
: public DifferentiablePotenialFunction1d
|
||||
{
|
||||
public :
|
||||
|
||||
/**
|
||||
* Inherited from DiffentiablePotenialFunction1d
|
||||
*
|
||||
* virtual
|
||||
* MT_Scalar
|
||||
* Evaluate1d(
|
||||
* MT_Scalar x
|
||||
* ) = 0;
|
||||
*
|
||||
* virtual
|
||||
* MT_Scalar
|
||||
* Derivative1d(
|
||||
* MT_Scalar x
|
||||
* ) = 0;
|
||||
*/
|
||||
|
||||
/// Methods to set the current line in N dimensions
|
||||
|
||||
void
|
||||
SetLineVector(
|
||||
const TNT::Vector<MT_Scalar> &pos,
|
||||
const TNT::Vector<MT_Scalar> &dir
|
||||
){
|
||||
m_line_pos = pos;
|
||||
m_line_dir = dir;
|
||||
};
|
||||
|
||||
virtual
|
||||
MT_Scalar
|
||||
Evaluate(
|
||||
const TNT::Vector<MT_Scalar> &x
|
||||
) = 0;
|
||||
|
||||
virtual
|
||||
void
|
||||
Derivative(
|
||||
const TNT::Vector<MT_Scalar> &x,
|
||||
TNT::Vector<MT_Scalar> &dy
|
||||
) = 0;
|
||||
|
||||
/// @return The dimension of the domain of the potenial function
|
||||
|
||||
virtual
|
||||
int
|
||||
Dimension(
|
||||
) const =0;
|
||||
|
||||
virtual
|
||||
~DifferentiablePotenialFunctionNd(
|
||||
){
|
||||
};
|
||||
|
||||
protected :
|
||||
|
||||
DifferentiablePotenialFunctionNd(){};
|
||||
|
||||
TNT::Vector<MT_Scalar> m_line_pos;
|
||||
TNT::Vector<MT_Scalar> m_line_dir;
|
||||
|
||||
};
|
||||
|
||||
|
||||
class IK_ConjugateGradientSolver
|
||||
: public MEM_NonCopyable
|
||||
{
|
||||
public :
|
||||
|
||||
/**
|
||||
* This class necessarily needs some (potenially large)
|
||||
* temporary vectors to aid computation. We therefore
|
||||
* insist creation of these objects on the heap.
|
||||
*/
|
||||
|
||||
static
|
||||
IK_ConjugateGradientSolver *
|
||||
New(
|
||||
);
|
||||
|
||||
/**
|
||||
* 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
|
||||
Solve(
|
||||
TNT::Vector<MT_Scalar> &p,
|
||||
MT_Scalar ftol,
|
||||
int &iter,
|
||||
MT_Scalar &fret,
|
||||
DifferentiablePotenialFunctionNd &potenial,
|
||||
int max_its = 200
|
||||
);
|
||||
|
||||
~IK_ConjugateGradientSolver(
|
||||
);
|
||||
|
||||
private :
|
||||
void
|
||||
LineMinimize(
|
||||
TNT::Vector<MT_Scalar> & p,
|
||||
const TNT::Vector<MT_Scalar> & xi,
|
||||
MT_Scalar &fret,
|
||||
DifferentiablePotenialFunctionNd &potenial
|
||||
);
|
||||
|
||||
IK_ConjugateGradientSolver(
|
||||
);
|
||||
|
||||
void
|
||||
ArmVectors(
|
||||
int dimension
|
||||
);
|
||||
|
||||
|
||||
TNT::Vector<MT_Scalar> m_g;
|
||||
TNT::Vector<MT_Scalar> m_h;
|
||||
TNT::Vector<MT_Scalar> m_xi;
|
||||
|
||||
TNT::Vector<MT_Scalar> m_xi_temp;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -1,166 +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 *****
|
||||
*/
|
||||
|
||||
#ifndef NAN_INCLUDED_IK_JacobianSolver_h
|
||||
|
||||
#define NAN_INCLUDED_IK_JacobianSolver_h
|
||||
|
||||
/**
|
||||
* @author Laurence Bourn
|
||||
* @date 28/6/2001
|
||||
*/
|
||||
|
||||
#include "TNT/cmat.h"
|
||||
#include <vector>
|
||||
#include "MT_Vector3.h"
|
||||
#include "IK_Chain.h"
|
||||
|
||||
class IK_JacobianSolver {
|
||||
|
||||
public :
|
||||
|
||||
/**
|
||||
* Create a new IK_JacobianSolver on the heap
|
||||
* @return A newly created IK_JacobianSolver you take ownership of the object
|
||||
* and responsibility for deleting it
|
||||
*/
|
||||
|
||||
|
||||
static
|
||||
IK_JacobianSolver *
|
||||
New(
|
||||
);
|
||||
|
||||
/**
|
||||
* 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 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_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
|
||||
);
|
||||
|
||||
~IK_JacobianSolver(
|
||||
);
|
||||
|
||||
|
||||
private :
|
||||
|
||||
/**
|
||||
* Private constructor to force use of New()
|
||||
*/
|
||||
|
||||
IK_JacobianSolver(
|
||||
);
|
||||
|
||||
|
||||
/**
|
||||
* Compute the inverse jacobian matrix of the chain.
|
||||
* Uses a damped least squares solution when the matrix is
|
||||
* is approaching singularity
|
||||
*/
|
||||
|
||||
int
|
||||
ComputeInverseJacobian(
|
||||
IK_Chain &chain,
|
||||
const MT_Scalar x_length,
|
||||
const MT_Scalar max_angle_change
|
||||
);
|
||||
|
||||
void
|
||||
ComputeBetas(
|
||||
IK_Chain &chain,
|
||||
const MT_Vector3 d_pos
|
||||
);
|
||||
|
||||
/**
|
||||
* Updates the angles of the chain with the newly
|
||||
* computed values
|
||||
**/
|
||||
|
||||
void
|
||||
UpdateChain(
|
||||
IK_Chain &chain
|
||||
);
|
||||
|
||||
/**
|
||||
* Make sure all the matrices are of the correct size
|
||||
**/
|
||||
|
||||
void
|
||||
ArmMatrices(
|
||||
int dof
|
||||
);
|
||||
|
||||
|
||||
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 contraint gradients for each angle.
|
||||
TNT::Vector<MT_Scalar> m_dh;
|
||||
|
||||
/// Space required for SVD computation
|
||||
|
||||
TNT::Vector<MT_Scalar> m_svd_w;
|
||||
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;
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -1,297 +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 *****
|
||||
*/
|
||||
|
||||
#ifndef NAN_INCLUDED_IK_LineMinimizer_h
|
||||
|
||||
#define NAN_INCLUDED_IK_LineMinimizer_h
|
||||
|
||||
/**
|
||||
* @author Laurence Bourn
|
||||
* @date 28/6/2001
|
||||
*/
|
||||
|
||||
#include "MT_Scalar.h"
|
||||
#include <vector>
|
||||
#include "TNT/tntmath.h"
|
||||
#include "MEM_NonCopyable.h"
|
||||
|
||||
|
||||
|
||||
#define GOLD 1.618034
|
||||
#define GLIMIT 100.0
|
||||
#define TINY 1.0e-20
|
||||
#define ZEPS 1.0e-10
|
||||
|
||||
/**
|
||||
* Routines for line - minization in n dimensions
|
||||
* these should be templated on the potenial function
|
||||
* p instead of using a virtual class. Please see
|
||||
* numerical recipes in c for more details. www.nr.com
|
||||
*/
|
||||
|
||||
class DifferentiablePotenialFunction1d {
|
||||
public :
|
||||
virtual
|
||||
MT_Scalar
|
||||
Evaluate(
|
||||
const MT_Scalar x
|
||||
) = 0;
|
||||
|
||||
virtual
|
||||
MT_Scalar
|
||||
Derivative(
|
||||
const MT_Scalar x
|
||||
) = 0;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* TODO get rid of this class and make some
|
||||
* template functions in a seperate namespace
|
||||
*/
|
||||
|
||||
class IK_LineMinimizer : public MEM_NonCopyable {
|
||||
|
||||
public :
|
||||
|
||||
/**
|
||||
* Before we proceed with line minimization
|
||||
* we need to construct an initial bracket
|
||||
* of a minima of the PotenialFunction
|
||||
*/
|
||||
|
||||
static
|
||||
void
|
||||
InitialBracket1d (
|
||||
MT_Scalar &ax,
|
||||
MT_Scalar &bx,
|
||||
MT_Scalar &cx,
|
||||
MT_Scalar &fa,
|
||||
MT_Scalar &fb,
|
||||
MT_Scalar &fc,
|
||||
DifferentiablePotenialFunction1d &potenial
|
||||
) {
|
||||
MT_Scalar ulim,u,r,q,fu;
|
||||
|
||||
fa = potenial.Evaluate(ax);
|
||||
fb = potenial.Evaluate(bx);
|
||||
|
||||
if (fb > fa) {
|
||||
std::swap(ax,bx);
|
||||
std::swap(fa,fb);
|
||||
}
|
||||
cx = bx + GOLD*(bx-ax);
|
||||
fc = potenial.Evaluate(cx);
|
||||
|
||||
while (fb > fc) {
|
||||
|
||||
r = (bx - ax) * (fb - fc);
|
||||
q = (bx - cx) * (fb - fa);
|
||||
u = bx - ((bx - cx)*q - (bx - ax) *r)/
|
||||
(2 * TNT::sign(TNT::max(TNT::abs(q-r),TINY),q-r));
|
||||
ulim = bx + GLIMIT*(cx-bx);
|
||||
|
||||
if ((bx-u)*(u-cx) > 0.0) {
|
||||
fu = potenial.Evaluate(u);
|
||||
if (fu < fc) {
|
||||
ax = bx;
|
||||
bx = u;
|
||||
fa = fb;
|
||||
fb = fu;
|
||||
return;
|
||||
} else if (fu > fb) {
|
||||
cx = u;
|
||||
fc = fu;
|
||||
return;
|
||||
}
|
||||
u = cx + GOLD*(cx-bx);
|
||||
fu = potenial.Evaluate(u);
|
||||
|
||||
} else if ((cx - u)*(u - ulim) > 0.0) {
|
||||
fu = potenial.Evaluate(u);
|
||||
|
||||
if (fu < fc) {
|
||||
bx = cx;
|
||||
cx = u;
|
||||
u = cx + GOLD*(cx - bx);
|
||||
fb = fc;
|
||||
fc = fu;
|
||||
fu = potenial.Evaluate(u);
|
||||
}
|
||||
} else if ((u-ulim)*(ulim-cx) >=0.0) {
|
||||
u = ulim;
|
||||
fu = potenial.Evaluate(u);
|
||||
} else {
|
||||
u = cx + GOLD*(cx-bx);
|
||||
fu = potenial.Evaluate(u);
|
||||
}
|
||||
ax = bx;
|
||||
bx = cx;
|
||||
cx = u;
|
||||
fa = fb;
|
||||
fb = fc;
|
||||
fc = fu;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* This is a 1 dimensional brent method for
|
||||
* line-minization with derivatives
|
||||
*/
|
||||
|
||||
|
||||
static
|
||||
MT_Scalar
|
||||
DerivativeBrent1d(
|
||||
MT_Scalar ax,
|
||||
MT_Scalar bx,
|
||||
MT_Scalar cx,
|
||||
DifferentiablePotenialFunction1d &potenial,
|
||||
MT_Scalar &x_min,
|
||||
const MT_Scalar tol,
|
||||
int max_iter = 100
|
||||
) {
|
||||
int iter;
|
||||
bool ok1,ok2;
|
||||
MT_Scalar a,b,d,d1,d2,du,dv,dw,dx,e(0);
|
||||
MT_Scalar fu,fv,fw,fx,olde,tol1,tol2,u,u1,u2,v,w,x,xm;
|
||||
|
||||
a = (ax < cx ? ax : cx);
|
||||
b = (ax > cx ? ax : cx);
|
||||
x = w = v = bx;
|
||||
fw = fv = fx = potenial.Evaluate(x);
|
||||
dw = dv = dx = potenial.Derivative(x);
|
||||
|
||||
for (iter = 1; iter <= max_iter; iter++) {
|
||||
xm = 0.5*(a+b);
|
||||
tol1 = tol*fabs(x) + ZEPS;
|
||||
tol2 = 2 * tol1;
|
||||
if (fabs(x - xm) <= (tol2 - 0.5*(b-a))) {
|
||||
x_min = x;
|
||||
return fx;
|
||||
}
|
||||
|
||||
if (fabs(e) > tol1) {
|
||||
d1 = 2*(b-a);
|
||||
d2 = d1;
|
||||
if (dw != dx) {
|
||||
d1 = (w-x)*dx/(dx-dw);
|
||||
}
|
||||
if (dv != dx) {
|
||||
d2 = (v-x)*dx/(dx-dv);
|
||||
}
|
||||
|
||||
u1 = x+d1;
|
||||
u2 = x+d2;
|
||||
ok1 = ((a-u1)*(u1-b) > 0.0) && ((dx*d1) <= 0.0);
|
||||
ok2 = ((a-u2)*(u2-b) > 0.0) && ((dx*d2) <= 0.0);
|
||||
olde = e;
|
||||
e = d;
|
||||
|
||||
if (ok1 || ok2) {
|
||||
if (ok1 && ok2) {
|
||||
d = fabs(d1) < fabs(d2) ? d1 : d2;
|
||||
} else if (ok1) {
|
||||
d = d1;
|
||||
} else {
|
||||
d = d2;
|
||||
}
|
||||
if (fabs(d) <= fabs(0.5*olde)) {
|
||||
u = x+d;
|
||||
if ((u-a < tol2) || (b-u < tol2)) {
|
||||
d = TNT::sign(tol1,xm-x);
|
||||
}
|
||||
} else {
|
||||
d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
|
||||
}
|
||||
} else {
|
||||
d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
|
||||
}
|
||||
} else {
|
||||
d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
|
||||
}
|
||||
|
||||
if (fabs(d) >= tol1) {
|
||||
u = x+d;
|
||||
fu = potenial.Evaluate(u);
|
||||
} else {
|
||||
u = x + TNT::sign(tol1,d);
|
||||
fu = potenial.Evaluate(u);
|
||||
if (fu > fx) {
|
||||
x_min = x;
|
||||
return fx;
|
||||
}
|
||||
}
|
||||
du = potenial.Derivative(u);
|
||||
if (fu <= fx) {
|
||||
if (u >= x) {
|
||||
a = x;
|
||||
} else {
|
||||
b = x;
|
||||
}
|
||||
v = w; fv = fw; dv = dw;
|
||||
w = x; fw = fx; dw = dx;
|
||||
x = u; fx = fu; dx = du;
|
||||
} else {
|
||||
if (u < x) {
|
||||
a = u;
|
||||
} else {
|
||||
b = u;
|
||||
}
|
||||
if (fu <= fw || w == x) {
|
||||
v = w; fv = fw; dv = dw;
|
||||
w = u; fw = fu; dw = du;
|
||||
} else if ( fu < fv || v == x || v == w) {
|
||||
v = u; fv = fu; dv = du;
|
||||
}
|
||||
}
|
||||
}
|
||||
// FIXME throw exception
|
||||
|
||||
assert(false);
|
||||
return MT_Scalar(0);
|
||||
};
|
||||
|
||||
private :
|
||||
|
||||
/// This class just contains static helper methods so no instantiation
|
||||
|
||||
IK_LineMinimizer();
|
||||
|
||||
};
|
||||
|
||||
#undef GOLD
|
||||
#undef GLIMIT
|
||||
#undef TINY
|
||||
#undef ZEPS
|
||||
|
||||
#endif
|
||||
|
@ -1,218 +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 *****
|
||||
*/
|
||||
|
||||
#ifndef NAN_INCLUDED_Segment_h
|
||||
|
||||
#define NAN_INCLUDED_Segment_h
|
||||
|
||||
/**
|
||||
* @author Laurence Bourn
|
||||
* @date 28/6/2001
|
||||
*/
|
||||
|
||||
|
||||
#include "MT_Vector3.h"
|
||||
#include "MT_Transform.h"
|
||||
#include <vector>
|
||||
|
||||
class IK_Segment {
|
||||
|
||||
public :
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
* @warning This class uses axis angles for it's parameterization.
|
||||
* Axis angles are a poor representation for joints of more than 1 DOF
|
||||
* because they suffer from Gimbal lock. This becomes noticeable in
|
||||
* IK solutions. A better solution is to do use a quaternion to represent
|
||||
* angles with 3 DOF
|
||||
*/
|
||||
|
||||
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
|
||||
);
|
||||
|
||||
|
||||
IK_Segment(
|
||||
);
|
||||
|
||||
|
||||
/**
|
||||
* @return The length of the segment
|
||||
*/
|
||||
|
||||
const
|
||||
MT_Scalar
|
||||
Length(
|
||||
) const ;
|
||||
|
||||
/**
|
||||
* @return The transform from adjacent
|
||||
* coordinate systems in the chain.
|
||||
*/
|
||||
|
||||
const
|
||||
MT_Transform &
|
||||
LocalTransform(
|
||||
) const ;
|
||||
|
||||
|
||||
/**
|
||||
* Get the segment to compute it's
|
||||
* global transform given the global transform
|
||||
* of the parent. This method also updtes the
|
||||
* global segment start
|
||||
*/
|
||||
|
||||
void
|
||||
UpdateGlobal(
|
||||
const MT_Transform & global
|
||||
);
|
||||
|
||||
/**
|
||||
* @return A const reference to the global trnasformation
|
||||
*/
|
||||
|
||||
const
|
||||
MT_Transform &
|
||||
GlobalTransform(
|
||||
) const;
|
||||
|
||||
/**
|
||||
* @return A const Reference to start of segment in global
|
||||
* coordinates
|
||||
*/
|
||||
|
||||
const
|
||||
MT_Vector3 &
|
||||
GlobalSegmentStart(
|
||||
) const;
|
||||
|
||||
/**
|
||||
* Computes the number of degrees of freedom of this segment
|
||||
*/
|
||||
|
||||
int
|
||||
DoF(
|
||||
) const;
|
||||
|
||||
|
||||
/**
|
||||
* Increment the active angles (at most DoF()) by
|
||||
* d_theta. Which angles are incremented depends
|
||||
* on which are active.
|
||||
* @return DoF()
|
||||
* @warning Bad interface
|
||||
*/
|
||||
|
||||
int
|
||||
IncrementAngles(
|
||||
MT_Scalar *d_theta
|
||||
);
|
||||
|
||||
|
||||
// FIXME - interface bloat
|
||||
|
||||
/**
|
||||
* @return the vectors about which the active
|
||||
* angles operate
|
||||
*/
|
||||
|
||||
const
|
||||
std::vector<MT_Vector3> &
|
||||
AngleVectors(
|
||||
) const;
|
||||
|
||||
/**
|
||||
* @return the ith active angle
|
||||
*/
|
||||
|
||||
MT_Scalar
|
||||
ActiveAngle(
|
||||
int i
|
||||
) const;
|
||||
|
||||
/**
|
||||
* @return the ith angle
|
||||
*/
|
||||
MT_Scalar
|
||||
Angle(
|
||||
int i
|
||||
) const;
|
||||
|
||||
|
||||
/**
|
||||
* Set the active angles from the array
|
||||
* @return the number of active angles
|
||||
*/
|
||||
|
||||
int
|
||||
SetAngles(
|
||||
const MT_Scalar *angles
|
||||
);
|
||||
|
||||
|
||||
private :
|
||||
|
||||
void
|
||||
UpdateLocalTransform(
|
||||
);
|
||||
|
||||
|
||||
|
||||
private :
|
||||
|
||||
/** The user defined transformation, composition of the
|
||||
* translation and rotation from constructor.
|
||||
*/
|
||||
|
||||
MT_Transform m_transform;
|
||||
MT_Scalar m_angles[3];
|
||||
MT_Scalar m_length;
|
||||
|
||||
MT_Transform m_local_transform;
|
||||
MT_Transform m_global_transform;
|
||||
|
||||
bool m_active_angles[3];
|
||||
|
||||
MT_Vector3 m_seg_start;
|
||||
|
||||
std::vector<MT_Vector3> m_angle_vectors;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -1,94 +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 *****
|
||||
*/
|
||||
|
||||
#ifndef NAN_INCLUDED_IK_Solver_Class
|
||||
|
||||
#define NAN_INCLUDED_IK_Solver_Class
|
||||
|
||||
#include "IK_Chain.h"
|
||||
#include "IK_JacobianSolver.h"
|
||||
#include "IK_Segment.h"
|
||||
#include "MEM_SmartPtr.h"
|
||||
|
||||
class IK_Solver_Class {
|
||||
|
||||
public :
|
||||
|
||||
static
|
||||
IK_Solver_Class *
|
||||
New(
|
||||
){
|
||||
MEM_SmartPtr<IK_Solver_Class> output (new IK_Solver_Class);
|
||||
|
||||
MEM_SmartPtr<IK_JacobianSolver> solver (IK_JacobianSolver::New());
|
||||
|
||||
if (output == NULL ||
|
||||
solver == NULL
|
||||
) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
output->m_solver = solver.Release();
|
||||
|
||||
return output.Release();
|
||||
};
|
||||
|
||||
IK_Chain &
|
||||
Chain(
|
||||
) {
|
||||
return m_chain;
|
||||
};
|
||||
|
||||
IK_JacobianSolver &
|
||||
Solver(
|
||||
) {
|
||||
return m_solver.Ref();
|
||||
}
|
||||
|
||||
~IK_Solver_Class(
|
||||
) {
|
||||
// nothing to do
|
||||
}
|
||||
|
||||
|
||||
private :
|
||||
|
||||
IK_Solver_Class(
|
||||
) {
|
||||
};
|
||||
|
||||
IK_Chain m_chain;
|
||||
MEM_SmartPtr<IK_JacobianSolver> m_solver;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -36,15 +36,9 @@
|
||||
#include "MyGlutMouseHandler.h"
|
||||
#include "MyGlutKeyHandler.h"
|
||||
#include "MT_Transform.h"
|
||||
#ifdef USE_QUATERNIONS
|
||||
# include "IK_Qsolver.h"
|
||||
# include "../intern/IK_QChain.h"
|
||||
# include "../intern/IK_QSolver_Class.h"
|
||||
#else
|
||||
# include "IK_solver.h"
|
||||
# include "../intern/IK_Chain.h"
|
||||
# include "../intern/IK_Solver_Class.h"
|
||||
#endif
|
||||
#include <GL/glut.h>
|
||||
|
||||
class ChainDrawer : public GlutDrawer
|
||||
|
Loading…
Reference in New Issue
Block a user