Add Chromium-only Blender WebEngine parity work

blender-5.2.0/intern/itasc/kdl/chain.cpp

@@ -0,0 +1,78 @@
+/** \file itasc/kdl/chain.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "chain.hpp"
+
+namespace KDL {
+    using namespace std;
+
+    Chain::Chain():
+        segments(0),
+        nrOfJoints(0),
+        nrOfSegments(0)
+    {
+    }
+
+    Chain::Chain(const Chain& in):nrOfJoints(0),
+                                  nrOfSegments(0)
+    {
+        for(unsigned int i=0;i<in.getNrOfSegments();i++)
+            this->addSegment(in.getSegment(i));
+    }
+
+    Chain& Chain::operator=(const Chain& arg)
+    {
+        nrOfJoints=0;
+        nrOfSegments=0;
+        segments.resize(0);
+        for(unsigned int i=0;i<arg.nrOfSegments;i++)
+            addSegment(arg.getSegment(i));
+        return *this;
+
+    }
+
+    void Chain::addSegment(const Segment& segment)
+    {
+        segments.push_back(segment);
+        nrOfSegments++;
+		nrOfJoints += segment.getJoint().getNDof();
+    }
+
+    void Chain::addChain(const Chain& chain)
+    {
+        for(unsigned int i=0;i<chain.getNrOfSegments();i++)
+            this->addSegment(chain.getSegment(i));
+    }
+
+    const Segment& Chain::getSegment(unsigned int nr) const
+    {
+        return segments[nr];
+    }
+
+    Chain::~Chain()
+    {
+    }
+
+}
+

blender-5.2.0/intern/itasc/kdl/chain.hpp

@@ -0,0 +1,97 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_CHAIN_HPP
+#define KDL_CHAIN_HPP
+
+#include "segment.hpp"
+#include <string>
+#include <Eigen/StdVector>
+
+namespace KDL {
+    /**
+	  * \brief This class encapsulates a <strong>serial</strong> kinematic
+	  * interconnection structure. It is build out of segments.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Chain {
+    private:
+        // Eigen allocator is needed for alignment of Eigen data types
+        std::vector<Segment, Eigen::aligned_allocator<Segment> > segments;
+        unsigned int nrOfJoints;
+        unsigned int nrOfSegments;
+    public:
+        /**
+         * The constructor of a chain, a new chain is always empty.
+         *
+         */
+        Chain();
+        Chain(const Chain& in);
+        Chain& operator = (const Chain& arg);
+
+        /**
+         * Adds a new segment to the <strong>end</strong> of the chain.
+         *
+         * @param segment The segment to add
+         */
+        void addSegment(const Segment& segment);
+        /**
+         * Adds a complete chain to the <strong>end</strong> of the chain
+         * The added chain is copied.
+         *
+         * @param chain The chain to add
+         */
+        void addChain(const Chain& chain);
+
+        /**
+         * Request the total number of joints in the chain.\n
+         * <strong> Important:</strong> It is not the
+         * same as the total number of segments since a segment does not
+         * need to have a joint. This function is important when
+         * creating a KDL::JntArray to use with this chain.
+         * @return total nr of joints
+         */
+        unsigned int getNrOfJoints()const {return nrOfJoints;};
+        /**
+         * Request the total number of segments in the chain.
+         * @return total number of segments
+         */
+        unsigned int getNrOfSegments()const {return nrOfSegments;};
+
+        /**
+         * Request the nr'd segment of the chain. There is no boundary
+         * checking.
+         *
+         * @param nr the nr of the segment starting from 0
+         *
+         * @return a constant reference to the nr'd segment
+         */
+        const Segment& getSegment(unsigned int nr)const;
+
+        virtual ~Chain();
+    };
+
+
+
+}//end of namespace KDL
+
+#endif

blender-5.2.0/intern/itasc/kdl/chainfksolver.hpp

@@ -0,0 +1,107 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_CHAIN_FKSOLVER_HPP
+#define KDL_CHAIN_FKSOLVER_HPP
+
+#include "chain.hpp"
+#include "framevel.hpp"
+#include "frameacc.hpp"
+#include "jntarray.hpp"
+#include "jntarrayvel.hpp"
+#include "jntarrayacc.hpp"
+
+namespace KDL {
+
+    /**
+	  * \brief This <strong>abstract</strong> class encapsulates a
+	  * solver for the forward position kinematics for a KDL::Chain.
+     *
+     * @ingroup KinematicFamily
+     */
+
+    //Forward definition
+    class ChainFkSolverPos {
+    public:
+        /**
+         * Calculate forward position kinematics for a KDL::Chain,
+         * from joint coordinates to cartesian pose.
+         *
+         * @param q_in input joint coordinates
+         * @param p_out reference to output cartesian pose
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0;
+        virtual ~ChainFkSolverPos(){};
+    };
+
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward velocity kinematics for a KDL::Chain.
+     *
+     * @ingroup KinematicFamily
+     */
+    class ChainFkSolverVel {
+    public:
+        /**
+         * Calculate forward position and velocity kinematics, from
+         * joint coordinates to cartesian coordinates.
+         *
+         * @param q_in input joint coordinates (position and velocity)
+         * @param out output cartesian coordinates (position and velocity)
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
+
+        virtual ~ChainFkSolverVel(){};
+    };
+
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward acceleration kinematics for a KDL::Chain.
+     *
+     * @ingroup KinematicFamily
+     */
+
+    class ChainFkSolverAcc {
+    public:
+        /**
+         * Calculate forward position, velocity and accelaration
+         * kinematics, from joint coordinates to cartesian coordinates
+         *
+         * @param q_in input joint coordinates (position, velocity and
+         * acceleration
+         @param out output cartesian coordinates (position, velocity
+         * and acceleration
+         *
+         * @return if < 0 something went wrong
+         */
+    virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
+
+        virtual ~ChainFkSolverAcc()=0;
+    };
+
+
+}//end of namespace KDL
+
+#endif

blender-5.2.0/intern/itasc/kdl/chainfksolverpos_recursive.cpp

@@ -0,0 +1,64 @@
+/** \file itasc/kdl/chainfksolverpos_recursive.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "chainfksolverpos_recursive.hpp"
+#include <iostream>
+
+namespace KDL {
+
+    ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain):
+        chain(_chain)
+    {
+    }
+
+    int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr)
+    {
+		unsigned int segNr = (unsigned int)segmentNr;
+        if(segmentNr<0)
+             segNr=chain.getNrOfSegments();
+
+        p_out = Frame::Identity();
+
+        if(q_in.rows()!=chain.getNrOfJoints())
+            return -1;
+        else if(segNr>chain.getNrOfSegments())
+            return -1;
+        else{
+            int j=0;
+            for(unsigned int i=0;i<segNr;i++){
+                p_out = p_out*chain.getSegment(i).pose(((JntArray&)q_in)(j));
+				j+=chain.getSegment(i).getJoint().getNDof();
+            }
+            return 0;
+        }
+    }
+
+
+    ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive()
+    {
+    }
+
+
+}

blender-5.2.0/intern/itasc/kdl/chainfksolverpos_recursive.hpp

@@ -0,0 +1,50 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDLCHAINFKSOLVERPOS_RECURSIVE_HPP
+#define KDLCHAINFKSOLVERPOS_RECURSIVE_HPP
+
+#include "chainfksolver.hpp"
+
+namespace KDL {
+
+    /**
+     * Implementation of a recursive forward position kinematics
+     * algorithm to calculate the position transformation from joint
+     * space to Cartesian space of a general kinematic chain (KDL::Chain).
+     *
+     * @ingroup KinematicFamily
+     */
+    class ChainFkSolverPos_recursive : public ChainFkSolverPos
+    {
+    public:
+        ChainFkSolverPos_recursive(const Chain& chain);
+        ~ChainFkSolverPos_recursive();
+
+        virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1);
+
+    private:
+        const Chain chain;
+    };
+
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/chainjnttojacsolver.cpp

@@ -0,0 +1,83 @@
+/** \file itasc/kdl/chainjnttojacsolver.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "chainjnttojacsolver.hpp"
+
+namespace KDL
+{
+    ChainJntToJacSolver::ChainJntToJacSolver(const Chain& _chain):
+        chain(_chain)
+    {
+    }
+
+    ChainJntToJacSolver::~ChainJntToJacSolver()
+    {
+    }
+
+    int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac)
+    {
+        assert(q_in.rows()==chain.getNrOfJoints()&&
+               q_in.rows()==jac.columns());
+
+
+		Frame T_local, T_joint;
+        T_total = Frame::Identity();
+        SetToZero(t_local);
+
+		int i=chain.getNrOfSegments()-1;
+		unsigned int q_nr = chain.getNrOfJoints();
+
+		//Lets recursively iterate until we are in the root segment
+		while (i >= 0) {
+			const Segment& segment = chain.getSegment(i);
+			int ndof = segment.getJoint().getNDof();
+			q_nr -= ndof;
+
+	        //get the pose of the joint.
+			T_joint = segment.getJoint().pose(((JntArray&)q_in)(q_nr));
+			// combine with the tip to have the tip pose
+			T_local = T_joint*segment.getFrameToTip();
+			//calculate new T_end:
+			T_total = T_local * T_total;
+
+			for (int dof=0; dof<ndof; dof++) {
+				// combine joint rotation with tip position to get a reference frame for the joint
+				T_joint.p = T_local.p;
+				// in which the twist can be computed (needed for NDof joint)
+				t_local = segment.twist(T_joint, 1.0, dof);
+				//transform the endpoint of the local twist to the global endpoint:
+				t_local = t_local.RefPoint(T_total.p - T_local.p);
+				//transform the base of the twist to the endpoint
+				t_local = T_total.M.Inverse(t_local);
+				//store the twist in the jacobian:
+				jac.twists[q_nr+dof] = t_local;
+			}
+			i--;
+		}//endwhile
+		//Change the base of the complete jacobian from the endpoint to the base
+		changeBase(jac, T_total.M, jac);
+        return 0;
+    }
+}
+

blender-5.2.0/intern/itasc/kdl/chainjnttojacsolver.hpp

@@ -0,0 +1,65 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_CHAINJNTTOJACSOLVER_HPP
+#define KDL_CHAINJNTTOJACSOLVER_HPP
+
+#include "frames.hpp"
+#include "jacobian.hpp"
+#include "jntarray.hpp"
+#include "chain.hpp"
+
+namespace KDL
+{
+    /**
+     * @brief  Class to calculate the jacobian of a general
+     * KDL::Chain, it is used by other solvers. It should not be used
+     * outside of KDL.
+     *
+     *
+     */
+
+    class ChainJntToJacSolver
+    {
+    public:
+        ChainJntToJacSolver(const Chain& chain);
+        ~ChainJntToJacSolver();
+        /**
+         * Calculate the jacobian expressed in the base frame of the
+         * chain, with reference point at the end effector of the
+         * *chain. The alghoritm is similar to the one used in
+         * KDL::ChainFkSolverVel_recursive
+         *
+         * @param q_in input joint positions
+         * @param jac output jacobian
+         *
+         * @return always returns 0
+         */
+        int JntToJac(const JntArray& q_in,Jacobian& jac);
+
+    private:
+        const Chain chain;
+        Twist t_local;
+        Frame T_total;
+    };
+}
+#endif
+

blender-5.2.0/intern/itasc/kdl/frameacc.cpp

@@ -0,0 +1,27 @@
+/** \file itasc/kdl/frameacc.cpp
+ * \ingroup intern_itasc
+ */
+/*****************************************************************************
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Name:  $
+ ****************************************************************************/
+
+
+#include "frameacc.hpp"
+
+namespace KDL {
+
+#ifndef KDL_INLINE
+    #include "frameacc.inl"
+#endif
+
+}
+

blender-5.2.0/intern/itasc/kdl/frameacc.hpp

@@ -0,0 +1,266 @@
+/*****************************************************************************
+ * \file
+ *      This file contains the definition of classes for a
+ *      Rall Algebra of (subset of) the classes defined in frames,
+ *      i.e. classes that contain a set (value,derivative,2nd derivative)
+ *      and define operations on that set
+ *      this classes are usefull for automatic differentiation ( <-> symbolic diff ,
+ *      <-> numeric diff).
+ *      Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc.
+ *      Look at the corresponding classes Vector Rotation Frame Twist and
+ *      Wrench for the semantics of the methods.
+ *
+ *      It also contains the 2nd derivative <-> RFrames.h
+ *
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ *      ORO_Geometry V0.2
+ *
+ * \par History
+ *      - $log$
+ *
+ * \par Release
+ *      $Name:  $
+ ****************************************************************************/
+
+#ifndef RRFRAMES_H
+#define RRFRAMES_H
+
+
+#include "utilities/rall2d.h"
+#include "frames.hpp"
+
+
+
+namespace KDL {
+
+class TwistAcc;
+typedef Rall2d<double,double,double> doubleAcc;
+
+
+class VectorAcc
+{
+public:
+    Vector p;   //!< position vector
+    Vector v;   //!< velocity vector
+    Vector dv;  //!< acceleration vector
+public:
+    VectorAcc():p(),v(),dv() {}
+    explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {}
+    VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {}
+    VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv):
+        p(_p),v(_v),dv(_dv) {}
+    IMETHOD VectorAcc& operator = (const VectorAcc& arg);
+    IMETHOD VectorAcc& operator = (const Vector& arg);
+    IMETHOD VectorAcc& operator += (const VectorAcc& arg);
+    IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
+    IMETHOD static VectorAcc Zero();
+    IMETHOD void ReverseSign();
+    IMETHOD doubleAcc Norm();
+    IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2);
+    IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2);
+    IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2);
+    IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1);
+    IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x);
+
+    IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2);
+    IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1);
+
+
+    IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps);
+    IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps);
+    IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps);
+    IMETHOD friend VectorAcc operator - (const VectorAcc& r);
+    IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs);
+    IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs);
+    IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs);
+};
+
+
+
+class RotationAcc
+{
+public:
+    Rotation R;     //!< rotation matrix
+    Vector   w;     //!< angular velocity vector
+    Vector   dw;    //!< angular acceration vector
+public:
+    RotationAcc():R(),w() {}
+    explicit RotationAcc(const Rotation& R_):R(R_),w(Vector::Zero()){}
+    RotationAcc(const Rotation& R_,const Vector& _w,const Vector& _dw):
+        R(R_),w(_w),dw(_dw) {}
+    IMETHOD RotationAcc& operator = (const RotationAcc& arg);
+    IMETHOD RotationAcc& operator = (const Rotation& arg);
+    IMETHOD static RotationAcc Identity();
+    IMETHOD RotationAcc Inverse() const;
+    IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
+    IMETHOD VectorAcc Inverse(const Vector& arg) const;
+    IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
+    IMETHOD VectorAcc operator*(const Vector& arg) const;
+
+    //  Rotations
+    // The SetRot.. functions set the value of *this to the appropriate rotation matrix.
+    // The Rot... static functions give the value of the appropriate rotation matrix back.
+    // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
+    // IMETHOD void DoRotX(const doubleAcc& angle);
+    // IMETHOD void DoRotY(const doubleAcc& angle);
+    // IMETHOD void DoRotZ(const doubleAcc& angle);
+    // IMETHOD static RRotation RotX(const doubleAcc& angle);
+    // IMETHOD static RRotation RotY(const doubleAcc& angle);
+    // IMETHOD static RRotation RotZ(const doubleAcc& angle);
+
+    // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
+    // Along an arbitrary axes.  The norm of rotvec is neglected.
+    // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
+    // rotvec has arbitrary norm
+    // rotation around a constant vector !
+    // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
+    // rotvec is normalized.
+    // rotation around a constant vector !
+
+    IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
+    IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
+    IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
+    IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps);
+    IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps);
+    IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps);
+    IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
+    IMETHOD TwistAcc Inverse(const Twist& arg) const;
+    IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
+    IMETHOD TwistAcc operator * (const Twist& arg) const;
+};
+
+
+
+
+class FrameAcc
+{
+public:
+    RotationAcc M;   //!< Rotation,angular velocity, and angular acceleration of frame.
+    VectorAcc   p;   //!< Translation, velocity and acceleration of origin.
+public:
+    FrameAcc(){}
+    explicit FrameAcc(const Frame& T_):M(T_.M),p(T_.p) {}
+    FrameAcc(const Frame& T_,const Twist& _t,const Twist& _dt):
+        M(T_.M,_t.rot,_dt.rot),p(T_.p,_t.vel,_dt.vel) {}
+    FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
+
+    IMETHOD FrameAcc& operator = (const FrameAcc& arg);
+    IMETHOD FrameAcc& operator = (const Frame& arg);
+    IMETHOD static FrameAcc Identity();
+    IMETHOD FrameAcc Inverse() const;
+    IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
+    IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
+    IMETHOD VectorAcc operator*(const Vector& arg) const;
+    IMETHOD VectorAcc Inverse(const Vector& arg) const;
+    IMETHOD Frame GetFrame() const;
+    IMETHOD Twist GetTwist() const;
+    IMETHOD Twist GetAccTwist() const;
+    IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
+    IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
+    IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
+    IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps);
+    IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps);
+    IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps);
+
+    IMETHOD TwistAcc  Inverse(const TwistAcc& arg) const;
+    IMETHOD TwistAcc  Inverse(const Twist& arg) const;
+    IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
+    IMETHOD TwistAcc operator * (const Twist& arg) const;
+};
+
+
+
+
+
+
+
+
+//very similar to Wrench class.
+class TwistAcc
+{
+public:
+    VectorAcc vel;       //!< translational velocity and its 1st and 2nd derivative
+    VectorAcc rot;       //!< rotational velocity and its 1st and 2nd derivative
+public:
+
+     TwistAcc():vel(),rot() {};
+     TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
+
+     IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
+     IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
+
+     IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
+     IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
+
+     IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
+     IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
+
+     IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
+
+     IMETHOD friend void SetToZero(TwistAcc& v);
+
+     static IMETHOD TwistAcc Zero();
+
+     IMETHOD void ReverseSign();
+
+     IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
+     // Changes the reference point of the RTwist.
+     // The RVector v_base_AB is expressed in the same base as the RTwist
+     // The RVector v_base_AB is a RVector from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+
+     IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps);
+     IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps);
+     IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps);
+
+
+     IMETHOD Twist GetTwist() const;
+     IMETHOD Twist GetTwistDot() const;
+
+    friend class RotationAcc;
+    friend class FrameAcc;
+
+};
+
+
+IMETHOD bool Equal(const VectorAcc&,   const VectorAcc&,   double = epsilon);
+IMETHOD bool Equal(const Vector&,      const VectorAcc&,   double = epsilon);
+IMETHOD bool Equal(const VectorAcc&,   const Vector&,      double = epsilon);
+IMETHOD bool Equal(const RotationAcc&, const RotationAcc&, double = epsilon);
+IMETHOD bool Equal(const Rotation&,    const RotationAcc&, double = epsilon);
+IMETHOD bool Equal(const RotationAcc&, const Rotation&,    double = epsilon);
+IMETHOD bool Equal(const FrameAcc&,    const FrameAcc&,    double = epsilon);
+IMETHOD bool Equal(const Frame&,       const FrameAcc&,    double = epsilon);
+IMETHOD bool Equal(const FrameAcc&,    const Frame&,       double = epsilon);
+IMETHOD bool Equal(const TwistAcc&,    const TwistAcc&,    double = epsilon);
+IMETHOD bool Equal(const Twist&,       const TwistAcc&,    double = epsilon);
+IMETHOD bool Equal(const TwistAcc&,    const Twist&,       double = epsilon);
+
+
+#ifdef KDL_INLINE
+#include "frameacc.inl"
+#endif
+
+}
+
+
+
+
+
+#endif

blender-5.2.0/intern/itasc/kdl/frameacc.inl

