diff options
Diffstat (limited to 'intern/iksolver')
67 files changed, 14360 insertions, 0 deletions
diff --git a/intern/iksolver/Makefile b/intern/iksolver/Makefile new file mode 100644 index 00000000000..8cdb81ca780 --- /dev/null +++ b/intern/iksolver/Makefile @@ -0,0 +1,52 @@ +# +# $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 ***** +# iksolver main makefile. +# + +include nan_definitions.mk + +LIBNAME = iksolver +SOURCEDIR = intern/$(LIBNAME) +DIR = $(OCGDIR)/$(SOURCEDIR) +DIRS = intern +TESTDIRS = test + +include nan_subdirs.mk + +install: all debug + @[ -d $(NAN_IKSOLVER) ] || mkdir $(NAN_IKSOLVER) + @[ -d $(NAN_IKSOLVER)/include ] || mkdir $(NAN_IKSOLVER)/include + @[ -d $(NAN_IKSOLVER)/lib ] || mkdir $(NAN_IKSOLVER)/lib + @[ -d $(NAN_IKSOLVER)/lib/debug ] || mkdir $(NAN_IKSOLVER)/lib/debug + cp -f $(DIR)/libiksolver.a $(NAN_IKSOLVER)/lib/ + cp -f $(DIR)/debug/libiksolver.a $(NAN_IKSOLVER)/lib/debug/ + cp -f extern/*.h $(NAN_IKSOLVER)/include/ + diff --git a/intern/iksolver/extern/IK_solver.h b/intern/iksolver/extern/IK_solver.h new file mode 100644 index 00000000000..57d06a3ba32 --- /dev/null +++ b/intern/iksolver/extern/IK_solver.h @@ -0,0 +1,207 @@ +/** + * $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 + * @mainpage IK - Blender inverse kinematics module. + * + * @section about About the IK module + * + * This module allows you to create segments and form them into + * chains. You can then define a goal point that the end of the + * chain should attempt to reach - an inverse kinematic problem. + * This module will then modify the segments in the chain in + * order to get the end of the chain as near as possible to the + * goal. This solver uses an inverse jacobian method to find + * a solution. + * + * @section issues Known issues with this IK solver. + * + * - The current solver works with only one type of segment. These + * segments always have 3 degress of freedom (DOF). i.e. the solver + * uses all these degrees to solve the IK problem. It would be + * nice to allow the user to specify different segment types such + * as 1 DOF joints in a given plane. 2 DOF joints about given axis. + * - There is currently no support for joint constraints in the + * solver. This is within the realms of possibility - please ask + * if this functionality is required. + * - The solver is slow, inverse jacobian methods in general give + * 'smooth' solutions and the method is also very flexible, it + * does not rely on specific angle parameterization and can be + * extended to deal with different joint types and joint + * constraints. However it is not suitable for real time use. + * Other algorithms exist which are more suitable for real-time + * applications, please ask if this functionality is required. + * + * @section dependencies Dependencies + * + * This module only depends on Moto. + */ + +#ifndef NAN_INCLUDED_IK_solver_h +#define NAN_INCLUDED_IK_solver_h + + + + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * External segment structure + */ + + +/** + * This structure defines a single segment of an IK chain. + * - Individual segments are always defined in local coordinates. + * - The segment is assumed to be oriented in the local + * y-direction. + * - seg_start is the start of the segment relative to the end + * of the parent segment. + * - basis is a column major matrix defining the rest position + * of the bone. + * - length is the simply the length of the bone. + * - basis_change is a 3x3 matrix representing the change + * from the rest position of the segment to the solved position. + * In fact it is the transpose of this matrix because blender + * does something weird with quaternion conversion. This is + * strictly an ouput variable for returning the results of an + * an ik solve back to you. + * The local transformation specified as a column major matrix + * of a segment is then defined as. + * translate(seg_start)*basis*transpose(basis_change)*translate(0,length,0) + */ + +typedef struct IK_Segment_Extern { + float seg_start[3]; + float basis[9]; + float length; + float basis_change[9]; +} IK_Segment_Extern; + +typedef IK_Segment_Extern* IK_Segment_ExternPtr; + +/** + * External chain structure. + * This structure is filled when you call IK_LoadChain. + * The first segment in the chain is the root segment. + * The end of the last segment is the end-effector of the chain + * this is the point that tries to move to the goal in the ik + * solver. + * - num_segments is the number of segments in the array pointed + * to by the member segments. + * - chain_dof is the number of degrees of freedom of the chain + * that is the number of independent ways the chain can be changed + * to reach the goal. + * - segments points to an array of IK_Segment_Extern structs + * containing the segments of this chain. + * - intern is pointer used by the module to store information + * about the chain. Please do not touch the member in any way. + */ + +typedef struct IK_Chain_Extern { + int num_segments; + int chain_dof; + IK_Segment_ExternPtr segments; + void * intern; +} IK_Chain_Extern; + +typedef IK_Chain_Extern* IK_Chain_ExternPtr; + + +/** + * Create a clean chain structure. + * @return A IK_Chain_Extern structure allocated on the heap. + * Do not attempt to delete or free this memory yourself please + * use the FreeChain(...) function for this. + */ + +extern IK_Chain_ExternPtr IK_CreateChain(void); + +/** + * Copy segment information into the chain structure. + * @param chain A chain to load the segments into. + * @param segments a ptr to an array of IK_Input_Segment_Extern structures + * @param num_segs the number of segments to load into the chain + * @return 1 if the chain was correctly loaded into the structure. + * @return 0 if an error occured loading the chain. This will normally + * occur when there is not enough memory to allocate internal chain data. + * In this case you should not use the chain structure and should call + * IK_FreeChain to free the memory associated with the chain. + */ + +extern int IK_LoadChain(IK_Chain_ExternPtr chain,IK_Segment_ExternPtr segments, int num_segs); + +/** + * Compute the solution of an inverse kinematic problem. + * @param chain a ptr to an IK_Segment_Extern loaded with the segments + * to solve for. + * @param goal the goal of the IK problem + * @param tolerance .The distance to the solution within which the chain is deemed + * to be solved. + * @param max_iterations. The maximum number of iterations to use in solving the + * problem. + * @param max_angle_change. The maximum allowed angular change. 0.1 is a good value here. + * @param output. Results of the solution are written to the segments pointed to by output. + * Only the basis and basis_change fields are written. You must make sure that you have + * allocated enough room for the output segments. + * @return 0 if the solved chain did not reach the goal. This occurs when the + * goal was unreachable by the chain end effector. + * @return 1 if the chain reached the goal. + */ + +extern int IK_SolveChain( + IK_Chain_ExternPtr chain, + float goal[3], + float tolerance, + int max_iterations, + float max_angle_change, + IK_Segment_ExternPtr output +); + +/** + * Free a chain and all it's internal memory. + */ + +extern void IK_FreeChain(IK_Chain_ExternPtr); + + +#ifdef __cplusplus +} +#endif + +#endif // NAN_INCLUDED_IK_solver_h
\ No newline at end of file diff --git a/intern/iksolver/intern/IK_CGChainSolver.cpp b/intern/iksolver/intern/IK_CGChainSolver.cpp new file mode 100644 index 00000000000..6af4df2a35b --- /dev/null +++ b/intern/iksolver/intern/IK_CGChainSolver.cpp @@ -0,0 +1,300 @@ +/** + * $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 ***** + */ + +#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; +}; + diff --git a/intern/iksolver/intern/IK_CGChainSolver.h b/intern/iksolver/intern/IK_CGChainSolver.h new file mode 100644 index 00000000000..f7aeee090a1 --- /dev/null +++ b/intern/iksolver/intern/IK_CGChainSolver.h @@ -0,0 +1,178 @@ +/** + * $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
\ No newline at end of file diff --git a/intern/iksolver/intern/IK_Chain.cpp b/intern/iksolver/intern/IK_Chain.cpp new file mode 100644 index 00000000000..875b8da1405 --- /dev/null +++ b/intern/iksolver/intern/IK_Chain.cpp @@ -0,0 +1,208 @@ +/** + * $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 ***** + */ + +#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; +} + + + + + + + + + + + + diff --git a/intern/iksolver/intern/IK_Chain.h b/intern/iksolver/intern/IK_Chain.h new file mode 100644 index 00000000000..40496b6087a --- /dev/null +++ b/intern/iksolver/intern/IK_Chain.h @@ -0,0 +1,194 @@ +/** + * $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
\ No newline at end of file diff --git a/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp b/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp new file mode 100644 index 00000000000..028b7dfe2b0 --- /dev/null +++ b/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp @@ -0,0 +1,162 @@ +/** + * $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 ***** + */ + +#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 + + + + + + + + diff --git a/intern/iksolver/intern/IK_ConjugateGradientSolver.h b/intern/iksolver/intern/IK_ConjugateGradientSolver.h new file mode 100644 index 00000000000..e50799a9fa1 --- /dev/null +++ b/intern/iksolver/intern/IK_ConjugateGradientSolver.h @@ -0,0 +1,195 @@ +/** + * $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 + + diff --git a/intern/iksolver/intern/IK_JacobianSolver.cpp b/intern/iksolver/intern/IK_JacobianSolver.cpp new file mode 100644 index 00000000000..de4531f1677 --- /dev/null +++ b/intern/iksolver/intern/IK_JacobianSolver.cpp @@ -0,0 +1,267 @@ +/** + * $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 ***** + */ + +#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(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); + +}; + + + + + + + + + + + + + + + + + diff --git a/intern/iksolver/intern/IK_JacobianSolver.h b/intern/iksolver/intern/IK_JacobianSolver.h new file mode 100644 index 00000000000..3f3372a2cb7 --- /dev/null +++ b/intern/iksolver/intern/IK_JacobianSolver.h @@ -0,0 +1,171 @@ +/** + * $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 + + + diff --git a/intern/iksolver/intern/IK_LineMinimizer.h b/intern/iksolver/intern/IK_LineMinimizer.h new file mode 100644 index 00000000000..635a5972c02 --- /dev/null +++ b/intern/iksolver/intern/IK_LineMinimizer.h @@ -0,0 +1,298 @@ +/** + * $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 diff --git a/intern/iksolver/intern/IK_QChain.cpp b/intern/iksolver/intern/IK_QChain.cpp new file mode 100644 index 00000000000..438729003a9 --- /dev/null +++ b/intern/iksolver/intern/IK_QChain.cpp @@ -0,0 +1,280 @@ +/** + * $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 + */ + +#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; + int i = 0; + + 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; +} + + + + + + + + + + + + + + + + + + + + diff --git a/intern/iksolver/intern/IK_QChain.h b/intern/iksolver/intern/IK_QChain.h new file mode 100644 index 00000000000..ed02bcce53e --- /dev/null +++ b/intern/iksolver/intern/IK_QChain.h @@ -0,0 +1,211 @@ +/** + * $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
\ No newline at end of file diff --git a/intern/iksolver/intern/IK_QJacobianSolver.cpp b/intern/iksolver/intern/IK_QJacobianSolver.cpp new file mode 100644 index 00000000000..32a703cb94d --- /dev/null +++ b/intern/iksolver/intern/IK_QJacobianSolver.cpp @@ -0,0 +1,327 @@ +/** + * $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 ***** + */ + +#include "IK_QJacobianSolver.h" + +#include "TNT/svd.h" + +using namespace std; + + IK_QJacobianSolver * +IK_QJacobianSolver:: +New( +){ + return new IK_QJacobianSolver(); +} + + 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 +){ + + const vector<IK_QSegment> & segs = chain.Segments(); + if (segs.size() == 0) return false; + + // 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; + } + + goal_dir.normalize(); + + + ArmMatrices(chain.DoF()); + + for (int iterations = 0; iterations < max_iterations; iterations++) { + + // check to see if the chain is pointing in the right direction + + if (iterations%32 && do_parallel_check && ParallelCheck(chain,goal_dir)) { + + return false; + } + + 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(); + + try { + ComputeInverseJacobian(chain,x_length,max_angle_change); + } + catch(...) { + return false; + } + + ComputeBetas(chain,d_pos); + UpdateChain(chain); + chain.UpdateGlobalTransformations(); + } + + + 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(); + + 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; + } +} + + diff --git a/intern/iksolver/intern/IK_QJacobianSolver.h b/intern/iksolver/intern/IK_QJacobianSolver.h new file mode 100644 index 00000000000..7e2f886d0c4 --- /dev/null +++ b/intern/iksolver/intern/IK_QJacobianSolver.h @@ -0,0 +1,184 @@ +/** + * $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_QJacobianSolver_h + +#define NAN_INCLUDED_IK_QJacobianSolver_h + +/** + * @author Laurence Bourn + * @date 28/6/2001 + */ + +#include "TNT/cmat.h" +#include <vector> +#include "MT_Vector3.h" +#include "IK_QChain.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( + ); + + /** + * 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_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 + ); + + ~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; + + +}; + + + + +#endif + + + diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp new file mode 100644 index 00000000000..2956dd815e2 --- /dev/null +++ b/intern/iksolver/intern/IK_QSegment.cpp @@ -0,0 +1,363 @@ +/** + * $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 + */ + +#include "IK_QSegment.h" +#include <iostream.h> + +IK_QSegment:: +IK_QSegment ( + const MT_Point3 tr1, + const MT_Matrix3x3 A, + const MT_Scalar length, + const MT_ExpMap q +) : + m_length (length), + m_q (q) +{ + + m_max_extension = tr1.length() + length; + + m_transform.setOrigin(tr1); + m_transform.setBasis(A); + + UpdateLocalTransform(); +}; + + +IK_QSegment:: +IK_QSegment ( +) : + m_length(0), + m_q (0,0,0), + m_max_extension(0) +{ + // Intentionally empty +} + + + +// accessors +//////////// + +// The length of the segment + +const + MT_Scalar +IK_QSegment:: +Length( +) const { + return m_length; +} + +const + MT_Scalar +IK_QSegment:: +MaxExtension( +) const { + return m_max_extension; +} + +// This is the transform from adjacent +// coordinate systems in the chain. + +const + MT_Transform & +IK_QSegment:: +LocalTransform( +) const { + return m_local_transform; +} + +const + MT_ExpMap & +IK_QSegment:: +LocalJointParameter( +) const { + return m_q; +} + + MT_Transform +IK_QSegment:: +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; + + const MT_Transform new_global = GlobalTransform(); + + m_seg_end = new_global.getOrigin(); + + return new_global; +} + + MT_Transform +IK_QSegment:: +GlobalTransform( +) const { + return m_global_transform * m_local_transform; +} + + + MT_Transform +IK_QSegment:: +UpdateAccumulatedLocal( + const MT_Transform & acc_local +){ + m_accum_local = acc_local; + return m_local_transform * m_accum_local; +} + +const + MT_Transform & +IK_QSegment:: +AccumulatedLocal( +) const { + return m_accum_local; +} + + MT_Vector3 +IK_QSegment:: +ComputeJacobianColumn( + int angle +) const { + + + MT_Transform translation; + translation.setIdentity(); + translation.translate(MT_Vector3(0,m_length,0)); + + + // we can compute the jacobian for one of the + // angles of this joint by first computing + // the partial derivative of the local transform dR/da + // and then computing + // dG/da = m_global_transform * dR/da * m_accum_local (0,0,0) + +#if 0 + + // use euler angles as a test of the matrices and this method. + + MT_Matrix3x3 dRda; + + MT_Quaternion rotx,roty,rotz; + + rotx.setRotation(MT_Vector3(1,0,0),m_q[0]); + roty.setRotation(MT_Vector3(0,1,0),m_q[1]); + rotz.setRotation(MT_Vector3(0,0,1),m_q[2]); + + MT_Matrix3x3 rotx_m(rotx); + MT_Matrix3x3 roty_m(roty); + MT_Matrix3x3 rotz_m(rotz); + + if (angle == 0) { + + MT_Scalar ci = cos(m_q[0]); + MT_Scalar si = sin(m_q[1]); + + dRda = MT_Matrix3x3( + 0, 0, 0, + 0,-si,-ci, + 0, ci,-si + ); + + dRda = rotz_m * roty_m * dRda; + } else + + if (angle == 1) { + + MT_Scalar cj = cos(m_q[1]); + MT_Scalar sj = sin(m_q[1]); + + dRda = MT_Matrix3x3( + -sj, 0, cj, + 0, 0, 0, + -cj, 0,-sj + ); + + dRda = rotz_m * dRda * rotx_m; + } else + + if (angle == 2) { + + MT_Scalar ck = cos(m_q[2]); + MT_Scalar sk = sin(m_q[2]); + + dRda = MT_Matrix3x3( + -sk,-ck, 0, + ck,-sk, 0, + 0, 0, 0 + ); + + dRda = dRda * roty_m * rotx_m; + } + + MT_Transform dRda_t(MT_Point3(0,0,0),dRda); + + // convert to 4x4 matrices coz dRda is singular. + MT_Matrix4x4 dRda_m(dRda_t); + dRda_m[3][3] = 0; + +#else + + // use exponential map derivatives + MT_Matrix4x4 dRda_m = m_q.partialDerivatives(angle); + +#endif + + + // Once we have computed the local derivatives we can compute + // derivatives of the end effector. + + // Imagine a chain consisting of 5 segments each with local + // transformation Li + // Then the global transformation G is L1 *L2 *L3 *L4 *L5 + // If we want to compute the partial derivatives of this expression + // w.r.t one of the angles x of L3 we should then compute + // dG/dx = d(L1 *L2 *L3 *L4 *L5)/dx + // = L1 *L2 * dL3/dx *L4 *L5 + // but L1 *L2 is the global transformation of the parent of this + // bone and L4 *L5 is the accumulated local transform of the children + // of this bone (m_accum_local) + // so dG/dx = m_global_transform * dL3/dx * m_accum_local + // + // so now we need to compute dL3/dx + // L3 = m_transform * rotation(m_q) * translate(0,m_length,0) + // do using the same procedure we get + // dL3/dx = m_transform * dR/dx * translate(0,m_length,0) + // dR/dx is the partial derivative of the exponential map w.r.t + // one of it's parameters. This is computed in MT_ExpMap + + MT_Matrix4x4 translation_m (translation); + MT_Matrix4x4 global_m(m_global_transform); + MT_Matrix4x4 accum_local_m(m_accum_local); + MT_Matrix4x4 transform_m(m_transform); + + + MT_Matrix4x4 dFda_m = global_m * transform_m * dRda_m * translation_m * accum_local_m; + + MT_Vector4 result = dFda_m * MT_Vector4(0,0,0,1); + return MT_Vector3(result[0],result[1],result[2]); +}; + + + +const + MT_Vector3 & +IK_QSegment:: +GlobalSegmentStart( +) const{ + return m_seg_start; +} + +const + MT_Vector3 & +IK_QSegment:: +GlobalSegmentEnd( +) const { + return m_seg_end; +} + + + void +IK_QSegment:: +IncrementAngle( + const MT_Vector3 &dq +){ + m_q.vector() += dq; + MT_Scalar theta(0); + m_q.reParameterize(theta); + + UpdateLocalTransform(); +} + + + void +IK_QSegment:: +SetAngle( + const MT_ExpMap &dq +){ + m_q = dq; + UpdateLocalTransform(); +} + + + void +IK_QSegment:: +UpdateLocalTransform( +){ +#if 0 + + //use euler angles - test + MT_Quaternion rotx,roty,rotz; + + rotx.setRotation(MT_Vector3(1,0,0),m_q[0]); + roty.setRotation(MT_Vector3(0,1,0),m_q[1]); + rotz.setRotation(MT_Vector3(0,0,1),m_q[2]); + + MT_Matrix3x3 rotx_m(rotx); + MT_Matrix3x3 roty_m(roty); + MT_Matrix3x3 rotz_m(rotz); + + MT_Matrix3x3 rot = rotz_m * roty_m * rotx_m; +#else + + //use exponential map + MT_Matrix3x3 rot = m_q.getMatrix(); + + +#endif + + 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; +}; + + + + + + diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h new file mode 100644 index 00000000000..0c0e3a7f8bc --- /dev/null +++ b/intern/iksolver/intern/IK_QSegment.h @@ -0,0 +1,290 @@ +/** + * $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_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 <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 + * local_transform = + * translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0) + * We use the standard moto column ordered matrices. You can read + * this as: + * - first translate by (0,length,0) + * - multiply by the rotation matrix derived from the current + * angle parameterization q. + * - multiply by the user defined matrix representing the rest + * position of the bone. + * - translate by the used defined translation (tr1) + * The ordering of these transformations is vital, you must + * use exactly the same transformations when displaying the segments + */ + +class IK_QSegment { + +public : + + /** + * 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 + ); + + /** + * Default constructor + * Defines identity local coordinate system, + * with a bone length of 1. + */ + + + IK_QSegment( + ); + + + /** + * @return The length of the segment + */ + + const + MT_Scalar + Length( + ) const ; + + /** + * @return the max distance of the end of this + * bone from the local origin. + */ + + const + MT_Scalar + MaxExtension( + ) const ; + + /** + * @return The transform from adjacent + * coordinate systems in the chain. + */ + + const + MT_Transform & + LocalTransform( + ) const ; + + const + MT_ExpMap & + LocalJointParameter( + ) const; + + /** + * 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. + */ + + MT_Transform + UpdateGlobal( + const MT_Transform & global + ); + + /** + * @return The global transformation + */ + + MT_Transform + GlobalTransform( + ) const; + + + /** + * 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 + */ + + MT_Transform + UpdateAccumulatedLocal( + const MT_Transform & acc_local + ); + + /** + * @return A const reference to accumulated local + * transform of this segment. + */ + + const + MT_Transform & + AccumulatedLocal( + ) const; + + /** + * @return A const Reference to start of segment in global + * coordinates + */ + + const + MT_Vector3 & + GlobalSegmentStart( + ) const; + + /** + * @return A const Reference to end of segment in global + * coordinates + */ + + const + MT_Vector3 & + GlobalSegmentEnd( + ) const; + + + /** + * @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] + */ + + MT_Vector3 + ComputeJacobianColumn( + int angle + ) const ; + + /** + * 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_ExpMap m_q; + MT_Scalar m_length; + + /** + * The maximum extension of this segment + * This is the magnitude of the user offset + the length of the + * chain + */ + + MT_Scalar m_max_extension; + + MT_Transform m_local_transform; + MT_Transform m_global_transform; + MT_Transform m_accum_local; + + MT_Vector3 m_seg_start; + MT_Vector3 m_seg_end; + +}; + +#endif
\ No newline at end of file diff --git a/intern/iksolver/intern/IK_QSolver_Class.h b/intern/iksolver/intern/IK_QSolver_Class.h new file mode 100644 index 00000000000..91ad9f9c186 --- /dev/null +++ b/intern/iksolver/intern/IK_QSolver_Class.h @@ -0,0 +1,107 @@ +/** + * $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 diff --git a/intern/iksolver/intern/IK_Segment.cpp b/intern/iksolver/intern/IK_Segment.cpp new file mode 100644 index 00000000000..f955a657dae --- /dev/null +++ b/intern/iksolver/intern/IK_Segment.cpp @@ -0,0 +1,277 @@ +/** + * $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 ***** + */ + +#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]; +} + + + + + diff --git a/intern/iksolver/intern/IK_Segment.h b/intern/iksolver/intern/IK_Segment.h new file mode 100644 index 00000000000..35018df7859 --- /dev/null +++ b/intern/iksolver/intern/IK_Segment.h @@ -0,0 +1,217 @@ +/** + * $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
\ No newline at end of file diff --git a/intern/iksolver/intern/IK_Solver.cpp b/intern/iksolver/intern/IK_Solver.cpp new file mode 100644 index 00000000000..11646597d6d --- /dev/null +++ b/intern/iksolver/intern/IK_Solver.cpp @@ -0,0 +1,196 @@ +/** + * $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 ***** + */ + +#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 <iostream.h> + + IK_Chain_ExternPtr +IK_CreateChain( + void +) { + + MEM_SmartPtr<IK_Chain_Extern> output (new IK_Chain_Extern); + MEM_SmartPtr<IK_QSolver_Class> output_void (IK_QSolver_Class::New()); + + + if (output == NULL || output_void == NULL) { + return NULL; + } + + output->chain_dof = 0; + output->num_segments = 0; + output->segments = NULL; + output->intern = output_void.Release(); + return output.Release(); +}; + + + 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() != 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; +} + + void +IK_FreeChain( + IK_Chain_ExternPtr chain +){ + IK_QSolver_Class *intern_cpp = (IK_QSolver_Class *)chain->intern; + + delete(intern_cpp); + delete(chain); + +} + + + + + + + + + + + + + diff --git a/intern/iksolver/intern/IK_Solver_Class.h b/intern/iksolver/intern/IK_Solver_Class.h new file mode 100644 index 00000000000..1ef6c22f946 --- /dev/null +++ b/intern/iksolver/intern/IK_Solver_Class.h @@ -0,0 +1,93 @@ +/** + * $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 diff --git a/intern/iksolver/intern/MT_ExpMap.cpp b/intern/iksolver/intern/MT_ExpMap.cpp new file mode 100644 index 00000000000..ea005a42096 --- /dev/null +++ b/intern/iksolver/intern/MT_ExpMap.cpp @@ -0,0 +1,268 @@ +/** + * $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 + */ + +#include "MT_ExpMap.h" + +/** + * Set the exponential map from a quaternion. The quaternion must be non-zero. + */ + + void +MT_ExpMap:: +setRotation( + const MT_Quaternion &q +) { + // ok first normailize 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(); + + MT_Vector3 axis(qt.x(),qt.y(),qt.z()); + MT_Scalar cosp = qt.w(); + MT_Scalar sinp = axis.length(); + axis /= sinp; + + MT_Scalar theta = atan2(double(sinp),double(cosp)); + + axis *= theta; + m_v = axis; +} + +/** + * Convert from an exponential map to a quaternion + * representation + */ + + MT_Quaternion +MT_ExpMap:: +getRotation( +) const { + bool rep=0; + 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; +} + +/** + * Convert the exponential map to a 3x3 matrix + */ + + MT_Matrix3x3 +MT_ExpMap:: +getMatrix( +) const { + + MT_Quaternion q = getRotation(); + return MT_Matrix3x3(q); +} + + + + +/** + * Force a reparameterization of the exponential + * map. + */ + + bool +MT_ExpMap:: +reParameterize( + MT_Scalar &theta +){ + 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; + +} + +/** + * 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 + */ + + MT_Matrix4x4 +MT_ExpMap:: +partialDerivatives( + const int i +) const { + + MT_Quaternion q = getRotation(); + MT_Quaternion dQdx; + + MT_Matrix4x4 output; + + compute_dQdVi(i,dQdx); + compute_dRdVi(q,dQdx,output); + + return output; +} + + void +MT_ExpMap:: +compute_dRdVi( + const MT_Quaternion &q, + const MT_Quaternion &dQdvi, + MT_Matrix4x4 & dRdvi +) const { + + 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' */ + + 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()); + + /* 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[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; +} + +// compute partial derivatives dQ/dVi + + void +MT_ExpMap:: +compute_dQdVi( + const int i, + MT_Quaternion & dQdX +) const { + + MT_Scalar theta = m_v.length(); + MT_Scalar cosp(cos(MT_Scalar(.5)*theta)), sinp(sin(MT_Scalar(.5)*theta)); + + MT_assert(i>=0 && i<3); + + /* 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); + + 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); + + 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; + } +} + + + + + + + + + + diff --git a/intern/iksolver/intern/MT_ExpMap.h b/intern/iksolver/intern/MT_ExpMap.h new file mode 100644 index 00000000000..10cacb59b78 --- /dev/null +++ b/intern/iksolver/intern/MT_ExpMap.h @@ -0,0 +1,219 @@ +/** + * $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 MT_ExpMap_H +#define MT_ExpMap_H + +#include <MT_assert.h> + +#include "MT_Vector3.h" +#include "MT_Quaternion.h" +#include "MT_Matrix4x4.h" + +const MT_Scalar MT_EXPMAP_MINANGLE (1e-7); + +/** + * MT_ExpMap an exponential map parameterization of rotations + * in 3D. This implementation is derived from the paper + * "F. Sebastian Grassia. Practical parameterization of + * rotations using the exponential map. Journal of Graphics Tools, + * 3(3):29-48, 1998" Please go to http://www.acm.org/jgt/papers/Grassia98/ + * for a thorough description of the theory and sample code used + * to derive this class. + * + * Basic overview of why this class is used. + * In an IK system we need to paramterize the joint angles in some + * way. Typically 2 parameterizations are used. + * - Euler Angles + * These suffer from singularities in the parameterization known + * as gimbal lock. They also do not interpolate well. For every + * set of euler angles there is exactly 1 corresponding 3d rotation. + * - Quaternions. + * Great for interpolating. Only unit quaternions are valid rotations + * means that in a differential ik solver we often stray outside of + * this manifold into invalid rotations. Means we have to do a lot + * of nasty normalizations all the time. Does not suffer from + * gimbal lock problems. More expensive to compute partial derivatives + * as there are 4 of them. + * + * So exponential map is similar to a quaternion axis/angle + * representation but we store the angle as the length of the + * axis. So require only 3 parameters. Means that all exponential + * maps are valid rotations. Suffers from gimbal lock. But it's + * possible to detect when gimbal lock is near and reparameterize + * away from it. Also nice for interpolating. + * Exponential maps are share some of the useful properties of + * euler and quaternion parameterizations. And are very useful + * for differential IK solvers. + */ + +class MT_ExpMap { +public: + + /** + * Default constructor + * @warning there is no initialization in the + * default constructor + */ + + MT_ExpMap() {} + MT_ExpMap(const MT_Vector3& v) : m_v(v) {} + + MT_ExpMap(const float v[3]) : m_v(v) {} + MT_ExpMap(const double v[3]) : m_v(v) {} + + MT_ExpMap(MT_Scalar x, MT_Scalar y, MT_Scalar z) : + m_v(x, y, z) {} + + /** + * Construct an exponential map from a quaternion + */ + + MT_ExpMap( + const MT_Quaternion &q + ) { + setRotation(q); + }; + + /** + * Accessors + * Decided not to inherit from MT_Vector3 but rather + * this class contains an MT_Vector3. This is because + * it is very dangerous to use MT_Vector3 functions + * on this class and some of them have no direct meaning. + */ + + MT_Vector3 & + vector( + ) { + return m_v; + }; + + const + MT_Vector3 & + vector( + ) const { + return m_v; + }; + + /** + * Set the exponential map from a quaternion + */ + + void + setRotation( + const MT_Quaternion &q + ); + + /** + * Convert from an exponential map to a quaternion + * representation + */ + + MT_Quaternion + getRotation( + ) const; + + /** + * Convert the exponential map to a 3x3 matrix + */ + + MT_Matrix3x3 + getMatrix( + ) 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. + */ + + bool + reParameterize( + MT_Scalar &theta + ); + + /** + * Compute the partial derivatives of the exponential + * map (dR/de - where R is a 4x4 matrix formed + * from the map) and return them as a 4x4 matrix + */ + + MT_Matrix4x4 + partialDerivatives( + const int i + ) const ; + +private : + + MT_Vector3 m_v; + + // 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 + ) const; + + // compute partial derivatives dQ/dVi + + void + compute_dQdVi( + int i, + MT_Quaternion & dQdX + ) const ; + + +}; + + + +#endif + + + diff --git a/intern/iksolver/intern/Makefile b/intern/iksolver/intern/Makefile new file mode 100644 index 00000000000..af32d816a80 --- /dev/null +++ b/intern/iksolver/intern/Makefile @@ -0,0 +1,44 @@ +# +# $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 ***** +# iksolver intern Makefile +# + +LIBNAME = iksolver +DIR = $(OCGDIR)/intern/$(LIBNAME) +CCSRCS = IK_QChain.cpp IK_QJacobianSolver.cpp IK_QSegment.cpp MT_ExpMap.cpp IK_Solver.cpp + +include nan_compile.mk + +CCFLAGS += $(LEVEL_2_CPP_WARNINGS) + +CPPFLAGS += -I$(NAN_MOTO)/include +CPPFLAGS += -I$(NAN_MEMUTIL)/include + diff --git a/intern/iksolver/intern/TNT/cholesky.h b/intern/iksolver/intern/TNT/cholesky.h new file mode 100644 index 00000000000..a3e0118f4e5 --- /dev/null +++ b/intern/iksolver/intern/TNT/cholesky.h @@ -0,0 +1,128 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +#ifndef CHOLESKY_H +#define CHOLESKY_H + +#include <cmath> + +// index method + +namespace TNT +{ + + +// +// Only upper part of A is used. Cholesky factor is returned in +// lower part of L. Returns 0 if successful, 1 otherwise. +// +template <class SPDMatrix, class SymmMatrix> +int Cholesky_upper_factorization(SPDMatrix &A, SymmMatrix &L) +{ + Subscript M = A.dim(1); + Subscript N = A.dim(2); + + assert(M == N); // make sure A is square + + // readjust size of L, if necessary + + if (M != L.dim(1) || N != L.dim(2)) + L = SymmMatrix(N,N); + + Subscript i,j,k; + + + typename SPDMatrix::element_type dot=0; + + + for (j=1; j<=N; j++) // form column j of L + { + dot= 0; + + for (i=1; i<j; i++) // for k= 1 TO j-1 + dot = dot + L(j,i)*L(j,i); + + L(j,j) = A(j,j) - dot; + + for (i=j+1; i<=N; i++) + { + dot = 0; + for (k=1; k<j; k++) + dot = dot + L(i,k)*L(j,k); + L(i,j) = A(j,i) - dot; + } + + if (L(j,j) <= 0.0) return 1; + + L(j,j) = sqrt( L(j,j) ); + + for (i=j+1; i<=N; i++) + L(i,j) = L(i,j) / L(j,j); + + } + + return 0; +} + + + + +} +// namespace TNT + +#endif +// CHOLESKY_H diff --git a/intern/iksolver/intern/TNT/cmat.h b/intern/iksolver/intern/TNT/cmat.h new file mode 100644 index 00000000000..80a82417e47 --- /dev/null +++ b/intern/iksolver/intern/TNT/cmat.h @@ -0,0 +1,661 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// C compatible matrix: row-oriented, 0-based [i][j] and 1-based (i,j) indexing +// + +#ifndef CMAT_H +#define CMAT_H + +#include "subscript.h" +#include "vec.h" +#include <stdlib.h> +#include <assert.h> +#include <iostream> +#include <strstream> +#ifdef TNT_USE_REGIONS +#include "region2d.h" +#endif + +namespace TNT +{ + +template <class T> +class Matrix +{ + + + public: + + typedef Subscript size_type; + typedef T value_type; + typedef T element_type; + typedef T* pointer; + typedef T* iterator; + typedef T& reference; + typedef const T* const_iterator; + typedef const T& const_reference; + + Subscript lbound() const { return 1;} + + protected: + Subscript m_; + Subscript n_; + Subscript mn_; // total size + T* v_; + T** row_; + T* vm1_ ; // these point to the same data, but are 1-based + T** rowm1_; + + // internal helper function to create the array + // of row pointers + + void initialize(Subscript M, Subscript N) + { + mn_ = M*N; + m_ = M; + n_ = N; + + v_ = new T[mn_]; + row_ = new T*[M]; + rowm1_ = new T*[M]; + + assert(v_ != NULL); + assert(row_ != NULL); + assert(rowm1_ != NULL); + + T* p = v_; + vm1_ = v_ - 1; + for (Subscript i=0; i<M; i++) + { + row_[i] = p; + rowm1_[i] = p-1; + p += N ; + + } + + rowm1_ -- ; // compensate for 1-based offset + } + + void copy(const T* v) + { + Subscript N = m_ * n_; + Subscript i; + +#ifdef TNT_UNROLL_LOOPS + Subscript Nmod4 = N & 3; + Subscript N4 = N - Nmod4; + + for (i=0; i<N4; i+=4) + { + v_[i] = v[i]; + v_[i+1] = v[i+1]; + v_[i+2] = v[i+2]; + v_[i+3] = v[i+3]; + } + + for (i=N4; i< N; i++) + v_[i] = v[i]; +#else + + for (i=0; i< N; i++) + v_[i] = v[i]; +#endif + } + + void set(const T& val) + { + Subscript N = m_ * n_; + Subscript i; + +#ifdef TNT_UNROLL_LOOPS + Subscript Nmod4 = N & 3; + Subscript N4 = N - Nmod4; + + for (i=0; i<N4; i+=4) + { + v_[i] = val; + v_[i+1] = val; + v_[i+2] = val; + v_[i+3] = val; + } + + for (i=N4; i< N; i++) + v_[i] = val; +#else + + for (i=0; i< N; i++) + v_[i] = val; + +#endif + } + + + + void destroy() + { + /* do nothing, if no memory has been previously allocated */ + if (v_ == NULL) return ; + + /* if we are here, then matrix was previously allocated */ + if (v_ != NULL) delete [] (v_); + if (row_ != NULL) delete [] (row_); + + /* return rowm1_ back to original value */ + rowm1_ ++; + if (rowm1_ != NULL ) delete [] (rowm1_); + } + + + public: + + operator T**(){ return row_; } + operator T**() const { return row_; } + + + Subscript size() const { return mn_; } + + // constructors + + Matrix() : m_(0), n_(0), mn_(0), v_(0), row_(0), vm1_(0), rowm1_(0) {}; + + Matrix(const Matrix<T> &A) + { + initialize(A.m_, A.n_); + copy(A.v_); + } + + Matrix(Subscript M, Subscript N, const T& value = T()) + { + initialize(M,N); + set(value); + } + + Matrix(Subscript M, Subscript N, const T* v) + { + initialize(M,N); + copy(v); + } + + Matrix(Subscript M, Subscript N, const char *s) + { + initialize(M,N); + std::istrstream ins(s); + + Subscript i, j; + + for (i=0; i<M; i++) + for (j=0; j<N; j++) + ins >> row_[i][j]; + } + + + // destructor + // + ~Matrix() + { + destroy(); + } + + + // reallocating + // + Matrix<T>& newsize(Subscript M, Subscript N) + { + if (num_rows() == M && num_cols() == N) + return *this; + + destroy(); + initialize(M,N); + + return *this; + } + + void + diagonal(Vector<T> &diag) + { + int sz = diag.dim(); + newsize(sz,sz); + set(0); + + Subscript i; + for (i = 0; i < sz; i++) { + row_[i][i] = diag[i]; + } + } + + + + // assignments + // + Matrix<T>& operator=(const Matrix<T> &A) + { + if (v_ == A.v_) + return *this; + + if (m_ == A.m_ && n_ == A.n_) // no need to re-alloc + copy(A.v_); + + else + { + destroy(); + initialize(A.m_, A.n_); + copy(A.v_); + } + + return *this; + } + + Matrix<T>& operator=(const T& scalar) + { + set(scalar); + return *this; + } + + + Subscript dim(Subscript d) const + { +#ifdef TNT_BOUNDS_CHECK + assert( d >= 1); + assert( d <= 2); +#endif + return (d==1) ? m_ : ((d==2) ? n_ : 0); + } + + Subscript num_rows() const { return m_; } + Subscript num_cols() const { return n_; } + + + + + inline T* operator[](Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(0<=i); + assert(i < m_) ; +#endif + return row_[i]; + } + + inline const T* operator[](Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(0<=i); + assert(i < m_) ; +#endif + return row_[i]; + } + + inline reference operator()(Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= mn_) ; +#endif + return vm1_[i]; + } + + inline const_reference operator()(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= mn_) ; +#endif + return vm1_[i]; + } + + + + inline reference operator()(Subscript i, Subscript j) + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= m_) ; + assert(1<=j); + assert(j <= n_); +#endif + return rowm1_[i][j]; + } + + + + inline const_reference operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= m_) ; + assert(1<=j); + assert(j <= n_); +#endif + return rowm1_[i][j]; + } + + + +#ifdef TNT_USE_REGIONS + + typedef Region2D<Matrix<T> > Region; + + + Region operator()(const Index1D &I, const Index1D &J) + { + return Region(*this, I,J); + } + + + typedef const_Region2D< Matrix<T> > const_Region; + const_Region operator()(const Index1D &I, const Index1D &J) const + { + return const_Region(*this, I,J); + } + +#endif + + +}; + + +/* *************************** I/O ********************************/ + +template <class T> +std::ostream& operator<<(std::ostream &s, const Matrix<T> &A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + s << M << " " << N << "\n"; + + for (Subscript i=0; i<M; i++) + { + for (Subscript j=0; j<N; j++) + { + s << A[i][j] << " "; + } + s << "\n"; + } + + + return s; +} + +template <class T> +std::istream& operator>>(std::istream &s, Matrix<T> &A) +{ + + Subscript M, N; + + s >> M >> N; + + if ( !(M == A.num_rows() && N == A.num_cols() )) + { + A.newsize(M,N); + } + + + for (Subscript i=0; i<M; i++) + for (Subscript j=0; j<N; j++) + { + s >> A[i][j]; + } + + + return s; +} + +// *******************[ basic matrix algorithms ]*************************** + +template <class T> +Matrix<T> operator+(const Matrix<T> &A, + const Matrix<T> &B) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M==B.num_rows()); + assert(N==B.num_cols()); + + Matrix<T> tmp(M,N); + Subscript i,j; + + for (i=0; i<M; i++) + for (j=0; j<N; j++) + tmp[i][j] = A[i][j] + B[i][j]; + + return tmp; +} + +template <class T> +Matrix<T> operator-(const Matrix<T> &A, + const Matrix<T> &B) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M==B.num_rows()); + assert(N==B.num_cols()); + + Matrix<T> tmp(M,N); + Subscript i,j; + + for (i=0; i<M; i++) + for (j=0; j<N; j++) + tmp[i][j] = A[i][j] - B[i][j]; + + return tmp; +} + +template <class T> +Matrix<T> mult_element(const Matrix<T> &A, + const Matrix<T> &B) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M==B.num_rows()); + assert(N==B.num_cols()); + + Matrix<T> tmp(M,N); + Subscript i,j; + + for (i=0; i<M; i++) + for (j=0; j<N; j++) + tmp[i][j] = A[i][j] * B[i][j]; + + return tmp; +} + +template <class T> +void transpose(const Matrix<T> &A, Matrix<T> &S) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M==S.num_cols()); + assert(N==S.num_rows()); + + Subscript i, j; + + for (i=0; i<M; i++) + for (j=0; j<N; j++) + S[j][i] = A[i][j]; + +} + + +template <class T> +inline void matmult(Matrix<T>& C, const Matrix<T> &A, + const Matrix<T> &B) +{ + + assert(A.num_cols() == B.num_rows()); + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + Subscript K = B.num_cols(); + + C.newsize(M,K); + + T sum; + + const T* row_i; + const T* col_k; + + for (Subscript i=0; i<M; i++) + for (Subscript k=0; k<K; k++) + { + row_i = &(A[i][0]); + col_k = &(B[0][k]); + sum = 0; + for (Subscript j=0; j<N; j++) + { + sum += *row_i * *col_k; + row_i++; + col_k += K; + } + C[i][k] = sum; + } + +} + +template <class T> +void matmult(Vector<T> &y, const Matrix<T> &A, const Vector<T> &x) +{ + +#ifdef TNT_BOUNDS_CHECK + assert(A.num_cols() == x.dim()); + assert(A.num_rows() == y.dim()); +#endif + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + T sum; + + for (Subscript i=0; i<M; i++) + { + sum = 0; + const T* rowi = A[i]; + for (Subscript j=0; j<N; j++) + sum = sum + rowi[j] * x[j]; + + y[i] = sum; + } +} + +template <class T> +inline void matmultdiag( + Matrix<T>& C, + const Matrix<T> &A, + const Vector<T> &diag +){ +#ifdef TNT_BOUNDS_CHECK + assert(A.num_cols() ==A.num_rows()== diag.dim()); +#endif + + Subscript M = A.num_rows(); + Subscript K = diag.dim(); + + C.newsize(M,K); + + const T* row_i; + const T* col_k; + + for (Subscript i=0; i<M; i++) { + for (Subscript k=0; k<K; k++) + { + C[i][k] = A[i,k] * diag[k]; + } + } +} + + +template <class T> +inline void matmultdiag( + Matrix<T>& C, + const Vector<T> &diag, + const Matrix<T> &A +){ +#ifdef TNT_BOUNDS_CHECK + assert(A.num_cols() ==A.num_rows()== diag.dim()); +#endif + + Subscript M = A.num_rows(); + Subscript K = diag.dim(); + + C.newsize(M,K); + + for (Subscript i=0; i<M; i++) { + + const T diag_element = diag[i]; + + for (Subscript k=0; k<K; k++) + { + C[i][k] = A[i][k] * diag_element; + } + } +} + + + + +} // namespace TNT + + + + +#endif +// CMAT_H diff --git a/intern/iksolver/intern/TNT/fcscmat.h b/intern/iksolver/intern/TNT/fcscmat.h new file mode 100644 index 00000000000..877364ad38c --- /dev/null +++ b/intern/iksolver/intern/TNT/fcscmat.h @@ -0,0 +1,197 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Templated compressed sparse column matrix (Fortran conventions). +// uses 1-based offsets in storing row indices. +// Used primarily to interface with Fortran sparse matrix libaries. +// (CANNOT BE USED AS AN STL CONTAINER.) + + +#ifndef FCSCMAT_H +#define FCSCMAT_H + +#include <iostream> +#include <cassert> +#include "tnt.h" +#include "vec.h" + +using namespace std; + +namespace TNT +{ + +template <class T> +class Fortran_Sparse_Col_Matrix +{ + + protected: + + Vector<T> val_; // data values (nz_ elements) + Vector<Subscript> rowind_; // row_ind (nz_ elements) + Vector<Subscript> colptr_; // col_ptr (n_+1 elements) + + int nz_; // number of nonzeros + Subscript m_; // global dimensions + Subscript n_; + + public: + + + Fortran_Sparse_Col_Matrix(void); + Fortran_Sparse_Col_Matrix(const Fortran_Sparse_Col_Matrix<T> &S) + : val_(S.val_), rowind_(S.rowind_), colptr_(S.colptr_), nz_(S.nz_), + m_(S.m_), n_(S.n_) {}; + Fortran_Sparse_Col_Matrix(Subscript M, Subscript N, + Subscript nz, const T *val, const Subscript *r, + const Subscript *c) : val_(nz, val), rowind_(nz, r), + colptr_(N+1, c), nz_(nz), m_(M), n_(N) {}; + + Fortran_Sparse_Col_Matrix(Subscript M, Subscript N, + Subscript nz, char *val, char *r, + char *c) : val_(nz, val), rowind_(nz, r), + colptr_(N+1, c), nz_(nz), m_(M), n_(N) {}; + + Fortran_Sparse_Col_Matrix(Subscript M, Subscript N, + Subscript nz, const T *val, Subscript *r, Subscript *c) + : val_(nz, val), rowind_(nz, r), colptr_(N+1, c), nz_(nz), + m_(M), n_(N) {}; + + ~Fortran_Sparse_Col_Matrix() {}; + + + T & val(Subscript i) { return val_(i); } + const T & val(Subscript i) const { return val_(i); } + + Subscript & row_ind(Subscript i) { return rowind_(i); } + const Subscript & row_ind(Subscript i) const { return rowind_(i); } + + Subscript col_ptr(Subscript i) { return colptr_(i);} + const Subscript col_ptr(Subscript i) const { return colptr_(i);} + + + Subscript num_cols() const { return m_;} + Subscript num_rows() const { return n_; } + + Subscript dim(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert( 1 <= i ); + assert( i <= 2 ); +#endif + if (i==1) return m_; + else if (i==2) return m_; + else return 0; + } + + Subscript num_nonzeros() const {return nz_;}; + Subscript lbound() const {return 1;} + + + + Fortran_Sparse_Col_Matrix& operator=(const + Fortran_Sparse_Col_Matrix &C) + { + val_ = C.val_; + rowind_ = C.rowind_; + colptr_ = C.colptr_; + nz_ = C.nz_; + m_ = C.m_; + n_ = C.n_; + + return *this; + } + + Fortran_Sparse_Col_Matrix& newsize(Subscript M, Subscript N, + Subscript nz) + { + val_.newsize(nz); + rowind_.newsize(nz); + colptr_.newsize(N+1); + return *this; + } +}; + +template <class T> +ostream& operator<<(ostream &s, const Fortran_Sparse_Col_Matrix<T> &A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + s << M << " " << N << " " << A.num_nonzeros() << endl; + + + for (Subscript k=1; k<=N; k++) + { + Subscript start = A.col_ptr(k); + Subscript end = A.col_ptr(k+1); + + for (Subscript i= start; i<end; i++) + { + s << A.row_ind(i) << " " << k << " " << A.val(i) << endl; + } + } + + return s; +} + + + +} // namespace TNT + +#endif +/* FCSCMAT_H */ + diff --git a/intern/iksolver/intern/TNT/fmat.h b/intern/iksolver/intern/TNT/fmat.h new file mode 100644 index 00000000000..5de9a63813e --- /dev/null +++ b/intern/iksolver/intern/TNT/fmat.h @@ -0,0 +1,609 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Fortran-compatible matrix: column oriented, 1-based (i,j) indexing + +#ifndef FMAT_H +#define FMAT_H + +#include "subscript.h" +#include "vec.h" +#include <cstdlib> +#include <cassert> +#include <iostream> +#include <strstream> +#ifdef TNT_USE_REGIONS +#include "region2d.h" +#endif + +// simple 1-based, column oriented Matrix class + +namespace TNT +{ + +template <class T> +class Fortran_Matrix +{ + + + public: + + typedef T value_type; + typedef T element_type; + typedef T* pointer; + typedef T* iterator; + typedef T& reference; + typedef const T* const_iterator; + typedef const T& const_reference; + + Subscript lbound() const { return 1;} + + protected: + T* v_; // these are adjusted to simulate 1-offset + Subscript m_; + Subscript n_; + T** col_; // these are adjusted to simulate 1-offset + + // internal helper function to create the array + // of row pointers + + void initialize(Subscript M, Subscript N) + { + // adjust col_[] pointers so that they are 1-offset: + // col_[j][i] is really col_[j-1][i-1]; + // + // v_[] is the internal contiguous array, it is still 0-offset + // + v_ = new T[M*N]; + col_ = new T*[N]; + + assert(v_ != NULL); + assert(col_ != NULL); + + + m_ = M; + n_ = N; + T* p = v_ - 1; + for (Subscript i=0; i<N; i++) + { + col_[i] = p; + p += M ; + + } + col_ --; + } + + void copy(const T* v) + { + Subscript N = m_ * n_; + Subscript i; + +#ifdef TNT_UNROLL_LOOPS + Subscript Nmod4 = N & 3; + Subscript N4 = N - Nmod4; + + for (i=0; i<N4; i+=4) + { + v_[i] = v[i]; + v_[i+1] = v[i+1]; + v_[i+2] = v[i+2]; + v_[i+3] = v[i+3]; + } + + for (i=N4; i< N; i++) + v_[i] = v[i]; +#else + + for (i=0; i< N; i++) + v_[i] = v[i]; +#endif + } + + void set(const T& val) + { + Subscript N = m_ * n_; + Subscript i; + +#ifdef TNT_UNROLL_LOOPS + Subscript Nmod4 = N & 3; + Subscript N4 = N - Nmod4; + + for (i=0; i<N4; i+=4) + { + v_[i] = val; + v_[i+1] = val; + v_[i+2] = val; + v_[i+3] = val; + } + + for (i=N4; i< N; i++) + v_[i] = val; +#else + + for (i=0; i< N; i++) + v_[i] = val; + +#endif + } + + + + void destroy() + { + /* do nothing, if no memory has been previously allocated */ + if (v_ == NULL) return ; + + /* if we are here, then matrix was previously allocated */ + delete [] (v_); + col_ ++; // changed back to 0-offset + delete [] (col_); + } + + + public: + + T* begin() { return v_; } + const T* begin() const { return v_;} + + T* end() { return v_ + m_*n_; } + const T* end() const { return v_ + m_*n_; } + + + // constructors + + Fortran_Matrix() : v_(0), m_(0), n_(0), col_(0) {}; + Fortran_Matrix(const Fortran_Matrix<T> &A) + { + initialize(A.m_, A.n_); + copy(A.v_); + } + + Fortran_Matrix(Subscript M, Subscript N, const T& value = T()) + { + initialize(M,N); + set(value); + } + + Fortran_Matrix(Subscript M, Subscript N, const T* v) + { + initialize(M,N); + copy(v); + } + + + Fortran_Matrix(Subscript M, Subscript N, char *s) + { + initialize(M,N); + std::istrstream ins(s); + + Subscript i, j; + + for (i=1; i<=M; i++) + for (j=1; j<=N; j++) + ins >> (*this)(i,j); + } + + // destructor + ~Fortran_Matrix() + { + destroy(); + } + + + // assignments + // + Fortran_Matrix<T>& operator=(const Fortran_Matrix<T> &A) + { + if (v_ == A.v_) + return *this; + + if (m_ == A.m_ && n_ == A.n_) // no need to re-alloc + copy(A.v_); + + else + { + destroy(); + initialize(A.m_, A.n_); + copy(A.v_); + } + + return *this; + } + + Fortran_Matrix<T>& operator=(const T& scalar) + { + set(scalar); + return *this; + } + + + Subscript dim(Subscript d) const + { +#ifdef TNT_BOUNDS_CHECK + assert( d >= 1); + assert( d <= 2); +#endif + return (d==1) ? m_ : ((d==2) ? n_ : 0); + } + + Subscript num_rows() const { return m_; } + Subscript num_cols() const { return n_; } + + Fortran_Matrix<T>& newsize(Subscript M, Subscript N) + { + if (num_rows() == M && num_cols() == N) + return *this; + + destroy(); + initialize(M,N); + + return *this; + } + + + + // 1-based element access + // + inline reference operator()(Subscript i, Subscript j) + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= m_) ; + assert(1<=j); + assert(j <= n_); +#endif + return col_[j][i]; + } + + inline const_reference operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= m_) ; + assert(1<=j); + assert(j <= n_); +#endif + return col_[j][i]; + } + + +#ifdef TNT_USE_REGIONS + + typedef Region2D<Fortran_Matrix<T> > Region; + typedef const_Region2D< Fortran_Matrix<T> > const_Region; + + Region operator()(const Index1D &I, const Index1D &J) + { + return Region(*this, I,J); + } + + const_Region operator()(const Index1D &I, const Index1D &J) const + { + return const_Region(*this, I,J); + } + +#endif + + +}; + + +/* *************************** I/O ********************************/ + +template <class T> +std::ostream& operator<<(std::ostream &s, const Fortran_Matrix<T> &A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + s << M << " " << N << "\n"; + + for (Subscript i=1; i<=M; i++) + { + for (Subscript j=1; j<=N; j++) + { + s << A(i,j) << " "; + } + s << "\n"; + } + + + return s; +} + +template <class T> +std::istream& operator>>(std::istream &s, Fortran_Matrix<T> &A) +{ + + Subscript M, N; + + s >> M >> N; + + if ( !(M == A.num_rows() && N == A.num_cols())) + { + A.newsize(M,N); + } + + + for (Subscript i=1; i<=M; i++) + for (Subscript j=1; j<=N; j++) + { + s >> A(i,j); + } + + + return s; +} + +// *******************[ basic matrix algorithms ]*************************** + + +template <class T> +Fortran_Matrix<T> operator+(const Fortran_Matrix<T> &A, + const Fortran_Matrix<T> &B) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M==B.num_rows()); + assert(N==B.num_cols()); + + Fortran_Matrix<T> tmp(M,N); + Subscript i,j; + + for (i=1; i<=M; i++) + for (j=1; j<=N; j++) + tmp(i,j) = A(i,j) + B(i,j); + + return tmp; +} + +template <class T> +Fortran_Matrix<T> operator-(const Fortran_Matrix<T> &A, + const Fortran_Matrix<T> &B) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M==B.num_rows()); + assert(N==B.num_cols()); + + Fortran_Matrix<T> tmp(M,N); + Subscript i,j; + + for (i=1; i<=M; i++) + for (j=1; j<=N; j++) + tmp(i,j) = A(i,j) - B(i,j); + + return tmp; +} + +// element-wise multiplication (use matmult() below for matrix +// multiplication in the linear algebra sense.) +// +// +template <class T> +Fortran_Matrix<T> mult_element(const Fortran_Matrix<T> &A, + const Fortran_Matrix<T> &B) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M==B.num_rows()); + assert(N==B.num_cols()); + + Fortran_Matrix<T> tmp(M,N); + Subscript i,j; + + for (i=1; i<=M; i++) + for (j=1; j<=N; j++) + tmp(i,j) = A(i,j) * B(i,j); + + return tmp; +} + + +template <class T> +Fortran_Matrix<T> transpose(const Fortran_Matrix<T> &A) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + Fortran_Matrix<T> S(N,M); + Subscript i, j; + + for (i=1; i<=M; i++) + for (j=1; j<=N; j++) + S(j,i) = A(i,j); + + return S; +} + + + +template <class T> +inline Fortran_Matrix<T> matmult(const Fortran_Matrix<T> &A, + const Fortran_Matrix<T> &B) +{ + +#ifdef TNT_BOUNDS_CHECK + assert(A.num_cols() == B.num_rows()); +#endif + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + Subscript K = B.num_cols(); + + Fortran_Matrix<T> tmp(M,K); + T sum; + + for (Subscript i=1; i<=M; i++) + for (Subscript k=1; k<=K; k++) + { + sum = 0; + for (Subscript j=1; j<=N; j++) + sum = sum + A(i,j) * B(j,k); + + tmp(i,k) = sum; + } + + return tmp; +} + +template <class T> +inline Fortran_Matrix<T> operator*(const Fortran_Matrix<T> &A, + const Fortran_Matrix<T> &B) +{ + return matmult(A,B); +} + +template <class T> +inline int matmult(Fortran_Matrix<T>& C, const Fortran_Matrix<T> &A, + const Fortran_Matrix<T> &B) +{ + + assert(A.num_cols() == B.num_rows()); + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + Subscript K = B.num_cols(); + + C.newsize(M,K); // adjust shape of C, if necessary + + + T sum; + + const T* row_i; + const T* col_k; + + for (Subscript i=1; i<=M; i++) + { + for (Subscript k=1; k<=K; k++) + { + row_i = &A(i,1); + col_k = &B(1,k); + sum = 0; + for (Subscript j=1; j<=N; j++) + { + sum += *row_i * *col_k; + row_i += M; + col_k ++; + } + + C(i,k) = sum; + } + + } + + return 0; +} + + +template <class T> +Vector<T> matmult(const Fortran_Matrix<T> &A, const Vector<T> &x) +{ + +#ifdef TNT_BOUNDS_CHECK + assert(A.num_cols() == x.dim()); +#endif + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + Vector<T> tmp(M); + T sum; + + for (Subscript i=1; i<=M; i++) + { + sum = 0; + for (Subscript j=1; j<=N; j++) + sum = sum + A(i,j) * x(j); + + tmp(i) = sum; + } + + return tmp; +} + +template <class T> +inline Vector<T> operator*(const Fortran_Matrix<T> &A, const Vector<T> &x) +{ + return matmult(A,x); +} + +template <class T> +inline Fortran_Matrix<T> operator*(const Fortran_Matrix<T> &A, const T &x) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + Subscript MN = M*N; + + Fortran_Matrix<T> res(M,N); + const T* a = A.begin(); + T* t = res.begin(); + T* tend = res.end(); + + for (t=res.begin(); t < tend; t++, a++) + *t = *a * x; + + return res; +} + +} // namespace TNT +#endif +// FMAT_H diff --git a/intern/iksolver/intern/TNT/fortran.h b/intern/iksolver/intern/TNT/fortran.h new file mode 100644 index 00000000000..4f513300588 --- /dev/null +++ b/intern/iksolver/intern/TNT/fortran.h @@ -0,0 +1,99 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Header file to define C/Fortran conventions (Platform specific) + +#ifndef FORTRAN_H +#define FORTRAN_H + +// help map between C/C++ data types and Fortran types + +typedef int Fortran_integer; +typedef float Fortran_float; +typedef double Fortran_double; + + +typedef Fortran_double *fda_; // (in/out) double precision array +typedef const Fortran_double *cfda_; // (in) double precsion array + +typedef Fortran_double *fd_; // (in/out) single double precision +typedef const Fortran_double *cfd_; // (in) single double precision + +typedef Fortran_float *ffa_; // (in/out) float precision array +typedef const Fortran_float *cffa_; // (in) float precsion array + +typedef Fortran_float *ff_; // (in/out) single float precision +typedef const Fortran_float *cff_; // (in) single float precision + +typedef Fortran_integer *fia_; // (in/out) single integer array +typedef const Fortran_integer *cfia_; // (in) single integer array + +typedef Fortran_integer *fi_; // (in/out) single integer +typedef const Fortran_integer *cfi_; // (in) single integer + +typedef char *fch_; // (in/out) single character +typedef char *cfch_; // (in) single character + + + +#ifndef TNT_SUBSCRIPT_TYPE +#define TNT_SUBSCRIPT_TYPE TNT::Fortran_integer +#endif + + +#endif +// FORTRAN_H diff --git a/intern/iksolver/intern/TNT/fspvec.h b/intern/iksolver/intern/TNT/fspvec.h new file mode 100644 index 00000000000..b8dde35b6c6 --- /dev/null +++ b/intern/iksolver/intern/TNT/fspvec.h @@ -0,0 +1,200 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + +// Templated sparse vector (Fortran conventions). +// Used primarily to interface with Fortran sparse matrix libaries. +// (CANNOT BE USED AS AN STL CONTAINER.) + +#ifndef FSPVEC_H +#define FSPVEC_H + +#include "tnt.h" +#include "vec.h" +#include <cstdlib> +#include <cassert> +#include <iostream> +#include <strstream> + +using namespace std; + +namespace TNT +{ + +template <class T> +class Fortran_Sparse_Vector +{ + + + public: + + typedef Subscript size_type; + typedef T value_type; + typedef T element_type; + typedef T* pointer; + typedef T* iterator; + typedef T& reference; + typedef const T* const_iterator; + typedef const T& const_reference; + + Subscript lbound() const { return 1;} + + protected: + Vector<T> val_; + Vector<Subscript> index_; + Subscript dim_; // prescribed dimension + + + public: + + // size and shape information + + Subscript dim() const { return dim_; } + Subscript num_nonzeros() const { return val_.dim(); } + + // access + + T& val(Subscript i) { return val_(i); } + const T& val(Subscript i) const { return val_(i); } + + Subscript &index(Subscript i) { return index_(i); } + const Subscript &index(Subscript i) const { return index_(i); } + + // constructors + + Fortran_Sparse_Vector() : val_(), index_(), dim_(0) {}; + Fortran_Sparse_Vector(Subscript N, Subscript nz) : val_(nz), + index_(nz), dim_(N) {}; + Fortran_Sparse_Vector(Subscript N, Subscript nz, const T *values, + const Subscript *indices): val_(nz, values), index_(nz, indices), + dim_(N) {} + + Fortran_Sparse_Vector(const Fortran_Sparse_Vector<T> &S): + val_(S.val_), index_(S.index_), dim_(S.dim_) {} + + // initialize from string, e.g. + // + // Fortran_Sparse_Vector<T> A(N, 2, "1.0 2.1", "1 3"); + // + Fortran_Sparse_Vector(Subscript N, Subscript nz, char *v, + char *ind) : val_(nz, v), index_(nz, ind), dim_(N) {} + + // assignments + + Fortran_Sparse_Vector<T> & newsize(Subscript N, Subscript nz) + { + val_.newsize(nz); + index_.newsize(nz); + dim_ = N; + return *this; + } + + Fortran_Sparse_Vector<T> & operator=( const Fortran_Sparse_Vector<T> &A) + { + val_ = A.val_; + index_ = A.index_; + dim_ = A.dim_; + + return *this; + } + + // methods + + + +}; + + +/* *************************** I/O ********************************/ + +template <class T> +ostream& operator<<(ostream &s, const Fortran_Sparse_Vector<T> &A) +{ + // output format is : N nz val1 ind1 val2 ind2 ... + Subscript nz=A.num_nonzeros(); + + s << A.dim() << " " << nz << endl; + + for (Subscript i=1; i<=nz; i++) + s << A.val(i) << " " << A.index(i) << endl; + s << endl; + + return s; +} + + +template <class T> +istream& operator>>(istream &s, Fortran_Sparse_Vector<T> &A) +{ + // output format is : N nz val1 ind1 val2 ind2 ... + + Subscript N; + Subscript nz; + + s >> N >> nz; + + A.newsize(N, nz); + + for (Subscript i=1; i<=nz; i++) + s >> A.val(i) >> A.index(i); + + + return s; +} + +} // namespace TNT + +#endif +// FSPVEC_H diff --git a/intern/iksolver/intern/TNT/index.h b/intern/iksolver/intern/TNT/index.h new file mode 100644 index 00000000000..885545d11b5 --- /dev/null +++ b/intern/iksolver/intern/TNT/index.h @@ -0,0 +1,115 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Vector/Matrix/Array Index Module + +#ifndef INDEX_H +#define INDEX_H + +#include "subscript.h" + +namespace TNT +{ + +class Index1D +{ + Subscript lbound_; + Subscript ubound_; + + public: + + Subscript lbound() const { return lbound_; } + Subscript ubound() const { return ubound_; } + + Index1D(const Index1D &D) : lbound_(D.lbound_), ubound_(D.ubound_) {} + Index1D(Subscript i1, Subscript i2) : lbound_(i1), ubound_(i2) {} + + Index1D & operator=(const Index1D &D) + { + lbound_ = D.lbound_; + ubound_ = D.ubound_; + return *this; + } + +}; + +inline Index1D operator+(const Index1D &D, Subscript i) +{ + return Index1D(i+D.lbound(), i+D.ubound()); +} + +inline Index1D operator+(Subscript i, const Index1D &D) +{ + return Index1D(i+D.lbound(), i+D.ubound()); +} + + + +inline Index1D operator-(Index1D &D, Subscript i) +{ + return Index1D(D.lbound()-i, D.ubound()-i); +} + +inline Index1D operator-(Subscript i, Index1D &D) +{ + return Index1D(i-D.lbound(), i-D.ubound()); +} + +} // namespace TNT + +#endif + diff --git a/intern/iksolver/intern/TNT/lapack.h b/intern/iksolver/intern/TNT/lapack.h new file mode 100644 index 00000000000..d3556a96071 --- /dev/null +++ b/intern/iksolver/intern/TNT/lapack.h @@ -0,0 +1,225 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Header file for Fortran Lapack + +#ifndef LAPACK_H +#define LAPACK_H + +// This file incomplete and included here to only demonstrate the +// basic framework for linking with the Fortran Lapack routines. + +#include "fortran.h" +#include "vec.h" +#include "fmat.h" + + +#define F77_DGESV dgesv_ +#define F77_DGELS dgels_ +#define F77_DSYEV dsyev_ +#define F77_DGEEV dgeev_ + +extern "C" +{ + + // linear equations (general) using LU factorizaiton + // + void F77_DGESV(cfi_ N, cfi_ nrhs, fda_ A, cfi_ lda, + fia_ ipiv, fda_ b, cfi_ ldb, fi_ info); + + // solve linear least squares using QR or LU factorization + // + void F77_DGELS(cfch_ trans, cfi_ M, + cfi_ N, cfi_ nrhs, fda_ A, cfi_ lda, fda_ B, cfi_ ldb, fda_ work, + cfi_ lwork, fi_ info); + + // solve symmetric eigenvalues + // + void F77_DSYEV( cfch_ jobz, cfch_ uplo, cfi_ N, fda_ A, cfi_ lda, + fda_ W, fda_ work, cfi_ lwork, fi_ info); + + // solve unsymmetric eigenvalues + // + void F77_DGEEV(cfch_ jobvl, cfch_ jobvr, cfi_ N, fda_ A, cfi_ lda, + fda_ wr, fda_ wi, fda_ vl, cfi_ ldvl, fda_ vr, + cfi_ ldvr, fda_ work, cfi_ lwork, fi_ info); + +} + +// solve linear equations using LU factorization + +using namespace TNT; + +Vector<double> Lapack_LU_linear_solve(const Fortran_Matrix<double> &A, + const Vector<double> &b) +{ + const Fortran_integer one=1; + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + Fortran_Matrix<double> Tmp(A); + Vector<double> x(b); + Vector<Fortran_integer> index(M); + Fortran_integer info = 0; + + F77_DGESV(&N, &one, &Tmp(1,1), &M, &index(1), &x(1), &M, &info); + + if (info != 0) return Vector<double>(0); + else + return x; +} + +// solve linear least squares problem using QR factorization +// +Vector<double> Lapack_LLS_QR_linear_solve(const Fortran_Matrix<double> &A, + const Vector<double> &b) +{ + const Fortran_integer one=1; + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + Fortran_Matrix<double> Tmp(A); + Vector<double> x(b); + Fortran_integer info = 0; + + char transp = 'N'; + Fortran_integer lwork = 5 * (M+N); // temporary work space + Vector<double> work(lwork); + + F77_DGELS(&transp, &M, &N, &one, &Tmp(1,1), &M, &x(1), &M, &work(1), + &lwork, &info); + + if (info != 0) return Vector<double>(0); + else + return x; +} + +// *********************** Eigenvalue problems ******************* + +// solve symmetric eigenvalue problem (eigenvalues only) +// +Vector<double> Upper_symmetric_eigenvalue_solve(const Fortran_Matrix<double> &A) +{ + char jobz = 'N'; + char uplo = 'U'; + Subscript N = A.num_rows(); + + assert(N == A.num_cols()); + + Vector<double> eigvals(N); + Fortran_integer worksize = 3*N; + Fortran_integer info = 0; + Vector<double> work(worksize); + Fortran_Matrix<double> Tmp = A; + + F77_DSYEV(&jobz, &uplo, &N, &Tmp(1,1), &N, eigvals.begin(), work.begin(), + &worksize, &info); + + if (info != 0) return Vector<double>(); + else + return eigvals; +} + + +// solve unsymmetric eigenvalue problems +// +int eigenvalue_solve(const Fortran_Matrix<double> &A, + Vector<double> &wr, Vector<double> &wi) +{ + char jobvl = 'N'; + char jobvr = 'N'; + + Fortran_integer N = A.num_rows(); + + + assert(N == A.num_cols()); + + if (N<1) return 1; + + Fortran_Matrix<double> vl(1,N); /* should be NxN ? **** */ + Fortran_Matrix<double> vr(1,N); + Fortran_integer one = 1; + + Fortran_integer worksize = 5*N; + Fortran_integer info = 0; + Vector<double> work(worksize, 0.0); + Fortran_Matrix<double> Tmp = A; + + wr.newsize(N); + wi.newsize(N); + +// void F77_DGEEV(cfch_ jobvl, cfch_ jobvr, cfi_ N, fda_ A, cfi_ lda, +// fda_ wr, fda_ wi, fda_ vl, cfi_ ldvl, fda_ vr, +// cfi_ ldvr, fda_ work, cfi_ lwork, fi_ info); + + F77_DGEEV(&jobvl, &jobvr, &N, &Tmp(1,1), &N, &(wr(1)), + &(wi(1)), &(vl(1,1)), &one, &(vr(1,1)), &one, + &(work(1)), &worksize, &info); + + return (info==0 ? 0: 1); +} + + + + + +#endif +// LAPACK_H + + + + diff --git a/intern/iksolver/intern/TNT/lu.h b/intern/iksolver/intern/TNT/lu.h new file mode 100644 index 00000000000..b86027aa386 --- /dev/null +++ b/intern/iksolver/intern/TNT/lu.h @@ -0,0 +1,236 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +#ifndef LU_H +#define LU_H + +// Solve system of linear equations Ax = b. +// +// Typical usage: +// +// Matrix(double) A; +// Vector(Subscript) ipiv; +// Vector(double) b; +// +// 1) LU_Factor(A,ipiv); +// 2) LU_Solve(A,ipiv,b); +// +// Now b has the solution x. Note that both A and b +// are overwritten. If these values need to be preserved, +// one can make temporary copies, as in +// +// O) Matrix(double) T = A; +// 1) LU_Factor(T,ipiv); +// 1a) Vector(double) x=b; +// 2) LU_Solve(T,ipiv,x); +// +// See details below. +// + + +// for fabs() +// +#include <cmath> + +// right-looking LU factorization algorithm (unblocked) +// +// Factors matrix A into lower and upper triangular matrices +// (L and U respectively) in solving the linear equation Ax=b. +// +// +// Args: +// +// A (input/output) Matrix(1:n, 1:n) In input, matrix to be +// factored. On output, overwritten with lower and +// upper triangular factors. +// +// indx (output) Vector(1:n) Pivot vector. Describes how +// the rows of A were reordered to increase +// numerical stability. +// +// Return value: +// +// int (0 if successful, 1 otherwise) +// +// + + +namespace TNT +{ + +template <class MaTRiX, class VecToRSubscript> +int LU_factor( MaTRiX &A, VecToRSubscript &indx) +{ + assert(A.lbound() == 1); // currently for 1-offset + assert(indx.lbound() == 1); // vectors and matrices + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + if (M == 0 || N==0) return 0; + if (indx.dim() != M) + indx.newsize(M); + + Subscript i=0,j=0,k=0; + Subscript jp=0; + + typename MaTRiX::element_type t; + + Subscript minMN = (M < N ? M : N) ; // min(M,N); + + for (j=1; j<= minMN; j++) + { + + // find pivot in column j and test for singularity. + + jp = j; + t = fabs(A(j,j)); + for (i=j+1; i<=M; i++) + if ( fabs(A(i,j)) > t) + { + jp = i; + t = fabs(A(i,j)); + } + + indx(j) = jp; + + // jp now has the index of maximum element + // of column j, below the diagonal + + if ( A(jp,j) == 0 ) + return 1; // factorization failed because of zero pivot + + + if (jp != j) // swap rows j and jp + for (k=1; k<=N; k++) + { + t = A(j,k); + A(j,k) = A(jp,k); + A(jp,k) =t; + } + + if (j<M) // compute elements j+1:M of jth column + { + // note A(j,j), was A(jp,p) previously which was + // guarranteed not to be zero (Label #1) + // + typename MaTRiX::element_type recp = 1.0 / A(j,j); + + for (k=j+1; k<=M; k++) + A(k,j) *= recp; + } + + + if (j < minMN) + { + // rank-1 update to trailing submatrix: E = E - x*y; + // + // E is the region A(j+1:M, j+1:N) + // x is the column vector A(j+1:M,j) + // y is row vector A(j,j+1:N) + + Subscript ii,jj; + + for (ii=j+1; ii<=M; ii++) + for (jj=j+1; jj<=N; jj++) + A(ii,jj) -= A(ii,j)*A(j,jj); + } + } + + return 0; +} + + + + +template <class MaTRiX, class VecToR, class VecToRSubscripts> +int LU_solve(const MaTRiX &A, const VecToRSubscripts &indx, VecToR &b) +{ + assert(A.lbound() == 1); // currently for 1-offset + assert(indx.lbound() == 1); // vectors and matrices + assert(b.lbound() == 1); + + Subscript i,ii=0,ip,j; + Subscript n = b.dim(); + typename MaTRiX::element_type sum = 0.0; + + for (i=1;i<=n;i++) + { + ip=indx(i); + sum=b(ip); + b(ip)=b(i); + if (ii) + for (j=ii;j<=i-1;j++) + sum -= A(i,j)*b(j); + else if (sum) ii=i; + b(i)=sum; + } + for (i=n;i>=1;i--) + { + sum=b(i); + for (j=i+1;j<=n;j++) + sum -= A(i,j)*b(j); + b(i)=sum/A(i,i); + } + + return 0; +} + +} // namespace TNT + +#endif +// LU_H diff --git a/intern/iksolver/intern/TNT/qr.h b/intern/iksolver/intern/TNT/qr.h new file mode 100644 index 00000000000..074551896b9 --- /dev/null +++ b/intern/iksolver/intern/TNT/qr.h @@ -0,0 +1,261 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + +#ifndef QR_H +#define QR_H + +// Classical QR factorization example, based on Stewart[1973]. +// +// +// This algorithm computes the factorization of a matrix A +// into a product of an orthognal matrix (Q) and an upper triangular +// matrix (R), such that QR = A. +// +// Parameters: +// +// A (in): Matrix(1:N, 1:N) +// +// Q (output): Matrix(1:N, 1:N), collection of Householder +// column vectors Q1, Q2, ... QN +// +// R (output): upper triangular Matrix(1:N, 1:N) +// +// Returns: +// +// 0 if successful, 1 if A is detected to be singular +// + + +#include <cmath> //for sqrt() & fabs() +#include "tntmath.h" // for sign() + +// Classical QR factorization, based on Stewart[1973]. +// +// +// This algorithm computes the factorization of a matrix A +// into a product of an orthognal matrix (Q) and an upper triangular +// matrix (R), such that QR = A. +// +// Parameters: +// +// A (in/out): On input, A is square, Matrix(1:N, 1:N), that represents +// the matrix to be factored. +// +// On output, Q and R is encoded in the same Matrix(1:N,1:N) +// in the following manner: +// +// R is contained in the upper triangular section of A, +// except that R's main diagonal is in D. The lower +// triangular section of A represents Q, where each +// column j is the vector Qj = I - uj*uj'/pi_j. +// +// C (output): vector of Pi[j] +// D (output): main diagonal of R, i.e. D(i) is R(i,i) +// +// Returns: +// +// 0 if successful, 1 if A is detected to be singular +// + +namespace TNT +{ + +template <class MaTRiX, class Vector> +int QR_factor(MaTRiX &A, Vector& C, Vector &D) +{ + assert(A.lbound() == 1); // ensure these are all + assert(C.lbound() == 1); // 1-based arrays and vectors + assert(D.lbound() == 1); + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(M == N); // make sure A is square + + Subscript i,j,k; + typename MaTRiX::element_type eta, sigma, sum; + + // adjust the shape of C and D, if needed... + + if (N != C.size()) C.newsize(N); + if (N != D.size()) D.newsize(N); + + for (k=1; k<N; k++) + { + // eta = max |M(i,k)|, for k <= i <= n + // + eta = 0; + for (i=k; i<=N; i++) + { + double absA = fabs(A(i,k)); + eta = ( absA > eta ? absA : eta ); + } + + if (eta == 0) // matrix is singular + { + cerr << "QR: k=" << k << "\n"; + return 1; + } + + // form Qk and premiltiply M by it + // + for(i=k; i<=N; i++) + A(i,k) = A(i,k) / eta; + + sum = 0; + for (i=k; i<=N; i++) + sum = sum + A(i,k)*A(i,k); + sigma = sign(A(k,k)) * sqrt(sum); + + + A(k,k) = A(k,k) + sigma; + C(k) = sigma * A(k,k); + D(k) = -eta * sigma; + + for (j=k+1; j<=N; j++) + { + sum = 0; + for (i=k; i<=N; i++) + sum = sum + A(i,k)*A(i,j); + sum = sum / C(k); + + for (i=k; i<=N; i++) + A(i,j) = A(i,j) - sum * A(i,k); + } + + D(N) = A(N,N); + } + + return 0; +} + +// modified form of upper triangular solve, except that the main diagonal +// of R (upper portion of A) is in D. +// +template <class MaTRiX, class Vector> +int R_solve(const MaTRiX &A, /*const*/ Vector &D, Vector &b) +{ + assert(A.lbound() == 1); // ensure these are all + assert(D.lbound() == 1); // 1-based arrays and vectors + assert(b.lbound() == 1); + + Subscript i,j; + Subscript N = A.num_rows(); + + assert(N == A.num_cols()); + assert(N == D.dim()); + assert(N == b.dim()); + + typename MaTRiX::element_type sum; + + if (D(N) == 0) + return 1; + + b(N) = b(N) / + D(N); + + for (i=N-1; i>=1; i--) + { + if (D(i) == 0) + return 1; + sum = 0; + for (j=i+1; j<=N; j++) + sum = sum + A(i,j)*b(j); + b(i) = ( b(i) - sum ) / + D(i); + } + + return 0; +} + + +template <class MaTRiX, class Vector> +int QR_solve(const MaTRiX &A, const Vector &c, /*const*/ Vector &d, + Vector &b) +{ + assert(A.lbound() == 1); // ensure these are all + assert(c.lbound() == 1); // 1-based arrays and vectors + assert(d.lbound() == 1); + + Subscript N=A.num_rows(); + + assert(N == A.num_cols()); + assert(N == c.dim()); + assert(N == d.dim()); + assert(N == b.dim()); + + Subscript i,j; + typename MaTRiX::element_type sum, tau; + + for (j=1; j<N; j++) + { + // form Q'*b + sum = 0; + for (i=j; i<=N; i++) + sum = sum + A(i,j)*b(i); + if (c(j) == 0) + return 1; + tau = sum / c(j); + for (i=j; i<=N; i++) + b(i) = b(i) - tau * A(i,j); + } + return R_solve(A, d, b); // solve Rx = Q'b +} + +} // namespace TNT + +#endif +// QR_H diff --git a/intern/iksolver/intern/TNT/region1d.h b/intern/iksolver/intern/TNT/region1d.h new file mode 100644 index 00000000000..0307ee02c39 --- /dev/null +++ b/intern/iksolver/intern/TNT/region1d.h @@ -0,0 +1,403 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + + +#ifndef REGION1D_H +#define REGION1D_H + + +#include "subscript.h" +#include "index.h" +#include <iostream> +#include <cassert> + +namespace TNT +{ + +template <class Array1D> +class const_Region1D; + +template <class Array1D> +class Region1D +{ + protected: + + Array1D & A_; + Subscript offset_; // 0-based + Subscript dim_; + + typedef typename Array1D::element_type T; + + public: + const Array1D & array() const { return A_; } + + Subscript offset() const { return offset_;} + Subscript dim() const { return dim_; } + + Subscript offset(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(i==TNT_BASE_OFFSET); +#endif + return offset_; + } + + Subscript dim(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(i== TNT_BASE_OFFSET); +#endif + return offset_; + } + + + Region1D(Array1D &A, Subscript i1, Subscript i2) : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i1 ); + assert(i2 <= A.dim() + (TNT_BASE_OFFSET-1)); + assert(i1 <= i2); +#endif + offset_ = i1 - TNT_BASE_OFFSET; + dim_ = i2-i1 + 1; + } + + Region1D(Array1D &A, const Index1D &I) : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <=I.lbound()); + assert(I.ubound() <= A.dim() + (TNT_BASE_OFFSET-1)); + assert(I.lbound() <= I.ubound()); +#endif + offset_ = I.lbound() - TNT_BASE_OFFSET; + dim_ = I.ubound() - I.lbound() + 1; + } + + Region1D(Region1D<Array1D> &A, Subscript i1, Subscript i2) : + A_(A.A_) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i1 ); + assert(i2 <= A.dim() + (TNT_BASE_OFFSET - 1)); + assert(i1 <= i2); +#endif + // (old-offset) (new-offset) + // + offset_ = (i1 - TNT_BASE_OFFSET) + A.offset_; + dim_ = i2-i1 + 1; + } + + Region1D<Array1D> operator()(Subscript i1, Subscript i2) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i1); + assert(i2 <= dim() + (TNT_BASE_OFFSET -1)); + assert(i1 <= i2); +#endif + // offset_ is 0-based, so no need for + // ( - TNT_BASE_OFFSET) + // + return Region1D<Array1D>(A_, i1+offset_, + offset_ + i2); + } + + + Region1D<Array1D> operator()(const Index1D &I) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET<=I.lbound()); + assert(I.ubound() <= dim() + (TNT_BASE_OFFSET-1)); + assert(I.lbound() <= I.ubound()); +#endif + return Region1D<Array1D>(A_, I.lbound()+offset_, + offset_ + I.ubound()); + } + + + + + T & operator()(Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i); + assert(i <= dim() + (TNT_BASE_OFFSET-1)); +#endif + return A_(i+offset_); + } + + const T & operator() (Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i); + assert(i <= dim() + (TNT_BASE_OFFSET-1)); +#endif + return A_(i+offset_); + } + + + Region1D<Array1D> & operator=(const Region1D<Array1D> &R) + { + // make sure both sides conform + assert(dim() == R.dim()); + + Subscript N = dim(); + Subscript i; + Subscript istart = TNT_BASE_OFFSET; + Subscript iend = istart + N-1; + + for (i=istart; i<=iend; i++) + (*this)(i) = R(i); + + return *this; + } + + + + Region1D<Array1D> & operator=(const const_Region1D<Array1D> &R) + { + // make sure both sides conform + assert(dim() == R.dim()); + + Subscript N = dim(); + Subscript i; + Subscript istart = TNT_BASE_OFFSET; + Subscript iend = istart + N-1; + + for (i=istart; i<=iend; i++) + (*this)(i) = R(i); + + return *this; + + } + + + Region1D<Array1D> & operator=(const T& t) + { + Subscript N=dim(); + Subscript i; + Subscript istart = TNT_BASE_OFFSET; + Subscript iend = istart + N-1; + + for (i=istart; i<= iend; i++) + (*this)(i) = t; + + return *this; + + } + + + Region1D<Array1D> & operator=(const Array1D &R) + { + // make sure both sides conform + Subscript N = dim(); + assert(dim() == R.dim()); + + Subscript i; + Subscript istart = TNT_BASE_OFFSET; + Subscript iend = istart + N-1; + + for (i=istart; i<=iend; i++) + (*this)(i) = R(i); + + return *this; + + } + +}; + +template <class Array1D> +std::ostream& operator<<(std::ostream &s, Region1D<Array1D> &A) +{ + Subscript N=A.dim(); + Subscript istart = TNT_BASE_OFFSET; + Subscript iend = N - 1 + TNT_BASE_OFFSET; + + for (Subscript i=istart; i<=iend; i++) + s << A(i) << endl; + + return s; +} + + +/* --------- class const_Region1D ------------ */ + +template <class Array1D> +class const_Region1D +{ + protected: + + const Array1D & A_; + Subscript offset_; // 0-based + Subscript dim_; + typedef typename Array1D::element_type T; + + public: + const Array1D & array() const { return A_; } + + Subscript offset() const { return offset_;} + Subscript dim() const { return dim_; } + + Subscript offset(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(i==TNT_BASE_OFFSET); +#endif + return offset_; + } + + Subscript dim(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(i== TNT_BASE_OFFSET); +#endif + return offset_; + } + + + const_Region1D(const Array1D &A, Subscript i1, Subscript i2) : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i1 ); + assert(i2 <= A.dim() + (TNT_BASE_OFFSET-1)); + assert(i1 <= i2); +#endif + offset_ = i1 - TNT_BASE_OFFSET; + dim_ = i2-i1 + 1; + } + + const_Region1D(const Array1D &A, const Index1D &I) : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <=I.lbound()); + assert(I.ubound() <= A.dim() + (TNT_BASE_OFFSET-1)); + assert(I.lbound() <= I.ubound()); +#endif + offset_ = I.lbound() - TNT_BASE_OFFSET; + dim_ = I.ubound() - I.lbound() + 1; + } + + const_Region1D(const_Region1D<Array1D> &A, Subscript i1, Subscript i2) : + A_(A.A_) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i1 ); + assert(i2 <= A.dim() + (TNT_BASE_OFFSET - 1)); + assert(i1 <= i2); +#endif + // (old-offset) (new-offset) + // + offset_ = (i1 - TNT_BASE_OFFSET) + A.offset_; + dim_ = i2-i1 + 1; + } + + const_Region1D<Array1D> operator()(Subscript i1, Subscript i2) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i1); + assert(i2 <= dim() + (TNT_BASE_OFFSET -1)); + assert(i1 <= i2); +#endif + // offset_ is 0-based, so no need for + // ( - TNT_BASE_OFFSET) + // + return const_Region1D<Array1D>(A_, i1+offset_, + offset_ + i2); + } + + + const_Region1D<Array1D> operator()(const Index1D &I) + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET<=I.lbound()); + assert(I.ubound() <= dim() + (TNT_BASE_OFFSET-1)); + assert(I.lbound() <= I.ubound()); +#endif + return const_Region1D<Array1D>(A_, I.lbound()+offset_, + offset_ + I.ubound()); + } + + + const T & operator() (Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(TNT_BASE_OFFSET <= i); + assert(i <= dim() + (TNT_BASE_OFFSET-1)); +#endif + return A_(i+offset_); + } + + + + +}; + +template <class Array1D> +std::ostream& operator<<(std::ostream &s, const_Region1D<Array1D> &A) +{ + Subscript N=A.dim(); + + for (Subscript i=1; i<=N; i++) + s << A(i) << endl; + + return s; +} + + +} // namespace TNT + +#endif +// const_Region1D_H diff --git a/intern/iksolver/intern/TNT/region2d.h b/intern/iksolver/intern/TNT/region2d.h new file mode 100644 index 00000000000..d612dfdaa79 --- /dev/null +++ b/intern/iksolver/intern/TNT/region2d.h @@ -0,0 +1,499 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + +// 2D Regions for arrays and matrices + +#ifndef REGION2D_H +#define REGION2D_H + +#include "index.h" +#include <iostream> +#include <cassert> + +namespace TNT +{ + +template <class Array2D> +class const_Region2D; + + +template <class Array2D> +class Region2D +{ + protected: + + Array2D & A_; + Subscript offset_[2]; // 0-offset internally + Subscript dim_[2]; + + public: + typedef typename Array2D::value_type T; + typedef Subscript size_type; + typedef T value_type; + typedef T element_type; + typedef T* pointer; + typedef T* iterator; + typedef T& reference; + typedef const T* const_iterator; + typedef const T& const_reference; + + Array2D & array() { return A_; } + const Array2D & array() const { return A_; } + Subscript lbound() const { return A_.lbound(); } + Subscript num_rows() const { return dim_[0]; } + Subscript num_cols() const { return dim_[1]; } + Subscript offset(Subscript i) const // 1-offset + { +#ifdef TNT_BOUNDS_CHECK + assert( A_.lbound() <= i); + assert( i<= dim_[0] + A_.lbound()-1); +#endif + return offset_[i-A_.lbound()]; + } + + Subscript dim(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert( A_.lbound() <= i); + assert( i<= dim_[0] + A_.lbound()-1); +#endif + return dim_[i-A_.lbound()]; + } + + + + Region2D(Array2D &A, Subscript i1, Subscript i2, Subscript j1, + Subscript j2) : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert( i1 <= i2 ); + assert( j1 <= j2); + assert( A.lbound() <= i1); + assert( i2<= A.dim(A.lbound()) + A.lbound()-1); + assert( A.lbound() <= j1); + assert( j2<= A.dim(A.lbound()+1) + A.lbound()-1 ); +#endif + + + offset_[0] = i1-A.lbound(); + offset_[1] = j1-A.lbound(); + dim_[0] = i2-i1+1; + dim_[1] = j2-j1+1; + } + + Region2D(Array2D &A, const Index1D &I, const Index1D &J) : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert( I.lbound() <= I.ubound() ); + assert( J.lbound() <= J.ubound() ); + assert( A.lbound() <= I.lbound()); + assert( I.ubound()<= A.dim(A.lbound()) + A.lbound()-1); + assert( A.lbound() <= J.lbound()); + assert( J.ubound() <= A.dim(A.lbound()+1) + A.lbound()-1 ); +#endif + + offset_[0] = I.lbound()-A.lbound(); + offset_[1] = J.lbound()-A.lbound(); + dim_[0] = I.ubound() - I.lbound() + 1; + dim_[1] = J.ubound() - J.lbound() + 1; + } + + Region2D(Region2D<Array2D> &A, Subscript i1, Subscript i2, + Subscript j1, Subscript j2) : A_(A.A_) + { +#ifdef TNT_BOUNDS_CHECK + assert( i1 <= i2 ); + assert( j1 <= j2); + assert( A.lbound() <= i1); + assert( i2<= A.dim(A.lbound()) + A.lbound()-1); + assert( A.lbound() <= j1); + assert( j2<= A.dim(A.lbound()+1) + A.lbound()-1 ); +#endif + offset_[0] = (i1 - A.lbound()) + A.offset_[0]; + offset_[1] = (j1 - A.lbound()) + A.offset_[1]; + dim_[0] = i2-i1 + 1; + dim_[1] = j2-j1+1; + } + + Region2D<Array2D> operator()(Subscript i1, Subscript i2, + Subscript j1, Subscript j2) + { +#ifdef TNT_BOUNDS_CHECK + assert( i1 <= i2 ); + assert( j1 <= j2); + assert( A_.lbound() <= i1); + assert( i2<= dim_[0] + A_.lbound()-1); + assert( A_.lbound() <= j1); + assert( j2<= dim_[1] + A_.lbound()-1 ); +#endif + return Region2D<Array2D>(A_, + i1+offset_[0], offset_[0] + i2, + j1+offset_[1], offset_[1] + j2); + } + + + Region2D<Array2D> operator()(const Index1D &I, + const Index1D &J) + { +#ifdef TNT_BOUNDS_CHECK + assert( I.lbound() <= I.ubound() ); + assert( J.lbound() <= J.ubound() ); + assert( A_.lbound() <= I.lbound()); + assert( I.ubound()<= dim_[0] + A_.lbound()-1); + assert( A_.lbound() <= J.lbound()); + assert( J.ubound() <= dim_[1] + A_.lbound()-1 ); +#endif + + return Region2D<Array2D>(A_, I.lbound()+offset_[0], + offset_[0] + I.ubound(), offset_[1]+J.lbound(), + offset_[1] + J.ubound()); + } + + inline T & operator()(Subscript i, Subscript j) + { +#ifdef TNT_BOUNDS_CHECK + assert( A_.lbound() <= i); + assert( i<= dim_[0] + A_.lbound()-1); + assert( A_.lbound() <= j); + assert( j<= dim_[1] + A_.lbound()-1 ); +#endif + return A_(i+offset_[0], j+offset_[1]); + } + + inline const T & operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert( A_.lbound() <= i); + assert( i<= dim_[0] + A_.lbound()-1); + assert( A_.lbound() <= j); + assert( j<= dim_[1] + A_.lbound()-1 ); +#endif + return A_(i+offset_[0], j+offset_[1]); + } + + + Region2D<Array2D> & operator=(const Region2D<Array2D> &R) + { + Subscript M = num_rows(); + Subscript N = num_cols(); + + // make sure both sides conform + assert(M == R.num_rows()); + assert(N == R.num_cols()); + + + Subscript start = R.lbound(); + Subscript Mend = start + M - 1; + Subscript Nend = start + N - 1; + for (Subscript i=start; i<=Mend; i++) + for (Subscript j=start; j<=Nend; j++) + (*this)(i,j) = R(i,j); + + return *this; + } + + Region2D<Array2D> & operator=(const const_Region2D<Array2D> &R) + { + Subscript M = num_rows(); + Subscript N = num_cols(); + + // make sure both sides conform + assert(M == R.num_rows()); + assert(N == R.num_cols()); + + + Subscript start = R.lbound(); + Subscript Mend = start + M - 1; + Subscript Nend = start + N - 1; + for (Subscript i=start; i<=Mend; i++) + for (Subscript j=start; j<=Nend; j++) + (*this)(i,j) = R(i,j); + + return *this; + } + + Region2D<Array2D> & operator=(const Array2D &R) + { + Subscript M = num_rows(); + Subscript N = num_cols(); + + // make sure both sides conform + assert(M == R.num_rows()); + assert(N == R.num_cols()); + + + Subscript start = R.lbound(); + Subscript Mend = start + M - 1; + Subscript Nend = start + N - 1; + for (Subscript i=start; i<=Mend; i++) + for (Subscript j=start; j<=Nend; j++) + (*this)(i,j) = R(i,j); + + return *this; + } + + Region2D<Array2D> & operator=(const T &scalar) + { + Subscript start = lbound(); + Subscript Mend = lbound() + num_rows() - 1; + Subscript Nend = lbound() + num_cols() - 1; + + + for (Subscript i=start; i<=Mend; i++) + for (Subscript j=start; j<=Nend; j++) + (*this)(i,j) = scalar; + + return *this; + } + +}; + +//************************ + +template <class Array2D> +class const_Region2D +{ + protected: + + const Array2D & A_; + Subscript offset_[2]; // 0-offset internally + Subscript dim_[2]; + + public: + typedef typename Array2D::value_type T; + typedef T value_type; + typedef T element_type; + typedef const T* const_iterator; + typedef const T& const_reference; + + const Array2D & array() const { return A_; } + Subscript lbound() const { return A_.lbound(); } + Subscript num_rows() const { return dim_[0]; } + Subscript num_cols() const { return dim_[1]; } + Subscript offset(Subscript i) const // 1-offset + { +#ifdef TNT_BOUNDS_CHECK + assert( TNT_BASE_OFFSET <= i); + assert( i<= dim_[0] + TNT_BASE_OFFSET-1); +#endif + return offset_[i-TNT_BASE_OFFSET]; + } + + Subscript dim(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert( TNT_BASE_OFFSET <= i); + assert( i<= dim_[0] + TNT_BASE_OFFSET-1); +#endif + return dim_[i-TNT_BASE_OFFSET]; + } + + + const_Region2D(const Array2D &A, Subscript i1, Subscript i2, + Subscript j1, Subscript j2) : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert( i1 <= i2 ); + assert( j1 <= j2); + assert( TNT_BASE_OFFSET <= i1); + assert( i2<= A.dim(TNT_BASE_OFFSET) + TNT_BASE_OFFSET-1); + assert( TNT_BASE_OFFSET <= j1); + assert( j2<= A.dim(TNT_BASE_OFFSET+1) + TNT_BASE_OFFSET-1 ); +#endif + + offset_[0] = i1-TNT_BASE_OFFSET; + offset_[1] = j1-TNT_BASE_OFFSET; + dim_[0] = i2-i1+1; + dim_[1] = j2-j1+1; + } + + const_Region2D(const Array2D &A, const Index1D &I, const Index1D &J) + : A_(A) + { +#ifdef TNT_BOUNDS_CHECK + assert( I.lbound() <= I.ubound() ); + assert( J.lbound() <= J.ubound() ); + assert( TNT_BASE_OFFSET <= I.lbound()); + assert( I.ubound()<= A.dim(TNT_BASE_OFFSET) + TNT_BASE_OFFSET-1); + assert( TNT_BASE_OFFSET <= J.lbound()); + assert( J.ubound() <= A.dim(TNT_BASE_OFFSET+1) + TNT_BASE_OFFSET-1 ); +#endif + + offset_[0] = I.lbound()-TNT_BASE_OFFSET; + offset_[1] = J.lbound()-TNT_BASE_OFFSET; + dim_[0] = I.ubound() - I.lbound() + 1; + dim_[1] = J.ubound() - J.lbound() + 1; + } + + + const_Region2D(const_Region2D<Array2D> &A, Subscript i1, + Subscript i2, + Subscript j1, Subscript j2) : A_(A.A_) + { +#ifdef TNT_BOUNDS_CHECK + assert( i1 <= i2 ); + assert( j1 <= j2); + assert( TNT_BASE_OFFSET <= i1); + assert( i2<= A.dim(TNT_BASE_OFFSET) + TNT_BASE_OFFSET-1); + assert( TNT_BASE_OFFSET <= j1); + assert( j2<= A.dim(TNT_BASE_OFFSET+1) + TNT_BASE_OFFSET-1 ); +#endif + offset_[0] = (i1 - TNT_BASE_OFFSET) + A.offset_[0]; + offset_[1] = (j1 - TNT_BASE_OFFSET) + A.offset_[1]; + dim_[0] = i2-i1 + 1; + dim_[1] = j2-j1+1; + } + + const_Region2D<Array2D> operator()(Subscript i1, Subscript i2, + Subscript j1, Subscript j2) + { +#ifdef TNT_BOUNDS_CHECK + assert( i1 <= i2 ); + assert( j1 <= j2); + assert( TNT_BASE_OFFSET <= i1); + assert( i2<= dim_[0] + TNT_BASE_OFFSET-1); + assert( TNT_BASE_OFFSET <= j1); + assert( j2<= dim_[0] + TNT_BASE_OFFSET-1 ); +#endif + return const_Region2D<Array2D>(A_, + i1+offset_[0], offset_[0] + i2, + j1+offset_[1], offset_[1] + j2); + } + + + const_Region2D<Array2D> operator()(const Index1D &I, + const Index1D &J) + { +#ifdef TNT_BOUNDS_CHECK + assert( I.lbound() <= I.ubound() ); + assert( J.lbound() <= J.ubound() ); + assert( TNT_BASE_OFFSET <= I.lbound()); + assert( I.ubound()<= dim_[0] + TNT_BASE_OFFSET-1); + assert( TNT_BASE_OFFSET <= J.lbound()); + assert( J.ubound() <= dim_[1] + TNT_BASE_OFFSET-1 ); +#endif + + return const_Region2D<Array2D>(A_, I.lbound()+offset_[0], + offset_[0] + I.ubound(), offset_[1]+J.lbound(), + offset_[1] + J.ubound()); + } + + + inline const T & operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert( TNT_BASE_OFFSET <= i); + assert( i<= dim_[0] + TNT_BASE_OFFSET-1); + assert( TNT_BASE_OFFSET <= j); + assert( j<= dim_[1] + TNT_BASE_OFFSET-1 ); +#endif + return A_(i+offset_[0], j+offset_[1]); + } + +}; + + +// ************** std::ostream algorithms ******************************* + +template <class Array2D> +std::ostream& operator<<(std::ostream &s, const const_Region2D<Array2D> &A) +{ + Subscript start = A.lbound(); + Subscript Mend=A.lbound()+ A.num_rows() - 1; + Subscript Nend=A.lbound() + A.num_cols() - 1; + + + s << A.num_rows() << " " << A.num_cols() << "\n"; + for (Subscript i=start; i<=Mend; i++) + { + for (Subscript j=start; j<=Nend; j++) + { + s << A(i,j) << " "; + } + s << "\n"; + } + + + return s; +} + +template <class Array2D> +std::ostream& operator<<(std::ostream &s, const Region2D<Array2D> &A) +{ + Subscript start = A.lbound(); + Subscript Mend=A.lbound()+ A.num_rows() - 1; + Subscript Nend=A.lbound() + A.num_cols() - 1; + + + s << A.num_rows() << " " << A.num_cols() << "\n"; + for (Subscript i=start; i<=Mend; i++) + { + for (Subscript j=start; j<=Nend; j++) + { + s << A(i,j) << " "; + } + s << "\n"; + } + + + return s; + +} + +} // namespace TNT + +#endif +// REGION2D_H diff --git a/intern/iksolver/intern/TNT/stopwatch.h b/intern/iksolver/intern/TNT/stopwatch.h new file mode 100644 index 00000000000..6079ec6f1b1 --- /dev/null +++ b/intern/iksolver/intern/TNT/stopwatch.h @@ -0,0 +1,115 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +#ifndef STPWATCH_H +#define STPWATCH_H + +// for clock() and CLOCKS_PER_SEC +#include <ctime> + +namespace TNT +{ + +/* Simple stopwatch object: + + void start() : start timing + double stop() : stop timing + void reset() : set elapsed time to 0.0 + double read() : read elapsed time (in seconds) + +*/ + +inline double seconds(void) +{ + static const double secs_per_tick = 1.0 / CLOCKS_PER_SEC; + return ( (double) clock() ) * secs_per_tick; +} + + +class stopwatch { + private: + int running; + double last_time; + double total; + + public: + stopwatch() : running(0), last_time(0.0), total(0.0) {} + void reset() { running = 0; last_time = 0.0; total=0.0; } + void start() { if (!running) { last_time = seconds(); running = 1;}} + double stop() { if (running) + { + total += seconds() - last_time; + running = 0; + } + return total; + } + double read() { if (running) + { + total+= seconds() - last_time; + last_time = seconds(); + } + return total; + } + +}; + +} // namespace TNT + +#endif + + + diff --git a/intern/iksolver/intern/TNT/subscript.h b/intern/iksolver/intern/TNT/subscript.h new file mode 100644 index 00000000000..4728a79a168 --- /dev/null +++ b/intern/iksolver/intern/TNT/subscript.h @@ -0,0 +1,90 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + +#ifndef SUBSCRPT_H +#define SUBSCRPT_H + + +//--------------------------------------------------------------------- +// This definition describes the default TNT data type used for +// indexing into TNT matrices and vectors. The data type should +// be wide enough to index into large arrays. It defaults to an +// "int", but can be overriden at compile time redefining TNT_SUBSCRIPT_TYPE, +// e.g. +// +// g++ -DTNT_SUBSCRIPT_TYPE='unsigned int' ... +// +//--------------------------------------------------------------------- +// + +#ifndef TNT_SUBSCRIPT_TYPE +#define TNT_SUBSCRIPT_TYPE int +#endif + +namespace TNT +{ + typedef TNT_SUBSCRIPT_TYPE Subscript; +} + + +// () indexing in TNT means 1-offset, i.e. x(1) and A(1,1) are the +// first elements. This offset is left as a macro for future +// purposes, but should not be changed in the current release. +// +// +#define TNT_BASE_OFFSET (1) + +#endif diff --git a/intern/iksolver/intern/TNT/svd.h b/intern/iksolver/intern/TNT/svd.h new file mode 100644 index 00000000000..9be2d98bd9f --- /dev/null +++ b/intern/iksolver/intern/TNT/svd.h @@ -0,0 +1,507 @@ +/** + * $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 SVD_H + +#define SVD_H + +// Compute the Single Value Decomposition of an arbitrary matrix A +// That is compute the 3 matrices U,W,V with U column orthogonal (m,n) +// ,W a diagonal matrix and V an orthogonal square matrix s.t. +// A = U.W.Vt. From this decomposition it is trivial to compute the +// inverse of A as Ainv = V.Winv.tranpose(U). +// work_space is a temporary vector used by this class to compute +// intermediate values during the computation of the SVD. This should +// be of length a.num_cols. This is not checked + +#include "tntmath.h" + +namespace TNT +{ + + +template <class MaTRiX, class VecToR > +void SVD(MaTRiX &a, VecToR &w, MaTRiX &v, VecToR &work_space) { + + int n = a.num_cols(); + int m = a.num_rows(); + + int flag,i,its,j,jj,k,l(0),nm(0); + typename MaTRiX::value_type c,f,h,x,y,z; + typename MaTRiX::value_type anorm(0),g(0),scale(0); + typename MaTRiX::value_type s(0); + + work_space.newsize(n); + + for (i=1;i<=n;i++) { + l=i+1; + work_space(i)=scale*g; + + g = (typename MaTRiX::value_type)0; + + s = (typename MaTRiX::value_type)0; + scale = (typename MaTRiX::value_type)0; + + if (i <= m) { + for (k=i;k<=m;k++) scale += TNT::abs(a(k,i)); + if (scale > (typename MaTRiX::value_type)0) { + for (k=i;k<=m;k++) { + a(k,i) /= scale; + s += a(k,i)*a(k,i); + } + f=a(i,i); + g = -TNT::sign(sqrt(s),f); + h=f*g-s; + a(i,i)=f-g; + if (i != n) { + for (j=l;j<=n;j++) { + s = (typename MaTRiX::value_type)0; + for (k=i;k<=m;k++) s += a(k,i)*a(k,j); + f=s/h; + for (k=i;k<=m;k++) a(k,j) += f*a(k,i); + } + } + for (k=i;k<=m;k++) a(k,i) *= scale; + } + } + w(i)=scale*g; + g = (typename MaTRiX::value_type)0; + s = (typename MaTRiX::value_type)0; + scale = (typename MaTRiX::value_type)0; + if (i <= m && i != n) { + for (k=l;k<=n;k++) scale += TNT::abs(a(i,k)); + if (scale > (typename MaTRiX::value_type)0) { + for (k=l;k<=n;k++) { + a(i,k) /= scale; + s += a(i,k)*a(i,k); + } + f=a(i,l); + g = -TNT::sign(sqrt(s),f); + h=f*g-s; + a(i,l)=f-g; + for (k=l;k<=n;k++) work_space(k)=a(i,k)/h; + if (i != m) { + for (j=l;j<=m;j++) { + s = (typename MaTRiX::value_type)0; + for (k=l;k<=n;k++) s += a(j,k)*a(i,k); + for (k=l;k<=n;k++) a(j,k) += s*work_space(k); + } + } + for (k=l;k<=n;k++) a(i,k) *= scale; + } + } + anorm=TNT::max(anorm,(TNT::abs(w(i))+TNT::abs(work_space(i)))); + } + for (i=n;i>=1;i--) { + if (i < n) { + if (g != (typename MaTRiX::value_type)0) { + for (j=l;j<=n;j++) + v(j,i)=(a(i,j)/a(i,l))/g; + for (j=l;j<=n;j++) { + s = (typename MaTRiX::value_type)0; + for (k=l;k<=n;k++) s += a(i,k)*v(k,j); + for (k=l;k<=n;k++) v(k,j) += s*v(k,i); + } + } + for (j=l;j<=n;j++) v(i,j)=v(j,i)= (typename MaTRiX::value_type)0; + } + v(i,i)= (typename MaTRiX::value_type)1; + g=work_space(i); + l=i; + } + for (i=n;i>=1;i--) { + l=i+1; + g=w(i); + if (i < n) { + for (j=l;j<=n;j++) a(i,j)= (typename MaTRiX::value_type)0; + } + if (g != (typename MaTRiX::value_type)0) { + g= ((typename MaTRiX::value_type)1)/g; + if (i != n) { + for (j=l;j<=n;j++) { + s = (typename MaTRiX::value_type)0; + for (k=l;k<=m;k++) s += a(k,i)*a(k,j); + f=(s/a(i,i))*g; + for (k=i;k<=m;k++) a(k,j) += f*a(k,i); + } + } + for (j=i;j<=m;j++) a(j,i) *= g; + } else { + for (j=i;j<=m;j++) a(j,i)= (typename MaTRiX::value_type)0; + } + ++a(i,i); + } + for (k=n;k>=1;k--) { + for (its=1;its<=30;its++) { + flag=1; + for (l=k;l>=1;l--) { + nm=l-1; + if (TNT::abs(work_space(l))+anorm == anorm) { + flag=0; + break; + } + if (TNT::abs(w(nm))+anorm == anorm) break; + } + if (flag) { + c= (typename MaTRiX::value_type)0; + s= (typename MaTRiX::value_type)1; + for (i=l;i<=k;i++) { + f=s*work_space(i); + if (TNT::abs(f)+anorm != anorm) { + g=w(i); + h= (typename MaTRiX::value_type)TNT::pythag(float(f),float(g)); + w(i)=h; + h= ((typename MaTRiX::value_type)1)/h; + c=g*h; + s=(-f*h); + for (j=1;j<=m;j++) { + y=a(j,nm); + z=a(j,i); + a(j,nm)=y*c+z*s; + a(j,i)=z*c-y*s; + } + } + } + } + z=w(k); + if (l == k) { + if (z < (typename MaTRiX::value_type)0) { + w(k) = -z; + for (j=1;j<=n;j++) v(j,k)=(-v(j,k)); + } + break; + } + + +#if 1 + if (its == 30) + { + TNTException an_exception; + an_exception.i = 0; + throw an_exception; + + return ; + assert(false); + } +#endif + x=w(l); + nm=k-1; + y=w(nm); + g=work_space(nm); + h=work_space(k); + f=((y-z)*(y+z)+(g-h)*(g+h))/(((typename MaTRiX::value_type)2)*h*y); + g=(typename MaTRiX::value_type)TNT::pythag(float(f), float(1)); + f=((x-z)*(x+z)+h*((y/(f+TNT::sign(g,f)))-h))/x; + c = (typename MaTRiX::value_type)1; + s = (typename MaTRiX::value_type)1; + for (j=l;j<=nm;j++) { + i=j+1; + g=work_space(i); + y=w(i); + h=s*g; + g=c*g; + z=(typename MaTRiX::value_type)TNT::pythag(float(f),float(h)); + work_space(j)=z; + c=f/z; + s=h/z; + f=x*c+g*s; + g=g*c-x*s; + h=y*s; + y=y*c; + for (jj=1;jj<=n;jj++) { + x=v(jj,j); + z=v(jj,i); + v(jj,j)=x*c+z*s; + v(jj,i)=z*c-x*s; + } + z=(typename MaTRiX::value_type)TNT::pythag(float(f),float(h)); + w(j)=z; + if (z != (typename MaTRiX::value_type)0) { + z= ((typename MaTRiX::value_type)1)/z; + c=f*z; + s=h*z; + } + f=(c*g)+(s*y); + x=(c*y)-(s*g); + for (jj=1;jj<=m;jj++) { + y=a(jj,j); + z=a(jj,i); + a(jj,j)=y*c+z*s; + a(jj,i)=z*c-y*s; + } + } + work_space(l)= (typename MaTRiX::value_type)0; + work_space(k)=f; + w(k)=x; + } + } +}; + + + +// A is replaced by the column orthogonal matrix U + + +template <class MaTRiX, class VecToR > +void SVD_a( MaTRiX &a, VecToR &w, MaTRiX &v) { + + int n = a.num_cols(); + int m = a.num_rows(); + + int flag,i,its,j,jj,k,l,nm; + typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z; + + VecToR work_space; + work_space.newsize(n); + + g = scale = anorm = 0.0; + + for (i=1;i <=n;i++) { + l = i+1; + work_space(i) = scale*g; + g = s=scale=0.0; + + if (i <= m) { + for(k=i; k<=m; k++) scale += abs(a(k,i)); + + if (scale) { + for (k = i; k <=m ; k++) { + a(k,i) /= scale; + s += a(k,i)*a(k,i); + } + f = a(i,i); + g = -sign(sqrt(s),f); + h = f*g -s; + a(i,i) = f-g; + + for (j = l; j <=n; j++) { + for (s = 0.0,k =i;k<=m;k++) s += a(k,i)*a(k,j); + f = s/h; + for (k = i; k <= m; k++) a(k,j) += f*a(k,i); + } + for (k = i; k <=m;k++) a(k,i) *= scale; + } + } + + w(i) = scale*g; + g = s = scale = 0.0; + + if (i <=m && i != n) { + for (k = l; k <=n;k++) scale += abs(a(i,k)); + if (scale) { + for(k = l;k <=n;k++) { + a(i,k) /= scale; + s += a(i,k) * a(i,k); + } + + f = a(i,l); + g = -sign(sqrt(s),f); + h= f*g -s; + a(i,l) = f-g; + for(k=l;k<=n;k++) work_space(k) = a(i,k)/h; + for (j=l;j<=m;j++) { + for(s = 0.0,k=l;k<=n;k++) s+= a(j,k)*a(i,k); + for(k=l;k<=n;k++) a(j,k) += s*work_space(k); + } + for(k=l;k<=n;k++) a(i,k)*=scale; + } + } + anorm = max(anorm,(abs(w(i)) + abs(work_space(i)))); + } + for (i=n;i>=1;i--) { + if (i <n) { + if (g) { + for(j=l;j<=n;j++) v(j,i) = (a(i,j)/a(i,l))/g; + for(j=l;j<=n;j++) { + for(s=0.0,k=l;k<=n;k++) s += a(i,k)*v(k,j); + for(k=l; k<=n;k++) v(k,j) +=s*v(k,i); + } + } + for(j=l;j <=n;j++) v(i,j) = v(j,i) = 0.0; + } + v(i,i) = 1.0; + g = work_space(i); + l = i; + } + + for (i = min(m,n);i>=1;i--) { + l = i+1; + g = w(i); + for (j=l;j <=n;j++) a(i,j) = 0.0; + if (g) { + g = 1.0/g; + for (j=l;j<=n;j++) { + for (s = 0.0,k=l;k<=m;k++) s += a(k,i)*a(k,j); + f = (s/a(i,i))*g; + for (k=i;k<=m;k++) a(k,j) += f*a(k,i); + } + for (j=i;j<=m;j++) a(j,i)*=g; + } else { + for (j=i;j<=m;j++) a(j,i) = 0.0; + } + ++a(i,i); + } + + for (k=n;k>=1;k--) { + for (its=1;its<=30;its++) { + flag=1; + for(l=k;l>=1;l--) { + nm = l-1; + if (abs(work_space(l)) + anorm == anorm) { + flag = 0; + break; + } + if (abs(w(nm)) + anorm == anorm) break; + } + if (flag) { + c = 0.0; + s = 1.0; + for (i=l;i<=k;i++) { + f = s*work_space(i); + work_space(i) = c*work_space(i); + if (abs(f) +anorm == anorm) break; + g = w(i); + h = pythag(f,g); + w(i) = h; + h = 1.0/h; + c = g*h; + s = -f*h; + for (j=1;j<=m;j++) { + y= a(j,nm); + z=a(j,i); + a(j,nm) = y*c + z*s; + a(j,i) = z*c - y*s; + } + } + } + z=w(k); + if (l==k) { + if (z <0.0) { + w(k) = -z; + for (j=1;j<=n;j++) v(j,k) = -v(j,k); + } + break; + } + + if (its == 30) assert(false); + + x=w(l); + nm=k-1; + y=w(nm); + g=work_space(nm); + h=work_space(k); + + f= ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y); + g = pythag(f,1.0); + f= ((x-z)*(x+z) + h*((y/(f + sign(g,f)))-h))/x; + c=s=1.0; + + for (j=l;j<=nm;j++) { + i=j+1; + g = work_space(i); + y=w(i); + h=s*g; + g=c*g; + z=pythag(f,h); + work_space(j) = z; + c=f/z; + s=h/z; + f=x*c + g*s; + g= g*c - x*s; + h=y*s; + y*=c; + for(jj=1;jj<=n;jj++) { + x=v(jj,j); + z=v(jj,i); + v(jj,j) = x*c + z*s; + v(jj,i) = z*c- x*s; + } + z=pythag(f,h); + w(j)=z; + if(z) { + z = 1.0/z; + c=f*z; + s=h*z; + } + f=c*g + s*y; + x= c*y-s*g; + + for(jj=1;jj<=m;jj++) { + y=a(jj,j); + z=a(jj,i); + a(jj,j) = y*c+z*s; + a(jj,i) = z*c - y*s; + } + } + + work_space(l) = 0.0; + work_space(k) = f; + w(k) = x; + } + } +} +} +#endif + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/intern/iksolver/intern/TNT/tnt.h b/intern/iksolver/intern/TNT/tnt.h new file mode 100644 index 00000000000..df27079f87a --- /dev/null +++ b/intern/iksolver/intern/TNT/tnt.h @@ -0,0 +1,127 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + +#ifndef TNT_H +#define TNT_H + +//--------------------------------------------------------------------- +// tnt.h TNT general header file. Defines default types +// and conventions. +//--------------------------------------------------------------------- + +//--------------------------------------------------------------------- +// Include current version +//--------------------------------------------------------------------- +#include "version.h" + +//--------------------------------------------------------------------- +// Define the data type used for matrix and vector Subscripts. +// This will default to "int", but it can be overriden at compile time, +// e.g. +// +// g++ -DTNT_SUBSCRIPT_TYPE='unsinged long' ... +// +// See subscript.h for details. +//--------------------------------------------------------------------- + +#include "subscript.h" + + + +//--------------------------------------------------------------------- +// Define this macro if you want TNT to ensure all refernces +// are within the bounds of the array. This encurs a run-time +// overhead, of course, but is recommended while developing +// code. It can be turned off for production runs. +// +// #define TNT_BOUNDS_CHECK +//--------------------------------------------------------------------- +// +#define TNT_BOUNDS_CHECK +#ifdef TNT_NO_BOUNDS_CHECK +#undef TNT_BOUNDS_CHECK +#endif + +//--------------------------------------------------------------------- +// Define this macro if you want to utilize matrix and vector +// regions. This is typically on, but you can save some +// compilation time by turning it off. If you do this and +// attempt to use regions you will get an error message. +// +// #define TNT_USE_REGIONS +//--------------------------------------------------------------------- +// +#define TNT_USE_REGIONS + +//--------------------------------------------------------------------- +// +//--------------------------------------------------------------------- +// if your system doesn't have abs() min(), and max() uncoment the following +//--------------------------------------------------------------------- +// +// +//#define __NEED_ABS_MIN_MAX_ + +#include "tntmath.h" + + + + + + + +#endif +// TNT_H diff --git a/intern/iksolver/intern/TNT/tntmath.h b/intern/iksolver/intern/TNT/tntmath.h new file mode 100644 index 00000000000..3342ff7f5c6 --- /dev/null +++ b/intern/iksolver/intern/TNT/tntmath.h @@ -0,0 +1,177 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Header file for scalar math functions + +#ifndef TNTMATH_H +#define TNTMATH_H + +// conventional functions required by several matrix algorithms + + + +namespace TNT +{ + +struct TNTException { + int i; +}; + + +inline double abs(double t) +{ + return ( t > 0 ? t : -t); +} + +inline double min(double a, double b) +{ + return (a < b ? a : b); +} + +inline double max(double a, double b) +{ + return (a > b ? a : b); +} + +inline float abs(float t) +{ + return ( t > 0 ? t : -t); +} + +inline float min(float a, float b) +{ + return (a < b ? a : b); +} + +inline int min(int a,int b) { + return (a < b ? a : b); +} + + +inline float max(float a, float b) +{ + return (a > b ? a : b); +} + +inline double sign(double a) +{ + return (a > 0 ? 1.0 : -1.0); +} + +inline double sign(double a,double b) { + return (b >= 0.0 ? TNT::abs(a) : -TNT::abs(a)); +} + +inline float sign(float a,float b) { + return (b >= 0.0f ? TNT::abs(a) : -TNT::abs(a)); +} + +inline float sign(float a) +{ + return (a > 0.0 ? 1.0f : -1.0f); +} + +inline float pythag(float a, float b) +{ + float absa,absb; + absa = abs(a); + absb = abs(b); + + if (absa > absb) { + float sqr = absb/absa; + sqr *= sqr; + return absa * float(sqrt(1 + sqr)); + } else { + if (absb > float(0)) { + float sqr = absa/absb; + sqr *= sqr; + return absb * float(sqrt(1 + sqr)); + } else { + return float(0); + } + } +} + +inline double pythag(double a, double b) +{ + double absa,absb; + absa = abs(a); + absb = abs(b); + + if (absa > absb) { + double sqr = absb/absa; + sqr *= sqr; + return absa * double(sqrt(1 + sqr)); + } else { + + if (absb > double(0)) { + double sqr = absa/absb; + sqr *= sqr; + return absb * double(sqrt(1 + sqr)); + } else { + return double(0); + } + } +} + + +} /* namespace TNT */ + + +#endif +/* TNTMATH_H */ + diff --git a/intern/iksolver/intern/TNT/tntreqs.h b/intern/iksolver/intern/TNT/tntreqs.h new file mode 100644 index 00000000000..c935902554e --- /dev/null +++ b/intern/iksolver/intern/TNT/tntreqs.h @@ -0,0 +1,102 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// The requirements for a bare-bones vector class: +// +// +// o) must have 0-based [] indexing for const and +// non-const objects (i.e. operator[] defined) +// +// o) must have size() method to denote the number of +// elements +// o) must clean up after itself when destructed +// (i.e. no memory leaks) +// +// -) must have begin() and end() methods (The begin() +// method is necessary, because relying on +// &v_[0] may not work on a empty vector (i.e. v_ is NULL.) +// +// o) must be templated +// o) must have X::value_type defined to be the types of elements +// o) must have X::X(const &x) copy constructor (by *value*) +// o) must have X::X(int N) constructor to N-length vector +// (NOTE: this constructor need *NOT* initalize elements) +// +// -) must have X::X(int N, T scalar) constructor to initalize +// elements to value of "scalar". +// +// ( removed, because valarray<> class uses (scalar, N) rather +// than (N, scalar) ) +// -) must have X::X(int N, const T* scalars) constructor to copy from +// any C linear array +// +// ( removed, because of same reverse order of valarray<> ) +// +// o) must have assignment A=B, by value +// +// NOTE: this class is *NOT* meant to be derived from, +// so its methods (particularly indexing) need not be +// declared virtual. +// +// +// Some things it *DOES NOT* need to do are +// +// o) bounds checking +// o) array referencing (e.g. reference counting) +// o) support () indexing +// o) I/O +// diff --git a/intern/iksolver/intern/TNT/transv.h b/intern/iksolver/intern/TNT/transv.h new file mode 100644 index 00000000000..47730350acc --- /dev/null +++ b/intern/iksolver/intern/TNT/transv.h @@ -0,0 +1,192 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Matrix Transpose Views + +#ifndef TRANSV_H +#define TRANSV_H + +#include <iostream> +#include <cassert> +#include "vec.h" + +namespace TNT +{ + +template <class Array2D> +class Transpose_View +{ + protected: + + const Array2D & A_; + + public: + + typedef typename Array2D::element_type T; + typedef T value_type; + typedef T element_type; + typedef T* pointer; + typedef T* iterator; + typedef T& reference; + typedef const T* const_iterator; + typedef const T& const_reference; + + + const Array2D & array() const { return A_; } + Subscript num_rows() const { return A_.num_cols();} + Subscript num_cols() const { return A_.num_rows();} + Subscript lbound() const { return A_.lbound(); } + Subscript dim(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert( A_.lbound() <= i); + assert( i<= A_.lbound()+1); +#endif + if (i== A_.lbound()) + return num_rows(); + else + return num_cols(); + } + + + Transpose_View(const Transpose_View<Array2D> &A) : A_(A.A_) {}; + Transpose_View(const Array2D &A) : A_(A) {}; + + + inline const typename Array2D::element_type & operator()( + Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(lbound()<=i); + assert(i<=A_.num_cols() + lbound() - 1); + assert(lbound()<=j); + assert(j<=A_.num_rows() + lbound() - 1); +#endif + + return A_(j,i); + } + + +}; + +template <class Matrix> +Transpose_View<Matrix> Transpose_view(const Matrix &A) +{ + return Transpose_View<Matrix>(A); +} + +template <class Matrix, class T> +Vector<T> matmult( + const Transpose_View<Matrix> & A, + const Vector<T> &B) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(B.dim() == N); + + Vector<T> x(N); + + Subscript i, j; + T tmp = 0; + + for (i=1; i<=M; i++) + { + tmp = 0; + for (j=1; j<=N; j++) + tmp += A(i,j) * B(j); + x(i) = tmp; + } + + return x; +} + +template <class Matrix, class T> +inline Vector<T> operator*(const Transpose_View<Matrix> & A, const Vector<T> &B) +{ + return matmult(A,B); +} + + +template <class Matrix> +std::ostream& operator<<(std::ostream &s, const Transpose_View<Matrix> &A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + Subscript start = A.lbound(); + Subscript Mend = M + A.lbound() - 1; + Subscript Nend = N + A.lbound() - 1; + + s << M << " " << N << endl; + for (Subscript i=start; i<=Mend; i++) + { + for (Subscript j=start; j<=Nend; j++) + { + s << A(i,j) << " "; + } + s << endl; + } + + + return s; +} + +} // namespace TNT + +#endif +// TRANSV_H diff --git a/intern/iksolver/intern/TNT/triang.h b/intern/iksolver/intern/TNT/triang.h new file mode 100644 index 00000000000..883caf138f7 --- /dev/null +++ b/intern/iksolver/intern/TNT/triang.h @@ -0,0 +1,670 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Triangular Matrices (Views and Adpators) + +#ifndef TRIANG_H +#define TRIANG_H + +// default to use lower-triangular portions of arrays +// for symmetric matrices. + +namespace TNT +{ + +template <class MaTRiX> +class LowerTriangularView +{ + protected: + + + const MaTRiX &A_; + const typename MaTRiX::element_type zero_; + + public: + + + typedef typename MaTRiX::const_reference const_reference; + typedef const typename MaTRiX::element_type element_type; + typedef const typename MaTRiX::element_type value_type; + typedef element_type T; + + Subscript dim(Subscript d) const { return A_.dim(d); } + Subscript lbound() const { return A_.lbound(); } + Subscript num_rows() const { return A_.num_rows(); } + Subscript num_cols() const { return A_.num_cols(); } + + + // constructors + + LowerTriangularView(/*const*/ MaTRiX &A) : A_(A), zero_(0) {} + + + inline const_reference get(Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(lbound()<=i); + assert(i<=A_.num_rows() + lbound() - 1); + assert(lbound()<=j); + assert(j<=A_.num_cols() + lbound() - 1); +#endif + if (i<j) + return zero_; + else + return A_(i,j); + } + + + inline const_reference operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(lbound()<=i); + assert(i<=A_.num_rows() + lbound() - 1); + assert(lbound()<=j); + assert(j<=A_.num_cols() + lbound() - 1); +#endif + if (i<j) + return zero_; + else + return A_(i,j); + } + +#ifdef TNT_USE_REGIONS + + typedef const_Region2D< LowerTriangularView<MaTRiX> > + const_Region; + + const_Region operator()(/*const*/ Index1D &I, + /*const*/ Index1D &J) const + { + return const_Region(*this, I, J); + } + + const_Region operator()(Subscript i1, Subscript i2, + Subscript j1, Subscript j2) const + { + return const_Region(*this, i1, i2, j1, j2); + } + + + +#endif +// TNT_USE_REGIONS + +}; + + +/* *********** Lower_triangular_view() algorithms ****************** */ + +template <class MaTRiX, class VecToR> +VecToR matmult(/*const*/ LowerTriangularView<MaTRiX> &A, VecToR &x) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(N == x.dim()); + + Subscript i, j; + typename MaTRiX::element_type sum=0.0; + VecToR result(M); + + Subscript start = A.lbound(); + Subscript Mend = M + A.lbound() -1 ; + + for (i=start; i<=Mend; i++) + { + sum = 0.0; + for (j=start; j<=i; j++) + sum = sum + A(i,j)*x(j); + result(i) = sum; + } + + return result; +} + +template <class MaTRiX, class VecToR> +inline VecToR operator*(/*const*/ LowerTriangularView<MaTRiX> &A, VecToR &x) +{ + return matmult(A,x); +} + +template <class MaTRiX> +class UnitLowerTriangularView +{ + protected: + + const MaTRiX &A_; + const typename MaTRiX::element_type zero; + const typename MaTRiX::element_type one; + + public: + + typedef typename MaTRiX::const_reference const_reference; + typedef typename MaTRiX::element_type element_type; + typedef typename MaTRiX::element_type value_type; + typedef element_type T; + + Subscript lbound() const { return 1; } + Subscript dim(Subscript d) const { return A_.dim(d); } + Subscript num_rows() const { return A_.num_rows(); } + Subscript num_cols() const { return A_.num_cols(); } + + + // constructors + + UnitLowerTriangularView(/*const*/ MaTRiX &A) : A_(A), zero(0), one(1) {} + + + inline const_reference get(Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i<=A_.dim(1)); + assert(1<=j); + assert(j<=A_.dim(2)); + assert(0<=i && i<A_.dim(0) && 0<=j && j<A_.dim(1)); +#endif + if (i>j) + return A_(i,j); + else if (i==j) + return one; + else + return zero; + } + + + inline const_reference operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i<=A_.dim(1)); + assert(1<=j); + assert(j<=A_.dim(2)); +#endif + if (i>j) + return A_(i,j); + else if (i==j) + return one; + else + return zero; + } + + +#ifdef TNT_USE_REGIONS + // These are the "index-aware" features + + typedef const_Region2D< UnitLowerTriangularView<MaTRiX> > + const_Region; + + const_Region operator()(/*const*/ Index1D &I, + /*const*/ Index1D &J) const + { + return const_Region(*this, I, J); + } + + const_Region operator()(Subscript i1, Subscript i2, + Subscript j1, Subscript j2) const + { + return const_Region(*this, i1, i2, j1, j2); + } +#endif +// TNT_USE_REGIONS +}; + +template <class MaTRiX> +LowerTriangularView<MaTRiX> Lower_triangular_view( + /*const*/ MaTRiX &A) +{ + return LowerTriangularView<MaTRiX>(A); +} + + +template <class MaTRiX> +UnitLowerTriangularView<MaTRiX> Unit_lower_triangular_view( + /*const*/ MaTRiX &A) +{ + return UnitLowerTriangularView<MaTRiX>(A); +} + +template <class MaTRiX, class VecToR> +VecToR matmult(/*const*/ UnitLowerTriangularView<MaTRiX> &A, VecToR &x) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(N == x.dim()); + + Subscript i, j; + typename MaTRiX::element_type sum=0.0; + VecToR result(M); + + Subscript start = A.lbound(); + Subscript Mend = M + A.lbound() -1 ; + + for (i=start; i<=Mend; i++) + { + sum = 0.0; + for (j=start; j<i; j++) + sum = sum + A(i,j)*x(j); + result(i) = sum + x(i); + } + + return result; +} + +template <class MaTRiX, class VecToR> +inline VecToR operator*(/*const*/ UnitLowerTriangularView<MaTRiX> &A, VecToR &x) +{ + return matmult(A,x); +} + + +//********************** Algorithms ************************************* + + + +template <class MaTRiX> +std::ostream& operator<<(std::ostream &s, const LowerTriangularView<MaTRiX>&A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + s << M << " " << N << endl; + + for (Subscript i=1; i<=M; i++) + { + for (Subscript j=1; j<=N; j++) + { + s << A(i,j) << " "; + } + s << endl; + } + + + return s; +} + +template <class MaTRiX> +std::ostream& operator<<(std::ostream &s, + const UnitLowerTriangularView<MaTRiX>&A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + s << M << " " << N << endl; + + for (Subscript i=1; i<=M; i++) + { + for (Subscript j=1; j<=N; j++) + { + s << A(i,j) << " "; + } + s << endl; + } + + + return s; +} + + + +// ******************* Upper Triangular Section ************************** + +template <class MaTRiX> +class UpperTriangularView +{ + protected: + + + /*const*/ MaTRiX &A_; + /*const*/ typename MaTRiX::element_type zero_; + + public: + + + typedef typename MaTRiX::const_reference const_reference; + typedef /*const*/ typename MaTRiX::element_type element_type; + typedef /*const*/ typename MaTRiX::element_type value_type; + typedef element_type T; + + Subscript dim(Subscript d) const { return A_.dim(d); } + Subscript lbound() const { return A_.lbound(); } + Subscript num_rows() const { return A_.num_rows(); } + Subscript num_cols() const { return A_.num_cols(); } + + + // constructors + + UpperTriangularView(/*const*/ MaTRiX &A) : A_(A), zero_(0) {} + + + inline const_reference get(Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(lbound()<=i); + assert(i<=A_.num_rows() + lbound() - 1); + assert(lbound()<=j); + assert(j<=A_.num_cols() + lbound() - 1); +#endif + if (i>j) + return zero_; + else + return A_(i,j); + } + + + inline const_reference operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(lbound()<=i); + assert(i<=A_.num_rows() + lbound() - 1); + assert(lbound()<=j); + assert(j<=A_.num_cols() + lbound() - 1); +#endif + if (i>j) + return zero_; + else + return A_(i,j); + } + +#ifdef TNT_USE_REGIONS + + typedef const_Region2D< UpperTriangularView<MaTRiX> > + const_Region; + + const_Region operator()(const Index1D &I, + const Index1D &J) const + { + return const_Region(*this, I, J); + } + + const_Region operator()(Subscript i1, Subscript i2, + Subscript j1, Subscript j2) const + { + return const_Region(*this, i1, i2, j1, j2); + } + + + +#endif +// TNT_USE_REGIONS + +}; + + +/* *********** Upper_triangular_view() algorithms ****************** */ + +template <class MaTRiX, class VecToR> +VecToR matmult(/*const*/ UpperTriangularView<MaTRiX> &A, VecToR &x) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(N == x.dim()); + + Subscript i, j; + typename VecToR::element_type sum=0.0; + VecToR result(M); + + Subscript start = A.lbound(); + Subscript Mend = M + A.lbound() -1 ; + + for (i=start; i<=Mend; i++) + { + sum = 0.0; + for (j=i; j<=N; j++) + sum = sum + A(i,j)*x(j); + result(i) = sum; + } + + return result; +} + +template <class MaTRiX, class VecToR> +inline VecToR operator*(/*const*/ UpperTriangularView<MaTRiX> &A, VecToR &x) +{ + return matmult(A,x); +} + +template <class MaTRiX> +class UnitUpperTriangularView +{ + protected: + + const MaTRiX &A_; + const typename MaTRiX::element_type zero; + const typename MaTRiX::element_type one; + + public: + + typedef typename MaTRiX::const_reference const_reference; + typedef typename MaTRiX::element_type element_type; + typedef typename MaTRiX::element_type value_type; + typedef element_type T; + + Subscript lbound() const { return 1; } + Subscript dim(Subscript d) const { return A_.dim(d); } + Subscript num_rows() const { return A_.num_rows(); } + Subscript num_cols() const { return A_.num_cols(); } + + + // constructors + + UnitUpperTriangularView(/*const*/ MaTRiX &A) : A_(A), zero(0), one(1) {} + + + inline const_reference get(Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i<=A_.dim(1)); + assert(1<=j); + assert(j<=A_.dim(2)); + assert(0<=i && i<A_.dim(0) && 0<=j && j<A_.dim(1)); +#endif + if (i<j) + return A_(i,j); + else if (i==j) + return one; + else + return zero; + } + + + inline const_reference operator() (Subscript i, Subscript j) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i<=A_.dim(1)); + assert(1<=j); + assert(j<=A_.dim(2)); +#endif + if (i<j) + return A_(i,j); + else if (i==j) + return one; + else + return zero; + } + + +#ifdef TNT_USE_REGIONS + // These are the "index-aware" features + + typedef const_Region2D< UnitUpperTriangularView<MaTRiX> > + const_Region; + + const_Region operator()(const Index1D &I, + const Index1D &J) const + { + return const_Region(*this, I, J); + } + + const_Region operator()(Subscript i1, Subscript i2, + Subscript j1, Subscript j2) const + { + return const_Region(*this, i1, i2, j1, j2); + } +#endif +// TNT_USE_REGIONS +}; + +template <class MaTRiX> +UpperTriangularView<MaTRiX> Upper_triangular_view( + /*const*/ MaTRiX &A) +{ + return UpperTriangularView<MaTRiX>(A); +} + + +template <class MaTRiX> +UnitUpperTriangularView<MaTRiX> Unit_upper_triangular_view( + /*const*/ MaTRiX &A) +{ + return UnitUpperTriangularView<MaTRiX>(A); +} + +template <class MaTRiX, class VecToR> +VecToR matmult(/*const*/ UnitUpperTriangularView<MaTRiX> &A, VecToR &x) +{ + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + assert(N == x.dim()); + + Subscript i, j; + typename VecToR::element_type sum=0.0; + VecToR result(M); + + Subscript start = A.lbound(); + Subscript Mend = M + A.lbound() -1 ; + + for (i=start; i<=Mend; i++) + { + sum = x(i); + for (j=i+1; j<=N; j++) + sum = sum + A(i,j)*x(j); + result(i) = sum + x(i); + } + + return result; +} + +template <class MaTRiX, class VecToR> +inline VecToR operator*(/*const*/ UnitUpperTriangularView<MaTRiX> &A, VecToR &x) +{ + return matmult(A,x); +} + + +//********************** Algorithms ************************************* + + + +template <class MaTRiX> +std::ostream& operator<<(std::ostream &s, + /*const*/ UpperTriangularView<MaTRiX>&A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + s << M << " " << N << endl; + + for (Subscript i=1; i<=M; i++) + { + for (Subscript j=1; j<=N; j++) + { + s << A(i,j) << " "; + } + s << endl; + } + + + return s; +} + +template <class MaTRiX> +std::ostream& operator<<(std::ostream &s, + /*const*/ UnitUpperTriangularView<MaTRiX>&A) +{ + Subscript M=A.num_rows(); + Subscript N=A.num_cols(); + + s << M << " " << N << endl; + + for (Subscript i=1; i<=M; i++) + { + for (Subscript j=1; j<=N; j++) + { + s << A(i,j) << " "; + } + s << endl; + } + + + return s; +} + +} // namespace TNT + + + + + +#endif +//TRIANG_H + diff --git a/intern/iksolver/intern/TNT/trisolve.h b/intern/iksolver/intern/TNT/trisolve.h new file mode 100644 index 00000000000..6a4f6e1a5b3 --- /dev/null +++ b/intern/iksolver/intern/TNT/trisolve.h @@ -0,0 +1,216 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +// Triangular Solves + +#ifndef TRISLV_H +#define TRISLV_H + + +#include "triang.h" + +namespace TNT +{ + +template <class MaTriX, class VecToR> +VecToR Lower_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b) +{ + Subscript N = A.num_rows(); + + // make sure matrix sizes agree; A must be square + + assert(A.num_cols() == N); + assert(b.dim() == N); + + VecToR x(N); + + Subscript i; + for (i=1; i<=N; i++) + { + typename MaTriX::element_type tmp=0; + + for (Subscript j=1; j<i; j++) + tmp = tmp + A(i,j)*x(j); + + x(i) = (b(i) - tmp)/ A(i,i); + } + + return x; +} + + +template <class MaTriX, class VecToR> +VecToR Unit_lower_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b) +{ + Subscript N = A.num_rows(); + + // make sure matrix sizes agree; A must be square + + assert(A.num_cols() == N); + assert(b.dim() == N); + + VecToR x(N); + + Subscript i; + for (i=1; i<=N; i++) + { + + typename MaTriX::element_type tmp=0; + + for (Subscript j=1; j<i; j++) + tmp = tmp + A(i,j)*x(j); + + x(i) = b(i) - tmp; + } + + return x; +} + + +template <class MaTriX, class VecToR> +VecToR linear_solve(/*const*/ LowerTriangularView<MaTriX> &A, + /*const*/ VecToR &b) +{ + return Lower_triangular_solve(A, b); +} + +template <class MaTriX, class VecToR> +VecToR linear_solve(/*const*/ UnitLowerTriangularView<MaTriX> &A, + /*const*/ VecToR &b) +{ + return Unit_lower_triangular_solve(A, b); +} + + + +//********************** Upper triangular section **************** + +template <class MaTriX, class VecToR> +VecToR Upper_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b) +{ + Subscript N = A.num_rows(); + + // make sure matrix sizes agree; A must be square + + assert(A.num_cols() == N); + assert(b.dim() == N); + + VecToR x(N); + + Subscript i; + for (i=N; i>=1; i--) + { + + typename MaTriX::element_type tmp=0; + + for (Subscript j=i+1; j<=N; j++) + tmp = tmp + A(i,j)*x(j); + + x(i) = (b(i) - tmp)/ A(i,i); + } + + return x; +} + + +template <class MaTriX, class VecToR> +VecToR Unit_upper_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b) +{ + Subscript N = A.num_rows(); + + // make sure matrix sizes agree; A must be square + + assert(A.num_cols() == N); + assert(b.dim() == N); + + VecToR x(N); + + Subscript i; + for (i=N; i>=1; i--) + { + + typename MaTriX::element_type tmp=0; + + for (Subscript j=i+1; j<i; j++) + tmp = tmp + A(i,j)*x(j); + + x(i) = b(i) - tmp; + } + + return x; +} + + +template <class MaTriX, class VecToR> +VecToR linear_solve(/*const*/ UpperTriangularView<MaTriX> &A, + /*const*/ VecToR &b) +{ + return Upper_triangular_solve(A, b); +} + +template <class MaTriX, class VecToR> +VecToR linear_solve(/*const*/ UnitUpperTriangularView<MaTriX> &A, + /*const*/ VecToR &b) +{ + return Unit_upper_triangular_solve(A, b); +} + + +} // namespace TNT + +#endif +// TRISLV_H diff --git a/intern/iksolver/intern/TNT/vec.h b/intern/iksolver/intern/TNT/vec.h new file mode 100644 index 00000000000..beb034ce315 --- /dev/null +++ b/intern/iksolver/intern/TNT/vec.h @@ -0,0 +1,535 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + +// Basic TNT numerical vector (0-based [i] AND 1-based (i) indexing ) +// + +#ifndef VEC_H +#define VEC_H + +#include "subscript.h" +#include <stdlib.h> +#include <assert.h> +#include <iostream> +#include <strstream> + +namespace TNT +{ + +template <class T> +class Vector +{ + + + public: + + typedef Subscript size_type; + typedef T value_type; + typedef T element_type; + typedef T* pointer; + typedef T* iterator; + typedef T& reference; + typedef const T* const_iterator; + typedef const T& const_reference; + + Subscript lbound() const { return 1;} + + protected: + T* v_; + T* vm1_; // pointer adjustment for optimzied 1-offset indexing + Subscript n_; + + // internal helper function to create the array + // of row pointers + + void initialize(Subscript N) + { + // adjust pointers so that they are 1-offset: + // v_[] is the internal contiguous array, it is still 0-offset + // + assert(v_ == NULL); + v_ = new T[N]; + assert(v_ != NULL); + vm1_ = v_-1; + n_ = N; + } + + void copy(const T* v) + { + Subscript N = n_; + Subscript i; + +#ifdef TNT_UNROLL_LOOPS + Subscript Nmod4 = N & 3; + Subscript N4 = N - Nmod4; + + for (i=0; i<N4; i+=4) + { + v_[i] = v[i]; + v_[i+1] = v[i+1]; + v_[i+2] = v[i+2]; + v_[i+3] = v[i+3]; + } + + for (i=N4; i< N; i++) + v_[i] = v[i]; +#else + + for (i=0; i< N; i++) + v_[i] = v[i]; +#endif + } + + void set(const T& val) + { + Subscript N = n_; + Subscript i; + +#ifdef TNT_UNROLL_LOOPS + Subscript Nmod4 = N & 3; + Subscript N4 = N - Nmod4; + + for (i=0; i<N4; i+=4) + { + v_[i] = val; + v_[i+1] = val; + v_[i+2] = val; + v_[i+3] = val; + } + + for (i=N4; i< N; i++) + v_[i] = val; +#else + + for (i=0; i< N; i++) + v_[i] = val; + +#endif + } + + + + void destroy() + { + /* do nothing, if no memory has been previously allocated */ + if (v_ == NULL) return ; + + /* if we are here, then matrix was previously allocated */ + delete [] (v_); + + v_ = NULL; + vm1_ = NULL; + } + + + public: + + // access + + iterator begin() { return v_;} + iterator end() { return v_ + n_; } + const iterator begin() const { return v_;} + const iterator end() const { return v_ + n_; } + + // destructor + + ~Vector() + { + destroy(); + } + + // constructors + + Vector() : v_(0), vm1_(0), n_(0) {}; + + Vector(const Vector<T> &A) : v_(0), vm1_(0), n_(0) + { + initialize(A.n_); + copy(A.v_); + } + + Vector(Subscript N, const T& value = T()) : v_(0), vm1_(0), n_(0) + { + initialize(N); + set(value); + } + + Vector(Subscript N, const T* v) : v_(0), vm1_(0), n_(0) + { + initialize(N); + copy(v); + } + + Vector(Subscript N, char *s) : v_(0), vm1_(0), n_(0) + { + initialize(N); + std::istrstream ins(s); + + Subscript i; + + for (i=0; i<N; i++) + ins >> v_[i]; + } + + + // methods + // + Vector<T>& newsize(Subscript N) + { + if (n_ == N) return *this; + + destroy(); + initialize(N); + + return *this; + } + + + // assignments + // + Vector<T>& operator=(const Vector<T> &A) + { + if (v_ == A.v_) + return *this; + + if (n_ == A.n_) // no need to re-alloc + copy(A.v_); + + else + { + destroy(); + initialize(A.n_); + copy(A.v_); + } + + return *this; + } + + Vector<T>& operator=(const T& scalar) + { + set(scalar); + return *this; + } + + inline Subscript dim() const + { + return n_; + } + + inline Subscript size() const + { + return n_; + } + + + inline reference operator()(Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= n_) ; +#endif + return vm1_[i]; + } + + inline const_reference operator() (Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i <= n_) ; +#endif + return vm1_[i]; + } + + inline reference operator[](Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(0<=i); + assert(i < n_) ; +#endif + return v_[i]; + } + + inline const_reference operator[](Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(0<=i) ; + assert(i < n_) ; +#endif + return v_[i]; + } + + + +}; + + +/* *************************** I/O ********************************/ + +template <class T> +std::ostream& operator<<(std::ostream &s, const Vector<T> &A) +{ + Subscript N=A.dim(); + + s << N << endl; + + for (Subscript i=0; i<N; i++) + s << A[i] << " " << endl; + s << endl; + + return s; +} + +template <class T> +std::istream & operator>>(std::istream &s, Vector<T> &A) +{ + + Subscript N; + + s >> N; + + if ( !(N == A.size() )) + { + A.newsize(N); + } + + + for (Subscript i=0; i<N; i++) + s >> A[i]; + + + return s; +} + +// *******************[ basic matrix algorithms ]*************************** + + +template <class T> +Vector<T> operator+(const Vector<T> &A, + const Vector<T> &B) +{ + Subscript N = A.dim(); + + assert(N==B.dim()); + + Vector<T> tmp(N); + Subscript i; + + for (i=0; i<N; i++) + tmp[i] = A[i] + B[i]; + + return tmp; +} + +template <class T> +Vector<T> operator-(const Vector<T> &A, + const Vector<T> &B) +{ + Subscript N = A.dim(); + + assert(N==B.dim()); + + Vector<T> tmp(N); + Subscript i; + + for (i=0; i<N; i++) + tmp[i] = A[i] - B[i]; + + return tmp; +} + +template <class T> +Vector<T> operator*(const Vector<T> &A, + const Vector<T> &B) +{ + Subscript N = A.dim(); + + assert(N==B.dim()); + + Vector<T> tmp(N); + Subscript i; + + for (i=0; i<N; i++) + tmp[i] = A[i] * B[i]; + + return tmp; +} + +template <class T> +Vector<T> operator*(const Vector<T> &A, + const T &B) +{ + Subscript N = A.dim(); + + Vector<T> tmp(N); + Subscript i; + + for (i=0; i<N; i++) + tmp[i] = A[i] * B; + + return tmp; +} + + +template <class T> +T dot_prod(const Vector<T> &A, const Vector<T> &B) +{ + Subscript N = A.dim(); + assert(N == B.dim()); + + Subscript i; + T sum = 0; + + for (i=0; i<N; i++) + sum += A[i] * B[i]; + + return sum; +} + +// inplace versions of the above template functions + +// A = A + B + +template <class T> +void vectoradd( + Vector<T> &A, + const Vector<T> &B) +{ + const Subscript N = A.dim(); + assert(N==B.dim()); + Subscript i; + + for (i=0; i<N; i++) + A[i] += B[i]; +} + +// same with seperate output vector + +template <class T> +void vectoradd( + Vector<T> &C, + const Vector<T> &A, + const Vector<T> &B) +{ + const Subscript N = A.dim(); + assert(N==B.dim()); + Subscript i; + + for (i=0; i<N; i++) + C[i] = A[i] + B[i]; +} + +// A = A - B + +template <class T> +void vectorsub( + Vector<T> &A, + const Vector<T> &B) +{ + const Subscript N = A.dim(); + assert(N==B.dim()); + Subscript i; + + for (i=0; i<N; i++) + A[i] -= B[i]; +} + +template <class T> +void vectorsub( + Vector<T> &C, + const Vector<T> &A, + const Vector<T> &B) +{ + const Subscript N = A.dim(); + assert(N==B.dim()); + Subscript i; + + for (i=0; i<N; i++) + C[i] = A[i] - B[i]; +} + +template <class T> +void vectorscale( + Vector<T> &C, + const Vector<T> &A, + const T &B) +{ + const Subscript N = A.dim(); + Subscript i; + + for (i=0; i<N; i++) + C[i] = A[i] * B; +} + +template <class T> +void vectorscale( + Vector<T> &A, + const T &B) +{ + const Subscript N = A.dim(); + Subscript i; + + for (i=0; i<N; i++) + A[i] *= B; +} + + + + + +} /* namespace TNT */ + +#endif +// VEC_H diff --git a/intern/iksolver/intern/TNT/vecadaptor.h b/intern/iksolver/intern/TNT/vecadaptor.h new file mode 100644 index 00000000000..337748c58bd --- /dev/null +++ b/intern/iksolver/intern/TNT/vecadaptor.h @@ -0,0 +1,321 @@ +/** + * $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 ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +#ifndef VECADAPTOR_H +#define VECADAPTOR_H + +#include <cstdlib> +#include <iostream> +#include <strstream> +#include <cassert> + +#include "subscript.h" + +#ifdef TNT_USE_REGIONS +#include "region1d.h" +#endif + +namespace TNT +{ + +// see "tntreq.h" for TNT requirements for underlying vector +// class. This need NOT be the STL vector<> class, but a subset +// that provides minimal services. +// +// This is a container adaptor that provides the following services. +// +// o) adds 1-offset operator() access ([] is always 0 offset) +// o) adds TNT_BOUNDS_CHECK to () and [] +// o) adds initialization from strings, e.g. "1.0 2.0 3.0"; +// o) adds newsize(N) function (does not preserve previous values) +// o) adds dim() and dim(1) +// o) adds free() function to release memory used by vector +// o) adds regions, e.g. A(Index(1,10)) = ... +// o) add getVector() method to return adapted container +// o) adds simple I/O for ostreams + +template <class BBVec> +class Vector_Adaptor +{ + + public: + typedef typename BBVec::value_type T; + typedef T value_type; + typedef T element_type; + typedef T* pointer; + typedef T* iterator; + typedef T& reference; + typedef const T* const_iterator; + typedef const T& const_reference; + + Subscript lbound() const { return 1; } + + protected: + BBVec v_; + T* vm1_; + + public: + + Subscript size() const { return v_.size(); } + + // These were removed so that the ANSI C++ valarray class + // would work as a possible storage container. + // + // + //iterator begin() { return v_.begin();} + //iterator begin() { return &v_[0];} + // + //iterator end() { return v_.end(); } + //iterator end() { return &v_[0] + v_.size(); } + // + //const_iterator begin() const { return v_.begin();} + //const_iterator begin() const { return &v_[0];} + // + //const_iterator end() const { return v_.end(); } + //const_iterator end() const { return &v_[0] + v_.size(); } + + BBVec& getVector() { return v_; } + Subscript dim() const { return v_.size(); } + Subscript dim(Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(i==TNT_BASE_OFFSET); +#endif + return (i==TNT_BASE_OFFSET ? v_.size() : 0 ); + } + Vector_Adaptor() : v_() {}; + Vector_Adaptor(const Vector_Adaptor<BBVec> &A) : v_(A.v_) + { + vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); + + } + + Vector_Adaptor(Subscript N, /*const*/ char *s) : v_(N) + { + istrstream ins(s); + for (Subscript i=0; i<N; i++) + ins >> v_[i] ; + + vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); + }; + + Vector_Adaptor(Subscript N, const T& value = T()) : v_(N) + { + for (Subscript i=0; i<N; i++) + v_[i] = value; + + vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); + } + + Vector_Adaptor(Subscript N, const T* values) : v_(N) + { + for (Subscript i=0; i<N; i++) + v_[i] = values[i]; + vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); + } + Vector_Adaptor(const BBVec & A) : v_(A) + { + vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); + } + + // NOTE: this assumes that BBVec(0) constructor creates an + // null vector that does not take up space... It would be + // great to require that BBVec have a corresponding free() + // function, but in particular STL vectors do not. + // + Vector_Adaptor<BBVec>& free() + { + return *this = Vector_Adaptor<BBVec>(0); + } + + Vector_Adaptor<BBVec>& operator=(const Vector_Adaptor<BBVec> &A) + { + v_ = A.v_ ; + vm1_ = ( v_.size() > 0 ? &(v_[0]) -1 : NULL); + return *this; + } + + Vector_Adaptor<BBVec>& newsize(Subscript N) + { + // NOTE: this is not as efficient as it could be + // but to retain compatiblity with STL interface + // we cannot assume underlying implementation + // has a newsize() function. + + return *this = Vector_Adaptor<BBVec>(N); + + } + + Vector_Adaptor<BBVec>& operator=(const T &a) + { + Subscript i; + Subscript N = v_.size(); + for (i=0; i<N; i++) + v_[i] = a; + + return *this; + } + + Vector_Adaptor<BBVec>& resize(Subscript N) + { + if (N == size()) return *this; + + Vector_Adaptor<BBVec> tmp(N); + Subscript n = (N < size() ? N : size()); // min(N, size()); + Subscript i; + + for (i=0; i<n; i++) + tmp[i] = v_[i]; + + + return (*this = tmp); + + } + + + reference operator()(Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i<=dim()); +#endif + return vm1_[i]; + } + + const_reference operator()(Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(1<=i); + assert(i<=dim()); +#endif + return vm1_[i]; + } + + reference operator[](Subscript i) + { +#ifdef TNT_BOUNDS_CHECK + assert(0<=i); + assert(i<dim()); +#endif + return v_[i]; + } + + const_reference operator[](Subscript i) const + { +#ifdef TNT_BOUNDS_CHECK + assert(0<=i); + assert(i<dim()); +#endif + return v_[i]; + } + + +#ifdef TNT_USE_REGIONS + // "index-aware" features, all of these are 1-based offsets + + typedef Region1D<Vector_Adaptor<BBVec> > Region; + + typedef const_Region1D< Vector_Adaptor<BBVec> > const_Region; + + Region operator()(const Index1D &I) + { return Region(*this, I); } + + Region operator()(const Subscript i1, Subscript i2) + { return Region(*this, i1, i2); } + + const_Region operator()(const Index1D &I) const + { return const_Region(*this, I); } + + const_Region operator()(const Subscript i1, Subscript i2) const + { return const_Region(*this, i1, i2); } +#endif +// TNT_USE_REGIONS + + +}; + +#include <iostream> + +template <class BBVec> +std::ostream& operator<<(std::ostream &s, const Vector_Adaptor<BBVec> &A) +{ + Subscript M=A.size(); + + s << M << endl; + for (Subscript i=1; i<=M; i++) + s << A(i) << endl; + return s; +} + +template <class BBVec> +std::istream& operator>>(std::istream &s, Vector_Adaptor<BBVec> &A) +{ + Subscript N; + + s >> N; + + A.resize(N); + + for (Subscript i=1; i<=N; i++) + s >> A(i); + + return s; +} + +} // namespace TNT + +#endif diff --git a/intern/iksolver/intern/TNT/version.h b/intern/iksolver/intern/TNT/version.h new file mode 100644 index 00000000000..c2759e0ce44 --- /dev/null +++ b/intern/iksolver/intern/TNT/version.h @@ -0,0 +1,52 @@ +/** + * $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 ***** + */ + +// Template Numerical Toolkit (TNT) for Linear Algebra + +// +// BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +// Please see http://math.nist.gov/tnt for updates +// +// R. Pozo +// Mathematical and Computational Sciences Division +// National Institute of Standards and Technology + + +#ifndef TNT_VERSION_H +#define TNT_VERSION_H + + +#define TNT_MAJOR_VERSION '0' +#define TNT_MINOR_VERSION '9' +#define TNT_SUBMINOR_VERSION '4' +#define TNT_VERSION_STRING "0.9.4" + +#endif diff --git a/intern/iksolver/make/msvc_6_0/iksolver.dsp b/intern/iksolver/make/msvc_6_0/iksolver.dsp new file mode 100644 index 00000000000..fb12812b7a7 --- /dev/null +++ b/intern/iksolver/make/msvc_6_0/iksolver.dsp @@ -0,0 +1,244 @@ +# Microsoft Developer Studio Project File - Name="iksolver" - Package Owner=<4> +# Microsoft Developer Studio Generated Build File, Format Version 6.00 +# ** DO NOT EDIT ** + +# TARGTYPE "Win32 (x86) Static Library" 0x0104 + +CFG=iksolver - Win32 Debug +!MESSAGE This is not a valid makefile. To build this project using NMAKE, +!MESSAGE use the Export Makefile command and run +!MESSAGE +!MESSAGE NMAKE /f "iksolver.mak". +!MESSAGE +!MESSAGE You can specify a configuration when running NMAKE +!MESSAGE by defining the macro CFG on the command line. For example: +!MESSAGE +!MESSAGE NMAKE /f "iksolver.mak" CFG="iksolver - Win32 Debug" +!MESSAGE +!MESSAGE Possible choices for configuration are: +!MESSAGE +!MESSAGE "iksolver - Win32 Release" (based on "Win32 (x86) Static Library") +!MESSAGE "iksolver - Win32 Debug" (based on "Win32 (x86) Static Library") +!MESSAGE + +# Begin Project +# PROP AllowPerConfigDependencies 0 +# PROP Scc_ProjName "" +# PROP Scc_LocalPath "" +CPP=cl.exe +RSC=rc.exe + +!IF "$(CFG)" == "iksolver - Win32 Release" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 0 +# PROP BASE Output_Dir "Release" +# PROP BASE Intermediate_Dir "Release" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 0 +# PROP Output_Dir "Release" +# PROP Intermediate_Dir "Release" +# PROP Target_Dir "" +LINK32=link.exe -lib +# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c +# ADD CPP /nologo /W3 /GX /O2 /Ob2 /I "..\..\..\..\intern\moto\include" /I "..\..\..\..\intern\memutil" /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /FD /c +# SUBTRACT CPP /YX +# ADD BASE RSC /l 0x413 /d "NDEBUG" +# ADD RSC /l 0x413 /d "NDEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LIB32=link.exe -lib +# ADD BASE LIB32 /nologo +# ADD LIB32 /nologo /out:"..\..\lib\windows\release\iksolver_rmtd.lib" + +!ELSEIF "$(CFG)" == "iksolver - Win32 Debug" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 1 +# PROP BASE Output_Dir "Debug" +# PROP BASE Intermediate_Dir "Debug" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 1 +# PROP Output_Dir "Debug" +# PROP Intermediate_Dir "Debug" +# PROP Target_Dir "" +LINK32=link.exe -lib +# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c +# ADD CPP /nologo /W3 /Gm /GX /ZI /Od /I "..\..\..\..\intern\moto\include" /I "..\..\..\..\intern\memutil" /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /FD /GZ /c +# SUBTRACT CPP /YX +# ADD BASE RSC /l 0x413 /d "_DEBUG" +# ADD RSC /l 0x413 /d "_DEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LIB32=link.exe -lib +# ADD BASE LIB32 /nologo +# ADD LIB32 /nologo /out:"..\..\lib\windows\debug\iksolver_dmtd.lib" + +!ENDIF + +# Begin Target + +# Name "iksolver - Win32 Release" +# Name "iksolver - Win32 Debug" +# Begin Group "intern" + +# PROP Default_Filter "" +# Begin Group "common" + +# PROP Default_Filter "" +# End Group +# Begin Group "TNT" + +# PROP Default_Filter "" +# Begin Source File + +SOURCE=..\..\intern\TNT\cholesky.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\cmat.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\fcscmat.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\fmat.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\fortran.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\fspvec.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\index.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\lapack.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\lu.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\qr.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\region1d.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\region2d.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\stopwatch.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\subscript.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\svd.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\tnt.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\tntmath.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\tntreqs.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\transv.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\triang.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\trisolve.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\vec.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\vecadaptor.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\TNT\version.h +# End Source File +# End Group +# Begin Source File + +SOURCE=..\..\intern\IK_QChain.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\intern\IK_QChain.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\IK_QJacobianSolver.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\intern\IK_QJacobianSolver.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\IK_QSegment.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\intern\IK_QSegment.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\IK_QSolver_Class.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\IK_Solver.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\intern\MT_ExpMap.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\intern\MT_ExpMap.h +# End Source File +# End Group +# Begin Group "extern" + +# PROP Default_Filter "" +# Begin Source File + +SOURCE=..\..\extern\IK_solver.h +# End Source File +# End Group +# End Target +# End Project diff --git a/intern/iksolver/make/msvc_6_0/iksolver.dsw b/intern/iksolver/make/msvc_6_0/iksolver.dsw new file mode 100644 index 00000000000..90a123d9ce8 --- /dev/null +++ b/intern/iksolver/make/msvc_6_0/iksolver.dsw @@ -0,0 +1,35 @@ +Microsoft Developer Studio Workspace File, Format Version 6.00 +# WARNING: DO NOT EDIT OR DELETE THIS WORKSPACE FILE! + +############################################################################### + +Project: "iksolver"=.\iksolver.dsp - Package Owner=<4> + +Package=<5> +{{{ +}}} + +Package=<4> +{{{ +}}} + +############################################################################### + +Global: + +Package=<5> +{{{ +}}} + +Package=<3> +{{{ +}}} + +############################################################################### + + + + + + + diff --git a/intern/iksolver/test/Makefile b/intern/iksolver/test/Makefile new file mode 100644 index 00000000000..f59e487add0 --- /dev/null +++ b/intern/iksolver/test/Makefile @@ -0,0 +1,67 @@ +# +# $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 ***** +# iksolver test makefile. +# + +LIBNAME = iksolver +SOURCEDIR = intern/$(LIBNAME)/test +DIR = $(OCGDIR)/$(SOURCEDIR) + +include nan_compile.mk + +DIRS = ik_glut_test + +include nan_subdirs.mk + +include nan_link.mk + +LIBS = $(OCGDIR)/intern/$(LIBNAME)/test/ik_glut_test/intern/$(DEBUG_DIR)libintern.a +LIBS += $(OCGDIR)/intern/$(LIBNAME)/test/ik_glut_test/common/$(DEBUG_DIR)libcommon.a +LIBS += $(OCGDIR)/intern/$(LIBNAME)/$(DEBUG_DIR)libiksolver.a + +SLIBS += $(NAN_MOTO)/lib/$(DEBUG_DIR)libmoto.a + +ifeq ($(OS),$(findstring $(OS), "beos darwin linux freebsd openbsd")) + LLIBS = -L/usr/X11R6/lib -lglut -pthread +endif + +all debug:: $(LIBS) $(DIR)/$(DEBUG_DIR)iksolvertest + +$(DIR)/$(DEBUG_DIR)iksolvertest: + @echo "****> linking $@ in $(DIR)" + $(CCC) $(LDFLAGS) -o $(DIR)/$(DEBUG_DIR)iksolvertest $(LIBS) $(SLIBS) $(LLIBS) $(DADD) + +clean:: + $(RM) $(DIR)/iksolvertest $(DIR)/debug/iksolvertest + +test:: all + $(DIR)/iksolvertest + diff --git a/intern/iksolver/test/ik_glut_test/Makefile b/intern/iksolver/test/ik_glut_test/Makefile new file mode 100644 index 00000000000..4b47af16599 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/Makefile @@ -0,0 +1,42 @@ +# +# $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 ***** +# iksolver subdir bouncer. Pure waste. +# + +include nan_definitions.mk + +LIBNAME = ik_glut_test +SOURCEDIR = intern/iksolver/test/$(LIBNAME) +DIR = $(OCGDIR)/$(SOURCEDIR) +DIRS = common intern + +include nan_subdirs.mk + diff --git a/intern/iksolver/test/ik_glut_test/common/GlutDrawer.cpp b/intern/iksolver/test/ik_glut_test/common/GlutDrawer.cpp new file mode 100644 index 00000000000..40c6a4a95c9 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/common/GlutDrawer.cpp @@ -0,0 +1,95 @@ +/** + * $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 ***** + */ + +#include "GlutDrawer.h" + +#include "MT_assert.h" + +MEM_SmartPtr<GlutDrawManager> GlutDrawManager::m_s_instance = MEM_SmartPtr<GlutDrawManager>(); + + GlutDrawManager * +GlutDrawManager:: +Instance( +){ + if (m_s_instance == NULL) { + m_s_instance = new GlutDrawManager(); + } + + return m_s_instance; +} + + +// this is the function you should pass to glut + + void +GlutDrawManager:: +Draw( +){ + GlutDrawManager *manager = GlutDrawManager::Instance(); + + if (manager->m_drawer != NULL) { + manager->m_drawer->Draw(); + } +} + + void +GlutDrawManager:: +InstallDrawer( + GlutDrawer * drawer +){ + + MT_assert(m_drawer == NULL); + m_drawer = drawer; +} + + void +GlutDrawManager:: +ReleaseDrawer( +){ + m_drawer = NULL; +} + + +GlutDrawManager:: +~GlutDrawManager( +){ + + delete(m_drawer); +} + + + + + + + + + diff --git a/intern/iksolver/test/ik_glut_test/common/GlutDrawer.h b/intern/iksolver/test/ik_glut_test/common/GlutDrawer.h new file mode 100644 index 00000000000..7e56a02dd47 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/common/GlutDrawer.h @@ -0,0 +1,100 @@ +/** + * $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_GlutDrawer + +#define NAN_INCLUDED_GlutDrawer + +#include "MEM_NonCopyable.h" +#include "MEM_SmartPtr.h" + +// So pissed off with Glut callback stuff +// that is impossible to call objects unless they are global + +// inherit from GlutDrawer and installl the drawer in the singleton +// class GlutDrawManager. + +class GlutDrawer { +public : + + virtual + void + Draw( + )= 0; + + virtual + ~GlutDrawer( + ){}; +}; + +class GlutDrawManager : public MEM_NonCopyable{ + +public : + + static + GlutDrawManager * + Instance( + ); + + // this is the function you should pass to glut + + static + void + Draw( + ); + + void + InstallDrawer( + GlutDrawer * + ); + + void + ReleaseDrawer( + ); + + ~GlutDrawManager( + ); + +private : + + GlutDrawManager ( + ) : + m_drawer (0) + { + }; + + GlutDrawer * m_drawer; + + static MEM_SmartPtr<GlutDrawManager> m_s_instance; +}; + + +#endif
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.cpp b/intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.cpp new file mode 100644 index 00000000000..0d1d52b542d --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.cpp @@ -0,0 +1,89 @@ +/** + * $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 ***** + */ + +#include "GlutKeyboardManager.h" + +#include "MT_assert.h" + +MEM_SmartPtr<GlutKeyboardManager> GlutKeyboardManager::m_s_instance = MEM_SmartPtr<GlutKeyboardManager>(); + + GlutKeyboardManager * +GlutKeyboardManager:: +Instance( +){ + if (m_s_instance == NULL) { + m_s_instance = new GlutKeyboardManager(); + } + + return m_s_instance; +} + + +// this is the function you should pass to glut + + void +GlutKeyboardManager:: +HandleKeyboard( + unsigned char key, + int x, + int y +){ + GlutKeyboardManager *manager = GlutKeyboardManager::Instance(); + + if (manager->m_handler != NULL) { + manager->m_handler->HandleKeyboard(key,x,y); + } +} + + void +GlutKeyboardManager:: +InstallHandler( + GlutKeyboardHandler * handler +){ + + MT_assert(m_handler == NULL); + m_handler = handler; +} + + void +GlutKeyboardManager:: +ReleaseHandler( +){ + m_handler = NULL; +} + + +GlutKeyboardManager:: +~GlutKeyboardManager( +){ + + delete(m_handler); +}
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.h b/intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.h new file mode 100644 index 00000000000..39174b5ff1a --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/common/GlutKeyboardManager.h @@ -0,0 +1,106 @@ +/** + * $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_GlutKeyboardManager + +#define NAN_INCLUDED_GlutKeyboardManager + +#include "MEM_NonCopyable.h" +#include "MEM_SmartPtr.h" + +// So pissed off with Glut callback stuff +// that is impossible to call objects unless they are global + +// inherit from GlutKeyboardHandler and installl the drawer in the singleton +// class GlutKeyboardManager. + +class GlutKeyboardHandler : public MEM_NonCopyable { +public : + + virtual + void + HandleKeyboard( + unsigned char key, + int x, + int y + )= 0; + + virtual + ~GlutKeyboardHandler( + ){}; +}; + +class GlutKeyboardManager : public MEM_NonCopyable{ + +public : + + static + GlutKeyboardManager * + Instance( + ); + + // this is the function you should pass to glut + + static + void + HandleKeyboard( + unsigned char key, + int x, + int y + ); + + void + InstallHandler( + GlutKeyboardHandler * + ); + + void + ReleaseHandler( + ); + + ~GlutKeyboardManager( + ); + +private : + + GlutKeyboardManager ( + ) : + m_handler (0) + { + }; + + GlutKeyboardHandler * m_handler; + + static MEM_SmartPtr<GlutKeyboardManager> m_s_instance; +}; + + +#endif
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/common/GlutMouseManager.cpp b/intern/iksolver/test/ik_glut_test/common/GlutMouseManager.cpp new file mode 100644 index 00000000000..57f4a65327e --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/common/GlutMouseManager.cpp @@ -0,0 +1,104 @@ +/** + * $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 ***** + */ + +#include "GlutMouseManager.h" + +#include "MT_assert.h" + +MEM_SmartPtr<GlutMouseManager> GlutMouseManager::m_s_instance = MEM_SmartPtr<GlutMouseManager>(); + + + GlutMouseManager * +GlutMouseManager:: +Instance( +){ + if (m_s_instance == NULL) { + m_s_instance = new GlutMouseManager(); + } + + return m_s_instance; +} + +// these are the functions you should pass to GLUT + + void +GlutMouseManager:: +Mouse( + int button, + int state, + int x, + int y +){ + GlutMouseManager *manager = GlutMouseManager::Instance(); + + if (manager->m_handler != NULL) { + manager->m_handler->Mouse(button,state,x,y); + } +} + + void +GlutMouseManager:: +Motion( + int x, + int y +){ + GlutMouseManager *manager = GlutMouseManager::Instance(); + + if (manager->m_handler != NULL) { + manager->m_handler->Motion(x,y); + } +} + + void +GlutMouseManager:: +InstallHandler( + GlutMouseHandler *handler +){ + + MT_assert(m_handler == NULL); + m_handler = handler; +} + + void +GlutMouseManager:: +ReleaseHandler( +){ + m_handler = NULL; +} + +GlutMouseManager:: +~GlutMouseManager( +){ + + delete(m_handler); +} + + diff --git a/intern/iksolver/test/ik_glut_test/common/GlutMouseManager.h b/intern/iksolver/test/ik_glut_test/common/GlutMouseManager.h new file mode 100644 index 00000000000..95700bc14dd --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/common/GlutMouseManager.h @@ -0,0 +1,116 @@ +/** + * $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_GlutMouseManager_h + +#define NAN_INCLUDED_GlutMouseManager_h + +#include "MEM_NonCopyable.h" +#include "MEM_SmartPtr.h" + +class GlutMouseHandler { +public : + + virtual + void + Mouse( + int button, + int state, + int x, + int y + ) = 0; + + virtual + void + Motion( + int x, + int y + ) = 0; + + virtual + ~GlutMouseHandler( + ){}; +}; + +class GlutMouseManager : public MEM_NonCopyable{ + +public : + + static + GlutMouseManager * + Instance( + ); + + // these are the functions you should pass to GLUT + + static + void + Mouse( + int button, + int state, + int x, + int y + ); + + static + void + Motion( + int x, + int y + ); + + void + InstallHandler( + GlutMouseHandler * + ); + + void + ReleaseHandler( + ); + + ~GlutMouseManager( + ); + +private : + + GlutMouseManager ( + ) : + m_handler (0) + { + }; + + GlutMouseHandler * m_handler; + + static MEM_SmartPtr<GlutMouseManager> m_s_instance; +}; + + +#endif
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/common/Makefile b/intern/iksolver/test/ik_glut_test/common/Makefile new file mode 100644 index 00000000000..07ad1837a7d --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/common/Makefile @@ -0,0 +1,44 @@ +# +# $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 ***** +# iksolver test intern Makefile +# + +LIBNAME = common +SOURCEDIR = intern/iksolver/test/ik_glut_test/$(LIBNAME) +DIR = $(OCGDIR)/$(SOURCEDIR) + +include nan_compile.mk + +CCFLAGS += $(LEVEL_2_CPP_WARNINGS) + +CPPFLAGS += -I$(NAN_MOTO)/include +CPPFLAGS += -I$(NAN_MEMUTIL)/include + diff --git a/intern/iksolver/test/ik_glut_test/intern/ChainDrawer.h b/intern/iksolver/test/ik_glut_test/intern/ChainDrawer.h new file mode 100644 index 00000000000..a164e686e37 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/ChainDrawer.h @@ -0,0 +1,387 @@ +/** + * $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_ChainDrawer_h + +#define NAN_INCLUDED_ChainDrawer_h + +#include "../common/GlutDrawer.h" +#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 +{ +public : + static + ChainDrawer * + New( + ) { + return new ChainDrawer(); + } + + void + SetMouseHandler( + MyGlutMouseHandler *mouse_handler + ) { + m_mouse_handler = mouse_handler; + } + + void + SetKeyHandler ( + MyGlutKeyHandler *key_handler + ) { + m_key_handler = key_handler; + } + + void + SetChain( + IK_Chain_ExternPtr *chains,int chain_num + ) { + m_chain_num = chain_num; + m_chains = chains; + } + + + // inherited from GlutDrawer + void + Draw( + ) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glPopMatrix(); + glPushMatrix(); + glRotatef(m_mouse_handler->AngleX(), 0.0, 1.0, 0.0); + glRotatef(m_mouse_handler->AngleY(), 1.0, 0.0, 0.0); + + DrawScene(); + glutSwapBuffers(); + + } + + ~ChainDrawer( + ){ + // nothing to do + }; + +private : + + void + DrawScene( + ){ + + // draw a little cross at the position of the key handler + // coordinates + + MT_Vector3 line_x(4,0,0); + MT_Vector3 line_y(0.0,4,0); + MT_Vector3 line_z(0.0,0.0,4); + + MT_Vector3 cross_origin = m_mouse_handler->Position(); + MT_Vector3 temp; + + glDisable(GL_LIGHTING); + + + glBegin(GL_LINES); + + glColor3f (1.0f,1.0f,1.0f); + + temp = cross_origin - line_x; + glVertex3f(temp[0],temp[1],temp[2]); + temp = cross_origin + line_x; + glVertex3f(temp[0],temp[1],temp[2]); + + temp = cross_origin - line_y; + glVertex3f(temp[0],temp[1],temp[2]); + temp = cross_origin + line_y; + glVertex3f(temp[0],temp[1],temp[2]); + + temp = cross_origin - line_z; + glVertex3f(temp[0],temp[1],temp[2]); + temp = cross_origin + line_z; + glVertex3f(temp[0],temp[1],temp[2]); + + glEnd(); + glEnable(GL_LIGHTING); + + + IK_Chain_ExternPtr chain; + + int chain_num; + for (chain_num = 0; chain_num < m_chain_num; chain_num++) { + chain = m_chains[chain_num]; + + + IK_Segment_ExternPtr segs = chain->segments; + IK_Segment_ExternPtr seg_start = segs; + const IK_Segment_ExternPtr seg_end = segs + chain->num_segments; + float ogl_matrix[16]; + + glColor3f (0.0f,1.0f,0.0f); + + MT_Vector3 previous_origin(0,0,0); + + MT_Transform global_transform; + global_transform.setIdentity(); + + for (; seg_start != seg_end; ++seg_start) { + + glPushMatrix(); + + // fill ogl_matrix with zeros + + std::fill(ogl_matrix,ogl_matrix + 16,float(0)); + + // we have to do a bit of work here to compute the chain's + // bone values + + // first compute all the matrices we need + + MT_Transform translation; + translation.setIdentity(); + translation.translate(MT_Vector3(0,seg_start->length,0)); + + MT_Matrix3x3 seg_rot( + seg_start->basis_change[0],seg_start->basis_change[1],seg_start->basis_change[2], + seg_start->basis_change[3],seg_start->basis_change[4],seg_start->basis_change[5], + seg_start->basis_change[6],seg_start->basis_change[7],seg_start->basis_change[8] + ); + + seg_rot.transpose(); + + MT_Matrix3x3 seg_pre_rot( + seg_start->basis[0],seg_start->basis[1],seg_start->basis[2], + seg_start->basis[3],seg_start->basis[4],seg_start->basis[5], + seg_start->basis[6],seg_start->basis[7],seg_start->basis[8] + ); + + + MT_Transform seg_t_pre_rot( + MT_Point3( + seg_start->seg_start[0], + seg_start->seg_start[1], + seg_start->seg_start[2] + ), + seg_pre_rot + ); + // start of the bone is just the current global transform + // multiplied by the seg_start vector + + + + MT_Transform seg_t_rot(MT_Point3(0,0,0),seg_rot); + MT_Transform seg_local = seg_t_pre_rot * seg_t_rot * translation; + + MT_Vector3 bone_start = global_transform * + MT_Point3( + seg_start->seg_start[0], + seg_start->seg_start[1], + seg_start->seg_start[2] + ); + + + global_transform = global_transform * seg_local; + + global_transform.getValue(ogl_matrix); + MT_Vector3 bone_end = global_transform.getOrigin(); + + glMultMatrixf(ogl_matrix); +// glutSolidSphere(0.5,5,5); + + glPopMatrix(); + + glDisable(GL_LIGHTING); + + glBegin(GL_LINES); + + // draw lines of the principle axis of the local transform + + MT_Vector3 x_axis(1,0,0); + MT_Vector3 y_axis(0,1,0); + MT_Vector3 z_axis(0,0,1); + + x_axis = global_transform.getBasis() * x_axis * 5; + y_axis = global_transform.getBasis() * y_axis * 5; + z_axis = global_transform.getBasis() * z_axis * 5; + + + x_axis = x_axis + bone_start; + y_axis = y_axis + bone_start; + z_axis = z_axis + bone_start; + + glColor3f(1,0,0); + + glVertex3f(x_axis.x(),x_axis.y(),x_axis.z()); + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + + glColor3f(0,1,0); + + glVertex3f(y_axis.x(),y_axis.y(),y_axis.z()); + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + + glColor3f(0,1,1); + + glVertex3f(z_axis.x(),z_axis.y(),z_axis.z()); + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + + glColor3f(0,0,1); + + glVertex3f( + bone_start.x(), + bone_start.y(), + bone_start.z() + ); + glVertex3f(bone_end[0],bone_end[1],bone_end[2]); + + glEnd(); + glEnable(GL_LIGHTING); + } +#if 0 + // draw jacobian column vectors + + // hack access to internals + + IK_Solver_Class * internals = static_cast<IK_Solver_Class *>(chain->intern); + + glDisable(GL_LIGHTING); + + glBegin(GL_LINES); + + const TNT::Matrix<MT_Scalar> & jac = internals->Chain().TransposedJacobian(); + + int i = 0; + for (i=0; i < jac.num_rows(); i++) { + glColor3f(1,1,1); + + previous_origin = internals->Chain().Segments()[i/3].GlobalSegmentStart(); + + glVertex3f(previous_origin[0],previous_origin[1],previous_origin[2]); + glVertex3f(jac[i][0] + previous_origin[0],jac[i][1] + previous_origin[1],jac[i][2] + previous_origin[2]); + + + } + glEnd(); + glEnable(GL_LIGHTING); +#endif + + } + + glColor3f(1.0,1.0,1.0); + + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + + MT_Scalar cube_size = 50; + glVertex3f(cube_size,cube_size,cube_size); + glVertex3f(-cube_size,cube_size,cube_size); + + glVertex3f(cube_size,-cube_size,cube_size); + glVertex3f(-cube_size,-cube_size,cube_size); + + glVertex3f(cube_size,cube_size,-cube_size); + glVertex3f(-cube_size,cube_size,-cube_size); + + glVertex3f(cube_size,-cube_size,-cube_size); + glVertex3f(-cube_size,-cube_size,-cube_size); + + + glVertex3f(-cube_size,cube_size,cube_size); + glVertex3f(-cube_size,-cube_size,cube_size); + + glVertex3f(cube_size,cube_size,-cube_size); + glVertex3f(cube_size,-cube_size,-cube_size); + + glVertex3f(cube_size,cube_size,cube_size); + glVertex3f(cube_size,-cube_size,cube_size); + + glVertex3f(-cube_size,cube_size,-cube_size); + glVertex3f(-cube_size,-cube_size,-cube_size); + + + glVertex3f(cube_size,cube_size,cube_size); + glVertex3f(cube_size,cube_size,-cube_size); + + glVertex3f(cube_size,-cube_size,cube_size); + glVertex3f(cube_size,-cube_size,-cube_size); + + glVertex3f(-cube_size,cube_size,cube_size); + glVertex3f(-cube_size,cube_size,-cube_size); + + glVertex3f(-cube_size,-cube_size,cube_size); + glVertex3f(-cube_size,-cube_size,-cube_size); + glEnd(); + glEnable(GL_LIGHTING); + + }; + + + +private : + + MyGlutMouseHandler * m_mouse_handler; + MyGlutKeyHandler *m_key_handler; + IK_Chain_ExternPtr *m_chains; + + int m_chain_num; + ChainDrawer ( + ) : m_chains (NULL), + m_mouse_handler (NULL), + m_chain_num (0) + { + }; + +}; + + + +#endif
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/intern/Makefile b/intern/iksolver/test/ik_glut_test/intern/Makefile new file mode 100644 index 00000000000..fe1f4b54182 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/Makefile @@ -0,0 +1,51 @@ +# +# $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 ***** +# iksolver test intern Makefile +# + +LIBNAME = intern +SOURCEDIR = intern/iksolver/test/ik_glut_test/$(LIBNAME) +DIR = $(OCGDIR)/$(SOURCEDIR) + +include nan_compile.mk + +CCFLAGS += $(LEVEL_2_CPP_WARNINGS) + +CPPFLAGS += -I$(OPENGL_HEADERS) +CPPFLAGS += -I../../../extern +CPPFLAGS += -I../common +CPPFLAGS += -I$(NAN_MOTO)/include +CPPFLAGS += -I$(NAN_MEMUTIL)/include + +ifeq ($(OS),windows) + CPPFLAGS += -I$(NAN_LIBDIR)/windows/glut-3.7/include +endif + diff --git a/intern/iksolver/test/ik_glut_test/intern/MyGlutKeyHandler.h b/intern/iksolver/test/ik_glut_test/intern/MyGlutKeyHandler.h new file mode 100644 index 00000000000..5f88bda378c --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/MyGlutKeyHandler.h @@ -0,0 +1,84 @@ +/** + * $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_MyGlutKeyHandler_h + +#define NAN_INCLUDED_MyGlutKeyHandler_h + +#include "../common/GlutKeyboardManager.h" + +class MyGlutKeyHandler : public GlutKeyboardHandler +{ +public : + static + MyGlutKeyHandler * + New( + ) { + MEM_SmartPtr<MyGlutKeyHandler> output = new MyGlutKeyHandler(); + + if (output == NULL + ) { + return NULL; + } + return output.Release(); + + } + + void + HandleKeyboard( + unsigned char key, + int x, + int y + ){ + + switch (key) { + + case 27 : + + exit(0); + } + } + + ~MyGlutKeyHandler( + ) + { + }; + +private : + + MyGlutKeyHandler( + ) + { + } + +}; +#endif + diff --git a/intern/iksolver/test/ik_glut_test/intern/MyGlutMouseHandler.h b/intern/iksolver/test/ik_glut_test/intern/MyGlutMouseHandler.h new file mode 100644 index 00000000000..e7d210beffb --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/MyGlutMouseHandler.h @@ -0,0 +1,211 @@ +/** + * $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_MyGlutMouseHandler_h + +#define NAN_INCLUDED_MyGlutMouseHandler_h + +#include "../common/GlutMouseManager.h" +#include <GL/glut.h> +#include "IK_solver.h" + +class MyGlutMouseHandler : public GlutMouseHandler +{ + +public : + + static + MyGlutMouseHandler * + New( + ) { + MEM_SmartPtr<MyGlutMouseHandler> output = new MyGlutMouseHandler(); + if (output == NULL + ) { + return NULL; + } + return output.Release(); + + } + + void + SetChain( + IK_Chain_ExternPtr *chains, int num_chains + ){ + m_chains = chains; + m_num_chains = num_chains; + } + + void + Mouse( + int button, + int state, + int x, + int y + ){ + if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) { + m_moving = true; + m_begin_x = x; + m_begin_y = y; + } + if (button == GLUT_LEFT_BUTTON && state == GLUT_UP) { + m_moving = false; + } + + if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN) { + m_tracking = true; + } + if (button == GLUT_RIGHT_BUTTON && state == GLUT_UP) { + m_tracking = false; + } + + if (button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN) { + m_cg_on = true; + } + if (button == GLUT_MIDDLE_BUTTON && state == GLUT_UP) { + m_cg_on = false; + } + + } + + + void + Motion( + int x, + int y + ){ + if (m_moving) { + m_angle_x = m_angle_x + (x - m_begin_x); + m_begin_x = x; + + m_angle_y = m_angle_y + (y - m_begin_y); + m_begin_y = y; + + glutPostRedisplay(); + } + if (m_tracking) { + + int w_h = glutGet((GLenum)GLUT_WINDOW_HEIGHT); + + y = w_h - y; + + double mvmatrix[16]; + double projmatrix[16]; + GLint viewport[4]; + + double px, py, pz,sz; + + /* Get the matrices needed for gluUnProject */ + glGetIntegerv(GL_VIEWPORT, viewport); + glGetDoublev(GL_MODELVIEW_MATRIX, mvmatrix); + glGetDoublev(GL_PROJECTION_MATRIX, projmatrix); + + // work out the position of the end effector in screen space + + GLdouble ex,ey,ez; + ex = m_pos.x(); + ey = m_pos.y(); + ez = m_pos.z(); + + gluProject(ex, ey, ez, mvmatrix, projmatrix, viewport, &px, &py, &sz); + gluUnProject((GLdouble) x, (GLdouble) y, sz, mvmatrix, projmatrix, viewport, &px, &py, &pz); + + m_pos = MT_Vector3(px,py,pz); + + } + if (m_tracking || m_cg_on) { + float temp[3]; + m_pos.getValue(temp); + + IK_SolveChain(m_chains[0],temp,0.01,200,0.1,m_chains[1]->segments); + IK_LoadChain(m_chains[0],m_chains[0]->segments,m_chains[0]->num_segments); + + glutPostRedisplay(); + } + + + } + + const + float + AngleX( + ) const { + return m_angle_x; + } + + const + float + AngleY( + ) const { + return m_angle_y; + } + + const + MT_Vector3 + Position( + ) const { + return m_pos; + } + + +private : + + MyGlutMouseHandler ( + ) : + m_angle_x(0), + m_angle_y(0), + m_begin_x(0), + m_begin_y(0), + m_moving (false), + m_tracking (false), + m_pos(0,0,0), + m_cg_on (false), + m_chains(NULL), + m_num_chains(0) + { + }; + + float m_angle_x; + float m_angle_y; + float m_begin_x; + float m_begin_y; + + bool m_moving; + bool m_tracking; + bool m_cg_on; + MT_Vector3 m_pos; + + IK_Chain_ExternPtr *m_chains; + int m_num_chains; + +}; + + +#endif
\ No newline at end of file diff --git a/intern/iksolver/test/ik_glut_test/intern/main.cpp b/intern/iksolver/test/ik_glut_test/intern/main.cpp new file mode 100644 index 00000000000..137c9bd4470 --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/intern/main.cpp @@ -0,0 +1,361 @@ +/** + * $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$ + * ***** 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 ***** + */ + + +#include "MEM_SmartPtr.h" + +#ifdef USE_QUATERNIONS +#include "IK_Qsolver.h" +#else +#include "IK_solver.h" +#endif + +#include <GL/glut.h> +#include "MT_Vector3.h" +#include "MT_Quaternion.h" +#include "MT_Matrix3x3.h" +#include "MyGlutMouseHandler.h" +#include "MyGlutKeyHandler.h" +#include "ChainDrawer.h" + +void +init(MT_Vector3 min,MT_Vector3 max) +{ + + GLfloat light_diffuse0[] = {1.0, 0.0, 0.0, 1.0}; /* Red diffuse light. */ + GLfloat light_position0[] = {1.0, 1.0, 1.0, 0.0}; /* Infinite light location. */ + + GLfloat light_diffuse1[] = {1.0, 1.0, 1.0, 1.0}; /* Red diffuse light. */ + GLfloat light_position1[] = {1.0, 0, 0, 0.0}; /* Infinite light location. */ + + /* Enable a single OpenGL light. */ + glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse0); + glLightfv(GL_LIGHT0, GL_POSITION, light_position0); + + glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse1); + glLightfv(GL_LIGHT1, GL_POSITION, light_position1); + + glEnable(GL_LIGHT0); + glEnable(GL_LIGHT1); + glEnable(GL_LIGHTING); + + /* Use depth buffering for hidden surface elimination. */ + glEnable(GL_DEPTH_TEST); + + /* Setup the view of the cube. */ + glMatrixMode(GL_PROJECTION); + + // centre of the box + 3* depth of box + + MT_Vector3 centre = (min + max) * 0.5; + MT_Vector3 diag = max - min; + + float depth = diag.length(); + float distance = 2; + + gluPerspective( + /* field of view in degree */ 40.0, + /* aspect ratio */ 1.0, + /* Z near */ 1.0, + /* Z far */ distance * depth * 2 + ); + glMatrixMode(GL_MODELVIEW); + + + gluLookAt( + centre.x(), centre.y(), centre.z() + distance*depth, /* eye is at (0,0,5) */ + centre.x(), centre.y(), centre.z(), /* center is at (0,0,0) */ + 0.0, 1.0, 0.); /* up is in positive Y direction */ + + glPushMatrix(); + + +} +int +main(int argc, char **argv) +{ + + + const int seg_num = 5; + const MT_Scalar seg_length = 15; + + const float seg_startA[3] = {0,0,0}; + const float seg_startB[3] = {0,-20,0}; + + // create some segments to solve with + + // First chain + ////////////// + + + IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num]; + IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num]; + + IK_Segment_ExternPtr seg_it = segmentsA; + IK_Segment_ExternPtr seg_itB = segmentsB; + + + { + +// MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2); + MT_Quaternion qmat(MT_Vector3(0,0,1),0); + MT_Matrix3x3 mat(qmat); + + seg_it->seg_start[0] = seg_startA[0]; + seg_it->seg_start[1] = seg_startA[1]; + seg_it->seg_start[2] = seg_startA[2]; + + float temp[12]; + mat.getValue(temp); + + seg_it->basis[0] = temp[0]; + seg_it->basis[1] = temp[1]; + seg_it->basis[2] = temp[2]; + + seg_it->basis[3] = temp[4]; + seg_it->basis[4] = temp[5]; + seg_it->basis[5] = temp[6]; + + seg_it->basis[6] = temp[8]; + seg_it->basis[7] = temp[9]; + seg_it->basis[8] = temp[10]; + + seg_it->length = seg_length; + + MT_Quaternion q; + q.setEuler(0,0,0); + + + MT_Matrix3x3 qrot(q); + + seg_it->basis_change[0] = 1; + seg_it->basis_change[1] = 0; + seg_it->basis_change[2] = 0; + seg_it->basis_change[3] = 0; + seg_it->basis_change[4] = 1; + seg_it->basis_change[5] = 0; + seg_it->basis_change[6] = 0; + seg_it->basis_change[7] = 0; + seg_it->basis_change[8] = 1; + + + seg_it ++; + + seg_itB->seg_start[0] = seg_startA[0]; + seg_itB->seg_start[1] = seg_startA[1]; + seg_itB->seg_start[2] = seg_startA[2]; + + seg_itB->basis[0] = temp[0]; + seg_itB->basis[1] = temp[1]; + seg_itB->basis[2] = temp[2]; + + seg_itB->basis[3] = temp[4]; + seg_itB->basis[4] = temp[5]; + seg_itB->basis[5] = temp[6]; + + seg_itB->basis[6] = temp[8]; + seg_itB->basis[7] = temp[9]; + seg_itB->basis[8] = temp[10]; + + seg_itB->length = seg_length; + + seg_itB->basis_change[0] = 1; + seg_itB->basis_change[1] = 0; + seg_itB->basis_change[2] = 0; + seg_itB->basis_change[3] = 0; + seg_itB->basis_change[4] = 1; + seg_itB->basis_change[5] = 0; + seg_itB->basis_change[6] = 0; + seg_itB->basis_change[7] = 0; + seg_itB->basis_change[8] = 1; + + + seg_itB ++; + + + } + + + int i; + for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) { + + MT_Quaternion qmat(MT_Vector3(0,0,1),0.3); + MT_Matrix3x3 mat(qmat); + + seg_it->seg_start[0] = 0; + seg_it->seg_start[1] = 0; + seg_it->seg_start[2] = 0; + + float temp[12]; + mat.getValue(temp); + + seg_it->basis[0] = temp[0]; + seg_it->basis[1] = temp[1]; + seg_it->basis[2] = temp[2]; + + seg_it->basis[3] = temp[4]; + seg_it->basis[4] = temp[5]; + seg_it->basis[5] = temp[6]; + + seg_it->basis[6] = temp[8]; + seg_it->basis[7] = temp[9]; + seg_it->basis[8] = temp[10]; + + seg_it->length = seg_length; + + MT_Quaternion q; + q.setEuler(0,0,0); + + + MT_Matrix3x3 qrot(q); + + seg_it->basis_change[0] = 1; + seg_it->basis_change[1] = 0; + seg_it->basis_change[2] = 0; + seg_it->basis_change[3] = 0; + seg_it->basis_change[4] = 1; + seg_it->basis_change[5] = 0; + seg_it->basis_change[6] = 0; + seg_it->basis_change[7] = 0; + seg_it->basis_change[8] = 1; + + + /////////////////////////////// + + seg_itB->seg_start[0] = 0; + seg_itB->seg_start[1] = 0; + seg_itB->seg_start[2] = 0; + + seg_itB->basis[0] = temp[0]; + seg_itB->basis[1] = temp[1]; + seg_itB->basis[2] = temp[2]; + + seg_itB->basis[3] = temp[4]; + seg_itB->basis[4] = temp[5]; + seg_itB->basis[5] = temp[6]; + + seg_itB->basis[6] = temp[8]; + seg_itB->basis[7] = temp[9]; + seg_itB->basis[8] = temp[10]; + + seg_itB->length = seg_length; + + seg_itB->basis_change[0] = 1; + seg_itB->basis_change[1] = 0; + seg_itB->basis_change[2] = 0; + seg_itB->basis_change[3] = 0; + seg_itB->basis_change[4] = 1; + seg_itB->basis_change[5] = 0; + seg_itB->basis_change[6] = 0; + seg_itB->basis_change[7] = 0; + seg_itB->basis_change[8] = 1; + + + + } + + // create the chains + + const int num_chains = 2; + + IK_Chain_ExternPtr chains[num_chains]; + + chains[0] = IK_CreateChain(); + chains[1] = IK_CreateChain(); + + // load segments into chain + + IK_LoadChain(chains[0],segmentsA,seg_num); + IK_LoadChain(chains[1],segmentsB,seg_num); + + // make and install a mouse handler + + MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New()); + GlutMouseManager::Instance()->InstallHandler(mouse_handler); + + mouse_handler->SetChain(chains,num_chains); + + // make and install a keyhandler + MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New()); + GlutKeyboardManager::Instance()->InstallHandler(key_handler); + + // instantiate the drawing class + + MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New()); + GlutDrawManager::Instance()->InstallDrawer(drawer); + + drawer->SetMouseHandler(mouse_handler); + drawer->SetChain(chains,num_chains); + drawer->SetKeyHandler(key_handler); + + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); + glutCreateWindow("ik"); + glutDisplayFunc(GlutDrawManager::Draw); + glutMouseFunc(GlutMouseManager::Mouse); + glutMotionFunc(GlutMouseManager::Motion); + glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard); + + init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50)); + glutMainLoop(); + return 0; /* ANSI C requires main to return int. */ +} diff --git a/intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsp b/intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsp new file mode 100644 index 00000000000..8688b2fcc2c --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsp @@ -0,0 +1,130 @@ +# Microsoft Developer Studio Project File - Name="ik_glut_test" - Package Owner=<4> +# Microsoft Developer Studio Generated Build File, Format Version 6.00 +# ** DO NOT EDIT ** + +# TARGTYPE "Win32 (x86) Console Application" 0x0103 + +CFG=ik_glut_test - Win32 Debug +!MESSAGE This is not a valid makefile. To build this project using NMAKE, +!MESSAGE use the Export Makefile command and run +!MESSAGE +!MESSAGE NMAKE /f "ik_glut_test.mak". +!MESSAGE +!MESSAGE You can specify a configuration when running NMAKE +!MESSAGE by defining the macro CFG on the command line. For example: +!MESSAGE +!MESSAGE NMAKE /f "ik_glut_test.mak" CFG="ik_glut_test - Win32 Debug" +!MESSAGE +!MESSAGE Possible choices for configuration are: +!MESSAGE +!MESSAGE "ik_glut_test - Win32 Release" (based on "Win32 (x86) Console Application") +!MESSAGE "ik_glut_test - Win32 Debug" (based on "Win32 (x86) Console Application") +!MESSAGE + +# Begin Project +# PROP AllowPerConfigDependencies 0 +# PROP Scc_ProjName "" +# PROP Scc_LocalPath "" +CPP=cl.exe +RSC=rc.exe + +!IF "$(CFG)" == "ik_glut_test - Win32 Release" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 0 +# PROP BASE Output_Dir "Release" +# PROP BASE Intermediate_Dir "Release" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 0 +# PROP Output_Dir "Release" +# PROP Intermediate_Dir "Release" +# PROP Ignore_Export_Lib 0 +# PROP Target_Dir "" +# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /c +# ADD CPP /nologo /MT /W3 /GX /O2 /I "..\..\..\..\extern" /I "..\..\..\..\..\..\lib\windows\glut-3.7\include" /I "..\..\..\..\..\..\lib\windows\moto\include" /I "..\..\..\..\..\..\lib\windows\memutil\include" /D "WIN32" /D "NDEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /c +# ADD BASE RSC /l 0x413 /d "NDEBUG" +# ADD RSC /l 0x413 /d "NDEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LINK32=link.exe +# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /machine:I386 +# ADD LINK32 iksolver_rmtd.lib libmoto.a /nologo /subsystem:console /machine:I386 /libpath:"..\..\..\..\lib\windows\release" /libpath:"..\..\..\..\..\..\lib\windows\glut-3.7\lib" /libpath:"..\..\..\..\..\..\lib\windows\moto\lib" + +!ELSEIF "$(CFG)" == "ik_glut_test - Win32 Debug" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 1 +# PROP BASE Output_Dir "Debug" +# PROP BASE Intermediate_Dir "Debug" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 1 +# PROP Output_Dir "Debug" +# PROP Intermediate_Dir "Debug" +# PROP Ignore_Export_Lib 0 +# PROP Target_Dir "" +# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /GZ /c +# ADD CPP /nologo /W3 /Gm /GX /ZI /Od /I "..\..\..\..\extern" /I "..\..\..\..\..\..\lib\windows\glut-3.7\include" /I "..\..\..\..\..\..\lib\windows\moto\include" /I "..\..\..\..\..\..\lib\windows\memutil\include" /D "WIN32" /D "_DEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /GZ /c +# ADD BASE RSC /l 0x413 /d "_DEBUG" +# ADD RSC /l 0x413 /d "_DEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LINK32=link.exe +# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /debug /machine:I386 /pdbtype:sept +# ADD LINK32 iksolver_dmtd.lib libmoto.a /nologo /subsystem:console /debug /machine:I386 /nodefaultlib:"LIBCMTD.lib" /pdbtype:sept /libpath:"..\..\..\..\lib\windows\debug" /libpath:"..\..\..\..\..\..\lib\windows\glut-3.7\lib" /libpath:"..\..\..\..\..\..\lib\windows\moto\lib\debug" + +!ENDIF + +# Begin Target + +# Name "ik_glut_test - Win32 Release" +# Name "ik_glut_test - Win32 Debug" +# Begin Group "common" + +# PROP Default_Filter "" +# Begin Source File + +SOURCE=..\..\common\GlutDrawer.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\common\GlutDrawer.h +# End Source File +# Begin Source File + +SOURCE=..\..\common\GlutKeyboardManager.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\common\GlutKeyboardManager.h +# End Source File +# Begin Source File + +SOURCE=..\..\common\GlutMouseManager.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\common\GlutMouseManager.h +# End Source File +# End Group +# Begin Source File + +SOURCE=..\..\intern\ChainDrawer.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\main.cpp +# End Source File +# Begin Source File + +SOURCE=..\..\intern\MyGlutKeyHandler.h +# End Source File +# Begin Source File + +SOURCE=..\..\intern\MyGlutMouseHandler.h +# End Source File +# End Target +# End Project diff --git a/intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsw b/intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsw new file mode 100644 index 00000000000..09b7094137b --- /dev/null +++ b/intern/iksolver/test/ik_glut_test/make/msvc_6_0/ik_glut_test.dsw @@ -0,0 +1,49 @@ +Microsoft Developer Studio Workspace File, Format Version 6.00 +# WARNING: DO NOT EDIT OR DELETE THIS WORKSPACE FILE! + +############################################################################### + +Project: "ik_glut_test"=.\ik_glut_test.dsp - Package Owner=<4> + +Package=<5> +{{{ +}}} + +Package=<4> +{{{ + Begin Project Dependency + Project_Dep_Name iksolver + End Project Dependency +}}} + +############################################################################### + +Project: "iksolver"=..\..\..\..\make\msvc_6_0\iksolver.dsp - Package Owner=<4> + +Package=<5> +{{{ +}}} + +Package=<4> +{{{ +}}} + +############################################################################### + +Global: + +Package=<5> +{{{ +}}} + +Package=<3> +{{{ +}}} + +############################################################################### + + + + + + |