@@ -0,0 +1,596 @@
+/*****************************************************************************
+ * \file  
+ *      provides inline functions of rrframes.h
+ *       
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version 
+ *      ORO_Geometry V0.2
+ *
+ *  \par History
+ *      - $log$
+ *
+ *  \par Release
+ *      $Name:  $ 
+ ****************************************************************************/
+
+
+
+
+/////////////////// VectorAcc /////////////////////////////////////
+
+VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv);
+}
+
+VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv);
+}
+VectorAcc operator + (const Vector& r1,const VectorAcc& r2) {
+    return VectorAcc(r1+r2.p,r2.v,r2.dv);
+}
+
+VectorAcc operator - (const Vector& r1,const VectorAcc& r2) {
+    return VectorAcc(r1-r2.p, -r2.v, -r2.dv);
+}
+VectorAcc operator + (const VectorAcc& r1,const Vector& r2) {
+    return VectorAcc(r1.p+r2,r1.v,r1.dv);
+}
+
+VectorAcc operator - (const VectorAcc& r1,const Vector& r2) {
+    return VectorAcc(r1.p-r2, r1.v, r1.dv);
+}
+
+// unary -
+VectorAcc operator - (const VectorAcc& r) {
+    return VectorAcc(-r.p,-r.v,-r.dv);
+}
+
+// cross prod.
+VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.p*r2.p, 
+                    r1.p*r2.v+r1.v*r2.p,
+                    r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv
+                    );
+}
+
+VectorAcc operator * (const VectorAcc& r1,const Vector& r2) {
+    return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
+}
+
+VectorAcc operator * (const Vector& r1,const VectorAcc& r2) {
+    return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
+}
+
+
+
+// scalar mult.
+VectorAcc operator * (double r1,const VectorAcc& r2) {
+    return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
+}
+
+VectorAcc operator * (const VectorAcc& r1,double r2) {
+    return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
+}
+
+VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.t*r2.p, 
+                   r1.t*r2.v  + r1.d*r2.p,
+                   r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
+                   );
+}
+
+VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) {
+    return VectorAcc(r1.t*r2.p, 
+               r1.t*r2.v  + r1.d*r2.p,
+               r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
+               );
+}
+
+VectorAcc& VectorAcc::operator = (const VectorAcc& arg) {
+    p=arg.p;
+    v=arg.v;
+    dv=arg.dv;
+    return *this;
+}
+
+VectorAcc& VectorAcc::operator = (const Vector& arg) {
+    p=arg;
+    v=Vector::Zero();
+    dv=Vector::Zero();
+    return *this;
+}
+
+VectorAcc& VectorAcc::operator += (const VectorAcc& arg) {
+    p+=arg.p;
+    v+=arg.v;
+    dv+= arg.dv;
+    return *this;
+}
+VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) {
+    p-=arg.p;
+    v-=arg.v;
+    dv-=arg.dv;
+    return *this;
+}
+
+VectorAcc VectorAcc::Zero() {
+    return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero());
+}
+
+void VectorAcc::ReverseSign() {
+    p.ReverseSign();
+    v.ReverseSign();
+    dv.ReverseSign();
+}
+
+doubleAcc VectorAcc::Norm() {
+    doubleAcc res;
+    res.t  = p.Norm();
+    res.d  = dot(p,v)/res.t;
+    res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t;
+    return res;
+}
+
+doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) {
+    return doubleAcc( dot(lhs.p,rhs.p),
+                    dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p),
+                    dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p)
+                    );
+}
+
+doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) {
+    return doubleAcc( dot(lhs.p,rhs),
+                     dot(lhs.v,rhs),
+                     dot(lhs.dv,rhs)
+                    );
+}
+
+doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) {
+    return doubleAcc( dot(lhs,rhs.p),
+                    dot(lhs,rhs.v),
+                    dot(lhs,rhs.dv)
+                    );
+}
+
+
+bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) {
+    return (Equal(r1.p,r2.p,eps) 
+         && Equal(r1.v,r2.v,eps)
+         && Equal(r1.dv,r2.dv,eps)
+         );
+}
+
+bool Equal(const Vector& r1,const VectorAcc& r2,double eps) {
+    return (Equal(r1,r2.p,eps) 
+         && Equal(Vector::Zero(),r2.v,eps)
+         && Equal(Vector::Zero(),r2.dv,eps)
+         );
+}
+
+bool Equal(const VectorAcc& r1,const Vector& r2,double eps) {
+    return (Equal(r1.p,r2,eps) 
+         && Equal(r1.v,Vector::Zero(),eps)
+         && Equal(r1.dv,Vector::Zero(),eps)
+         );
+}
+
+VectorAcc operator / (const VectorAcc& r1,double r2) {
+    return r1*(1.0/r2);
+}
+
+VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) {
+    return r2*(1.0/r1);
+}
+
+
+
+/////////////////// RotationAcc /////////////////////////////////////
+
+RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) {
+    return RotationAcc( r1.R  * r2.R, 
+                      r1.w  + r1.R*r2.w,  
+                      r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw 
+                      );
+}
+
+RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) {
+    return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw);
+}
+
+RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) {
+    return RotationAcc( r1.R*r2, r1.w, r1.dw );
+}
+
+RotationAcc& RotationAcc::operator = (const RotationAcc& arg) {
+    R=arg.R;
+    w=arg.w;
+    dw=arg.dw;
+    return *this;
+}
+RotationAcc& RotationAcc::operator = (const Rotation& arg) {
+    R = arg;
+    w = Vector::Zero();
+    dw = Vector::Zero();
+    return *this;
+}
+
+RotationAcc RotationAcc::Identity() {
+    return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero());
+}
+
+RotationAcc RotationAcc::Inverse() const {
+    return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw));
+}
+
+VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const {
+    VectorAcc tmp;
+    tmp.p  = R.Inverse(arg.p);
+    tmp.v  = R.Inverse(arg.v - w * arg.p);
+    tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v));
+    return tmp;
+}
+
+VectorAcc RotationAcc::Inverse(const Vector& arg) const {
+    VectorAcc tmp;
+    tmp.p  = R.Inverse(arg);
+    tmp.v  = R.Inverse(-w*arg);
+    tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v));
+    return tmp;
+}
+
+
+VectorAcc RotationAcc::operator*(const VectorAcc& arg) const {
+    VectorAcc tmp;
+    tmp.p = R*arg.p;
+    tmp.dv = R*arg.v;
+    tmp.v = w*tmp.p + tmp.dv;
+    tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv;
+    return tmp;
+}
+
+VectorAcc operator*(const Rotation& R,const VectorAcc& x) {
+    return VectorAcc(R*x.p,R*x.v,R*x.dv);
+}
+
+VectorAcc RotationAcc::operator*(const Vector& arg) const {
+    VectorAcc tmp;
+    tmp.p = R*arg;
+    tmp.v = w*tmp.p;
+    tmp.dv = dw*tmp.p + w*tmp.v;
+    return tmp;
+}
+
+/*
+        // = Rotations
+        // The Rot... static functions give the value of the appropriate rotation matrix back.
+        // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
+
+        void RRotation::DoRotX(const RDouble& angle) {
+            w+=R*Vector(angle.grad,0,0);
+            R.DoRotX(angle.t);
+        }
+RotationAcc RotationAcc::RotX(const doubleAcc& angle) {
+    return RotationAcc(Rotation::RotX(angle.t),
+                      Vector(angle.d,0,0),
+                      Vector(angle.dd,0,0)
+                      );
+}
+
+        void RRotation::DoRotY(const RDouble& angle) {
+            w+=R*Vector(0,angle.grad,0);
+            R.DoRotY(angle.t);
+        }
+RotationAcc RotationAcc::RotY(const doubleAcc& angle) {
+    return RotationAcc(
+              Rotation::RotX(angle.t),
+              Vector(0,angle.d,0),
+              Vector(0,angle.dd,0)
+            );
+}
+
+        void RRotation::DoRotZ(const RDouble& angle) {
+            w+=R*Vector(0,0,angle.grad);
+            R.DoRotZ(angle.t);
+        }
+RotationAcc RotationAcc::RotZ(const doubleAcc& angle) {
+    return RotationAcc(
+                Rotation::RotZ(angle.t),
+                Vector(0,0,angle.d),
+                Vector(0,0,angle.dd)
+            );
+}
+
+
+        RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle) 
+        // rotvec has arbitrary norm
+        // rotation around a constant vector !
+        {
+            Vector v = rotvec.Normalize();
+            return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad);
+        }
+
+        RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle) 
+            // rotvec is normalized.
+        {
+            return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
+        }
+
+*/
+
+bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) {
+    return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) );
+}
+bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) {
+    return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) && 
+            Equal(Vector::Zero(),r2.dw,eps) );
+}
+bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) {
+    return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) && 
+            Equal(r1.dw,Vector::Zero(),eps) );
+}
+
+
+// Methods and operators related to FrameAcc
+// They all delegate most of the work to RotationAcc and VectorAcc
+FrameAcc& FrameAcc::operator = (const FrameAcc& arg) {
+    M=arg.M;
+    p=arg.p;
+    return *this;
+}
+
+FrameAcc FrameAcc::Identity() {
+    return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero());
+}
+
+
+FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs)
+{
+    return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs)
+{
+    return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs)
+{
+    return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+
+VectorAcc FrameAcc::operator *(const VectorAcc & arg) const
+{
+    return M*arg+p;
+}
+VectorAcc FrameAcc::operator *(const Vector & arg) const
+{
+    return M*arg+p;
+}
+
+VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+VectorAcc FrameAcc::Inverse(const Vector& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+FrameAcc FrameAcc::Inverse() const
+{
+    return FrameAcc(M.Inverse(),-M.Inverse(p));
+}
+
+FrameAcc& FrameAcc::operator =(const Frame & arg)
+{ 
+    M = arg.M;
+    p = arg.p;
+    return *this;
+}
+
+bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const Frame& r1,const FrameAcc& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const FrameAcc& r1,const Frame& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+
+
+Frame FrameAcc::GetFrame() const {
+    return Frame(M.R,p.p);
+}
+
+
+Twist FrameAcc::GetTwist() const {
+    return Twist(p.v,M.w);
+}
+
+
+Twist FrameAcc::GetAccTwist() const {
+    return Twist(p.dv,M.dw);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+TwistAcc TwistAcc::Zero()
+{
+    return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
+}
+
+
+void TwistAcc::ReverseSign()
+{   
+    vel.ReverseSign();
+    rot.ReverseSign();
+}
+
+TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB)
+     // Changes the reference point of the TwistAcc.
+     // The RVector v_base_AB is expressed in the same base as the TwistAcc
+     // The RVector v_base_AB is a RVector from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+{
+    return TwistAcc(this->vel+this->rot*v_base_AB,this->rot);
+}
+
+TwistAcc& TwistAcc::operator-=(const TwistAcc& arg)
+{
+    vel-=arg.vel;
+    rot -=arg.rot;
+    return *this;
+}
+
+TwistAcc& TwistAcc::operator+=(const TwistAcc& arg)
+{
+    vel+=arg.vel;
+    rot +=arg.rot;
+    return *this;
+}
+
+
+TwistAcc operator*(const TwistAcc& lhs,double rhs)
+{
+    return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistAcc operator*(double lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistAcc operator/(const TwistAcc& lhs,double rhs)
+{
+    return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs)
+{
+    return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs)
+{
+    return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+
+// addition of TwistAcc's
+TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
+}
+
+TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
+}
+
+// unary -
+TwistAcc operator-(const TwistAcc& arg) 
+{
+    return TwistAcc(-arg.vel,-arg.rot);
+}
+
+
+
+
+
+TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const
+{
+    return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistAcc RotationAcc::operator * (const TwistAcc& arg) const
+{
+    return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
+}
+
+TwistAcc RotationAcc::Inverse(const Twist& arg) const
+{
+    return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistAcc RotationAcc::operator * (const Twist& arg) const
+{
+    return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
+}
+
+
+TwistAcc FrameAcc::operator * (const TwistAcc& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistAcc FrameAcc::operator * (const Twist& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+TwistAcc FrameAcc::Inverse(const Twist& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+Twist TwistAcc::GetTwist() const {
+    return Twist(vel.p,rot.p);
+}
+
+Twist TwistAcc::GetTwistDot() const {
+    return Twist(vel.v,rot.v);
+}
+
+bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const Twist& a,const TwistAcc& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const TwistAcc& a,const Twist& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+

blender-5.2.0/intern/itasc/kdl/frames.cpp

@@ -0,0 +1,392 @@
+/** \file itasc/kdl/frames.cpp
+ * \ingroup intern_itasc
+ */
+/***************************************************************************
+                        frames.cxx -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ ***************************************************************************
+ *   This library is free software; you can redistribute it and/or         *
+ *   modify it under the terms of the GNU Lesser General Public            *
+ *   License as published by the Free Software Foundation; either          *
+ *   version 2.1 of the License, or (at your option) any later version.    *
+ *                                                                         *
+ *   This library 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     *
+ *   Lesser General Public License for more details.                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU Lesser General Public      *
+ *   License along with this library; if not, write to the Free Software   *
+ *   Foundation, Inc., 51 Franklin Street,                                    *
+ *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
+ *                                                                         *
+ ***************************************************************************/
+
+#include "frames.hpp"
+
+namespace KDL {
+
+#ifndef KDL_INLINE
+#include "frames.inl"
+#endif
+
+void Frame::Make4x4(double * d)
+{
+    int i;
+    int j;
+    for (i=0;i<3;i++) {
+        for (j=0;j<3;j++)
+            d[i*4+j]=M(i,j);
+        d[i*4+3] = p(i)/1000;
+    }
+    for (j=0;j<3;j++)
+        d[12+j] = 0.;
+    d[15] = 1;
+}
+
+Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta)
+// returns Modified Denavit-Hartenberg parameters (According to Craig)
+{
+    double ct,st,ca,sa;
+    ct = cos(theta);
+    st = sin(theta);
+    sa = sin(alpha);
+    ca = cos(alpha);
+    return Frame(Rotation(
+                    ct,       -st,     0,
+                    st*ca,  ct*ca,   -sa,
+                    st*sa,  ct*sa,    ca   ),
+                 Vector(
+                    a,      -sa*d,  ca*d   )
+            );
+}
+
+Frame Frame::DH(double a,double alpha,double d,double theta)
+// returns Denavit-Hartenberg parameters (Non-Modified DH)
+{
+    double ct,st,ca,sa;
+    ct = cos(theta);
+    st = sin(theta);
+    sa = sin(alpha);
+    ca = cos(alpha);
+    return Frame(Rotation(
+                    ct,    -st*ca,   st*sa,
+                    st,     ct*ca,  -ct*sa,
+                     0,        sa,      ca   ),
+                 Vector(
+                    a*ct,      a*st,  d   )
+            );
+}
+
+double Vector2::Norm() const
+{
+    double tmp0 = fabs(data[0]);
+    double tmp1 = fabs(data[1]);
+    if (tmp0 >= tmp1) {
+		if (tmp1 == 0)
+			return 0;
+        return tmp0*sqrt(1+sqr(tmp1/tmp0));
+    } else {
+        return tmp1*sqrt(1+sqr(tmp0/tmp1));
+    }
+}
+// makes v a unitvector and returns the norm of v.
+// if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+// if this is not good, check the return value of this method.
+double Vector2::Normalize(double eps) {
+	double v = this->Norm();
+	if (v < eps) {
+		*this = Vector2(1,0);
+		return v;
+	} else {
+		*this = (*this)/v;
+		return v;
+	}
+}
+
+
+// do some effort not to lose precision
+double Vector::Norm() const
+{
+    double tmp1;
+    double tmp2;
+    tmp1 = fabs(data[0]);
+    tmp2 = fabs(data[1]);
+    if (tmp1 >= tmp2) {
+        tmp2=fabs(data[2]);
+        if (tmp1 >= tmp2) {
+        	if (tmp1 == 0) {
+        		// only to everything exactly zero case, all other are handled correctly
+        		return 0;
+        	}
+            return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
+        } else {
+            return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
+        }
+    } else {
+        tmp1=fabs(data[2]);
+        if (tmp2 > tmp1) {
+            return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
+        } else {
+            return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
+        }
+    }
+}
+
+// makes v a unitvector and returns the norm of v.
+// if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+// if this is not good, check the return value of this method.
+double Vector::Normalize(double eps) {
+	double v = this->Norm();
+	if (v < eps) {
+		*this = Vector(1,0,0);
+		return v;
+	} else {
+		*this = (*this)/v;
+		return v;
+	}
+}
+
+
+bool Equal(const Rotation& a,const Rotation& b,double eps) {
+    return (Equal(a.data[0],b.data[0],eps) &&
+            Equal(a.data[1],b.data[1],eps) &&
+            Equal(a.data[2],b.data[2],eps) &&
+            Equal(a.data[3],b.data[3],eps) &&
+            Equal(a.data[4],b.data[4],eps) &&
+            Equal(a.data[5],b.data[5],eps) &&
+            Equal(a.data[6],b.data[6],eps) &&
+            Equal(a.data[7],b.data[7],eps) &&
+            Equal(a.data[8],b.data[8],eps)    );
+}
+
+void Rotation::Ortho()
+{
+	double n;
+	n=sqrt(sqr(data[0])+sqr(data[3])+sqr(data[6]));n=(n>1e-10)?1.0/n:0.0;data[0]*=n;data[3]*=n;data[6]*=n; 
+	n=sqrt(sqr(data[1])+sqr(data[4])+sqr(data[7]));n=(n>1e-10)?1.0/n:0.0;data[1]*=n;data[4]*=n;data[7]*=n; 
+	n=sqrt(sqr(data[2])+sqr(data[5])+sqr(data[8]));n=(n>1e-10)?1.0/n:0.0;data[2]*=n;data[5]*=n;data[8]*=n; 
+}
+
+Rotation operator *(const Rotation& lhs,const Rotation& rhs)
+// Complexity : 27M+27A
+{
+    return Rotation(
+        lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6],
+        lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7],
+        lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8],
+        lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6],
+        lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7],
+        lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8],
+        lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6],
+        lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7],
+        lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8]
+    );
+
+}
+
+
+Rotation Rotation::RPY(double roll,double pitch,double yaw)
+    {
+        double ca1,cb1,cc1,sa1,sb1,sc1;
+        ca1 = cos(yaw); sa1 = sin(yaw);
+        cb1 = cos(pitch);sb1 = sin(pitch);
+        cc1 = cos(roll);sc1 = sin(roll);
+        return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
+                   sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
+                   -sb1,cb1*sc1,cb1*cc1);
+    }
+
+// Gives back a rotation matrix specified with RPY convention
+void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
+    {
+        if (fabs(data[6]) > 1.0 - epsilon ) {
+            roll = -sign(data[6]) * atan2(data[1], data[4]);
+            pitch= -sign(data[6]) * PI / 2;
+            yaw  = 0.0 ;
+        } else {
+            roll  = atan2(data[7], data[8]);
+            pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
+            yaw   = atan2(data[3], data[0]);
+        }
+    }
+
+Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
+        double sa,ca,sb,cb,sg,cg;
+        sa  = sin(Alfa);ca = cos(Alfa);
+        sb  = sin(Beta);cb = cos(Beta);
+        sg  = sin(Gamma);cg = cos(Gamma);
+        return Rotation( ca*cb*cg-sa*sg,     -ca*cb*sg-sa*cg,        ca*sb,
+                 sa*cb*cg+ca*sg,     -sa*cb*sg+ca*cg,        sa*sb,
+                 -sb*cg ,                sb*sg,              cb
+                );
+
+     }
+
+
+void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const {
+        if (fabs(data[6]) < epsilon ) {
+            alfa=0.0;
+            if (data[8]>0) {
+                beta = 0.0;
+                gamma= atan2(-data[1],data[0]);
+            } else {
+                beta = PI;
+                gamma= atan2(data[1],-data[0]);
+            }
+        } else {
+            alfa=atan2(data[5], data[2]);
+            beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
+            gamma=atan2(data[7], -data[6]);
+        }
+ }
+
+Rotation Rotation::Rot(const Vector& rotaxis,double angle) {
+    // The formula is
+    // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
+    // can be found by multiplying it with an arbitrary vector p
+    // and noting that this vector is rotated.
+    double ct = cos(angle);
+    double st = sin(angle);
+    double vt = 1-ct;
+    Vector rotvec = rotaxis;
+	rotvec.Normalize();
+    return Rotation(
+        ct            +  vt*rotvec(0)*rotvec(0),
+        -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
+        rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
+        rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
+        ct            +  vt*rotvec(1)*rotvec(1),
+        -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
+        -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
+        rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
+        ct            +  vt*rotvec(2)*rotvec(2)
+        );
+    }
+
+Rotation Rotation::Rot2(const Vector& rotvec,double angle) {
+    // rotvec should be normalized !
+    // The formula is
+    // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
+    // can be found by multiplying it with an arbitrary vector p
+    // and noting that this vector is rotated.
+    double ct = cos(angle);
+    double st = sin(angle);
+    double vt = 1-ct;
+    return Rotation(
+        ct            +  vt*rotvec(0)*rotvec(0),
+        -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
+        rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
+        rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
+        ct            +  vt*rotvec(1)*rotvec(1),
+        -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
+        -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
+        rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
+        ct            +  vt*rotvec(2)*rotvec(2)
+        );
+}
+
+
+
+Vector Rotation::GetRot() const
+         // Returns a vector with the direction of the equiv. axis
+         // and its norm is angle
+     {
+       Vector axis  = Vector((data[7]-data[5]),
+			     (data[2]-data[6]),
+			     (data[3]-data[1]) )/2;
+
+       double sa    = axis.Norm();
+       double ca    = (data[0]+data[4]+data[8]-1)/2.0;
+       double alfa;
+       if (sa > epsilon)
+           alfa = ::atan2(sa,ca)/sa;
+	   else {
+		   if (ca < 0.0) {
+			   alfa = KDL::PI;
+			   axis.data[0] = 0.0;
+			   axis.data[1] = 0.0;
+			   axis.data[2] = 0.0;
+			   if (data[0] > 0.0) {
+				   axis.data[0] = 1.0;
+			   } else if (data[4] > 0.0) {
+				   axis.data[1] = 1.0;
+			   } else {
+				   axis.data[2] = 1.0;
+			   }
+		   } else {
+			   alfa = 0.0;
+		   }
+	   }
+       return axis * alfa;
+     }
+
+Vector2 Rotation::GetXZRot() const
+{
+	// [0,1,0] x Y
+	Vector2 axis(data[7], -data[1]);
+	double norm = axis.Normalize();
+	if (norm < epsilon) {
+		norm = (data[4] < 0.0) ? PI : 0.0;
+	} else {
+		norm = acos(data[4]);
+	}
+	return axis*norm;
+}
+
+
+/** Returns the rotation angle around the equiv. axis
+ * @param axis the rotation axis is returned in this variable
+ * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
+ *                                         to be +/- Z-axis
+ *               in the case of angle == PI : 2 solutions, positive Z-component
+ *                                            of the axis is choosen.
+ * @result returns the rotation angle (between [0..PI] )
+ * /todo :
+ *   Check corresponding routines in rframes and rrframes
+ */
+double Rotation::GetRotAngle(Vector& axis,double eps) const {
+	double ca    = (data[0]+data[4]+data[8]-1)/2.0;
+	if (ca>1-eps) {
+		// undefined choose the Z-axis, and angle 0
+		axis = Vector(0,0,1);
+		return 0;
+	}
+	if (ca < -1+eps) {
+		// two solutions, choose a positive Z-component of the axis
+		double z = sqrt( (data[8]+1)/2 );
+		double x = (data[2])/2/z;
+		double y = (data[5])/2/z;
+		axis = Vector( x,y,z  );
+		return PI;
+	}
+	double angle = acos(ca);
+	double sa    = sin(angle);
+	axis  = Vector((data[7]-data[5])/2/sa,
+                       (data[2]-data[6])/2/sa,
+                       (data[3]-data[1])/2/sa  );
+	return angle;
+}
+
+bool operator==(const Rotation& a,const Rotation& b) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a,b);
+#else
+    return ( a.data[0]==b.data[0] &&
+             a.data[1]==b.data[1] &&
+             a.data[2]==b.data[2] &&
+             a.data[3]==b.data[3] &&
+             a.data[4]==b.data[4] &&
+             a.data[5]==b.data[5] &&
+             a.data[6]==b.data[6] &&
+             a.data[7]==b.data[7] &&
+             a.data[8]==b.data[8]  );
+#endif
+}
+}

blender-5.2.0/intern/itasc/kdl/frames_io.cpp

@@ -0,0 +1,313 @@
+/** \file itasc/kdl/frames_io.cpp
+ * \ingroup intern_itasc
+ */
+
+/***************************************************************************
+                        frames_io.h -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ ***************************************************************************
+ *   This library is free software; you can redistribute it and/or         *
+ *   modify it under the terms of the GNU Lesser General Public            *
+ *   License as published by the Free Software Foundation; either          *
+ *   version 2.1 of the License, or (at your option) any later version.    *
+ *                                                                         *
+ *   This library 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     *
+ *   Lesser General Public License for more details.                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU Lesser General Public      *
+ *   License along with this library; if not, write to the Free Software   *
+ *   Foundation, Inc., 51 Franklin Street,                                    *
+ *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
+ *                                                                         *
+ ***************************************************************************/
+
+#include "utilities/error.h"
+#include "utilities/error_stack.h"
+#include "frames.hpp"
+#include "frames_io.hpp"
+
+#include <stdlib.h>
+#include <ctype.h>
+#include <string.h>
+#include <iostream>
+
+namespace KDL {
+
+
+std::ostream& operator << (std::ostream& os,const Vector& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os,const Twist& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2)
+       << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os,const Wrench& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.force(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2)
+       << "]";
+    return os;
+}
+
+
+std::ostream& operator << (std::ostream& os,const Rotation& R) {
+#ifdef KDL_ROTATION_PROPERTIES_RPY
+    double r,p,y;
+    R.GetRPY(r,p,y);
+    os << "[RPY]"<<endl;
+    os << "[";
+    os << std::setw(KDL_FRAME_WIDTH) << r << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << p << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << y << "]";
+#else
+# ifdef KDL_ROTATION_PROPERTIES_EULER
+    double z,y,x;
+    R.GetEulerZYX(z,y,x);
+    os << "[EULERZYX]"<<endl;
+    os << "[";
+    os << std::setw(KDL_FRAME_WIDTH) << z << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << y << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << x << "]";
+# else
+    os << "[";
+    for (int i=0;i<=2;i++) {
+        os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," <<
+                       std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," <<
+                       std::setw(KDL_FRAME_WIDTH) << R(i,2);
+        if (i<2)
+            os << ";"<< std::endl << " ";
+        else
+            os << "]";
+    }
+# endif
+#endif
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os, const Frame& T)
+{
+    os << "[" << T.M << std::endl<< T.p << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os,const Vector2& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
+       << "]";
+    return os;
+}
+
+// Rotation2 gives back an angle in degrees with the << and >> operators.
+std::ostream& operator << (std::ostream& os,const Rotation2& R) {
+    os << "[" << R.GetRot()*rad2deg << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os, const Frame2& T)
+{
+    os << T.M << T.p;
+    return os;
+}
+
+std::istream& operator >> (std::istream& is,Vector& v)
+{   IOTrace("Stream input Vector (vector or ZERO)");
+    char storage[10];
+    EatWord(is,"[]",storage,10);
+    if (storage[0]=='\0') {
+        Eat(is,'[');
+        is >> v(0);
+        Eat(is,',');
+        is >> v(1);
+        Eat(is,',');
+        is >> v(2);
+        EatEnd(is,']');
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"ZERO")==0) {
+        v = Vector::Zero();
+        IOTracePop();
+        return is;
+    }
+    throw Error_Frame_Vector_Unexpected_id();
+}
+
+std::istream& operator >> (std::istream& is,Twist& v)
+{   IOTrace("Stream input Twist");
+    Eat(is,'[');
+    is >> v.vel(0);
+    Eat(is,',');
+    is >> v.vel(1);
+    Eat(is,',');
+    is >> v.vel(2);
+    Eat(is,',');
+    is >> v.rot(0);
+    Eat(is,',');
+    is >> v.rot(1);
+    Eat(is,',');
+    is >> v.rot(2);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Wrench& v)
+{   IOTrace("Stream input Wrench");
+    Eat(is,'[');
+    is >> v.force(0);
+    Eat(is,',');
+    is >> v.force(1);
+    Eat(is,',');
+    is >> v.force(2);
+    Eat(is,',');
+    is >> v.torque(0);
+    Eat(is,',');
+    is >> v.torque(1);
+    Eat(is,',');
+    is >> v.torque(2);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Rotation& r)
+{   IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)");
+    char storage[10];
+    EatWord(is,"[]",storage,10);
+    if (storage[0]=='\0') {
+        Eat(is,'[');
+        for (int i=0;i<3;i++) {
+            is >> r(i,0);
+            Eat(is,',') ;
+            is >> r(i,1);
+            Eat(is,',');
+            is >> r(i,2);
+            if (i<2)
+                Eat(is,';');
+            else
+                EatEnd(is,']');
+        }
+        IOTracePop();
+        return is;
+    }
+    Vector v;
+    if (strcmp(storage,"EULERZYX")==0) {
+        is >> v;
+        v=v*deg2rad;
+        r = Rotation::EulerZYX(v(0),v(1),v(2));
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"EULERZYZ")==0) {
+        is >> v;
+        v=v*deg2rad;
+        r = Rotation::EulerZYZ(v(0),v(1),v(2));
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"RPY")==0) {
+        is >> v;
+        v=v*deg2rad;
+        r = Rotation::RPY(v(0),v(1),v(2));
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"ROT")==0) {
+        is >> v;
+        double angle;
+        Eat(is,'[');
+        is >> angle;
+        EatEnd(is,']');
+        r = Rotation::Rot(v,angle*deg2rad);
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"IDENTITY")==0) {
+        r = Rotation::Identity();
+        IOTracePop();
+        return is;
+    }
+    throw Error_Frame_Rotation_Unexpected_id();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Frame& T)
+{   IOTrace("Stream input Frame (Rotation,Vector) or DH[...]");
+    char storage[10];
+    EatWord(is,"[",storage,10);
+    if (storage[0]=='\0') {
+        Eat(is,'[');
+        is >> T.M;
+        is >> T.p;
+        EatEnd(is,']');
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"DH")==0) {
+        double a,alpha,d,theta;
+        Eat(is,'[');
+        is >> a;
+        Eat(is,',');
+        is >> alpha;
+        Eat(is,',');
+        is >> d;
+        Eat(is,',');
+        is >> theta;
+        EatEnd(is,']');
+        T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad);
+        IOTracePop();
+        return is;
+    }
+    throw Error_Frame_Frame_Unexpected_id();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Vector2& v)
+{   IOTrace("Stream input Vector2");
+    Eat(is,'[');
+    is >> v(0);
+    Eat(is,',');
+    is >> v(1);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+std::istream& operator >> (std::istream& is,Rotation2& r)
+{   IOTrace("Stream input Rotation2");
+    Eat(is,'[');
+    double val;
+    is >> val;
+    r.Rot(val*deg2rad);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+std::istream& operator >> (std::istream& is,Frame2& T)
+{   IOTrace("Stream input Frame2");
+    is >> T.M;
+    is >> T.p;
+    IOTracePop();
+    return is;
+}
+
+} // namespace Frame

blender-5.2.0/intern/itasc/kdl/frames_io.hpp

@@ -0,0 +1,114 @@
+/***************************************************************************
+                        frames_io.h -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ Ruben Smits - Added output for jacobian and jntarray 06/2007
+
+ ***************************************************************************
+ *   This library is free software; you can redistribute it and/or         *
+ *   modify it under the terms of the GNU Lesser General Public            *
+ *   License as published by the Free Software Foundation; either          *
+ *   version 2.1 of the License, or (at your option) any later version.    *
+ *                                                                         *
+ *   This library 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     *
+ *   Lesser General Public License for more details.                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU Lesser General Public      *
+ *   License along with this library; if not, write to the Free Software   *
+ *   Foundation, Inc., 51 Franklin Street,                                    *
+ *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
+ *                                                                         *
+ ***************************************************************************/
+/**
+//
+// \file
+//      Defines routines for I/O of Frame and related objects.
+// \verbatim
+//      Spaces, tabs and newlines do not have any importance.
+//      Comments are allowed C-style,C++-style, make/perl/csh -style
+//      Description of the I/O :
+//        Vector  : OUTPUT : e.g. [10,20,30]
+//                  INPUT  :
+//                         1) [10,20,30]
+//                         2) Zero
+//        Twist   : e.g. [1,2,3,4,5,6]
+//           where [1,2,3] is velocity vector
+//           where [4,5,6] is rotational velocity vector
+//        Wrench  : e.g. [1,2,3,4,5,6]
+//           where [1,2,3] represents a force vector
+//           where [4,5,6] represents a torque vector
+//        Rotation : output :
+//                 [1,2,3;
+//                  4,5,6;
+//                  7,8,9] cfr definition of Rotation object.
+//                  input :
+//                    1) like the output
+//                    2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. :
+//                        Eulerzyx[10,20,30]
+//                   (ANGLES are always expressed in DEGREES for I/O)
+//                   (ANGELS are always expressed in RADIANS for internal representation)
+//                    3) Rot [1,2,3] [20]  Rotates around axis [1,2,3] with an angle
+//                   of 20 degrees.
+//                    4) Identity          returns identity rotation matrix.
+//       Frames   : output : [ Rotationmatrix positionvector ]
+//                   e.g. [ [1,0,0;0,1,0;0,0,1] [1,2,3] ]
+//                  Input  :
+//                     1) [ Rotationmatrix positionvector ]
+//                     2) DH [ 10,10,50,30]  Denavit-Hartenberg representation
+//                     ( is in fact not the representation of a Frame, but more
+//                       limited, cfr. documentation of Frame object.)
+//  \endverbatim
+//
+// \warning
+//   You can use iostream.h or iostream header files for file I/O,
+//   if one declares the define WANT_STD_IOSTREAM then the standard C++
+//   iostreams headers are included instead of the compiler-dependent version
+//
+ *
+ ****************************************************************************/
+#ifndef FRAMES_IO_H
+#define FRAMES_IO_H
+
+#include "utilities/utility_io.h"
+#include "frames.hpp"
+#include "jntarray.hpp"
+#include "jacobian.hpp"
+
+namespace KDL {
+
+    //! width to be used when printing variables out with frames_io.h
+    //! global variable, can be changed.
+
+
+    // I/O to C++ stream.
+    std::ostream& operator << (std::ostream& os,const Vector& v);
+    std::ostream& operator << (std::ostream& os,const Rotation& R);
+    std::ostream& operator << (std::ostream& os,const Frame& T);
+    std::ostream& operator << (std::ostream& os,const Twist& T);
+    std::ostream& operator << (std::ostream& os,const Wrench& T);
+    std::ostream& operator << (std::ostream& os,const Vector2& v);
+    std::ostream& operator << (std::ostream& os,const Rotation2& R);
+    std::ostream& operator << (std::ostream& os,const Frame2& T);
+
+
+
+    std::istream& operator >> (std::istream& is,Vector& v);
+    std::istream& operator >> (std::istream& is,Rotation& R);
+    std::istream& operator >> (std::istream& is,Frame& T);
+    std::istream& operator >> (std::istream& os,Twist& T);
+    std::istream& operator >> (std::istream& os,Wrench& T);
+    std::istream& operator >> (std::istream& is,Vector2& v);
+    std::istream& operator >> (std::istream& is,Rotation2& R);
+    std::istream& operator >> (std::istream& is,Frame2& T);
+
+
+} // namespace Frame
+
+#endif

blender-5.2.0/intern/itasc/kdl/framevel.cpp

@@ -0,0 +1,28 @@
+/** \file itasc/kdl/framevel.cpp
+ * \ingroup intern_itasc
+ */
+/*****************************************************************************
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Name:  $
+ ****************************************************************************/
+
+
+#include "framevel.hpp"
+
+namespace KDL {
+
+#ifndef KDL_INLINE
+    #include "framevel.inl"
+#endif
+
+
+
+}

blender-5.2.0/intern/itasc/kdl/framevel.hpp

@@ -0,0 +1,393 @@
+/*****************************************************************************
+ * \file
+ *      This file contains the definition of classes for a
+ *      Rall Algebra of (subset of) the classes defined in frames,
+ *      i.e. classes that contain a pair (value,derivative) and define operations on that pair
+ *      this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
+ *      Defines VectorVel, RotationVel, FrameVel.  Look at Frames.h for details on how to work
+ *      with Frame objects.
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ *      ORO_Geometry V0.2
+ *
+ * \par History
+ *      - $log$
+ *
+ * \par Release
+ *      $Name:  $
+ ****************************************************************************/
+
+#ifndef KDL_FRAMEVEL_H
+#define KDL_FRAMEVEL_H
+
+#include "utilities/utility.h"
+#include "utilities/rall1d.h"
+#include "utilities/traits.h"
+
+#include "frames.hpp"
+
+
+
+namespace KDL {
+
+typedef Rall1d<double> doubleVel;
+
+IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
+	return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
+}
+
+IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
+	return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
+}
+
+IMETHOD void random(doubleVel& F) {
+	random(F.t);
+	random(F.grad);
+}
+IMETHOD void posrandom(doubleVel& F) {
+	posrandom(F.t);
+	posrandom(F.grad);
+}
+
+}
+
+template <>
+struct Traits<KDL::doubleVel> {
+	typedef double valueType;
+	typedef KDL::doubleVel derivType;
+};
+
+namespace KDL {
+
+class TwistVel;
+class VectorVel;
+class FrameVel;
+class RotationVel;
+
+class VectorVel
+// = TITLE
+//     An VectorVel is a Vector and its first derivative
+// = CLASS TYPE
+//     Concrete
+{
+public:
+    Vector p;       // position vector
+    Vector v;       // velocity vector
+public:
+    VectorVel():p(),v(){}
+    VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
+    explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
+
+    Vector value() const { return p;}
+    Vector deriv() const { return v;}
+
+    IMETHOD VectorVel& operator = (const VectorVel& arg);
+    IMETHOD VectorVel& operator = (const Vector& arg);
+    IMETHOD VectorVel& operator += (const VectorVel& arg);
+    IMETHOD VectorVel& operator -= (const VectorVel& arg);
+    IMETHOD static VectorVel Zero();
+    IMETHOD void ReverseSign();
+    IMETHOD doubleVel Norm() const;
+    IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
+    IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
+    IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
+    IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
+    IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
+
+    IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
+    IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
+    IMETHOD friend void SetToZero(VectorVel& v);
+
+
+    IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps);
+    IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps);
+    IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps);
+    IMETHOD friend VectorVel operator - (const VectorVel& r);
+    IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
+    IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
+    IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
+};
+
+
+
+class RotationVel
+// = TITLE
+//     An RotationVel is a Rotation and its first derivative, a rotation vector
+// = CLASS TYPE
+//     Concrete
+{
+public:
+    Rotation R; // Rotation matrix
+    Vector   w; // rotation vector
+public:
+    RotationVel():R(),w() {}
+    explicit RotationVel(const Rotation& R_):R(R_),w(Vector::Zero()){}
+    RotationVel(const Rotation& R_,const Vector& _w):R(R_),w(_w){}
+
+
+    Rotation value() const { return R;}
+    Vector   deriv() const { return w;}
+
+
+    IMETHOD RotationVel& operator = (const RotationVel& arg);
+    IMETHOD RotationVel& operator = (const Rotation& arg);
+	IMETHOD VectorVel UnitX() const;
+	IMETHOD VectorVel UnitY() const;
+	IMETHOD VectorVel UnitZ() const;
+    IMETHOD static RotationVel Identity();
+    IMETHOD RotationVel Inverse() const;
+    IMETHOD VectorVel Inverse(const VectorVel& arg) const;
+    IMETHOD VectorVel Inverse(const Vector& arg) const;
+    IMETHOD VectorVel operator*(const VectorVel& arg) const;
+    IMETHOD VectorVel operator*(const Vector& arg) const;
+    IMETHOD void DoRotX(const doubleVel& angle);
+    IMETHOD void DoRotY(const doubleVel& angle);
+    IMETHOD void DoRotZ(const doubleVel& angle);
+    IMETHOD static RotationVel RotX(const doubleVel& angle);
+    IMETHOD static RotationVel RotY(const doubleVel& angle);
+    IMETHOD static RotationVel RotZ(const doubleVel& angle);
+    IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
+    // rotvec has arbitrary norm
+    // rotation around a constant vector !
+    IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
+    // rotvec is normalized.
+    // rotation around a constant vector !
+    IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
+    IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
+    IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
+    IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps);
+    IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps);
+    IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps);
+
+    IMETHOD TwistVel Inverse(const TwistVel& arg) const;
+    IMETHOD TwistVel Inverse(const Twist& arg) const;
+    IMETHOD TwistVel operator * (const TwistVel& arg) const;
+    IMETHOD TwistVel operator * (const Twist& arg) const;
+};
+
+
+
+
+class FrameVel
+// = TITLE
+//     An FrameVel is a Frame and its first derivative, a Twist vector
+// = CLASS TYPE
+//     Concrete
+// = CAVEATS
+//
+{
+public:
+    RotationVel M;
+    VectorVel   p;
+public:
+    FrameVel(){}
+
+    explicit FrameVel(const Frame& T_):
+        M(T_.M),p(T_.p) {}
+
+    FrameVel(const Frame& T_,const Twist& _t):
+        M(T_.M,_t.rot),p(T_.p,_t.vel) {}
+
+    FrameVel(const RotationVel& _M,const VectorVel& _p):
+        M(_M),p(_p) {}
+
+
+    Frame value() const { return Frame(M.value(),p.value());}
+    Twist deriv() const { return Twist(p.deriv(),M.deriv());}
+
+
+    IMETHOD FrameVel& operator = (const Frame& arg);
+    IMETHOD FrameVel& operator = (const FrameVel& arg);
+    IMETHOD static FrameVel Identity();
+    IMETHOD FrameVel Inverse() const;
+    IMETHOD VectorVel Inverse(const VectorVel& arg) const;
+    IMETHOD VectorVel operator*(const VectorVel& arg) const;
+    IMETHOD VectorVel operator*(const Vector& arg) const;
+    IMETHOD VectorVel Inverse(const Vector& arg) const;
+    IMETHOD Frame GetFrame() const;
+    IMETHOD Twist GetTwist() const;
+    IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
+    IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
+    IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
+    IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps);
+    IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps);
+    IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps);
+
+    IMETHOD TwistVel  Inverse(const TwistVel& arg) const;
+    IMETHOD TwistVel  Inverse(const Twist& arg) const;
+    IMETHOD TwistVel operator * (const TwistVel& arg) const;
+    IMETHOD TwistVel operator * (const Twist& arg) const;
+};
+
+
+
+
+
+//very similar to Wrench class.
+class TwistVel
+// = TITLE
+// This class represents a TwistVel. This is a velocity and rotational velocity together
+{
+public:
+    VectorVel vel;
+    VectorVel rot;
+public:
+
+// = Constructors
+    TwistVel():vel(),rot() {};
+    TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
+    TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
+    TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
+
+    Twist value() const {
+        return Twist(vel.value(),rot.value());
+    }
+    Twist deriv() const {
+        return Twist(vel.deriv(),rot.deriv());
+    }
+// = Operators
+     IMETHOD TwistVel& operator-=(const TwistVel& arg);
+     IMETHOD TwistVel& operator+=(const TwistVel& arg);
+
+// = External operators
+     IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
+     IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
+
+     IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
+     IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
+
+     IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator-(const TwistVel& arg);
+     IMETHOD friend void SetToZero(TwistVel& v);
+
+
+// = Zero
+     static IMETHOD TwistVel Zero();
+
+// = Reverse Sign
+     IMETHOD void ReverseSign();
+
+// = Change Reference point
+     IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
+     // Changes the reference point of the TwistVel.
+     // The VectorVel v_base_AB is expressed in the same base as the TwistVel
+     // The VectorVel v_base_AB is a VectorVel from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+
+     // = Equality operators
+     // do not use operator == because the definition of Equal(.,.) is slightly
+     // different.  It compares whether the 2 arguments are equal in an eps-interval
+     IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps);
+     IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps);
+     IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps);
+
+// = Conversion to other entities
+     IMETHOD Twist GetTwist() const;
+     IMETHOD Twist GetTwistDot() const;
+// = Friends
+    friend class RotationVel;
+    friend class FrameVel;
+
+};
+
+IMETHOD bool Equal(const VectorVel&,   const VectorVel&,   double = epsilon);
+IMETHOD bool Equal(const Vector&,      const VectorVel&,   double = epsilon);
+IMETHOD bool Equal(const VectorVel&,   const Vector&,      double = epsilon);
+IMETHOD bool Equal(const RotationVel&, const RotationVel&, double = epsilon);
+IMETHOD bool Equal(const Rotation&,    const RotationVel&, double = epsilon);
+IMETHOD bool Equal(const RotationVel&, const Rotation&,    double = epsilon);
+IMETHOD bool Equal(const FrameVel&,    const FrameVel&,    double = epsilon);
+IMETHOD bool Equal(const Frame&,       const FrameVel&,    double = epsilon);
+IMETHOD bool Equal(const FrameVel&,    const Frame&,       double = epsilon);
+IMETHOD bool Equal(const TwistVel&,    const TwistVel&,    double = epsilon);
+IMETHOD bool Equal(const Twist&,       const TwistVel&,    double = epsilon);
+IMETHOD bool Equal(const TwistVel&,    const Twist&,       double = epsilon);
+
+IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
+	return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
+}
+
+IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
+	return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
+}
+IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
+	return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
+}
+
+IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
+	return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
+}
+
+IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
+	return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
+}
+
+IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
+	return FrameVel(
+			addDelta(a.M,da.rot,dt),
+			addDelta(a.p,da.vel,dt)
+		   );
+}
+
+IMETHOD void random(VectorVel& a) {
+	random(a.p);
+	random(a.v);
+}
+IMETHOD void random(TwistVel& a) {
+	random(a.vel);
+	random(a.rot);
+}
+
+IMETHOD void random(RotationVel& R) {
+	random(R.R);
+	random(R.w);
+}
+
+IMETHOD void random(FrameVel& F) {
+	random(F.M);
+	random(F.p);
+}
+IMETHOD void posrandom(VectorVel& a) {
+	posrandom(a.p);
+	posrandom(a.v);
+}
+IMETHOD void posrandom(TwistVel& a) {
+	posrandom(a.vel);
+	posrandom(a.rot);
+}
+
+IMETHOD void posrandom(RotationVel& R) {
+	posrandom(R.R);
+	posrandom(R.w);
+}
+
+IMETHOD void posrandom(FrameVel& F) {
+	posrandom(F.M);
+	posrandom(F.p);
+}
+
+#ifdef KDL_INLINE
+#include "framevel.inl"
+#endif
+
+} // namespace
+
+#endif
+
+
+
+

blender-5.2.0/intern/itasc/kdl/framevel.inl

@@ -0,0 +1,532 @@
+/*****************************************************************************
+ * \file  
+ *      provides inline functions of rframes.h
+ *       
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version 
+ *      ORO_Geometry V0.2
+ *
+ *  \par History
+ *      - $log$
+ *
+ *  \par Release
+ *      $Name:  $ 
+ ****************************************************************************/
+
+
+// Methods and operators related to FrameVelVel
+// They all delegate most of the work to RotationVelVel and VectorVelVel
+FrameVel& FrameVel::operator = (const FrameVel& arg) {
+    M=arg.M;
+    p=arg.p;
+    return *this;
+}
+
+FrameVel FrameVel::Identity() {
+    return FrameVel(RotationVel::Identity(),VectorVel::Zero());
+}
+
+
+FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs)
+{
+    return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameVel operator *(const FrameVel& lhs,const Frame& rhs)
+{
+    return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameVel operator *(const Frame& lhs,const FrameVel& rhs)
+{
+    return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p );
+}
+
+VectorVel FrameVel::operator *(const VectorVel & arg) const
+{
+    return M*arg+p;
+}
+VectorVel FrameVel::operator *(const Vector & arg) const
+{
+    return M*arg+p;
+}
+
+VectorVel FrameVel::Inverse(const VectorVel& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+VectorVel FrameVel::Inverse(const Vector& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+FrameVel FrameVel::Inverse() const
+{
+    return FrameVel(M.Inverse(),-M.Inverse(p));
+}
+
+FrameVel& FrameVel::operator = (const Frame& arg) {
+    M = arg.M;
+    p = arg.p;
+    return *this;
+}
+bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const Frame& r1,const FrameVel& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const FrameVel& r1,const Frame& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+
+Frame FrameVel::GetFrame() const {
+    return Frame(M.R,p.p);
+}
+
+Twist FrameVel::GetTwist() const {
+    return Twist(p.v,M.w);
+}
+
+
+RotationVel operator* (const RotationVel& r1,const RotationVel& r2) {
+    return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w );
+}
+
+RotationVel operator* (const Rotation& r1,const RotationVel& r2) {
+    return RotationVel( r1*r2.R, r1*r2.w );
+}
+
+RotationVel operator* (const RotationVel& r1,const Rotation& r2) {
+    return RotationVel( r1.R*r2, r1.w );
+}
+
+RotationVel& RotationVel::operator = (const RotationVel& arg) {
+        R=arg.R;
+        w=arg.w;
+        return *this;
+    }
+RotationVel& RotationVel::operator = (const Rotation& arg) {
+    R=arg;
+    w=Vector::Zero();
+    return *this;
+}
+
+VectorVel   RotationVel::UnitX() const {
+	return VectorVel(R.UnitX(),w*R.UnitX()); 
+}
+
+VectorVel   RotationVel::UnitY() const {
+	return VectorVel(R.UnitY(),w*R.UnitY()); 
+}
+
+VectorVel   RotationVel::UnitZ() const {
+	return VectorVel(R.UnitZ(),w*R.UnitZ()); 
+}
+
+
+
+RotationVel RotationVel::Identity() {
+    return RotationVel(Rotation::Identity(),Vector::Zero());
+}
+
+RotationVel RotationVel::Inverse() const {
+    return RotationVel(R.Inverse(),-R.Inverse(w));
+}
+
+VectorVel RotationVel::Inverse(const VectorVel& arg) const {
+    Vector tmp=R.Inverse(arg.p);
+    return VectorVel(tmp,
+                    R.Inverse(arg.v-w*arg.p)
+                    );
+}
+
+VectorVel RotationVel::Inverse(const Vector& arg) const {
+    Vector tmp=R.Inverse(arg);
+    return VectorVel(tmp,
+                    R.Inverse(-w*arg)
+                    );
+}
+
+
+VectorVel RotationVel::operator*(const VectorVel& arg) const {
+    Vector tmp=R*arg.p;
+    return VectorVel(tmp,w*tmp+R*arg.v);
+}
+
+VectorVel RotationVel::operator*(const Vector& arg) const {
+    Vector tmp=R*arg;
+    return VectorVel(tmp,w*tmp);
+}
+
+
+// = Rotations
+// The Rot... static functions give the value of the appropriate rotation matrix back.
+// The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
+
+void RotationVel::DoRotX(const doubleVel& angle) {
+    w+=R*Vector(angle.grad,0,0);
+    R.DoRotX(angle.t);
+}
+RotationVel RotationVel::RotX(const doubleVel& angle) {
+    return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0));
+}
+
+void RotationVel::DoRotY(const doubleVel& angle) {
+    w+=R*Vector(0,angle.grad,0);
+    R.DoRotY(angle.t);
+}
+RotationVel RotationVel::RotY(const doubleVel& angle) {
+    return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0));
+}
+
+void RotationVel::DoRotZ(const doubleVel& angle) {
+    w+=R*Vector(0,0,angle.grad);
+    R.DoRotZ(angle.t);
+}
+RotationVel RotationVel::RotZ(const doubleVel& angle) {
+    return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad));
+}
+
+
+RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle) 
+// rotvec has arbitrary norm
+// rotation around a constant vector !
+{
+    Vector v(rotvec);
+	v.Normalize();
+    return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad);
+}
+
+RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle) 
+    // rotvec is normalized.
+{
+    return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
+}
+
+
+VectorVel operator + (const VectorVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.p+r2.p,r1.v+r2.v);
+}
+
+VectorVel operator - (const VectorVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.p-r2.p,r1.v-r2.v);
+}
+
+VectorVel operator + (const VectorVel& r1,const Vector& r2) {
+    return VectorVel(r1.p+r2,r1.v);
+}
+
+VectorVel operator - (const VectorVel& r1,const Vector& r2) {
+    return VectorVel(r1.p-r2,r1.v);
+}
+
+VectorVel operator + (const Vector& r1,const VectorVel& r2) {
+    return VectorVel(r1+r2.p,r2.v);
+}
+
+VectorVel operator - (const Vector& r1,const VectorVel& r2) {
+    return VectorVel(r1-r2.p,-r2.v);
+}
+
+// unary -
+VectorVel operator - (const VectorVel& r) {
+    return VectorVel(-r.p,-r.v);
+}
+
+void SetToZero(VectorVel& v){
+    SetToZero(v.p);
+    SetToZero(v.v);
+}
+
+// cross prod.
+VectorVel operator * (const VectorVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p);
+}
+
+VectorVel operator * (const VectorVel& r1,const Vector& r2) {
+    return VectorVel(r1.p*r2, r1.v*r2);
+}
+
+VectorVel operator * (const Vector& r1,const VectorVel& r2) {
+    return VectorVel(r1*r2.p, r1*r2.v);
+}
+
+
+
+// scalar mult.
+VectorVel operator * (double r1,const VectorVel& r2) {
+    return VectorVel(r1*r2.p, r1*r2.v);
+}
+
+VectorVel operator * (const VectorVel& r1,double r2) {
+    return VectorVel(r1.p*r2, r1.v*r2);
+}
+
+
+
+VectorVel operator * (const doubleVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
+}
+
+VectorVel operator * (const VectorVel& r2,const doubleVel& r1) {
+    return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
+}
+
+VectorVel operator / (const VectorVel& r1,double r2) {
+    return VectorVel(r1.p/r2, r1.v/r2);
+}
+
+VectorVel operator / (const VectorVel& r2,const doubleVel& r1) {
+    return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t);
+}
+
+VectorVel operator*(const Rotation& R,const VectorVel& x) {
+    return VectorVel(R*x.p,R*x.v);
+}
+
+VectorVel& VectorVel::operator = (const VectorVel& arg) {
+    p=arg.p;
+    v=arg.v;
+    return *this;
+}
+VectorVel& VectorVel::operator = (const Vector& arg) {
+    p=arg;
+    v=Vector::Zero();
+    return *this;
+}
+VectorVel& VectorVel::operator += (const VectorVel& arg) {
+    p+=arg.p;
+    v+=arg.v;
+    return *this;
+}
+VectorVel& VectorVel::operator -= (const VectorVel& arg) {
+    p-=arg.p;
+    v-=arg.v;
+    return *this;
+}
+
+VectorVel VectorVel::Zero() {
+    return VectorVel(Vector::Zero(),Vector::Zero());
+}
+void VectorVel::ReverseSign() {
+    p.ReverseSign();
+    v.ReverseSign();
+}
+doubleVel VectorVel::Norm() const {
+    double n = p.Norm();
+    return doubleVel(n,dot(p,v)/n);
+}
+
+bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {
+    return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps));
+}
+bool Equal(const Vector& r1,const VectorVel& r2,double eps) {
+    return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps));
+}
+bool Equal(const VectorVel& r1,const Vector& r2,double eps) {
+    return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps));
+}
+
+bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) {
+    return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps));
+}
+bool Equal(const Rotation& r1,const RotationVel& r2,double eps) {
+    return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps));
+}
+bool Equal(const RotationVel& r1,const Rotation& r2,double eps) {
+    return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps));
+}
+bool Equal(const TwistVel& a,const TwistVel& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const Twist& a,const TwistVel& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const TwistVel& a,const Twist& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+
+
+
+IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) {
+    return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p));
+}
+IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) {
+    return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs));
+}
+IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) {
+    return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v));
+}
+
+
+
+
+
+
+
+
+
+
+
+
+TwistVel TwistVel::Zero()
+{
+    return TwistVel(VectorVel::Zero(),VectorVel::Zero());
+}
+
+
+void TwistVel::ReverseSign()
+{   
+    vel.ReverseSign();
+    rot.ReverseSign();
+}
+
+TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB)
+     // Changes the reference point of the TwistVel.
+     // The VectorVel v_base_AB is expressed in the same base as the TwistVel
+     // The VectorVel v_base_AB is a VectorVel from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+{
+    return TwistVel(this->vel+this->rot*v_base_AB,this->rot);
+}
+
+TwistVel& TwistVel::operator-=(const TwistVel& arg)
+{
+    vel-=arg.vel;
+    rot -=arg.rot;
+    return *this;
+}
+
+TwistVel& TwistVel::operator+=(const TwistVel& arg)
+{
+    vel+=arg.vel;
+    rot +=arg.rot;
+    return *this;
+}
+
+
+TwistVel operator*(const TwistVel& lhs,double rhs)
+{
+    return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistVel operator*(double lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistVel operator/(const TwistVel& lhs,double rhs)
+{
+    return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs)
+{
+    return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs)
+{
+    return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+
+// addition of TwistVel's
+TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
+}
+
+TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
+}
+
+// unary -
+TwistVel operator-(const TwistVel& arg) 
+{
+    return TwistVel(-arg.vel,-arg.rot);
+}
+
+void SetToZero(TwistVel& v)
+{
+   SetToZero(v.vel);
+   SetToZero(v.rot);
+}
+
+
+
+
+
+TwistVel RotationVel::Inverse(const TwistVel& arg) const
+{
+    return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistVel RotationVel::operator * (const TwistVel& arg) const
+{
+    return TwistVel((*this)*arg.vel,(*this)*arg.rot);
+}
+
+TwistVel RotationVel::Inverse(const Twist& arg) const
+{
+    return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistVel RotationVel::operator * (const Twist& arg) const
+{
+    return TwistVel((*this)*arg.vel,(*this)*arg.rot);
+}
+
+
+TwistVel FrameVel::operator * (const TwistVel& arg) const
+{
+    TwistVel tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistVel FrameVel::operator * (const Twist& arg) const
+{
+    TwistVel tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistVel FrameVel::Inverse(const TwistVel& arg) const
+{
+    TwistVel tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+TwistVel FrameVel::Inverse(const Twist& arg) const
+{
+    TwistVel tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+Twist TwistVel::GetTwist() const {
+    return Twist(vel.p,rot.p);
+}
+
+Twist TwistVel::GetTwistDot() const {
+    return Twist(vel.v,rot.v);
+}

blender-5.2.0/intern/itasc/kdl/inertia.cpp

@@ -0,0 +1,51 @@
+/** \file itasc/kdl/inertia.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "inertia.hpp"
+
+#include <Eigen/Core>
+
+namespace KDL {
+using namespace Eigen;
+
+Inertia::Inertia(double m,double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz):
+data(Matrix<double,6,6>::Zero())
+{
+    data(0,0)=Ixx;
+    data(1,1)=Iyy;
+    data(2,2)=Izz;
+    data(2,1)=data(1,2)=Ixy;
+    data(3,1)=data(1,3)=Ixz;
+    data(3,2)=data(2,3)=Iyz;
+
+	data.block(3,3,3,3)=m*Matrix<double,3,3>::Identity();
+}
+
+Inertia::~Inertia()
+{
+}
+
+
+
+}

blender-5.2.0/intern/itasc/kdl/inertia.hpp

@@ -0,0 +1,70 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDLINERTIA_HPP
+#define KDLINERTIA_HPP
+
+#include <Eigen/Core>
+#include "frames.hpp"
+
+namespace KDL {
+
+using namespace Eigen;
+
+/**
+ *	This class offers the inertia-structure of a body
+ *	An inertia is defined in a certain reference point and a certain reference base.
+ *	The reference point does not have to coincide with the origin of the reference frame.
+ */
+class Inertia{
+public:
+
+	/**
+	 * 	This constructor creates a cartesian space inertia matrix,
+	 * 	the arguments are the mass and the inertia moments in the cog.
+	 */
+	Inertia(double m=0,double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0);
+
+    static inline Inertia Zero(){
+        return Inertia(0,0,0,0,0,0,0);
+    };
+
+    friend class Rotation;
+    friend class Frame;
+
+    /**
+     * F = m*a
+     */
+   // Wrench operator* (const AccelerationTwist& acc);
+
+
+    ~Inertia();
+private:
+    Matrix<double,6,6,RowMajor> data;
+
+};
+
+
+
+
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/jacobian.cpp

@@ -0,0 +1,132 @@
+/** \file itasc/kdl/jacobian.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "jacobian.hpp"
+
+namespace KDL
+{
+    Jacobian::Jacobian(unsigned int _size,unsigned int _nr_blocks):
+        size(_size),nr_blocks(_nr_blocks)
+    {
+        twists = new Twist[size*nr_blocks];
+    }
+
+    Jacobian::Jacobian(const Jacobian& arg):
+                       size(arg.columns()),
+                       nr_blocks(arg.nr_blocks)
+    {
+        twists = new Twist[size*nr_blocks];
+        for(unsigned int i=0;i<size*nr_blocks;i++)
+            twists[i] = arg.twists[i];
+    }
+
+    Jacobian& Jacobian::operator = (const Jacobian& arg)
+    {
+        assert(size==arg.size);
+        assert(nr_blocks==arg.nr_blocks);
+        for(unsigned int i=0;i<size;i++)
+            twists[i]=arg.twists[i];
+        return *this;
+    }
+
+
+    Jacobian::~Jacobian()
+    {
+        delete [] twists;
+    }
+
+    double Jacobian::operator()(int i,int j)const
+    {
+        assert(i<6*(int)nr_blocks&&j<(int)size);
+        return twists[j+6*(int)(floor((double)i/6))](i%6);
+    }
+
+    double& Jacobian::operator()(int i,int j)
+    {
+        assert(i<6*(int)nr_blocks&&j<(int)size);
+        return twists[j+6*(int)(floor((double)i/6))](i%6);
+    }
+
+    unsigned int Jacobian::rows()const
+    {
+        return 6*nr_blocks;
+    }
+
+    unsigned int Jacobian::columns()const
+    {
+        return size;
+    }
+
+    void SetToZero(Jacobian& jac)
+    {
+        for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++)
+            SetToZero(jac.twists[i]);
+    }
+
+    void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
+    {
+        assert(src1.size==dest.size);
+        assert(src1.nr_blocks==dest.nr_blocks);
+        for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
+            dest.twists[i]=src1.twists[i].RefPoint(base_AB);
+    }
+
+    void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
+    {
+        assert(src1.size==dest.size);
+        assert(src1.nr_blocks==dest.nr_blocks);
+        for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
+            dest.twists[i]=rot*src1.twists[i];
+    }
+
+    void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
+    {
+        assert(src1.size==dest.size);
+        assert(src1.nr_blocks==dest.nr_blocks);
+        for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
+            dest.twists[i]=frame*src1.twists[i];
+    }
+
+    bool Jacobian::operator ==(const Jacobian& arg)
+    {
+        return Equal((*this),arg);
+    }
+    
+    bool Jacobian::operator!=(const Jacobian& arg)
+    {
+        return !Equal((*this),arg);
+    }
+    
+    bool Equal(const Jacobian& a,const Jacobian& b,double eps)
+    {
+        if(a.rows()==b.rows()&&a.columns()==b.columns()){
+            bool rc=true;
+            for(unsigned int i=0;i<a.columns();i++)
+                rc&=Equal(a.twists[i],b.twists[i],eps);
+            return rc;
+        }else
+            return false;
+    }
+    
+}

blender-5.2.0/intern/itasc/kdl/jacobian.hpp

@@ -0,0 +1,69 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_JACOBIAN_HPP
+#define KDL_JACOBIAN_HPP
+
+#include "frames.hpp"
+
+namespace KDL
+{
+    //Forward declaration
+    class ChainJntToJacSolver;
+
+    class Jacobian
+    {
+        friend class ChainJntToJacSolver;
+    private:
+        unsigned int size;
+        unsigned int nr_blocks;
+    public:
+        Twist* twists;
+        Jacobian(unsigned int size,unsigned int nr=1);
+        Jacobian(const Jacobian& arg);
+
+        Jacobian& operator=(const Jacobian& arg);
+
+        bool operator ==(const Jacobian& arg);
+        bool operator !=(const Jacobian& arg);
+        
+        friend bool Equal(const Jacobian& a,const Jacobian& b,double eps);
+        
+
+        ~Jacobian();
+
+        double operator()(int i,int j)const;
+        double& operator()(int i,int j);
+        unsigned int rows()const;
+        unsigned int columns()const;
+
+        friend void SetToZero(Jacobian& jac);
+
+        friend void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
+        friend void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
+        friend void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);
+
+
+    };
+	bool Equal(const Jacobian&, const Jacobian&, double = epsilon);
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/jntarray.cpp

@@ -0,0 +1,160 @@
+/** \file itasc/kdl/jntarray.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "jntarray.hpp"
+
+namespace KDL
+{
+    JntArray::JntArray():
+            size(0),
+            data(NULL)
+    {
+    }
+
+    JntArray::JntArray(unsigned int _size):
+        size(_size)
+    {
+        assert(0 < size);
+        data = new double[size];
+        SetToZero(*this);
+    }
+
+
+    JntArray::JntArray(const JntArray& arg):
+        size(arg.size)
+    {
+        data = ((0 < size) ? new double[size] : NULL);
+        for(unsigned int i=0;i<size;i++)
+            data[i]=arg.data[i];
+    }
+
+    JntArray& JntArray::operator = (const JntArray& arg)
+    {
+        assert(size==arg.size);
+        for(unsigned int i=0;i<size;i++)
+            data[i]=arg.data[i];
+        return *this;
+    }
+
+
+    JntArray::~JntArray()
+    {
+        delete [] data;
+    }
+
+    void JntArray::resize(unsigned int newSize)
+    {
+        delete [] data;
+        size = newSize;
+        data = new double[size];
+        SetToZero(*this);
+    }
+
+    double JntArray::operator[](unsigned int i)const
+    {
+        assert(i<size);
+        return data[i];
+    }
+
+    double& JntArray::operator[](unsigned int i)
+    {
+        assert(i<size);
+        return data[i];
+    }
+
+    double* JntArray::operator()(unsigned int i)
+    {
+        if (i>=size)
+			return NULL;
+        return &data[i];
+    }
+
+    unsigned int JntArray::rows()const
+    {
+        return size;
+    }
+
+    unsigned int JntArray::columns()const
+    {
+        return 0;
+    }
+
+    void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
+    {
+        assert(src1.size==src2.size&&src1.size==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=src1.data[i]+src2.data[i];
+    }
+
+    void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
+    {
+        assert(src1.size==src2.size&&src1.size==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=src1.data[i]-src2.data[i];
+    }
+
+    void Multiply(const JntArray& src,const double& factor,JntArray& dest)
+    {
+        assert(src.size==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=factor*src.data[i];
+    }
+
+    void Divide(const JntArray& src,const double& factor,JntArray& dest)
+    {
+        assert(src.rows()==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=src.data[i]/factor;
+    }
+
+    void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
+    {
+        assert(jac.columns()==src.size);
+        SetToZero(dest);
+        for(unsigned int i=0;i<6;i++)
+            for(unsigned int j=0;j<src.size;j++)
+                dest(i)+=jac(i,j)*src.data[j];
+    }
+
+    void SetToZero(JntArray& array)
+    {
+        for(unsigned int i=0;i<array.size;i++)
+            array.data[i]=0;
+    }
+
+    bool Equal(const JntArray& src1, const JntArray& src2,double eps)
+    {
+        assert(src1.size==src2.size);
+        bool ret = true;
+        for(unsigned int i=0;i<src1.size;i++)
+            ret = ret && Equal(src1.data[i],src2.data[i],eps);
+        return ret;
+    }
+
+    bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
+    //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
+
+}
+
+

blender-5.2.0/intern/itasc/kdl/jntarray.hpp

@@ -0,0 +1,223 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_JNTARRAY_HPP
+#define KDL_JNTARRAY_HPP
+
+#include "frames.hpp"
+#include "jacobian.hpp"
+
+namespace KDL
+{
+    /**
+     * @brief This class represents an fixed size array containing
+     * joint values of a KDL::Chain.
+     *
+     * \warning An object constructed with the default constructor provides
+     * a valid, but inert, object. Many of the member functions will do
+     * the correct thing and have no affect on this object, but some 
+     * member functions can _NOT_ deal with an inert/empty object. These 
+     * functions will assert() and exit the program instead. The intended use 
+     * case for the default constructor (in an RTT/OCL setting) is outlined in
+     * code below - the default constructor plus the resize() function allow
+     * use of JntArray objects whose size is set within a configureHook() call
+     * (typically based on a size determined from a property).
+	 
+\code
+class MyTask : public RTT::TaskContext
+{
+   JntArray		j;
+   MyTask() 
+   {}			// invokes j's default constructor
+
+   bool configureHook()
+   {
+       unsigned int size = some_property.rvalue();
+	   j.resize(size)
+	   ...
+   }
+
+   void updateHook()
+   {
+   ** use j here
+   }
+};
+\endcode
+
+     */	
+
+    class JntArray
+    {
+    private:
+        unsigned int size;
+        double* data;
+    public:
+        /** Construct with _no_ data array
+         * @post NULL == data
+         * @post 0 == rows()
+         * @warning use of an object constructed like this, without
+         * a resize() first, may result in program exit! See class
+         * documentation.
+         */
+        JntArray();
+        /**
+         * Constructor of the joint array
+         *
+         * @param size size of the array, this cannot be changed
+         * afterwards.
+         * @pre 0 < size
+         * @post NULL != data
+         * @post 0 < rows()
+         * @post all elements in data have 0 value
+         */
+        JntArray(unsigned int size);
+        /** Copy constructor 
+         * @note Will correctly copy an empty object
+         */
+        JntArray(const JntArray& arg);
+        ~JntArray();
+        /** Resize the array 
+         * @warning This causes a dynamic allocation (and potentially 	
+         * also a dynamic deallocation). This _will_ negatively affect
+         * real-time performance! 
+         *
+         * @post newSize == rows()
+         * @post NULL != data
+         * @post all elements in data have 0 value
+         */
+        void resize(unsigned int newSize);
+		
+        JntArray& operator = ( const JntArray& arg);
+        /**
+         * get_item operator for the joint array
+         *
+         *
+         * @return the joint value at position i, starting from 0
+         * @pre 0 != size (ie non-default constructor or resize() called)
+         */
+        double operator[](unsigned int i) const;
+        /**
+         * set_item operator
+         *
+         * @return reference to the joint value at position i,starting
+         *from zero.
+         * @pre 0 != size (ie non-default constructor or resize() called)
+         */
+        double& operator[](unsigned int i);
+        /**
+         * access operator for the joint array. Use pointer here to allow
+		 * access to sequential joint angles (required for ndof joints)
+         * 
+         *
+         * @return the joint value at position i, NULL if i is outside the valid range
+         */
+        double* operator()(unsigned int i);
+		/**
+         * Returns the number of rows (size) of the array
+         *
+         */
+        unsigned int rows()const;
+        /**
+         * Returns the number of columns of the array, always 1.
+         */
+        unsigned int columns()const;
+
+        /**
+         * Function to add two joint arrays, all the arguments must
+         * have the same size: A + B = C. This function is
+         * aliasing-safe, A or B can be the same array as C.
+         *
+         * @param src1 A
+         * @param src2 B
+         * @param dest C
+         */
+        friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest);
+        /**
+         * Function to subtract two joint arrays, all the arguments must
+         * have the same size: A - B = C. This function is
+         * aliasing-safe, A or B can be the same array as C.
+         *
+         * @param src1 A
+         * @param src2 B
+         * @param dest C
+         */
+        friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest);
+        /**
+         * Function to multiply all the array values with a scalar
+         * factor: A*b=C. This function is aliasing-safe, A can be the
+         * same array as C.
+         *
+         * @param src A
+         * @param factor b
+         * @param dest C
+         */
+        friend void Multiply(const JntArray& src,const double& factor,JntArray& dest);
+        /**
+         * Function to divide all the array values with a scalar
+         * factor: A/b=C. This function is aliasing-safe, A can be the
+         * same array as C.
+         *
+         * @param src A
+         * @param factor b
+         * @param dest C
+         */
+        friend void Divide(const JntArray& src,const double& factor,JntArray& dest);
+        /**
+         * Function to multiply a KDL::Jacobian with a KDL::JntArray
+         * to get a KDL::Twist, it should not be used to calculate the
+         * forward velocity kinematics, the solver classes are built
+         * for this purpose.
+         * J*q = t
+         *
+         * @param jac J
+         * @param src q
+         * @param dest t
+         * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty)
+         */
+        friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
+        /**
+         * Function to set all the values of the array to 0
+         *
+         * @param array
+         */
+        friend void SetToZero(JntArray& array);
+        /**
+         * Function to check if two arrays are the same with a
+         *precision of eps
+         *
+         * @param src1
+         * @param src2
+         * @param eps default: epsilon
+         * @return true if each element of src1 is within eps of the same
+		 * element in src2, or if both src1 and src2 have no data (ie 0==rows())
+         */
+        friend bool Equal(const JntArray& src1,const JntArray& src2,double eps);
+
+        friend bool operator==(const JntArray& src1,const JntArray& src2);
+        //friend bool operator!=(const JntArray& src1,const JntArray& src2);
+        };
+	bool Equal(const JntArray&,const JntArray&, double = epsilon);
+    bool operator==(const JntArray& src1,const JntArray& src2);
+    //bool operator!=(const JntArray& src1,const JntArray& src2);
+
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/jntarrayacc.cpp

@@ -0,0 +1,173 @@
+/** \file itasc/kdl/jntarrayacc.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "jntarrayacc.hpp"
+
+namespace KDL
+{
+    JntArrayAcc::JntArrayAcc(unsigned int size):
+        q(size),qdot(size),qdotdot(size)
+    {
+    }
+    JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin):
+        q(qin),qdot(qdotin),qdotdot(qdotdotin)
+    {
+        assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows());
+    }
+    JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin):
+        q(qin),qdot(qdotin),qdotdot(q.rows())
+    {
+        assert(q.rows()==qdot.rows());
+    }
+    JntArrayAcc::JntArrayAcc(const JntArray& qin):
+        q(qin),qdot(q.rows()),qdotdot(q.rows())
+    {
+    }
+
+    JntArray JntArrayAcc::value()const
+    {
+        return q;
+    }
+
+    JntArray JntArrayAcc::deriv()const
+    {
+        return qdot;
+    }
+    JntArray JntArrayAcc::dderiv()const
+    {
+        return qdotdot;
+    }
+
+    void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
+    {
+        Add(src1.q,src2.q,dest.q);
+        Add(src1.qdot,src2.qdot,dest.qdot);
+        Add(src1.qdotdot,src2.qdotdot,dest.qdotdot);
+    }
+    void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
+    {
+        Add(src1.q,src2.q,dest.q);
+        Add(src1.qdot,src2.qdot,dest.qdot);
+        dest.qdotdot=src1.qdotdot;
+    }
+    void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
+    {
+        Add(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+        dest.qdotdot=src1.qdotdot;
+    }
+
+    void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
+    {
+        Subtract(src1.q,src2.q,dest.q);
+        Subtract(src1.qdot,src2.qdot,dest.qdot);
+        Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot);
+    }
+    void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
+    {
+        Subtract(src1.q,src2.q,dest.q);
+        Subtract(src1.qdot,src2.qdot,dest.qdot);
+        dest.qdotdot=src1.qdotdot;
+    }
+    void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
+    {
+        Subtract(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+        dest.qdotdot=src1.qdotdot;
+    }
+
+    void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,factor,dest.q);
+        Multiply(src.qdot,factor,dest.qdot);
+        Multiply(src.qdotdot,factor,dest.qdotdot);
+    }
+    void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.qdot,factor.grad*2,dest.qdot);
+        Multiply(src.qdotdot,factor.t,dest.qdotdot);
+        Add(dest.qdot,dest.qdotdot,dest.qdotdot);
+        Multiply(src.q,factor.grad,dest.q);
+        Multiply(src.qdot,factor.t,dest.qdot);
+        Add(dest.qdot,dest.q,dest.qdot);
+        Multiply(src.q,factor.t,dest.q);
+    }
+    void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,factor.dd,dest.q);
+        Multiply(src.qdot,factor.d*2,dest.qdot);
+        Multiply(src.qdotdot,factor.t,dest.qdotdot);
+        Add(dest.qdotdot,dest.qdot,dest.qdotdot);
+        Add(dest.qdotdot,dest.q,dest.qdotdot);
+        Multiply(src.q,factor.d,dest.q);
+        Multiply(src.qdot,factor.t,dest.qdot);
+        Add(dest.qdot,dest.q,dest.qdot);
+        Multiply(src.q,factor.t,dest.q);
+    }
+
+    void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
+    {
+        Divide(src.q,factor,dest.q);
+        Divide(src.qdot,factor,dest.qdot);
+        Divide(src.qdotdot,factor,dest.qdotdot);
+    }
+    void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q);
+        Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot);
+        Divide(src.qdotdot,factor.t,dest.qdotdot);
+        Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
+        Add(dest.qdotdot,dest.q,dest.qdotdot);
+        Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q);
+        Divide(src.qdot,factor.t,dest.qdot);
+        Subtract(dest.qdot,dest.q,dest.qdot);
+        Divide(src.q,factor.t,dest.q);
+    }
+    void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q);
+        Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot);
+        Divide(src.qdotdot,factor.t,dest.qdotdot);
+        Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
+        Add(dest.qdotdot,dest.q,dest.qdotdot);
+        Multiply(src.q,factor.d/(factor.t*factor.t),dest.q);
+        Divide(src.qdot,factor.t,dest.qdot);
+        Subtract(dest.qdot,dest.q,dest.qdot);
+        Divide(src.q,factor.t,dest.q);
+    }
+
+    void SetToZero(JntArrayAcc& array)
+    {
+        SetToZero(array.q);
+        SetToZero(array.qdot);
+        SetToZero(array.qdotdot);
+    }
+
+    bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps)
+    {
+        return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps));
+    }
+}
+
+

blender-5.2.0/intern/itasc/kdl/jntarrayacc.hpp

@@ -0,0 +1,67 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_JNTARRAYACC_HPP
+#define KDL_JNTARRAYACC_HPP
+
+#include "utilities/utility.h"
+#include "jntarray.hpp"
+#include "jntarrayvel.hpp"
+#include "frameacc.hpp"
+
+namespace KDL
+{
+    class JntArrayAcc
+    {
+    public:
+        JntArray q;
+        JntArray qdot;
+        JntArray qdotdot;
+    public:
+        JntArrayAcc(unsigned int size);
+        JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot);
+        JntArrayAcc(const JntArray& q,const JntArray& qdot);
+        JntArrayAcc(const JntArray& q);
+
+        JntArray value()const;
+        JntArray deriv()const;
+        JntArray dderiv()const;
+
+        friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
+        friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
+        friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
+        friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
+        friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
+        friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
+        friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
+        friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
+        friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
+        friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
+        friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
+        friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
+        friend void SetToZero(JntArrayAcc& array);
+        friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps);
+    };
+
+    bool Equal(const JntArrayAcc&, const JntArrayAcc&, double = epsilon);
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/jntarrayvel.cpp

@@ -0,0 +1,114 @@
+/** \file itasc/kdl/jntarrayvel.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#include "jntarrayacc.hpp"
+
+namespace KDL
+{
+    JntArrayVel::JntArrayVel(unsigned int size):
+        q(size),qdot(size)
+    {
+    }
+    JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin):
+        q(qin),qdot(qdotin)
+    {
+        assert(q.rows()==qdot.rows());
+    }
+    JntArrayVel::JntArrayVel(const JntArray& qin):
+        q(qin),qdot(q.rows())
+    {
+    }
+
+    JntArray JntArrayVel::value()const
+    {
+        return q;
+    }
+
+    JntArray JntArrayVel::deriv()const
+    {
+        return qdot;
+    }
+
+    void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
+    {
+        Add(src1.q,src2.q,dest.q);
+        Add(src1.qdot,src2.qdot,dest.qdot);
+    }
+    void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
+    {
+        Add(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+    }
+
+    void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
+    {
+        Subtract(src1.q,src2.q,dest.q);
+        Subtract(src1.qdot,src2.qdot,dest.qdot);
+    }
+    void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
+    {
+        Subtract(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+    }
+
+    void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
+    {
+        Multiply(src.q,factor,dest.q);
+        Multiply(src.qdot,factor,dest.qdot);
+    }
+    void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
+    {
+        Multiply(src.q,factor.grad,dest.q);
+        Multiply(src.qdot,factor.t,dest.qdot);
+        Add(dest.qdot,dest.q,dest.qdot);
+        Multiply(src.q,factor.t,dest.q);
+    }
+
+    void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
+    {
+        Divide(src.q,factor,dest.q);
+        Divide(src.qdot,factor,dest.qdot);
+    }
+    void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
+    {
+        Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q);
+        Divide(src.qdot,factor.t,dest.qdot);
+        Subtract(dest.qdot,dest.q,dest.qdot);
+        Divide(src.q,factor.t,dest.q);
+    }
+
+    void SetToZero(JntArrayVel& array)
+    {
+        SetToZero(array.q);
+        SetToZero(array.qdot);
+    }
+
+    bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps)
+    {
+        return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps);
+    }
+}
+
+

blender-5.2.0/intern/itasc/kdl/jntarrayvel.hpp

@@ -0,0 +1,60 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_JNTARRAYVEL_HPP
+#define KDL_JNTARRAYVEL_HPP
+
+#include "utilities/utility.h"
+#include "jntarray.hpp"
+#include "framevel.hpp"
+
+namespace KDL
+{
+
+    class JntArrayVel
+    {
+    public:
+        JntArray q;
+        JntArray qdot;
+    public:
+        JntArrayVel(unsigned int size);
+        JntArrayVel(const JntArray& q,const JntArray& qdot);
+        JntArrayVel(const JntArray& q);
+
+        JntArray value()const;
+        JntArray deriv()const;
+
+        friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
+        friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
+        friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
+        friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
+        friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
+        friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
+        friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
+        friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
+        friend void SetToZero(JntArrayVel& array);
+        friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps);
+    };
+
+    bool Equal(const JntArrayVel&, const JntArrayVel&, double = epsilon);
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/joint.cpp

@@ -0,0 +1,164 @@
+/** \file itasc/kdl/joint.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "joint.hpp"
+
+namespace KDL {
+
+    Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
+                 const double& _inertia, const double& _damping, const double& _stiffness):
+        type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
+    {
+		// for sphere and swing, offset is not used, assume no offset
+    }
+
+    Joint::Joint(const Joint& in):
+	type(in.type),scale(in.scale),offset(in.offset),
+        inertia(in.inertia),damping(in.damping),stiffness(in.stiffness)
+    {
+    }
+
+    Joint& Joint::operator=(const Joint& in)
+    {
+        type=in.type;
+        scale=in.scale;
+        offset=in.offset;
+        inertia=in.inertia;
+        damping=in.damping;
+        stiffness=in.stiffness;
+		return *this;
+    }
+
+
+    Joint::~Joint()
+    {
+    }
+
+    Frame Joint::pose(const double* q)const
+    {
+
+        switch(type){
+        case RotX:
+			assert(q);
+            return Frame(Rotation::RotX(scale*q[0]+offset));
+            break;
+        case RotY:
+			assert(q);
+            return  Frame(Rotation::RotY(scale*q[0]+offset));
+            break;
+        case RotZ:
+			assert(q);
+            return  Frame(Rotation::RotZ(scale*q[0]+offset));
+            break;
+        case TransX:
+			assert(q);
+            return  Frame(Vector(scale*q[0]+offset,0.0,0.0));
+            break;
+        case TransY:
+			assert(q);
+            return Frame(Vector(0.0,scale*q[0]+offset,0.0));
+            break;
+        case TransZ:
+			assert(q);
+            return Frame(Vector(0.0,0.0,scale*q[0]+offset));
+            break;
+		case Sphere:
+			// the joint angles represent a rotation vector expressed in the base frame of the joint
+			// (= the frame you get when there is no offset nor rotation)
+			assert(q);
+			return Frame(Rot(Vector(q[0], q[1], q[2])));
+			break;
+		case Swing:
+			// the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint
+			// (= the frame you get when there is no offset nor rotation)
+			assert(q);
+			return Frame(Rot(Vector(q[0], 0.0, q[1])));
+			break;
+        default:
+            return Frame::Identity();
+            break;
+        }
+    }
+
+    Twist Joint::twist(const double& qdot, int dof)const
+    {
+        switch(type){
+        case RotX:
+            return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+            break;
+        case RotY:
+            return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
+            break;
+        case RotZ:
+            return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+            break;
+        case TransX:
+            return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
+            break;
+        case TransY:
+            return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
+            break;
+        case TransZ:
+            return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
+            break;
+		case Swing:
+			switch (dof) {
+			case 0:
+				return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+			case 1:
+				return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+			}
+            return Twist::Zero();
+		case Sphere:
+			switch (dof) {
+			case 0:
+				return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+			case 1:
+				return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
+			case 2:
+				return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+			}
+            return Twist::Zero();
+        default:
+            return Twist::Zero();
+            break;
+        }
+	}
+
+	unsigned int Joint::getNDof() const
+	{
+		switch (type) {
+		case Sphere:
+			return 3;
+		case Swing:
+			return 2;
+		case None:
+			return 0;
+		default:
+			return 1;
+		}
+	}
+
+} // end of namespace KDL
+

blender-5.2.0/intern/itasc/kdl/joint.hpp

@@ -0,0 +1,138 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_JOINT_HPP
+#define KDL_JOINT_HPP
+
+#include "frames.hpp"
+#include <string>
+
+namespace KDL {
+
+    /**
+	  * \brief This class encapsulates a simple joint, that is with one
+	  * parameterized degree of freedom and with scalar dynamic properties.
+     *
+     * A simple joint is described by the following properties :
+     *      - scale: ratio between motion input and motion output
+     *      - offset: between the "physical" and the "logical" zero position.
+     *      - type: revolute or translational, along one of the basic frame axes
+	  *      - inertia, stiffness and damping: scalars representing the physical
+	  *      effects along/about the joint axis only.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Joint {
+    public:
+        typedef enum { RotX,RotY,RotZ,TransX,TransY,TransZ,Sphere,Swing,None} JointType;
+        /**
+         * Constructor of a joint.
+         *
+         * @param type type of the joint, default: Joint::None
+         * @param scale scale between joint input and actual geometric
+         * movement, default: 1
+         * @param offset offset between joint input and actual
+         * geometric input, default: 0
+         * @param inertia 1D inertia along the joint axis, default: 0
+         * @param damping 1D damping along the joint axis, default: 0
+         * @param stiffness 1D stiffness along the joint axis,
+         * default: 0
+         */
+        Joint(const JointType& type=None,const double& scale=1,const double& offset=0,
+              const double& inertia=0,const double& damping=0,const double& stiffness=0);
+        Joint(const Joint& in);
+
+        Joint& operator=(const Joint& arg);
+
+        /**
+         * Request the 6D-pose between the beginning and the end of
+         * the joint at joint position q
+         *
+         * @param q the 1D joint position
+         *
+         * @return the resulting 6D-pose
+         */
+        Frame pose(const double* q)const;
+        /**
+         * Request the resulting 6D-velocity with a joint velocity qdot
+         *
+         * @param qdot the 1D joint velocity
+         *
+         * @return the resulting 6D-velocity
+         */
+        Twist twist(const double& qdot, int dof=0)const;
+
+        /**
+         * Request the type of the joint.
+         *
+         * @return const reference to the type
+         */
+        const JointType& getType() const
+        {
+            return type;
+        };
+
+        /**
+         * Request the stringified type of the joint.
+         *
+         * @return const string
+         */
+        const std::string getTypeName() const
+        {
+            switch (type) {
+            case RotX:
+                return "RotX";
+            case RotY:
+                return "RotY";
+            case RotZ:
+                return "RotZ";
+            case TransX:
+                return "TransX";
+            case TransY:
+                return "TransY";
+            case TransZ:
+                return "TransZ";
+			case Sphere:
+				return "Sphere";
+			case Swing:
+				return "Swing";
+            case None:
+                return "None";
+            default:
+                return "None";
+            }
+        };
+		unsigned int getNDof() const;
+
+        virtual ~Joint();
+
+    private:
+        Joint::JointType type;
+        double scale;
+        double offset;
+        double inertia;
+        double damping;
+        double stiffness;
+    };
+
+} // end of namespace KDL
+
+#endif

blender-5.2.0/intern/itasc/kdl/kinfam_io.cpp

@@ -0,0 +1,104 @@
+/** \file itasc/kdl/kinfam_io.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "kinfam_io.hpp"
+#include "frames_io.hpp"
+
+namespace KDL {
+std::ostream& operator <<(std::ostream& os, const Joint& joint) {
+	return os << joint.getTypeName();
+}
+
+std::istream& operator >>(std::istream& is, Joint& joint) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Segment& segment) {
+	os << "[" << segment.getJoint() << ",\n" << segment.getFrameToTip() << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, Segment& segment) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Chain& chain) {
+	os << "[";
+	for (unsigned int i = 0; i < chain.getNrOfSegments(); i++)
+		os << chain.getSegment(i) << "\n";
+	os << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, Chain& chain) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Tree& tree) {
+	SegmentMap::const_iterator root = tree.getSegment("root");
+	return os << root;
+}
+
+std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) {
+	//os<<root->first<<": "<<root->second.segment<<"\n";
+	os << root->first<<"(q_nr: "<<root->second.q_nr<<")"<<"\n \t";
+	for (unsigned int i = 0; i < root->second.children.size(); i++) {
+		os <<(root->second.children[i])<<"\t";
+	}
+	return os << "\n";
+}
+
+std::istream& operator >>(std::istream& is, Tree& tree) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const JntArray& array) {
+	os << "[";
+	for (unsigned int i = 0; i < array.rows(); i++)
+		os << std::setw(KDL_FRAME_WIDTH) << array[i];
+	os << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, JntArray& array) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Jacobian& jac) {
+	os << "[";
+	for (unsigned int i = 0; i < jac.rows(); i++) {
+		for (unsigned int j = 0; j < jac.columns(); j++)
+			os << std::setw(KDL_FRAME_WIDTH) << jac(i, j);
+		os << std::endl;
+	}
+	os << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, Jacobian& jac) {
+	return is;
+}
+
+}
+

blender-5.2.0/intern/itasc/kdl/kinfam_io.hpp

@@ -0,0 +1,70 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_KINFAM_IO_HPP
+#define KDL_KINFAM_IO_HPP
+
+#include <iostream>
+#include <fstream>
+
+#include "joint.hpp"
+#include "segment.hpp"
+#include "chain.hpp"
+#include "jntarray.hpp"
+#include "jacobian.hpp"
+#include "tree.hpp"
+
+namespace KDL {
+std::ostream& operator <<(std::ostream& os, const Joint& joint);
+std::istream& operator >>(std::istream& is, Joint& joint);
+std::ostream& operator <<(std::ostream& os, const Segment& segment);
+std::istream& operator >>(std::istream& is, Segment& segment);
+std::ostream& operator <<(std::ostream& os, const Chain& chain);
+std::istream& operator >>(std::istream& is, Chain& chain);
+
+std::ostream& operator <<(std::ostream& os, const Tree& tree);
+std::istream& operator >>(std::istream& is, Tree& tree);
+
+std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator it);
+
+std::ostream& operator <<(std::ostream& os, const JntArray& array);
+std::istream& operator >>(std::istream& is, JntArray& array);
+std::ostream& operator <<(std::ostream& os, const Jacobian& jac);
+std::istream& operator >>(std::istream& is, Jacobian& jac);
+
+template<typename T>
+std::ostream& operator<<(std::ostream& os, const std::vector<T>& vec) {
+	os << "[";
+	for (unsigned int i = 0; i < vec.size(); i++)
+		os << vec[i] << " ";
+	os << "]";
+	return os;
+}
+;
+
+template<typename T>
+std::istream& operator >>(std::istream& is, std::vector<T>& vec) {
+	return is;
+}
+;
+}
+#endif
+

blender-5.2.0/intern/itasc/kdl/segment.cpp

@@ -0,0 +1,71 @@
+/** \file itasc/kdl/segment.cpp
+ * \ingroup intern_itasc
+ */
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
+
+#include "segment.hpp"
+
+namespace KDL {
+
+    Segment::Segment(const Joint& _joint, const Frame& _f_tip, const Inertia& _M):
+        M(_M),joint(_joint),
+        f_tip(_f_tip)
+    {
+    }
+
+    Segment::Segment(const Segment& in):
+        M(in.M),joint(in.joint),
+        f_tip(in.f_tip)
+    {
+    }
+
+    Segment& Segment::operator=(const Segment& arg)
+    {
+        joint=arg.joint;
+        M=arg.M;
+        f_tip=arg.f_tip;
+        return *this;
+    }
+
+    Segment::~Segment()
+    {
+    }
+
+    Frame Segment::pose(const double* q)const
+    {
+        return joint.pose(q)*f_tip;
+    }
+
+    Twist Segment::twist(const double* q, const double& qdot, int dof)const
+    {
+        return joint.twist(qdot, dof).RefPoint(pose(q).p);
+    }
+
+    Twist Segment::twist(const Vector& p, const double& qdot, int dof)const
+    {
+        return joint.twist(qdot, dof).RefPoint(p);
+    }
+
+	Twist Segment::twist(const Frame& f, const double& qdot, int dof)const
+    {
+        return (f.M*joint.twist(qdot, dof)).RefPoint(f.p);
+    }
+}//end of namespace KDL
+

blender-5.2.0/intern/itasc/kdl/segment.hpp

@@ -0,0 +1,149 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#ifndef KDL_SEGMENT_HPP
+#define KDL_SEGMENT_HPP
+
+#include "frames.hpp"
+#include "inertia.hpp"
+#include "joint.hpp"
+#include <vector>
+
+namespace KDL {
+
+    /**
+	  * \brief This class encapsulates a simple segment, that is a "rigid
+	  * body" (i.e., a frame and an inertia) with a joint and with
+	  * "handles", root and tip to connect to other segments.
+     *
+     * A simple segment is described by the following properties :
+     *      - Joint
+     *      - inertia: of the rigid body part of the Segment
+     *      - Offset from the end of the joint to the tip of the segment:
+     *        the joint is located at the root of the segment.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Segment {
+        friend class Chain;
+    private:
+        Inertia M;
+        Joint joint;
+        Frame f_tip;
+
+    public:
+        /**
+         * Constructor of the segment
+         *
+         * @param joint joint of the segment, default:
+         * Joint(Joint::None)
+         * @param f_tip frame from the end of the joint to the tip of
+         * the segment, default: Frame::Identity()
+         * @param M rigid body inertia of the segment, default: Inertia::Zero()
+         */
+        Segment(const Joint& joint=Joint(), const Frame& f_tip=Frame::Identity(),const Inertia& M = Inertia::Zero());
+        Segment(const Segment& in);
+        Segment& operator=(const Segment& arg);
+
+        virtual ~Segment();
+
+        /**
+         * Request the pose of the segment, given the joint position q.
+         *
+         * @param q 1D position of the joint
+         *
+         * @return pose from the root to the tip of the segment
+         */
+        Frame pose(const double* q)const;
+        /**
+         * Request the 6D-velocity of the tip of the segment, given
+         * the joint position q and the joint velocity qdot.
+         *
+         * @param q ND position of the joint
+         * @param qdot ND velocity of the joint
+         *
+         * @return 6D-velocity of the tip of the segment, expressed
+         *in the base-frame of the segment(root) and with the tip of
+         *the segment as reference point.
+         */
+        Twist twist(const double* q,const double& qdot, int dof=0)const;
+
+        /**
+         * Request the 6D-velocity at a given point p, relative to base frame of the segment
+         * givven the joint velocity qdot.
+         *
+         * @param p reference point
+         * @param qdot ND velocity of the joint
+         *
+         * @return 6D-velocity at a given point p, expressed
+         * in the base-frame of the segment(root) 
+         */
+	    Twist twist(const Vector& p, const double& qdot, int dof=0)const;
+
+        /**
+         * Request the 6D-velocity at a given frame origin, relative to base frame of the segment
+         * assuming the frame rotation is the rotation of the joint.
+         *
+         * @param f joint pose frame + reference point
+         * @param qdot ND velocity of the joint
+         *
+         * @return 6D-velocity at frame reference point, expressed
+         * in the base-frame of the segment(root) 
+         */
+		Twist twist(const Frame& f, const double& qdot, int dof)const;
+
+        /**
+         * Request the joint of the segment
+         *
+         *
+         * @return const reference to the joint of the segment
+         */
+        const Joint& getJoint()const
+        {
+            return joint;
+        }
+        /**
+         * Request the inertia of the segment
+         *
+         *
+         * @return const reference to the inertia of the segment
+         */
+        const Inertia& getInertia()const
+        {
+            return M;
+        }
+
+        /**
+         * Request the pose from the joint end to the tip of the
+         *segment.
+         *
+         * @return const reference to the joint end - segment tip pose.
+         */
+        const Frame& getFrameToTip()const
+        {
+            return f_tip;
+        }
+
+    };
+}//end of namespace KDL
+
+#endif

blender-5.2.0/intern/itasc/kdl/tree.cpp

@@ -0,0 +1,126 @@
+/** \file itasc/kdl/tree.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "tree.hpp"
+#include <sstream>
+namespace KDL {
+using namespace std;
+
+Tree::Tree() :
+    nrOfJoints(0), nrOfSegments(0) {
+	TreeElement root;
+	std::pair<std::string, TreeElement> val("root", root);
+    segments.insert(val);
+}
+
+Tree::Tree(const Tree& in) {
+    segments.clear();
+    nrOfSegments = 0;
+    nrOfJoints = 0;
+	TreeElement root;
+	std::pair<std::string, TreeElement> val("root", root);
+    segments.insert(val);
+    this->addTree(in, "", "root");
+
+}
+
+Tree& Tree::operator=(const Tree& in) {
+    segments.clear();
+    nrOfSegments = 0;
+    nrOfJoints = 0;
+	TreeElement root;
+	std::pair<std::string, TreeElement> val("root", root);
+    segments.insert(val);
+    this->addTree(in, "", "root");
+    return *this;
+}
+
+bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
+        const std::string& hook_name) {
+    SegmentMap::iterator parent = segments.find(hook_name);
+    //check if parent exists
+    if (parent == segments.end())
+        return false;
+    pair<SegmentMap::iterator, bool> retval;
+    //insert new element
+	TreeElement elem(segment, *parent, nrOfJoints);
+	std::pair<std::string, TreeElement> val(segment_name, elem);
+
+    retval = segments.insert(val);
+    //check if insertion succeeded
+    if (!retval.second)
+        return false;
+    //add iterator to new element in parents children list
+    parent->second.children.push_back(retval.first);
+    //increase number of segments
+    nrOfSegments++;
+    //increase number of joints
+	nrOfJoints += segment.getJoint().getNDof();
+    return true;
+}
+
+bool Tree::addChain(const Chain& chain, const std::string& chain_name,
+        const std::string& hook_name) {
+    string parent_name = hook_name;
+    for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
+        ostringstream segment_name;
+        segment_name << chain_name << "Segment" << i;
+        if (this->addSegment(chain.getSegment(i), segment_name.str(),
+                parent_name))
+            parent_name = segment_name.str();
+        else
+            return false;
+    }
+    return true;
+}
+
+bool Tree::addTree(const Tree& tree, const std::string& tree_name,
+        const std::string& hook_name) {
+    return this->addTreeRecursive(tree.getSegment("root"), tree_name, hook_name);
+}
+
+bool Tree::addTreeRecursive(SegmentMap::const_iterator root,
+        const std::string& tree_name, const std::string& hook_name) {
+    //get iterator for root-segment
+    SegmentMap::const_iterator child;
+    //try to add all of root's children
+    for (unsigned int i = 0; i < root->second.children.size(); i++) {
+        child = root->second.children[i];
+        //Try to add the child
+        if (this->addSegment(child->second.segment, tree_name + child->first,
+                hook_name)) {
+            //if child is added, add all the child's children
+            if (!(this->addTreeRecursive(child, tree_name, tree_name
+                    + child->first)))
+                //if it didn't work, return false
+                return false;
+        } else
+            //If the child could not be added, return false
+            return false;
+    }
+    return true;
+}
+
+}
+

blender-5.2.0/intern/itasc/kdl/tree.hpp

@@ -0,0 +1,177 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_TREE_HPP
+#define KDL_TREE_HPP
+
+#include "segment.hpp"
+#include "chain.hpp"
+
+#include <string>
+#include <map>
+#include <Eigen/Core>
+
+namespace KDL
+{
+    //Forward declaration
+    class TreeElement;
+    // Eigen allocator is needed for alignment of Eigen data types
+    typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, TreeElement> > > SegmentMap;
+
+    class TreeElement
+    {
+    public:
+        TreeElement():q_nr(0),parent(0)
+        {};
+    public:
+        Segment segment;
+        unsigned int q_nr;
+        SegmentMap::value_type const *parent;
+        std::vector<SegmentMap::const_iterator > children;
+        TreeElement(const Segment& segment_in,const SegmentMap::value_type& parent_in,unsigned int q_nr_in)
+        {
+			q_nr=q_nr_in;
+            segment=segment_in;
+            parent=&parent_in;
+        };
+        static TreeElement Root()
+        {
+            return TreeElement();
+        };
+    };
+
+    /**
+     * \brief  This class encapsulates a <strong>tree</strong>
+     * kinematic interconnection structure. It is build out of segments.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Tree
+    {
+    private:
+        SegmentMap segments;
+        unsigned int nrOfJoints;
+        unsigned int nrOfSegments;
+
+        bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name);
+
+    public:
+        /**
+         * The constructor of a tree, a new tree is always empty
+         */
+        Tree();
+        Tree(const Tree& in);
+        Tree& operator= (const Tree& arg);
+
+        /**
+         * Adds a new segment to the end of the segment with
+         * hook_name as segment_name
+         *
+         * @param segment new segment to add
+         * @param segment_name name of the new segment
+         * @param hook_name name of the segment to connect this
+         * segment with.
+         *
+         * @return false if hook_name could not be found.
+         */
+        bool addSegment(const Segment& segment, const std::string& segment_name, const std::string& hook_name);
+
+        /**
+         * Adds a complete chain to the end of the segment with
+         * hook_name as segment_name. Segment i of
+         * the chain will get chain_name+".Segment"+i as segment_name.
+         *
+         * @param chain Chain to add
+         * @param chain_name name of the chain
+         * @param hook_name name of the segment to connect the chain with.
+         *
+         * @return false if hook_name could not be found.
+         */
+        bool addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name);
+
+        /**
+         * Adds a complete tree to the end of the segment with
+         * hookname as segment_name. The segments of the tree will get
+         * tree_name+segment_name as segment_name.
+         *
+         * @param tree Tree to add
+         * @param tree_name name of the tree
+         * @param hook_name name of the segment to connect the tree with
+         *
+         * @return false if hook_name could not be found
+         */
+        bool addTree(const Tree& tree, const std::string& tree_name,const std::string& hook_name);
+
+        /**
+         * Request the total number of joints in the tree.\n
+         * <strong> Important:</strong> It is not the same as the
+         * total number of segments since a segment does not need to have
+         * a joint.
+         *
+         * @return total nr of joints
+         */
+        unsigned int getNrOfJoints()const
+        {
+            return nrOfJoints;
+        };
+
+        /**
+         * Request the total number of segments in the tree.
+         * @return total number of segments
+         */
+        unsigned int getNrOfSegments()const {return nrOfSegments;};
+
+        /**
+         * Request the segment of the tree with name segment_name.
+         *
+         * @param segment_name the name of the requested segment
+         *
+         * @return constant iterator pointing to the requested segment
+         */
+        SegmentMap::const_iterator getSegment(const std::string& segment_name)const
+        {
+            return segments.find(segment_name);
+        };
+
+        SegmentMap::value_type const* getSegmentPtr(const std::string& segment_name)const
+        {
+            SegmentMap::const_iterator it = segments.find(segment_name);
+            
+            if (it == segments.end())
+                return 0;
+
+            return &*it;
+        };
+
+        const SegmentMap& getSegments()const
+        {
+            return segments;
+        }
+
+        virtual ~Tree(){};
+    };
+}
+#endif
+
+
+
+
+

blender-5.2.0/intern/itasc/kdl/treefksolver.hpp

@@ -0,0 +1,110 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2008  Julia Jesse
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_TREE_FKSOLVER_HPP
+#define KDL_TREE_FKSOLVER_HPP
+
+#include <string>
+
+#include "tree.hpp"
+//#include "framevel.hpp"
+//#include "frameacc.hpp"
+#include "jntarray.hpp"
+//#include "jntarrayvel.hpp"
+//#include "jntarrayacc.hpp"
+
+namespace KDL {
+
+    /**
+	  * \brief This <strong>abstract</strong> class encapsulates a
+	  * solver for the forward position kinematics for a KDL::Tree.
+     *
+     * @ingroup KinematicFamily
+     */
+
+    //Forward definition
+    class TreeFkSolverPos {
+    public:
+        /**
+         * Calculate forward position kinematics for a KDL::Tree,
+         * from joint coordinates to cartesian pose.
+         *
+         * @param q_in input joint coordinates
+         * @param p_out reference to output cartesian pose
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)=0;
+        virtual ~TreeFkSolverPos(){};
+    };
+
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward velocity kinematics for a KDL::Tree.
+     *
+     * @ingroup KinematicFamily
+     */
+//    class TreeFkSolverVel {
+//    public:
+        /**
+         * Calculate forward position and velocity kinematics, from
+         * joint coordinates to cartesian coordinates.
+         *
+         * @param q_in input joint coordinates (position and velocity)
+         * @param out output cartesian coordinates (position and velocity)
+         *
+         * @return if < 0 something went wrong
+         */
+//        virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
+
+//        virtual ~TreeFkSolverVel(){};
+//    };
+    
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward acceleration kinematics for a KDL::Tree.
+     *
+     * @ingroup KinematicFamily
+     */
+
+//    class TreeFkSolverAcc {
+//   public:
+        /**
+         * Calculate forward position, velocity and accelaration
+         * kinematics, from joint coordinates to cartesian coordinates
+         *
+         * @param q_in input joint coordinates (position, velocity and
+         * acceleration
+         @param out output cartesian coordinates (position, velocity
+         * and acceleration
+         *
+         * @return if < 0 something went wrong
+         */
+//    virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
+
+//        virtual ~TreeFkSolverAcc()=0;
+//    };
+
+
+}//end of namespace KDL
+
+#endif

blender-5.2.0/intern/itasc/kdl/treefksolverpos_recursive.cpp

@@ -0,0 +1,72 @@
+/** \file itasc/kdl/treefksolverpos_recursive.cpp
+ * \ingroup intern_itasc
+ */
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2008 Julia Jesse
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "treefksolverpos_recursive.hpp"
+#include <iostream>
+
+namespace KDL {
+
+    TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree& _tree):
+        tree(_tree)
+    {
+    }
+
+    int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
+    {      
+		SegmentMap::value_type const* it = tree.getSegmentPtr(segmentName); 
+		SegmentMap::value_type const* baseit = tree.getSegmentPtr(baseName); 
+        
+        if(q_in.rows() != tree.getNrOfJoints())
+    	    	return -1;
+        else if(!it) //if the segment name is not found
+         	return -2;
+        else if(!baseit) //if the base segment name is not found
+         	return -3;
+        else{
+			p_out = recursiveFk(q_in, it, baseit);
+        	return 0;
+        }
+    }
+
+	Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit)
+	{
+		//gets the frame for the current element (segment)
+		const TreeElement& currentElement = it->second;
+		
+		if(it == baseit){
+			return KDL::Frame::Identity();
+		}
+		else{
+			Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
+			return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame;
+		}
+	}
+
+    TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive()
+    {
+    }
+
+
+}

blender-5.2.0/intern/itasc/kdl/treefksolverpos_recursive.hpp

@@ -0,0 +1,53 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2008 Julia Jesse
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDLTREEFKSOLVERPOS_RECURSIVE_HPP
+#define KDLTREEFKSOLVERPOS_RECURSIVE_HPP
+
+#include "treefksolver.hpp"
+
+namespace KDL {
+
+    /**
+     * Implementation of a recursive forward position kinematics
+     * algorithm to calculate the position transformation from joint
+     * space to Cartesian space of a general kinematic tree (KDL::Tree).
+     *
+     * @ingroup KinematicFamily
+     */
+    class TreeFkSolverPos_recursive : public TreeFkSolverPos
+    {
+    public:
+        TreeFkSolverPos_recursive(const Tree& tree);
+        ~TreeFkSolverPos_recursive();
+
+		virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName);
+
+    private:
+        const Tree tree;
+        
+        Frame recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit);
+    };
+
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/treejnttojacsolver.cpp

@@ -0,0 +1,81 @@
+/** \file itasc/kdl/treejnttojacsolver.cpp
+ * \ingroup intern_itasc
+ */
+/*
+ * TreeJntToJacSolver.cpp
+ *
+ *  Created on: Nov 27, 2008
+ *      Author: rubensmits
+ */
+
+#include "treejnttojacsolver.hpp"
+#include <iostream>
+
+namespace KDL {
+
+TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) :
+    tree(tree_in) {
+}
+
+TreeJntToJacSolver::~TreeJntToJacSolver() {
+}
+
+int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
+        const std::string& segmentname) {
+    //First we check all the sizes:
+    if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
+            != tree.getNrOfJoints())
+        return -1;
+
+    //Lets search the tree-element
+    SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);
+
+    //If segmentname is not inside the tree, back out:
+    if (!it)
+        return -2;
+
+    //Let's make the jacobian zero:
+    SetToZero(jac);
+
+    SegmentMap::value_type const* root = tree.getSegmentPtr("root");
+
+    Frame T_total = Frame::Identity();
+	Frame T_local, T_joint;
+	Twist t_local;
+    //Lets recursively iterate until we are in the root segment
+    while (it != root) {
+        //get the corresponding q_nr for this TreeElement:
+        unsigned int q_nr = it->second.q_nr;
+
+        //get the pose of the joint.
+		T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
+		// combine with the tip to have the tip pose
+		T_local = T_joint*it->second.segment.getFrameToTip();
+        //calculate new T_end:
+        T_total = T_local * T_total;
+
+        //get the twist of the segment:
+		int ndof = it->second.segment.getJoint().getNDof();
+		for (int dof=0; dof<ndof; dof++) {
+			// combine joint rotation with tip position to get a reference frame for the joint
+			T_joint.p = T_local.p;
+			// in which the twist can be computed (needed for NDof joint)
+            t_local = it->second.segment.twist(T_joint, 1.0, dof);
+            //transform the endpoint of the local twist to the global endpoint:
+            t_local = t_local.RefPoint(T_total.p - T_local.p);
+            //transform the base of the twist to the endpoint
+            t_local = T_total.M.Inverse(t_local);
+            //store the twist in the jacobian:
+            jac.twists[q_nr+dof] = t_local;
+        }
+        //goto the parent
+        it = it->second.parent;
+    }//endwhile
+    //Change the base of the complete jacobian from the endpoint to the base
+    changeBase(jac, T_total.M, jac);
+
+    return 0;
+
+}//end JntToJac
+}//end namespace
+

blender-5.2.0/intern/itasc/kdl/treejnttojacsolver.hpp

@@ -0,0 +1,38 @@
+/*
+ * TreeJntToJacSolver.hpp
+ *
+ *  Created on: Nov 27, 2008
+ *      Author: rubensmits
+ */
+
+#ifndef TREEJNTTOJACSOLVER_HPP_
+#define TREEJNTTOJACSOLVER_HPP_
+
+#include "tree.hpp"
+#include "jacobian.hpp"
+#include "jntarray.hpp"
+
+namespace KDL {
+
+class TreeJntToJacSolver {
+public:
+    TreeJntToJacSolver(const Tree& tree);
+
+    virtual ~TreeJntToJacSolver();
+
+    /*
+     * Calculate the jacobian for a part of the tree: from a certain segment, given by segmentname to the root.
+     * The resulting jacobian is expressed in the baseframe of the tree ("root"), the reference point is in the end-segment
+     */
+
+    int JntToJac(const JntArray& q_in, Jacobian& jac,
+            const std::string& segmentname);
+
+private:
+    KDL::Tree tree;
+
+};
+
+}//End of namespace
+
+#endif /* TREEJNTTOJACSOLVER_H_ */

blender-5.2.0/intern/itasc/kdl/utilities/error.h

@@ -0,0 +1,243 @@
+/***************************************************************************
+  tag: Erwin Aertbelien  Mon Jan 10 16:38:38 CET 2005  error.h 
+
+                        error.h -  description
+                           -------------------
+    begin                : Mon January 10 2005
+    copyright            : (C) 2005 Erwin Aertbelien
+    email                : erwin.aertbelien@mech.kuleuven.ac.be
+ 
+ ***************************************************************************
+ *   This library is free software; you can redistribute it and/or         *
+ *   modify it under the terms of the GNU Lesser General Public            *
+ *   License as published by the Free Software Foundation; either          *
+ *   version 2.1 of the License, or (at your option) any later version.    *
+ *                                                                         *
+ *   This library 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     *
+ *   Lesser General Public License for more details.                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU Lesser General Public      *
+ *   License along with this library; if not, write to the Free Software   *
+ *   Foundation, Inc., 51 Franklin Street,                                    *
+ *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
+ *                                                                         *
+ ***************************************************************************/
+ 
+ 
+/*****************************************************************************
+ * \file  
+ *		 Defines the exception classes that can be thrown
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version 
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Name:  $ 
+ ****************************************************************************/
+#ifndef ERROR_H_84822 // to make it unique, a random number
+#define ERROR_H_84822
+
+#include "utility.h"
+#include <string>
+
+namespace KDL {
+
+/** 
+ *  Base class for errors generated by ORO_Geometry 
+ */
+class Error {
+public:
+    /** Returns a description string describing the error.
+     *  the returned pointer only guaranteed to exists as long as
+     * the Error object exists.
+     */
+	virtual ~Error() {}
+    virtual const char* Description() const {return "Unspecified Error\n";}
+
+    virtual int GetType() const {return 0;}
+};
+
+
+class Error_IO : public Error {
+	std::string msg;
+	int typenr;
+public:
+	Error_IO(const std::string& _msg="Unspecified I/O Error",int typenr=0):msg(_msg) {}
+    virtual const char* Description() const {return msg.c_str();}
+    virtual int GetType() const {return typenr;}
+};
+class Error_BasicIO : public Error_IO {};
+class Error_BasicIO_File : public Error_BasicIO {
+public:
+    virtual const char* Description() const {return "Error while reading stream";}
+    virtual int GetType() const {return 1;}
+};
+class Error_BasicIO_Exp_Delim : public Error_BasicIO {
+public:
+    virtual const char* Description() const {return "Expected Delimiter not encountered";}
+    virtual int GetType() const {return 2;}
+};
+class Error_BasicIO_Not_A_Space : public Error_BasicIO {
+public:
+    virtual const char* Description() const {return "Expected space,tab or newline not encountered";}
+    virtual int GetType() const {return 3;}
+};
+class Error_BasicIO_Unexpected : public Error_BasicIO {
+public:
+    virtual const char* Description() const {return "Unexpected character";}
+    virtual int GetType() const {return 4;}
+};
+
+class Error_BasicIO_ToBig : public Error_BasicIO {
+public:
+    virtual const char* Description() const {return "Word that is read out of stream is bigger than maxsize";}
+    virtual int GetType() const {return 5;}
+};
+
+class Error_BasicIO_Not_Opened : public Error_BasicIO {
+public:
+    virtual const char* Description() const {return "File cannot be opened";}
+    virtual int GetType() const {return 6;}
+};
+class Error_FrameIO : public Error_IO {};
+class Error_Frame_Vector_Unexpected_id : public Error_FrameIO {
+public:
+    virtual const char* Description() const {return "Unexpected identifier, expecting a vector (explicit or ZERO)";}
+    virtual int GetType() const {return 101;}
+};
+class Error_Frame_Frame_Unexpected_id : public Error_FrameIO {
+public:
+    virtual const char* Description() const {return "Unexpected identifier, expecting a Frame (explicit or DH)";}
+    virtual int GetType() const {return 102;}
+};
+class Error_Frame_Rotation_Unexpected_id : public Error_FrameIO {
+public:
+    virtual const char* Description() const {return "Unexpected identifier, expecting a Rotation (explicit or EULERZYX, EULERZYZ, RPY,ROT,IDENTITY)";}
+    virtual int GetType() const {return 103;}
+};
+class Error_ChainIO : public Error {};
+class Error_Chain_Unexpected_id : public Error_ChainIO {
+public:
+    virtual const char* Description() const {return "Unexpected identifier, expecting TRANS or ROT";}
+    virtual int GetType() const {return 201;}
+};
+//! Error_Redundancy indicates an error that occured during solving for redundancy.
+class Error_RedundancyIO:public Error_IO  {};
+class Error_Redundancy_Illegal_Resolutiontype : public Error_RedundancyIO {
+public:
+    virtual const char* Description() const {return "Illegal Resolutiontype is used in I/O with ResolutionTask";}
+    virtual int GetType() const {return 301;}
+};
+class Error_Redundancy:public Error  {};
+class Error_Redundancy_Unavoidable : public Error_Redundancy {
+public:
+    virtual const char* Description() const {return "Joint limits cannot be avoided";}
+    virtual int GetType() const {return 1002;}
+};
+class  Error_Redundancy_Low_Manip: public Error_Redundancy {
+public:
+    virtual const char* Description() const {return "Manipulability is very low";}
+    virtual int GetType() const {return 1003;}
+};
+class Error_MotionIO : public Error {};
+class Error_MotionIO_Unexpected_MotProf : public Error_MotionIO {
+public:
+    virtual const char* Description() const { return "Wrong keyword while reading motion profile";}
+    virtual int GetType() const {return 2001;}
+};
+class Error_MotionIO_Unexpected_Traj : public Error_MotionIO {
+public:
+    virtual const char* Description() const { return "Trajectory type keyword not known";}
+    virtual int GetType() const {return 2002;}
+};
+
+class Error_MotionPlanning : public Error {};
+
+class Error_MotionPlanning_Circle_ToSmall : public Error_MotionPlanning {
+public:
+    virtual const char* Description() const { return "Circle : radius is too small";}
+    virtual int GetType() const {return 3001;}
+};
+
+class Error_MotionPlanning_Circle_No_Plane : public Error_MotionPlanning {
+public:
+    virtual const char* Description() const { return "Circle : Plane for motion is not properly defined";}
+    virtual int GetType() const {return 3002;}
+};
+
+class Error_MotionPlanning_Incompatible: public Error_MotionPlanning {
+public:
+    virtual const char* Description() const { return "Acceleration of a rectangular velocityprofile cannot be used";}
+    virtual int GetType() const {return 3003;}
+};
+
+class Error_MotionPlanning_Not_Feasible: public Error_MotionPlanning {
+public:
+    virtual const char* Description() const { return "Motion Profile with requested parameters is not feasible";}
+    virtual int GetType() const {return 3004;}
+};
+
+class Error_MotionPlanning_Not_Applicable: public Error_MotionPlanning {
+public:
+    virtual const char* Description() const { return "Method is not applicable for this derived object";}
+    virtual int GetType() const {return 3004;}
+};
+//! Abstract subclass of all errors that can be thrown by Adaptive_Integrator
+class Error_Integrator : public Error {};
+
+//! Error_Stepsize_Underflow is thrown if the stepsize becomes to small
+class Error_Stepsize_Underflow  : public Error_Integrator {
+public: 
+    virtual const char* Description() const { return "Stepsize Underflow";}
+    virtual int GetType() const {return 4001;}
+};
+
+//! Error_To_Many_Steps is thrown if the number of steps needed to
+//! integrate to the desired accuracy becomes to big.
+class Error_To_Many_Steps : public Error_Integrator {
+public:
+    virtual const char* Description() const { return "To many steps"; }
+    virtual int GetType() const {return 4002;}
+};
+
+//! Error_Stepsize_To_Small is thrown if the stepsize becomes to small  
+class Error_Stepsize_To_Small : public Error_Integrator {
+public:
+    virtual const char* Description() const { return "Stepsize to small"; }
+    virtual int GetType() const {return 4003;}
+};
+
+class Error_Criterium : public Error {};
+
+class Error_Criterium_Unexpected_id: public Error_Criterium {
+public:
+    virtual const char* Description() const { return "Unexpected identifier while reading a criterium";   }
+    virtual int GetType() const {return 5001;}
+};
+
+class Error_Limits : public Error {};
+
+class Error_Limits_Unexpected_id: public Error_Limits {
+public:
+    virtual const char* Description() const { return "Unexpected identifier while reading a jointlimits"; }
+    virtual int GetType() const {return 6001;}
+};
+
+
+class Error_Not_Implemented: public Error {
+public:
+    virtual const char* Description() const { return "The requested object/method/function is not implemented"; }
+    virtual int GetType() const {return 7000;}
+};
+
+
+
+}
+
+#endif

blender-5.2.0/intern/itasc/kdl/utilities/error_stack.cpp

@@ -0,0 +1,60 @@
+/** \file itasc/kdl/utilities/error_stack.cpp
+ * \ingroup intern_itasc
+ */
+/*****************************************************************************
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version 
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Name:  $ 
+ ****************************************************************************/
+
+
+#include "error_stack.h"
+#include <stack>
+#include <vector>
+#include <string>
+#include <cstring>
+
+namespace KDL {
+
+// Trace of the call stack of the I/O routines to help user
+// interprete error messages from I/O
+typedef std::stack<std::string>  ErrorStack;
+
+ErrorStack errorstack;
+// should be in Thread Local Storage if this gets multithreaded one day...
+
+
+void IOTrace(const std::string& description) {
+    errorstack.push(description);   
+}
+
+
+void IOTracePop() {
+    errorstack.pop();
+}
+
+void IOTraceOutput(std::ostream& os) {
+    while (!errorstack.empty()) {
+        os << errorstack.top().c_str() << std::endl;
+        errorstack.pop();
+    }
+}
+
+
+void IOTracePopStr(char* buffer,int size) {
+    if (errorstack.empty()) {
+        *buffer = 0;
+        return;
+    }
+    strncpy(buffer,errorstack.top().c_str(),size);
+    errorstack.pop();
+}
+
+}

blender-5.2.0/intern/itasc/kdl/utilities/error_stack.h

@@ -0,0 +1,69 @@
+/***************************************************************************
+  tag: Erwin Aertbelien  Mon Jan 10 16:38:39 CET 2005  error_stack.h 
+
+                        error_stack.h -  description
+                           -------------------
+    begin                : Mon January 10 2005
+    copyright            : (C) 2005 Erwin Aertbelien
+    email                : erwin.aertbelien@mech.kuleuven.ac.be
+ 
+ ***************************************************************************
+ *   This library is free software; you can redistribute it and/or         *
+ *   modify it under the terms of the GNU Lesser General Public            *
+ *   License as published by the Free Software Foundation; either          *
+ *   version 2.1 of the License, or (at your option) any later version.    *
+ *                                                                         *
+ *   This library 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     *
+ *   Lesser General Public License for more details.                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU Lesser General Public      *
+ *   License along with this library; if not, write to the Free Software   *
+ *   Foundation, Inc., 51 Franklin Street,                                    *
+ *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
+ *                                                                         *
+ ***************************************************************************/
+ 
+ 
+/** 
+ * \file 
+ * \version 
+ *      ORO_Geometry V0.2
+ *   
+ * \par history
+ *   - changed layout of the comments to accommodate doxygen
+ */
+#ifndef ERROR_STACK_H
+#define ERROR_STACK_H
+
+#include "utility.h"
+#include "utility_io.h"
+#include <string>
+
+
+namespace KDL {
+
+/*
+ * \todo
+ *    IOTrace-routines store in static memory, should be in thread-local memory.
+ * pushes a description of the current routine on the IO-stack trace
+ */
+void IOTrace(const std::string& description);
+
+//! pops a description of the IO-stack
+void IOTracePop();
+
+
+//! outputs the IO-stack to a stream to provide a better errormessage.
+void IOTraceOutput(std::ostream& os);
+
+//! outputs one element of the IO-stack to the buffer (maximally size chars)
+//! returns empty string if no elements on the stack.
+void IOTracePopStr(char* buffer,int size);
+
+
+}
+
+#endif
+

blender-5.2.0/intern/itasc/kdl/utilities/kdl-config.h

@@ -0,0 +1,36 @@
+/** \file itasc/kdl/utilities/kdl-config.h
+ * \ingroup intern_itasc
+ */
+/* Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
+
+/* Version: 1.0 */
+/* Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
+/* Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
+/* URL: http://www.orocos.org/kdl */
+
+/* This library is free software; you can redistribute it and/or */
+/* modify it under the terms of the GNU Lesser General Public */
+/* License as published by the Free Software Foundation; either */
+/* version 2.1 of the License, or (at your option) any later version. */
+
+/* This library 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 */
+/* Lesser General Public License for more details. */
+
+/* You should have received a copy of the GNU Lesser General Public */
+/* License along with this library; if not, write to the Free Software */
+/* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA */
+
+
+/* Methods are inlined */
+#define KDL_INLINE 1
+
+/* Column width that is used form printing frames */
+#define KDL_FRAME_WIDTH 12
+
+/* Indices are checked when accessing members of the objects */
+#define KDL_INDEX_CHECK 1
+
+/* use KDL implementation for == operator */
+#define KDL_USE_EQUAL 1

blender-5.2.0/intern/itasc/kdl/utilities/rall1d.h

@@ -0,0 +1,476 @@
+ 
+/*****************************************************************************
+ * \file  
+ *      class for automatic differentiation on scalar values and 1st 
+ *      derivatives .
+ *       
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version 
+ *      ORO_Geometry V0.2
+ *
+ * \par Note
+ *      VC6++ contains a bug, concerning the use of inlined friend functions 
+ *      in combination with namespaces.  So, try to avoid inlined friend 
+ *      functions !  
+ *
+ * \par History
+ *      - $log$ 
+ *
+ * \par Release
+ *      $Name:  $ 
+ ****************************************************************************/
+ 
+#ifndef Rall1D_H
+#define Rall1D_H
+#include <assert.h>
+#include "utility.h"
+
+namespace KDL {
+/**
+ * Rall1d contains a value, and its gradient, and defines an algebraic structure on this pair.
+ *  This template class has 3 template parameters :
+ *  -   T contains the type of the value.
+ *  -   V contains the type of the gradient (can be a vector-like type).  
+ *  -   S defines a scalar type that can operate on Rall1d.  This is the type that 
+ *      is used to give back values of Norm() etc. 
+ *
+ * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. 
+ * derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ).
+ *
+ * S is always passed by value. 
+ *
+ * \par Class Type
+ * Concrete implementation
+ */
+template <typename T,typename V=T,typename S=T>
+class Rall1d
+    {
+    public:
+        typedef T valuetype;
+        typedef V gradienttype;
+        typedef S scalartype;
+    public :
+        T t;        //!< value
+        V grad;     //!< gradient
+    public :
+        INLINE Rall1d() {}
+
+        T value() const {
+            return t;
+        }
+        V deriv() const {
+            return grad;
+        }
+
+        explicit INLINE  Rall1d(typename TI<T>::Arg c)
+            {t=T(c);SetToZero(grad);}
+
+        INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
+
+        INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
+        //if one defines this constructor, it's better optimized then the
+        //automatically generated one ( this one set's up a loop to copy
+        // word by word.
+        
+        INLINE T& Value() {
+            return t;
+        }
+
+        INLINE V& Gradient() {
+            return grad;
+        }
+
+        INLINE static Rall1d<T,V,S> Zero() {
+            Rall1d<T,V,S> tmp;
+            SetToZero(tmp);
+            return tmp;
+        }
+        INLINE static Rall1d<T,V,S> Identity() {
+            Rall1d<T,V,S> tmp;
+            SetToIdentity(tmp);
+            return tmp;
+        }
+
+        INLINE Rall1d<T,V,S>& operator =(S c)
+            {t=c;SetToZero(grad);return *this;}
+
+        INLINE Rall1d<T,V,S>& operator =(const Rall1d<T,V,S>& r)
+            {t=r.t;grad=r.grad;return *this;}
+
+        INLINE Rall1d<T,V,S>& operator /=(const Rall1d<T,V,S>& rhs)
+            {
+            grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
+            t     /= rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator *=(const Rall1d<T,V,S>& rhs)
+            {
+            LinCombR(rhs.t,grad,t,rhs.grad,grad);
+            t *= rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator +=(const Rall1d<T,V,S>& rhs)
+            {
+            grad +=rhs.grad;
+            t    +=rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator -=(const Rall1d<T,V,S>& rhs)
+            {
+            grad -= rhs.grad;
+            t     -= rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator /=(S rhs)
+            {
+            grad /= rhs;
+            t    /= rhs;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator *=(S rhs)
+            {
+            grad *= rhs;
+            t    *= rhs;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator +=(S rhs)
+            {
+            t    += rhs;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator -=(S rhs)
+            {
+            t    -= rhs;
+            return *this;
+            }
+
+
+
+        // = operators
+        /* gives warnings on cygwin 
+        
+         template <class T2,class V2,class S2>
+         friend INLINE  Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
+         
+         friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
+         friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
+         friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
+         friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
+         friend INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
+         friend INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
+         friend INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
+
+        // = Mathematical functions that operate on Rall1d objects
+         friend INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
+         friend INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
+         friend INLINE  S Norm(const Rall1d<T,V,S>& value) ;
+         friend INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
+         
+        // = Utility functions to improve performance
+
+         friend INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
+            const T& beta,const Rall1d<T,V,S>& b );
+        
+         friend INLINE  void LinCombR(S alfa,const Rall1d<T,V,S>& a,
+            const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
+        
+        // = Setting value of a Rall1d object to 0 or 1
+
+         friend INLINE  void SetToZero(Rall1d<T,V,S>& value);
+         friend INLINE  void SetToOne(Rall1d<T,V,S>& value);
+        // = Equality in an eps-interval
+         friend INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
+         */
+    };
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
+    }
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg)
+    {
+    return Rall1d<T,V,S>(-arg.t,-arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s*v.t,s*v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t*s,v.grad*s);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s+v.t,v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t+s,v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s-v.t,-v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t-s,v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t/s,v.grad/s);
+    }
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v= (exp(arg.t));
+    return Rall1d<T,V,S>(v,v*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(log(arg.t));
+    return Rall1d<T,V,S>(v,arg.grad/arg.t);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(sin(arg.t));
+    return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(cos(arg.t));
+    return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(tan(arg.t));
+    return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(sinh(arg.t));
+    return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(cosh(arg.t));
+    return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(arg.t*arg.t);
+    return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) 
+    {
+    T v;
+    v=(pow(arg.t,m));
+    return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=sqrt(arg.t);
+    return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
+    }   
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(atan(x.t));
+    return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(hypot(y.t,x.t));
+    return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(asin(x.t));
+    return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(acos(x.t));
+    return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(Sign(x));
+    return Rall1d<T,V,S>(v*x,v*x.grad);
+}
+
+
+template <class T,class V,class S>
+INLINE  S Norm(const Rall1d<T,V,S>& value) 
+{
+    return Norm(value.t);
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg)
+{       
+    T v(tanh(arg.t));       
+    return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
+{
+    T v(x.t*x.t+y.t*y.t);
+    return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
+}
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
+    const T& beta,const Rall1d<T,V,S>& b ) {
+        return Rall1d<T,V,S>(
+            LinComb(alfa,a.t,beta,b.t),
+            LinComb(alfa,a.grad,beta,b.grad)
+        );
+}
+
+template <class T,class V,class S>
+INLINE  void LinCombR(S alfa,const Rall1d<T,V,S>& a,
+    const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) {
+            LinCombR(alfa, a.t,       beta, b.t,      result.t);
+            LinCombR(alfa, a.grad,    beta, b.grad,   result.grad);
+}
+
+
+template <class T,class V,class S>
+INLINE  void SetToZero(Rall1d<T,V,S>& value)
+    {
+    SetToZero(value.grad);
+    SetToZero(value.t);
+    }
+template <class T,class V,class S>
+INLINE  void SetToIdentity(Rall1d<T,V,S>& value)
+    {
+    SetToIdentity(value.t);
+    SetToZero(value.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
+{
+    return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
+}
+
+}
+
+
+
+#endif

blender-5.2.0/intern/itasc/kdl/utilities/rall2d.h

@@ -0,0 +1,536 @@
+
+/*****************************************************************************
+ * \file  
+ *      class for automatic differentiation on scalar values and 1st 
+ *      derivatives and 2nd derivative.
+ *       
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version 
+ *      ORO_Geometry V0.2
+ *
+ * \par Note
+ *      VC6++ contains a bug, concerning the use of inlined friend functions 
+ *      in combination with namespaces.  So, try to avoid inlined friend 
+ *      functions !  
+ *
+ * \par History
+ *      - $log$ 
+ *
+ * \par Release
+ *      $Name:  $ 
+ ****************************************************************************/
+
+#ifndef Rall2D_H
+#define Rall2D_H
+
+#include <math.h>
+#include <assert.h>
+#include "utility.h"
+
+
+namespace KDL {
+
+/**
+ * Rall2d contains a value, and its gradient and its 2nd derivative, and defines an algebraic 
+ * structure on this pair.
+ *  This template class has 3 template parameters :
+ *  -   T contains the type of the value.
+ *  -   V contains the type of the gradient (can be a vector-like type).  
+ *  -   S defines a scalar type that can operate on Rall1d.  This is the type that 
+ *      is used to give back values of Norm() etc. 
+ *
+ * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. 
+ * derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ).
+ *
+ * S is always passed by value. 
+ *
+ * \par Class Type
+ * Concrete implementation
+ */
+template <class T,class V=T,class S=T>
+class Rall2d                      
+    {
+    public :
+        T t;    //!< value
+        V d;    //!< 1st derivative
+        V dd;   //!< 2nd derivative
+    public :
+        // = Constructors
+        INLINE Rall2d() {}
+
+        explicit INLINE  Rall2d(typename TI<T>::Arg c)
+            {t=c;SetToZero(d);SetToZero(dd);}
+
+        INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);}
+
+        INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {}
+
+        // = Copy Constructor
+        INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
+            //if one defines this constructor, it's better optimized then the
+            //automatically generated one ( that one set's up a loop to copy
+            // word by word.
+        
+        // = Member functions to access internal structures :
+        INLINE T& Value() {
+            return t;
+        }
+
+        INLINE V& D() {
+            return d;
+        }
+
+        INLINE V& DD() {
+            return dd;
+        }
+        INLINE static Rall2d<T,V,S> Zero() {
+            Rall2d<T,V,S> tmp;
+            SetToZero(tmp);
+            return tmp;
+        }
+        INLINE static Rall2d<T,V,S> Identity() {
+            Rall2d<T,V,S> tmp;
+            SetToIdentity(tmp);
+            return tmp;
+        }
+
+        // = assignment operators
+        INLINE Rall2d<T,V,S>& operator =(S c)
+            {t=c;SetToZero(d);SetToZero(dd);return *this;}
+
+        INLINE Rall2d<T,V,S>& operator =(const Rall2d<T,V,S>& r)
+            {t=r.t;d=r.d;dd=r.dd;return *this;}
+
+        INLINE Rall2d<T,V,S>& operator /=(const Rall2d<T,V,S>& rhs)
+            {
+            t /= rhs.t;
+            d = (d-t*rhs.d)/rhs.t;
+            dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t;
+            return *this;
+            }
+
+        INLINE Rall2d<T,V,S>& operator *=(const Rall2d<T,V,S>& rhs)
+            {
+            t *= rhs.t;
+            d  = (d*rhs.t+t*rhs.d);
+            dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd);
+            return *this;
+            }
+
+        INLINE Rall2d<T,V,S>& operator +=(const Rall2d<T,V,S>& rhs)
+            {
+            t    +=rhs.t;
+            d +=rhs.d;
+            dd+=rhs.dd;
+            return *this;
+            }
+
+        INLINE Rall2d<T,V,S>& operator -=(const Rall2d<T,V,S>& rhs)
+            {
+            t     -= rhs.t;
+            d     -= rhs.d;
+            dd    -= rhs.dd;
+            return *this;
+            }
+
+        INLINE Rall2d<T,V,S>& operator /=(S rhs)
+            {
+            t    /= rhs;
+            d    /= rhs;
+            dd   /= rhs;
+            return *this;
+            }
+
+        INLINE Rall2d<T,V,S>& operator *=(S rhs)
+            {
+            t    *= rhs;
+            d    *= rhs;
+            dd   *= rhs;
+            return *this;
+            }
+
+        INLINE Rall2d<T,V,S>& operator -=(S rhs)
+            {
+            t    -= rhs;
+            return *this;
+            }
+
+        INLINE Rall2d<T,V,S>& operator +=(S rhs)
+            {
+            t    += rhs;
+            return *this;
+            }
+
+        // = Operators between Rall2d objects
+/*
+         friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+         friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+         friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+         friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+         friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v);
+         friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s);
+         friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v);
+         friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s);
+         friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v);
+         friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s);
+         friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v);
+         friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
+
+        // = Mathematical functions that operate on Rall2d objects
+
+         friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
+         friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
+         friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
+         friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
+         friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
+         friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
+        // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
+         friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
+        // returns Norm( value.Value() ).
+
+        // = Some utility functions to improve performance
+        // (should also be declared on primitive types to improve uniformity
+         friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
+            TI<T>::Arg beta,const Rall2d<T,V,S>& b );
+         friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
+            TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
+        // = Setting value of a Rall2d object to 0 or 1
+         friend INLINE void SetToZero(Rall2d<T,V,S>& value);
+         friend INLINE void SetToOne(Rall2d<T,V,S>& value);
+        // = Equality in an eps-interval
+         friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
+         */
+    };
+
+
+
+
+
+// = Operators between Rall2d objects
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+    {
+    Rall2d<T,V,S> tmp;
+    tmp.t = lhs.t/rhs.t;
+    tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
+    tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
+    return tmp;
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+    {
+    Rall2d<T,V,S> tmp;
+    tmp.t  = lhs.t*rhs.t;
+    tmp.d  = (lhs.d*rhs.t+lhs.t*rhs.d);
+    tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
+    return tmp;
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+    {
+    return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+    {
+    return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg)
+    {
+    return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v)
+    {
+    return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s)
+    {
+    return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v)
+    {
+    return Rall2d<T,V,S>(s+v.t,v.d,v.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s)
+    {
+    return Rall2d<T,V,S>(v.t+s,v.d,v.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v)
+    {
+    return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s)
+    {
+    return Rall2d<T,V,S>(v.t-s,v.d,v.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& rhs)
+    {
+    Rall2d<T,V,S> tmp;
+    tmp.t = s/rhs.t;
+    tmp.d = (-tmp.t*rhs.d)/rhs.t;
+    tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
+    return tmp;
+}
+
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s)
+    {
+    return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s);
+    }
+
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg)
+    {
+    Rall2d<T,V,S> tmp;
+    tmp.t  = exp(arg.t);
+    tmp.d  = tmp.t*arg.d;
+    tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
+    return tmp;
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg)
+    {
+    Rall2d<T,V,S> tmp;
+    tmp.t  = log(arg.t);
+    tmp.d  = arg.d/arg.t;
+    tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
+    return tmp;
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg)
+    {
+    T v1 = sin(arg.t);
+    T v2 = cos(arg.t);
+    return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg)
+    {
+    T v1 = cos(arg.t);
+    T v2 = -sin(arg.t);
+    return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg)
+    {
+    T v1 = tan(arg.t);
+    T v2 = S(1)+sqr(v1);
+    return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d))));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg)
+    {
+    T v1 = sinh(arg.t);
+    T v2 = cosh(arg.t);
+    return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg)
+    {
+    T v1 = cosh(arg.t);
+    T v2 = sinh(arg.t);
+    return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg)
+    {       
+    T v1 = tanh(arg.t);
+    T v2 = S(1)-sqr(v1);
+    return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d))));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg)
+    {
+    return Rall2d<T,V,S>(arg.t*arg.t,
+                        (S(2)*arg.t)*arg.d,
+                        S(2)*(sqr(arg.d)+arg.t*arg.dd)
+                        );
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) 
+    {
+    Rall2d<T,V,S> tmp;
+    tmp.t = pow(arg.t,m);
+    T v2  = (m/arg.t)*tmp.t;
+    tmp.d  = v2*arg.d;
+    tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
+    return tmp;
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg)
+    {
+    /* By inversion of sqr(x) */
+    Rall2d<T,V,S> tmp;
+    tmp.t  = sqrt(arg.t);
+    tmp.d  = (S(0.5)/tmp.t)*arg.d;
+    tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t;
+    return tmp;
+    }
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg)
+{
+    /* By inversion of sin(x) */
+    Rall2d<T,V,S> tmp;
+    tmp.t  = asin(arg.t);
+    T v = cos(tmp.t);
+    tmp.d  = arg.d/v;
+    tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
+    return tmp;
+}
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg)
+{
+    /* By inversion of cos(x) */
+    Rall2d<T,V,S> tmp;
+    tmp.t  = acos(arg.t);
+    T v = -sin(tmp.t);
+    tmp.d  = arg.d/v;
+    tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
+    return tmp;
+
+}
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x)
+{
+    /* By inversion of tan(x) */
+    Rall2d<T,V,S> tmp;
+    tmp.t  = atan(x.t);
+    T v    = S(1)+sqr(x.t);
+    tmp.d  = x.d/v;
+    tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d);
+    return tmp;
+}
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
+{
+    Rall2d<T,V,S> tmp;
+    tmp.t  = atan2(y.t,x.t);
+    T v    = sqr(y.t)+sqr(x.t);
+    tmp.d  = (x.t*y.d-x.d*y.t)/v;
+    tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
+    return tmp;
+}
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x)
+{
+    T v(Sign(x));
+    return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd);
+}
+
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
+{
+    Rall2d<T,V,S> tmp;
+    tmp.t  = hypot(y.t,x.t);
+    tmp.d  = (x.t*x.d+y.t*y.d)/tmp.t;
+    tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t;
+    return tmp;
+}
+// returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
+
+template <class T,class V,class S>
+INLINE  S Norm(const Rall2d<T,V,S>& value) 
+{
+    return Norm(value.t);
+}
+// returns Norm( value.Value() ).
+
+
+// (should also be declared on primitive types to improve uniformity
+template <class T,class V,class S>
+INLINE  Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
+    const T& beta,const Rall2d<T,V,S>& b ) {
+        return Rall2d<T,V,S>(
+            LinComb(alfa,a.t,beta,b.t),
+            LinComb(alfa,a.d,beta,b.d),
+            LinComb(alfa,a.dd,beta,b.dd) 
+        );
+}
+
+template <class T,class V,class S>
+INLINE  void LinCombR(S alfa,const Rall2d<T,V,S>& a,
+    const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
+            LinCombR(alfa, a.t,       beta, b.t,      result.t);
+            LinCombR(alfa, a.d,    beta, b.d,   result.d);
+            LinCombR(alfa, a.dd,    beta, b.dd,   result.dd);
+}
+
+template <class T,class V,class S>
+INLINE  void SetToZero(Rall2d<T,V,S>& value)
+    {
+    SetToZero(value.t);
+    SetToZero(value.d);
+    SetToZero(value.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  void SetToIdentity(Rall2d<T,V,S>& value)
+    {
+    SetToZero(value.d);
+    SetToIdentity(value.t);
+    SetToZero(value.dd);
+    }
+
+template <class T,class V,class S>
+INLINE  bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon)
+{
+    return (Equal(x.t,y.t,eps)&&
+            Equal(x.d,y.d,eps)&&
+            Equal(x.dd,y.dd,eps)
+            );
+}
+
+
+}
+
+
+#endif

blender-5.2.0/intern/itasc/kdl/utilities/svd_eigen_HH.hpp

@@ -0,0 +1,309 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+//Based on the svd of the KDL-0.2 library by Erwin Aertbelien
+#ifndef SVD_EIGEN_HH_HPP
+#define SVD_EIGEN_HH_HPP
+
+
+#include <Eigen/Core>
+#include <algorithm>
+
+namespace KDL
+{
+    template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) {
+        double at,bt,ct;
+        at = fabs(a);
+        bt = fabs(b);
+        if (at > bt ) {
+            ct=bt/at;
+            return Scalar(at*sqrt(1.0+ct*ct));
+        } else {
+            if (bt==0)
+                return Scalar(0.0);
+            else {
+                ct=at/bt;
+                return Scalar(bt*sqrt(1.0+ct*ct));
+            }
+        }
+    }
+
+
+    template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) {
+        return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
+    }
+
+    /**
+     * svd calculation of boost ublas matrices
+     *
+     * @param A matrix<double>(mxn)
+     * @param U matrix<double>(mxn)
+     * @param S vector<double> n
+     * @param V matrix<double>(nxn)
+     * @param tmp vector<double> n
+     * @param maxiter defaults to 150
+     *
+     * @return -2 if maxiter exceeded, 0 otherwise
+     */
+	template<typename MatrixA, typename MatrixUV, typename VectorS> 
+	int svd_eigen_HH(
+		const Eigen::MatrixBase<MatrixA>&		A,
+		Eigen::MatrixBase<MatrixUV>&			U,
+		Eigen::MatrixBase<VectorS>&				S,
+		Eigen::MatrixBase<MatrixUV>&			V,
+		Eigen::MatrixBase<VectorS>&				tmp,
+		int maxiter=150)
+	{
+        //get the rows/columns of the matrix
+        const int rows = A.rows();
+        const int cols = A.cols();
+        
+        U = A;
+        
+        int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
+        int ppi(0);
+        bool flag;
+        e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
+        
+        g=scale=anorm=e_scalar(0.0);
+        
+        /* Householder reduction to bidiagonal form. */
+        for (i=0;i<cols;i++) {
+            ppi=i+1;
+            tmp(i)=scale*g;
+            g=s=scale=e_scalar(0.0); 
+            if (i<rows) {
+                // compute the sum of the i-th column, starting from the i-th row
+                for (k=i;k<rows;k++) scale += fabs(U(k,i));
+                if (scale!=0) {
+                    // multiply the i-th column by 1.0/scale, start from the i-th element
+                    // sum of squares of column i, start from the i-th element
+                    for (k=i;k<rows;k++) {
+                        U(k,i) /= scale;
+                        s += U(k,i)*U(k,i);
+                    }
+                    f=U(i,i);  // f is the diag elem
+                    g = -SIGN(e_scalar(sqrt(s)),f);
+                    h=f*g-s;
+                    U(i,i)=f-g;
+                    for (j=ppi;j<cols;j++) {
+                        // dot product of columns i and j, starting from the i-th row
+                        for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
+                        f=s/h;
+                        // copy the scaled i-th column into the j-th column
+                        for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
+                    }
+                    for (k=i;k<rows;k++) U(k,i) *= scale;
+                }
+            }
+            // save singular value
+            S(i)=scale*g;
+            g=s=scale=e_scalar(0.0);
+            if ((i <rows) && (i+1 != cols)) {
+                // sum of row i, start from columns i+1
+                for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
+                if (scale!=0) {
+                    for (k=ppi;k<cols;k++) {
+                        U(i,k) /= scale;
+                        s += U(i,k)*U(i,k);
+                    }
+                    f=U(i,ppi);
+                    g = -SIGN(e_scalar(sqrt(s)),f);
+                    h=f*g-s;
+                    U(i,ppi)=f-g;
+                    for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
+                    for (j=ppi;j<rows;j++) {
+                        for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
+                        for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
+                    }
+                    for (k=ppi;k<cols;k++) U(i,k) *= scale;
+                }
+            }
+            maxarg1=anorm;
+            maxarg2=(fabs(S(i))+fabs(tmp(i)));
+            anorm = maxarg1 > maxarg2 ?	maxarg1 : maxarg2;		
+        }
+        /* Accumulation of right-hand transformations. */
+        for (i=cols-1;i>=0;i--) {
+            if (i<cols-1) {
+                if (g) {
+                    for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
+                    for (j=ppi;j<cols;j++) {
+                        for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
+                        for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
+                    }
+                }
+                for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
+            }
+            V(i,i)=1.0;
+            g=tmp(i);
+            ppi=i;
+        }
+        /* Accumulation of left-hand transformations. */
+        for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
+            ppi=i+1;
+            g=S(i);
+            for (j=ppi;j<cols;j++) U(i,j)=0.0;
+            if (g) {
+                g=e_scalar(1.0)/g;
+                for (j=ppi;j<cols;j++) {
+                    for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
+                    f=(s/U(i,i))*g;
+                    for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
+                }
+                for (j=i;j<rows;j++) U(j,i) *= g;
+            } else {
+                for (j=i;j<rows;j++) U(j,i)=0.0;
+            }
+            ++U(i,i);
+        }
+        
+        /* Diagonalization of the bidiagonal form. */
+        for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
+            for (its=1;its<=maxiter;its++) {  /* Loop over allowed iterations. */
+                flag=true;
+                for (ppi=k;ppi>=0;ppi--) {  /* Test for splitting. */
+                    nm=ppi-1;             /* Note that tmp(1) is always zero. */
+                    if ((fabs(tmp(ppi))+anorm) == anorm) {
+                        flag=false;
+                        break;
+                    }
+                    if ((fabs(S(nm)+anorm) == anorm)) break;
+                }
+                if (flag) {
+                    c=e_scalar(0.0);           /* Cancellation of tmp(l), if l>1: */
+                    s=e_scalar(1.);
+                    for (i=ppi;i<=k;i++) {
+                        f=s*tmp(i);
+                        tmp(i)=c*tmp(i);
+                        if ((fabs(f)+anorm) == anorm) break;
+                        g=S(i);
+                        h=PYTHAG(f,g);
+                        S(i)=h;
+                        h=e_scalar(1.0)/h;
+                        c=g*h;
+                        s=(-f*h);
+                        for (j=0;j<rows;j++) {
+                            y=U(j,nm);
+                            z=U(j,i);
+                            U(j,nm)=y*c+z*s;
+                            U(j,i)=z*c-y*s;
+                        }
+                    }
+                }
+                z=S(k);
+                
+                if (ppi == k) {       /* Convergence. */
+                    if (z < e_scalar(0.0)) {   /* Singular value is made nonnegative. */
+                        S(k) = -z;
+                        for (j=0;j<cols;j++) V(j,k)=-V(j,k);
+                    }
+                    break;
+                }
+                
+                x=S(ppi);            /* Shift from bottom 2-by-2 minor: */
+                nm=k-1;
+                y=S(nm);
+                g=tmp(nm);
+                h=tmp(k);
+                f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y);
+                
+                g=PYTHAG(f,e_scalar(1.0));
+                f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
+                
+                /* Next QR transformation: */
+                c=s=1.0;
+                for (j=ppi;j<=nm;j++) {
+                    i=j+1;
+                    g=tmp(i);
+                    y=S(i);
+                    h=s*g;
+                    g=c*g;
+                    z=PYTHAG(f,h);
+                    tmp(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=0;jj<cols;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);
+                    S(j)=z;
+                    if (z) {
+                        z=e_scalar(1.0)/z;
+                        c=f*z;
+                        s=h*z;
+                    }
+                    f=(c*g)+(s*y);
+                    x=(c*y)-(s*g);
+                    for (jj=0;jj<rows;jj++) {
+                        y=U(jj,j);
+                        z=U(jj,i);
+                        U(jj,j)=y*c+z*s;
+                        U(jj,i)=z*c-y*s;
+                    }
+                }
+                tmp(ppi)=0.0;
+                tmp(k)=f;
+                S(k)=x;
+            }
+        }
+
+        //Sort eigen values:
+        for (i=0; i<cols; i++){
+            
+            double S_max = S(i);
+            int i_max = i;
+            for (j=i+1; j<cols; j++){
+                double Sj = S(j);
+                if (Sj > S_max){
+                    S_max = Sj;
+                    i_max = j;
+                }
+            }
+            if (i_max != i){
+                /* swap eigenvalues */
+                e_scalar tmp = S(i);
+                S(i)=S(i_max);
+                S(i_max)=tmp;
+                
+                /* swap eigenvectors */
+                U.col(i).swap(U.col(i_max));
+                V.col(i).swap(V.col(i_max));
+            }
+        }
+        
+        
+        if (its == maxiter) 
+            return (-2);
+        else 
+            return (0);
+    }
+
+}
+#endif

blender-5.2.0/intern/itasc/kdl/utilities/traits.h

@@ -0,0 +1,114 @@
+/** \file itasc/kdl/utilities/traits.h
+ * \ingroup intern_itasc
+ */
+#ifndef KDLPV_TRAITS_H 
+#define KDLPV_TRAITS_H 
+
+#include "utility.h"
+
+ 
+// forwards declarations :
+namespace KDL {
+   class Frame;
+    class Rotation;
+    class Vector;
+    class Twist;
+    class Wrench;
+	class FrameVel;
+	class RotationVel;
+	class VectorVel;
+	class TwistVel;
+}
+
+
+/**
+ * @brief Traits are traits classes to determine the type of a derivative of another type.
+ *
+ * For geometric objects the "geometric" derivative is chosen.  For example the derivative of a Rotation
+ * matrix is NOT a 3x3 matrix containing the derivative of the elements of a rotation matrix.  The derivative
+ * of the rotation matrix is a Vector corresponding the rotational velocity.  Mostly used in template classes
+ * and routines to derive a correct type when needed.
+ * 
+ * You can see this as a compile-time lookuptable to find the type of the derivative.
+ *
+ * Example
+ * \verbatim
+	Rotation R;
+    Traits<Rotation> dR;
+   \endverbatim
+ */
+template <typename T>
+struct Traits {
+	typedef T valueType;
+	typedef T derivType;
+};
+
+template <>
+struct Traits<KDL::Frame> {
+	typedef KDL::Frame valueType;
+	typedef KDL::Twist derivType;
+};
+template <>
+struct Traits<KDL::Twist> {
+	typedef KDL::Twist valueType;
+	typedef KDL::Twist derivType;
+};
+template <>
+struct Traits<KDL::Wrench> {
+	typedef KDL::Wrench valueType;
+	typedef KDL::Wrench derivType;
+};
+
+template <>
+struct Traits<KDL::Rotation> {
+	typedef KDL::Rotation valueType;
+	typedef KDL::Vector derivType;
+};
+
+template <>
+struct Traits<KDL::Vector> {
+	typedef KDL::Vector valueType;
+	typedef KDL::Vector derivType;
+};
+
+template <>
+struct Traits<double> {
+	typedef double valueType;
+	typedef double derivType;
+};
+
+template <>
+struct Traits<float> {
+	typedef float valueType;
+	typedef float derivType;
+};
+
+template <>
+struct Traits<KDL::FrameVel> {
+	typedef KDL::Frame valueType;
+	typedef KDL::TwistVel derivType;
+};
+template <>
+struct Traits<KDL::TwistVel> {
+	typedef KDL::Twist valueType;
+	typedef KDL::TwistVel derivType;
+};
+
+template <>
+struct Traits<KDL::RotationVel> {
+	typedef KDL::Rotation valueType;
+	typedef KDL::VectorVel derivType;
+};
+
+template <>
+struct Traits<KDL::VectorVel> {
+	typedef KDL::Vector valueType;
+	typedef KDL::VectorVel derivType;
+};
+
+
+
+#endif
+
+
+

blender-5.2.0/intern/itasc/kdl/utilities/utility.cpp

@@ -0,0 +1,24 @@
+/** \file itasc/kdl/utilities/utility.cpp
+ * \ingroup intern_itasc
+ */
+/** @file   utility.cpp
+ *  @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *  @version 
+ *      ORO_Geometry V0.2
+ *   
+ *  @par history
+ *   - changed layout of the comments to accommodate doxygen
+ */
+ 
+#include "utility.h"
+
+namespace KDL {
+
+int STREAMBUFFERSIZE=10000;
+int MAXLENFILENAME = 255;
+const double PI=       3.1415926535897932384626433832795;
+const double deg2rad = 0.01745329251994329576923690768488;
+const double rad2deg = 57.2957795130823208767981548141052;
+double epsilon = 0.000001;
+double epsilon2 = 0.000001*0.000001;
+}

blender-5.2.0/intern/itasc/kdl/utilities/utility.h

@@ -0,0 +1,301 @@
+/***************************************************************************** 
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version 
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Name:  $ 
+ * \file
+ *    Included by most lrl-files to provide some general
+ *    functions and macro definitions.
+ *  
+ * \par history
+ *   - changed layout of the comments to accommodate doxygen
+ */
+
+
+#ifndef KDL_UTILITY_H
+#define KDL_UTILITY_H
+
+#include "kdl-config.h"
+#include <cstdlib>
+#include <cassert>
+#include <cmath>
+
+#ifdef NDEBUG
+#undef assert
+#define assert(e) ((void)0)
+#endif
+
+/////////////////////////////////////////////////////////////
+// configurable options for the frames library.
+
+#ifdef KDL_INLINE
+    #ifdef _MSC_VER
+        // Microsoft Visual C
+        #define IMETHOD __forceinline
+    #else
+        // Some other compiler, e.g. gcc
+        #define IMETHOD inline
+    #endif
+#else
+    #define IMETHOD
+#endif
+
+
+
+//! turn on or off frames bounds checking. If turned on, assert() can still
+//! be turned off with -DNDEBUG.
+#ifdef KDL_INDEX_CHECK
+    #define FRAMES_CHECKI(a) assert(a)
+#else
+    #define FRAMES_CHECKI(a)
+#endif
+
+
+namespace KDL {
+
+#ifdef __GNUC__
+    // so that sin,cos can be overloaded and complete 
+    // resolution of overloaded functions work.
+    using ::sin;
+    using ::cos;
+    using ::exp;
+    using ::log;
+    using ::sin;
+    using ::cos;
+    using ::tan;
+    using ::sinh;
+    using ::cosh;
+    using ::pow;
+    using ::sqrt;
+    using ::atan;
+    using ::hypot;
+    using ::asin;
+    using ::acos;
+    using ::tanh;
+    using ::atan2;
+#endif
+#ifndef __GNUC__
+    //only real solution : get Rall1d and varia out of namespaces.
+    #pragma warning (disable:4786)
+
+    inline double sin(double a) {
+        return ::sin(a);
+    }
+    
+    inline double cos(double a) {
+        return ::cos(a);
+    }
+    inline double exp(double a) {
+        return ::exp(a);
+    }
+    inline double log(double a) {
+        return ::log(a);
+    }
+    inline double tan(double a) {
+        return ::tan(a);
+    }
+    inline double cosh(double a) {
+        return ::cosh(a);
+    }
+    inline double sinh(double a) {
+        return ::sinh(a);
+    }
+    inline double sqrt(double a) {
+        return ::sqrt(a);
+    }
+    inline double atan(double a) {
+        return ::atan(a);
+    }
+    inline double acos(double a) {
+        return ::acos(a);
+    }
+    inline double asin(double a) {
+        return ::asin(a);
+    }
+    inline double tanh(double a) {
+        return ::tanh(a);
+    }
+    inline double pow(double a,double b) {
+        return ::pow(a,b);
+    }
+    inline double atan2(double a,double b) {
+        return ::atan2(a,b);
+    }
+#endif  
+
+
+
+
+
+/** 
+ * Auxiliary class for argument types (Trait-template class )
+ * 
+ * Is used to pass doubles by value, and arbitrary objects by const reference.
+ * This is TWICE as fast (2 x less memory access) and avoids bugs in VC6++ concerning
+ * the assignment of the result of intrinsic functions to const double&-typed variables,
+ * and optimization on.
+ */
+template <class T>
+class TI
+{
+    public:
+        typedef const T& Arg; //!< Arg is used for passing the element to a function.
+};
+
+template <>
+class TI<double> {
+public:
+    typedef double Arg;
+};
+
+template <>
+class TI<int> {
+public:
+    typedef int Arg;
+};
+
+
+
+
+
+/** 
+ * /note linkage
+ * Something fishy about the difference between C++ and C
+ * in C++ const values default to INTERNAL linkage, in C they default
+ * to EXTERNAL linkage.  Here the constants should have EXTERNAL linkage
+ * because they, for at least some of them, can be changed by the user.
+ * If you want to explicitly declare internal linkage, use "static".
+ */
+//! 
+extern int          STREAMBUFFERSIZE;
+
+//! maximal length of a file name
+extern int          MAXLENFILENAME;
+
+//! the value of pi
+extern const double PI;
+
+//! the value pi/180
+extern const double deg2rad;
+
+//! the value 180/pi
+extern const double rad2deg;
+
+//! default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
+extern double     epsilon;
+
+//! power or 2 of epsilon
+extern double     epsilon2;
+
+//! the number of derivatives used in the RN-... objects.
+extern int          VSIZE;
+
+
+
+#ifndef _MFC_VER
+#undef max
+inline double max(double a,double b) {
+    if (b<a) 
+        return a;
+    else
+        return b;
+}
+
+#undef min
+inline double min(double a,double b) {
+    if (b<a) 
+        return b;
+    else
+        return a;
+}
+#endif
+
+
+#ifdef _MSC_VER
+    //#pragma inline_depth( 255 )
+    //#pragma inline_recursion( on )
+    #define INLINE __forceinline
+    //#define INLINE inline
+#else
+    #define INLINE inline
+#endif
+
+
+inline double LinComb(double alfa,double a,
+        double beta,double b ) {
+            return alfa*a+beta*b;
+}
+
+inline void LinCombR(double alfa,double a,
+        double beta,double b,double& result ) {
+            result=alfa*a+beta*b;
+         }
+
+//! to uniformly set double, RNDouble,Vector,... objects to zero in template-classes
+inline void SetToZero(double& arg) {
+    arg=0;
+}
+
+//! to uniformly set double, RNDouble,Vector,... objects to the identity element in template-classes
+inline void SetToIdentity(double& arg) {
+    arg=1;
+}
+
+inline double sign(double arg) {
+    return (arg<0)?(-1):(1);
+}
+
+inline double sqr(double arg) { return arg*arg;}
+inline double Norm(double arg) {
+    return fabs(  (double)arg );
+}
+
+
+#if defined(__WIN32__) && !defined(__GNUC__)
+inline double hypot(double y,double x) { return ::_hypot(y,x);}
+inline double abs(double x) { return ::fabs(x);}
+#endif
+
+// compares whether 2 doubles are equal in an eps-interval.
+// Does not check whether a or b represents numbers
+// On VC6, if a/b is -INF, it returns false;
+inline bool Equal(double a,double b,double eps=epsilon)
+{
+    double tmp=(a-b);
+    return ((eps>tmp)&& (tmp>-eps) );
+}
+
+inline void random(double& a) {
+	a = 1.98*rand()/(double)RAND_MAX -0.99;
+}
+
+inline void posrandom(double& a) {
+	a = 0.001+0.99*rand()/(double)RAND_MAX;
+}
+
+inline double diff(double a,double b,double dt) {
+	return (b-a)/dt;
+}
+//inline float diff(float a,float b,double dt) {
+//return (b-a)/dt;
+//}
+inline double addDelta(double a,double da,double dt) {
+	return a+da*dt;
+}
+
+//inline float addDelta(float a,float da,double dt) {
+//	return a+da*dt;
+//}
+
+
+}
+
+
+
+#endif

blender-5.2.0/intern/itasc/kdl/utilities/utility_io.cpp

@@ -0,0 +1,209 @@
+/** \file itasc/kdl/utilities/utility_io.cpp
+ * \ingroup intern_itasc
+ */
+/*****************************************************************************
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version 
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Name:  $ 
+ * \todo
+ *   make IO routines more robust against the differences between DOS/UNIX end-of-line style.
+ ****************************************************************************/
+
+
+#include "utility_io.h"
+#include "error.h"
+
+#include <stdlib.h>
+#include <ctype.h>
+#include <string.h>
+
+namespace KDL {
+
+//
+//  _functions are private functions 
+//
+
+    void _check_istream(std::istream& is)
+    {
+        if ((!is.good())&&(is.eof()) )
+            {
+            throw Error_BasicIO_File();
+            }
+    }
+// Eats until the end of the line
+static int _EatUntilEndOfLine( std::istream& is, int* countp=NULL) {
+    int ch;
+    int count;
+    count = 0;
+    do {
+        ch = is.get();
+        count++;
+        _check_istream(is);
+    } while (ch!='\n');
+    if (countp!=NULL) *countp = count;
+    return ch;
+}
+
+// Eats until the end of the comment
+static int _EatUntilEndOfComment( std::istream& is, int* countp=NULL) {
+    int ch;
+    int count;
+    count = 0;
+    int prevch;
+    ch = 0;
+    do {
+        prevch = ch;
+        ch = is.get();
+        count++;
+        _check_istream(is);
+        if ((prevch=='*')&&(ch=='/')) {
+            break;
+        }
+    } while (true);
+    if (countp!=NULL) *countp = count;
+    ch = is.get();
+    return ch;
+}
+
+// Eats space-like characters and comments
+// possibly returns the number of space-like characters eaten.
+static int _EatSpace( std::istream& is,int* countp=NULL) {
+    int ch;
+    int count;
+    count=-1;
+    do { 
+        _check_istream(is);
+
+        ch = is.get(); 
+        count++;
+        if (ch == '#') {
+            ch = _EatUntilEndOfLine(is,&count);
+        }
+        if (ch == '/') {
+            ch = is.get();
+            if (ch == '/') {
+                ch = _EatUntilEndOfLine(is,&count);
+            } else  if (ch == '*') {
+                ch = _EatUntilEndOfComment(is,&count);
+            } else {
+                is.putback(ch);
+                ch = '/';
+            }
+        }
+    } while ((ch==' ')||(ch=='\n')||(ch=='\t'));
+    if (countp!=NULL) *countp =  count;
+    return ch;
+}
+
+
+
+// Eats whites, returns, tabs and the delim character
+//  Checks whether delim char. is encountered.
+void Eat( std::istream& is, int delim )
+{   
+    int ch;
+    ch=_EatSpace(is);
+    if (ch != delim) {
+       throw Error_BasicIO_Exp_Delim();
+    }
+    ch=_EatSpace(is);   
+    is.putback(ch);
+}
+
+// Eats whites, returns, tabs and the delim character
+//  Checks whether delim char. is encountered.
+// EatEnd does not eat all space-like char's at the end.
+void EatEnd( std::istream& is, int delim )
+{   
+    int ch;
+    ch=_EatSpace(is);
+    if (ch != delim) {
+       throw Error_BasicIO_Exp_Delim();
+    }
+}
+
+
+
+// For each space in descript, this routine eats whites,tabs, and newlines (at least one)
+// There should be no consecutive spaces in the description.
+// for each letter in descript, its reads the corresponding letter in the output
+// the routine is case insensitive.
+
+
+// Simple routine, enough for our purposes.
+//  works with ASCII chars
+inline char Upper(char ch) 
+{
+    /*if (('a'<=ch)&&(ch<='z'))
+        return (ch-'a'+'A');
+    else
+        return ch;
+    */
+    return toupper(ch);
+}
+
+void Eat(std::istream& is,const char* descript)
+{
+    // eats whites before word
+    char ch;
+    char chdescr;
+    ch=_EatSpace(is);   
+    is.putback(ch);
+    const char* p;
+    p = descript;
+    while ((*p)!=0) {
+        chdescr = (char)Upper(*p);
+        if (chdescr==' ') {
+            int count=0;
+            ch=_EatSpace(is,&count);
+            is.putback(ch);
+            if (count==0) {
+                throw Error_BasicIO_Not_A_Space();
+            }
+        } else {
+            ch=(char)is.get();
+            if (chdescr!=Upper(ch)) {
+               throw Error_BasicIO_Unexpected();
+            }
+        }
+        p++;
+    }
+    
+}
+
+
+
+void EatWord(std::istream& is,const char* delim,char* storage,int maxsize)
+{
+    int ch;
+    char* p;
+    int size;
+    // eat white before word
+    ch=_EatSpace(is);
+    p = storage;
+    size=0;
+    int count = 0;
+    while ((count==0)&&(strchr(delim,ch)==NULL)) {
+        *p = (char) toupper(ch);
+        ++p;
+        if (size==maxsize) {
+           throw Error_BasicIO_ToBig();
+        }
+        _check_istream(is);
+        ++size;
+        //ch = is.get();
+        ch =_EatSpace(is,&count);
+    }
+    *p=0;
+    is.putback(ch);
+}
+
+
+}

blender-5.2.0/intern/itasc/kdl/utilities/utility_io.h

@@ -0,0 +1,77 @@
+/***************************************************************************** 
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version 
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Name:  $ 
+ *
+ * \file   utility_io.h
+ *     Included by most lrl-files to provide some general
+ *     functions and macro definitions related to file/stream I/O.
+ */
+
+#ifndef KDL_UTILITY_IO_H_84822
+#define KDL_UTILITY_IO_H_84822
+
+//#include <kdl/kdl-config.h>
+
+
+// Standard includes
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+
+
+namespace KDL {
+
+
+/**
+ * checks validity of basic io of is
+ */
+void _check_istream(std::istream& is);
+
+
+/** 
+ * Eats characters of the stream until the character delim is encountered
+ * @param is a stream
+ * @param delim eat until this character is encountered
+ */
+void Eat(std::istream& is, int delim );
+
+/** 
+ * Eats characters of the stream as long as they satisfy the description in descript
+ * @param is a stream
+ * @param descript description string. A sequence of spaces, tabs, 
+ *           new-lines and comments is regarded as 1 space in the description string.
+ */
+void Eat(std::istream& is,const char* descript);
+
+/**
+ * Eats a word of the stream delimited by the letters in delim or space(tabs...)
+ * @param is a stream
+ * @param delim a string containing the delimmiting characters
+ * @param storage for returning the word
+ * @param maxsize a word can be maximally maxsize-1 long.
+ */
+void EatWord(std::istream& is,const char* delim,char* storage,int maxsize);
+
+/** 
+ * Eats characters of the stream until the character delim is encountered
+ * similar to Eat(is,delim) but spaces at the end are not read.
+ * @param is a stream
+ * @param delim eat until this character is encountered
+ */
+void EatEnd( std::istream& is, int delim );
+
+
+
+
+}
+
+
+#endif