Add Chromium-only Blender WebEngine parity work

blender-5.2.0/intern/itasc/.clang-format

@@ -0,0 +1,2 @@
+DisableFormat: true
+SortIncludes: false

blender-5.2.0/intern/itasc/Armature.cpp

@@ -0,0 +1,778 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "Armature.hpp"
+#include <algorithm>
+#include <string.h>
+#include <stdlib.h>
+
+namespace iTaSC {
+
+#if 0
+// a joint constraint is characterized by 5 values: tolerance, K, alpha, yd, yddot
+static const unsigned int constraintCacheSize = 5;
+#endif
+std::string Armature::m_root = "root";
+
+Armature::Armature():
+	ControlledObject(),
+	m_tree(),
+	m_njoint(0),
+	m_nconstraint(0),
+	m_noutput(0),
+	m_neffector(0),
+	m_finalized(false),
+	m_cache(NULL),
+	m_buf(NULL),
+	m_qCCh(-1),
+	m_qCTs(0),
+	m_yCCh(-1),
+#if 0
+	m_yCTs(0),
+#endif
+	m_qKdl(),
+	m_oldqKdl(),
+	m_newqKdl(),
+	m_qdotKdl(),
+	m_jac(NULL),
+	m_armlength(0.0),
+	m_jacsolver(NULL),
+	m_fksolver(NULL)
+{
+}
+
+Armature::~Armature()
+{
+	if (m_jac)
+		delete m_jac;
+	if (m_jacsolver)
+		delete m_jacsolver;
+	if (m_fksolver)
+		delete m_fksolver;
+	for (JointConstraintList::iterator it=m_constraints.begin(); it != m_constraints.end(); it++) {
+		if (*it != NULL)
+			delete (*it);
+	}
+	if (m_buf)
+		delete [] m_buf;
+	m_constraints.clear();
+}
+
+Armature::JointConstraint_struct::JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep):
+	segment(_segment), value(), values(), function(_function), y_nr(_y_nr), param(_param), freeParam(_freeParam), substep(_substep)
+{
+	memset(values, 0, sizeof(values));
+	memset(value, 0, sizeof(value));
+	values[0].feedback = 20.0;
+	values[1].feedback = 20.0;
+	values[2].feedback = 20.0;
+	values[0].tolerance = 1.0;
+	values[1].tolerance = 1.0;
+	values[2].tolerance = 1.0;
+	values[0].values = &value[0];
+	values[1].values = &value[1];
+	values[2].values = &value[2];
+	values[0].number = 1;
+	values[1].number = 1;
+	values[2].number = 1;
+	switch (segment->second.segment.getJoint().getType()) {
+	case Joint::RotX:
+		value[0].id = ID_JOINT_RX;		
+		values[0].id = ID_JOINT_RX;		
+		v_nr = 1;
+		break;
+	case Joint::RotY:
+		value[0].id = ID_JOINT_RY;		
+		values[0].id = ID_JOINT_RY;		
+		v_nr = 1;
+		break;
+	case Joint::RotZ:
+		value[0].id = ID_JOINT_RZ;		
+		values[0].id = ID_JOINT_RZ;		
+		v_nr = 1;
+		break;
+	case Joint::TransX:
+		value[0].id = ID_JOINT_TX;		
+		values[0].id = ID_JOINT_TX;		
+		v_nr = 1;
+		break;
+	case Joint::TransY:
+		value[0].id = ID_JOINT_TY;		
+		values[0].id = ID_JOINT_TY;		
+		v_nr = 1;
+		break;
+	case Joint::TransZ:
+		value[0].id = ID_JOINT_TZ;		
+		values[0].id = ID_JOINT_TZ;		
+		v_nr = 1;
+		break;
+	case Joint::Sphere:
+		values[0].id = value[0].id = ID_JOINT_RX;		
+		values[1].id = value[1].id = ID_JOINT_RY;
+		values[2].id = value[2].id = ID_JOINT_RZ;		
+		v_nr = 3;
+		break;
+	case Joint::Swing:
+		values[0].id = value[0].id = ID_JOINT_RX;		
+		values[1].id = value[1].id = ID_JOINT_RZ;		
+		v_nr = 2;
+		break;
+	case Joint::None:
+		break;
+	}
+}
+
+Armature::JointConstraint_struct::~JointConstraint_struct()
+{
+	if (freeParam && param)
+		free(param);
+}
+
+void Armature::initCache(Cache *_cache)
+{
+	m_cache = _cache;
+	m_qCCh = -1;
+	m_yCCh = -1;
+	m_buf = NULL;
+	if (m_cache) {
+		// add a special channel for the joint
+		m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double));
+#if 0
+		// for the constraints, instead of creating many different channels, we will
+		// create a single channel for all the constraints
+		if (m_nconstraint) {
+			m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize*sizeof(double));
+			m_buf = new double[m_nconstraint*constraintCacheSize];
+		}
+		// store the initial cache position at timestamp 0
+		pushConstraints(0);
+#endif
+		pushQ(0);
+	}
+}
+
+void Armature::pushQ(CacheTS timestamp)
+{
+	if (m_qCCh >= 0) {
+		// try to keep the cache if the joints are the same
+		m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, m_qKdl(0), m_qKdl.rows(), KDL::epsilon);
+		m_qCTs = timestamp;
+	}
+}
+
+/* return true if a m_cache position was loaded */
+bool Armature::popQ(CacheTS timestamp)
+{
+	if (m_qCCh >= 0) {
+		double* item;
+		item = (double *)m_cache->getPreviousCacheItem(this, m_qCCh, &timestamp);
+		if (item && m_qCTs != timestamp) {
+			double* q = m_qKdl(0);
+			memcpy(q, item, m_qKdl.rows()*sizeof(double));
+			m_qCTs = timestamp;
+			// changing the joint => recompute the jacobian
+			updateJacobian();
+		}
+		return (item) ? true : false;
+	}
+	return true;
+}
+#if 0
+void Armature::pushConstraints(CacheTS timestamp)
+{
+	if (m_yCCh >= 0) {
+		double *buf = NULL;
+		if (m_nconstraint) {
+			double *item = m_buf;
+			for (unsigned int i=0; i<m_nconstraint; i++) {
+				JointConstraint_struct* pConstraint = m_constraints[i];
+				*item++ = pConstraint->values.feedback;
+				*item++ = pConstraint->values.tolerance;
+				*item++ = pConstraint->value.yd;
+				*item++ = pConstraint->value.yddot;
+				*item++ = pConstraint->values.alpha;
+			}
+		}
+		m_cache->addCacheVectorIfDifferent(this, m_yCCh, timestamp, m_buf, m_nconstraint*constraintCacheSize, KDL::epsilon);
+		m_yCTs = timestamp;
+	}
+}
+
+/* return true if a cache position was loaded */
+bool Armature::popConstraints(CacheTS timestamp)
+{
+	if (m_yCCh >= 0) {
+		double *item = (double*)m_cache->getPreviousCacheItem(this, m_yCCh, &timestamp);
+		if (item && m_yCTs != timestamp) {
+			for (unsigned int i=0; i<m_nconstraint; i++) {
+				JointConstraint_struct* pConstraint = m_constraints[i];
+				if (pConstraint->function != Joint1DOFLimitCallback) {
+					pConstraint->values.feedback = *item++;
+					pConstraint->values.tolerance = *item++;
+					pConstraint->value.yd = *item++;
+					pConstraint->value.yddot = *item++;
+					pConstraint->values.alpha = *item++;
+				} else {
+					item += constraintCacheSize;
+				}
+			}
+			m_yCTs = timestamp;
+		}
+		return (item) ? true : false;
+	}
+	return true;
+}
+#endif
+
+bool Armature::addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip, const Inertia& M)
+{
+	if (m_finalized)
+		return false;
+
+	Segment segment(joint, f_tip, M);
+	if (!m_tree.addSegment(segment, segment_name, hook_name))
+		return false;
+	int ndof = joint.getNDof();
+	for (int dof=0; dof<ndof; dof++) {
+		Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]);
+		m_joints.push_back(js);
+	}
+	m_njoint+=ndof;
+	return true;
+}
+
+bool Armature::getSegment(const std::string& name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip)
+{
+	SegmentMap::const_iterator sit = m_tree.getSegment(name);
+	if (sit == m_tree.getSegments().end())
+		return false;
+	p_joint = &sit->second.segment.getJoint();
+	if (q_size < p_joint->getNDof())
+		return false;
+	p_tip = &sit->second.segment.getFrameToTip();
+	for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) {
+		(&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest;
+		(&q)[dof] = m_qKdl[sit->second.q_nr+dof];
+	}
+	return true;
+}
+
+double Armature::getMaxJointChange()
+{
+	if (!m_finalized)
+		return 0.0;
+	double maxJoint = 0.0;
+	for (unsigned int i=0; i<m_njoint; i++) {
+		// this is a very rough calculation, it doesn't work well for spherical joint
+		double joint = fabs(m_oldqKdl[i]-m_qKdl[i]);
+		if (maxJoint < joint)
+			maxJoint = joint;
+	}
+	return maxJoint;
+}
+
+double Armature::getMaxEndEffectorChange()
+{
+	if (!m_finalized)
+		return 0.0;
+	double maxDelta = 0.0;
+	double delta;
+	Twist twist;
+	for (unsigned int i = 0; i<m_neffector; i++) {
+		twist = diff(m_effectors[i].pose, m_effectors[i].oldpose);
+		delta = twist.rot.Norm();
+		if (delta > maxDelta)
+			maxDelta = delta;
+		delta = twist.vel.Norm();
+		if (delta > maxDelta)
+			maxDelta = delta;
+	}
+	return maxDelta;
+}
+
+int Armature::addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep)
+{
+	SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
+	// not suitable for NDof joints
+	if (segment_it == m_tree.getSegments().end()) {
+		if (_freeParam && _param)
+			free(_param);
+		return -1;
+	}
+	JointConstraintList::iterator constraint_it;
+	JointConstraint_struct* pConstraint;
+	int iConstraint;
+	for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) {
+		pConstraint = *constraint_it;
+		if (pConstraint->segment == segment_it) {
+			// redefining a constraint
+			if (pConstraint->freeParam && pConstraint->param) {
+				free(pConstraint->param);
+			}
+			pConstraint->function = _function;
+			pConstraint->param = _param;
+			pConstraint->freeParam = _freeParam;
+			pConstraint->substep = _substep;
+			return iConstraint;
+		}
+	}
+	if (m_finalized) {
+		if (_freeParam && _param)
+			free(_param);
+		return -1;
+	}
+	// new constraint, append
+	pConstraint = new JointConstraint_struct(segment_it, m_noutput, _function, _param, _freeParam, _substep);
+	m_constraints.push_back(pConstraint);
+	m_noutput += pConstraint->v_nr;
+	return m_nconstraint++;
+}
+
+int Armature::addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max)
+{
+	SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
+	if (segment_it == m_tree.getSegments().end())
+		return -1;
+	const Joint& joint = segment_it->second.segment.getJoint();
+	if (joint.getNDof() != 1 && joint.getType() != Joint::Swing) {
+		// not suitable for Sphere joints
+		return -1;
+	}
+	if ((joint.getNDof() == 1 && dof > 0) || (joint.getNDof() == 2 && dof > 1))
+		return -1;
+	Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof];
+	p_joint.min = _min;
+	p_joint.max = _max;
+	p_joint.useLimit = true;
+	return 0;
+}
+
+int Armature::addEndEffector(const std::string& name)
+{
+	const SegmentMap& segments = m_tree.getSegments();
+	if (segments.find(name) == segments.end())
+		return -1;
+
+	EffectorList::const_iterator it;
+	int ee;
+	for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) {
+		if (it->name == name)
+			return ee;
+	}
+	if (m_finalized)
+		return -1;
+	Effector_struct effector(name);
+	m_effectors.push_back(effector);
+	return m_neffector++;
+}
+
+bool Armature::finalize()
+{
+	unsigned int i, j, c;
+	if (m_finalized)
+		return true;
+	if (m_njoint == 0)
+		return false;
+	initialize(m_njoint, m_noutput, m_neffector);
+	for (i=c=0; i<m_nconstraint; i++) {
+		JointConstraint_struct* pConstraint = m_constraints[i];
+		for (j=0; j<pConstraint->v_nr; j++, c++) {
+			m_Cq(c,pConstraint->segment->second.q_nr+j) = 1.0;
+			m_Wy(c) = pConstraint->values[j].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
+		}
+	}
+	m_jacsolver= new KDL::TreeJntToJacSolver(m_tree);
+	m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree);
+	m_jac = new Jacobian(m_njoint);
+	m_qKdl.resize(m_njoint);
+	m_oldqKdl.resize(m_njoint);
+	m_newqKdl.resize(m_njoint);
+	m_qdotKdl.resize(m_njoint);
+	for (i=0; i<m_njoint; i++) {
+		m_newqKdl[i] = m_oldqKdl[i] = m_qKdl[i] = m_joints[i].rest;
+	}
+	updateJacobian();
+	// estimate the maximum size of the robot arms
+	double length;
+	m_armlength = 0.0;
+	for (i=0; i<m_neffector; i++) {
+		length = 0.0;
+		KDL::SegmentMap::value_type const *sit = m_tree.getSegmentPtr(m_effectors[i].name);
+		while (sit->first != "root") {
+			Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
+			length += tip.p.Norm();
+			sit = sit->second.parent;
+		}
+		if (length > m_armlength)
+			m_armlength = length;
+	}
+	if (m_armlength < KDL::epsilon)
+		m_armlength = KDL::epsilon;
+	m_finalized = true;
+	return true;
+}
+
+void Armature::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache) {
+		pushQ(timestamp.cacheTimestamp);
+		//pushConstraints(timestamp.cacheTimestamp);
+	}
+}
+
+bool Armature::setJointArray(const KDL::JntArray& joints)
+{
+	if (!m_finalized)
+		return false;
+	if (joints.rows() != m_qKdl.rows())
+		return false;
+	m_qKdl = joints;
+	updateJacobian();
+	return true;
+}
+
+const KDL::JntArray& Armature::getJointArray()
+{
+	return m_qKdl;
+}
+
+bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback)
+{
+	if (!m_finalized)
+		return false;
+	
+	// integration and joint limit
+	// for spherical joint we must use a more sophisticated method
+	unsigned int q_nr;
+	double* qdot=m_qdotKdl(0);
+	double* q=m_qKdl(0);
+	double* newq=m_newqKdl(0);
+	double norm, qx, qz, CX, CZ, sx, sz;
+	bool locked = false;
+	int unlocked = 0;
+
+	for (q_nr=0; q_nr<m_nq; ++q_nr)
+		qdot[q_nr]=m_qdot[q_nr];
+
+	for (q_nr=0; q_nr<m_nq; ) {
+		Joint_struct* joint = &m_joints[q_nr];
+		if (!joint->locked) {
+			switch (joint->type) {
+			case KDL::Joint::Swing:
+			{
+				KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1]));
+				(base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq);
+				if (joint[0].useLimit) {
+					if (joint[1].useLimit) {
+						// elliptical limit
+						sx = sz = 1.0;
+						qx = newq[0];
+						qz = newq[1];
+						// determine in which quadrant we are
+						if (qx > 0.0 && qz > 0.0) {
+							CX = joint[0].max;
+							CZ = joint[1].max;
+						} else if (qx <= 0.0 && qz > 0.0) {
+							CX = -joint[0].min;
+							CZ = joint[1].max;
+							qx = -qx;
+							sx = -1.0;
+						} else if (qx <= 0.0 && qz <= 0.0) {
+							CX = -joint[0].min;
+							CZ = -joint[1].min;
+							qx = -qx;
+							qz = -qz;
+							sx = sz = -1.0;
+						} else {
+							CX = joint[0].max;
+							CZ = -joint[0].min;
+							qz = -qz;
+							sz = -1.0;
+						}
+						if (CX < KDL::epsilon || CZ < KDL::epsilon) {
+							// quadrant is degenerated
+							if (qx > CX) {
+								newq[0] = CX*sx;
+								joint[0].locked = true;
+							}
+							if (qz > CZ) {
+								newq[1] = CZ*sz;
+								joint[0].locked = true;
+							}
+						} else {
+							// general case
+							qx /= CX;
+							qz /= CZ;
+							norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz));
+							if (norm > 1.0) {
+								norm = 1.0/norm;
+								newq[0] = qx*norm*CX*sx;
+								newq[1] = qz*norm*CZ*sz;
+								joint[0].locked = true;
+							}
+						}
+					} else {
+						// limit on X only
+						qx = newq[0];
+						if (qx > joint[0].max) {
+							newq[0] = joint[0].max;
+							joint[0].locked = true;
+						} else if (qx < joint[0].min) {
+							newq[0] = joint[0].min;
+							joint[0].locked = true;
+						}
+					}
+				} else if (joint[1].useLimit) {
+					// limit on Z only
+					qz = newq[1];
+					if (qz > joint[1].max) {
+						newq[1] = joint[1].max;
+						joint[0].locked = true;
+					} else if (qz < joint[1].min) {
+						newq[1] = joint[1].min;
+						joint[0].locked = true;
+					}
+				}
+				if (joint[0].locked) {
+					// check the difference from previous position
+					locked = true;
+					norm = KDL::sqr(newq[0]-q[0])+KDL::sqr(newq[1]-q[1]);
+					if (norm < KDL::epsilon2) {
+						// joint didn't move, no need to update the jacobian
+						callback.lockJoint(q_nr, 2);
+					} else {
+						// joint moved, compute the corresponding velocity
+						double deltaq[2];
+						(base.Inverse()*KDL::Rot(KDL::Vector(newq[0],0.0,newq[1]))).GetXZRot().GetValue(deltaq);
+						deltaq[0] /= timestamp.realTimestep;
+						deltaq[1] /= timestamp.realTimestep;
+						callback.lockJoint(q_nr, 2, deltaq);
+						// no need to update the other joints, it will be done after next rerun
+						goto end_loop;
+					}
+				} else
+					unlocked++;
+				break;
+			}
+			case KDL::Joint::Sphere:
+			{
+				(KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq);
+				// no limit on this joint
+				unlocked++;
+				break;
+			}
+			default:
+				for (unsigned int i=0; i<joint->ndof; i++) {
+					newq[i] = q[i]+qdot[i]*timestamp.realTimestep;
+					if (joint[i].useLimit) {
+						if (newq[i] > joint[i].max) {
+							newq[i] = joint[i].max;
+							joint[0].locked = true;
+						} else if (newq[i] < joint[i].min) {
+							newq[i] = joint[i].min;
+							joint[0].locked = true;
+						}
+					}
+				}
+				if (joint[0].locked) {
+					locked = true;
+					norm = 0.0;
+					// compute delta to locked position
+					for (unsigned int i=0; i<joint->ndof; i++) {
+						qdot[i] = newq[i] - q[i];
+						norm += qdot[i]*qdot[i];
+					}
+					if (norm < KDL::epsilon2) {
+						// joint didn't move, no need to update the jacobian
+						callback.lockJoint(q_nr, joint->ndof);
+					} else {
+						// solver needs velocity, compute equivalent velocity
+						for (unsigned int i=0; i<joint->ndof; i++) {
+							qdot[i] /= timestamp.realTimestep;
+						}
+						callback.lockJoint(q_nr, joint->ndof, qdot);
+						goto end_loop;
+					}
+				} else 
+					unlocked++;
+			}
+		}
+		qdot += joint->ndof;
+		q    += joint->ndof;
+		newq += joint->ndof;
+		q_nr += joint->ndof;
+	}
+end_loop:
+	// check if there any other unlocked joint
+	for ( ; q_nr<m_nq; ) {
+		Joint_struct* joint = &m_joints[q_nr];
+		if (!joint->locked)
+			unlocked++;
+		q_nr += joint->ndof;
+	}
+	// if all joints have been locked no need to run the solver again
+	return (unlocked) ? locked : false;
+}
+
+void Armature::updateKinematics(const Timestamp& timestamp){
+
+    //Integrate m_qdot
+	if (!m_finalized)
+		return;
+
+	// the new joint value have been computed already, just copy
+	memcpy(m_qKdl(0), m_newqKdl(0), sizeof(double)*m_qKdl.rows());
+	pushCache(timestamp);
+	updateJacobian();
+	// here update the desired output.
+	// We assume constant desired output for the joint limit constraint, no need to update it.
+}
+
+void Armature::updateJacobian()
+{
+    //calculate pose and jacobian
+	for (unsigned int ee=0; ee<m_nee; ee++) {
+		m_fksolver->JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root);
+		m_jacsolver->JntToJac(m_qKdl,*m_jac,m_effectors[ee].name);
+		// get the jacobian for the base point, to prepare transformation to world reference
+		changeRefPoint(*m_jac,-m_effectors[ee].pose.p,*m_jac);
+		//copy to Jq:
+		e_matrix& Jq = m_JqArray[ee];
+		for(unsigned int i=0;i<6;i++) {
+			for(unsigned int j=0;j<m_nq;j++)
+				Jq(i,j)=(*m_jac)(i,j);
+		}
+	}
+	// remember that this object has moved 
+	m_updated = true;
+}
+
+const Frame& Armature::getPose(const unsigned int ee)
+{
+	if (!m_finalized)
+		return F_identity;
+	return (ee >= m_nee) ? F_identity : m_effectors[ee].pose;
+}
+
+bool Armature::getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name)
+{
+	if (!m_finalized)
+		return false;
+	return (m_fksolver->JntToCart(m_qKdl,result,segment_name,base_name) < 0) ? false : true;
+}
+
+void Armature::updateControlOutput(const Timestamp& timestamp)
+{
+	if (!m_finalized)
+		return;
+
+
+	if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) {
+		popQ(timestamp.cacheTimestamp);
+		//popConstraints(timestamp.cacheTimestamp);
+	}
+
+	if (!timestamp.substep) {
+		// save previous joint state for getMaxJointChange()
+		memcpy(m_oldqKdl(0), m_qKdl(0), sizeof(double)*m_qKdl.rows());
+		for (unsigned int i=0; i<m_neffector; i++) {
+			m_effectors[i].oldpose = m_effectors[i].pose;
+		}
+	}
+
+	// remove all joint lock
+	for (JointList::iterator jit=m_joints.begin(); jit!=m_joints.end(); ++jit) {
+		(*jit).locked = false;
+	}
+
+	JointConstraintList::iterator it;
+	unsigned int iConstraint;
+
+	// scan through the constraints and call the callback functions
+	for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) {
+		JointConstraint_struct* pConstraint = *it;
+		unsigned int nr, i;
+		for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) {
+			*(double *)&pConstraint->value[i].y = m_qKdl[nr];
+			*(double *)&pConstraint->value[i].ydot = m_qdotKdl[nr];
+		}
+		if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) {
+			(*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param);
+		}
+		// recompute the weight in any case, that's the most likely modification
+		for (i=0, nr=pConstraint->y_nr; i<pConstraint->v_nr; i++, nr++) {
+			m_Wy(nr) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
+			m_ydot(nr)=pConstraint->value[i].yddot+pConstraint->values[i].feedback*(pConstraint->value[i].yd-pConstraint->value[i].y);
+		}
+	}
+}
+
+bool Armature::setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep)
+{
+	unsigned int lastid, i;
+	if (constraintId == CONSTRAINT_ID_ALL) {
+		constraintId = 0;
+		lastid = m_nconstraint;
+	} else if (constraintId < m_nconstraint) {
+		lastid = constraintId+1;
+	} else {
+		return false;
+	}
+	for ( ; constraintId<lastid; ++constraintId) {
+		JointConstraint_struct* pConstraint = m_constraints[constraintId];
+		if (valueId == ID_JOINT) {
+			for (i=0; i<pConstraint->v_nr; i++) {
+				switch (action) {
+				case ACT_TOLERANCE:
+					pConstraint->values[i].tolerance = value;
+					break;
+				case ACT_FEEDBACK:
+					pConstraint->values[i].feedback = value;
+					break;
+				case ACT_ALPHA:
+					pConstraint->values[i].alpha = value;
+					break;
+				default:
+					break;
+				}
+			}
+		} else {
+			for (i=0; i<pConstraint->v_nr; i++) {
+				if (valueId == pConstraint->value[i].id) {
+					switch (action) {
+					case ACT_VALUE:
+						pConstraint->value[i].yd = value;
+						break;
+					case ACT_VELOCITY:
+						pConstraint->value[i].yddot = value;
+						break;
+					case ACT_TOLERANCE:
+						pConstraint->values[i].tolerance = value;
+						break;
+					case ACT_FEEDBACK:
+						pConstraint->values[i].feedback = value;
+						break;
+					case ACT_ALPHA:
+						pConstraint->values[i].alpha = value;
+						break;
+					case ACT_NONE:
+						break;
+					}
+				}
+			}
+		}
+		if (m_finalized) {
+			for (i=0; i<pConstraint->v_nr; i++) 
+				m_Wy(pConstraint->y_nr+i) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
+		}
+	}
+	return true;
+}
+
+}
+

blender-5.2.0/intern/itasc/Armature.hpp

@@ -0,0 +1,140 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef ARMATURE_HPP_
+#define ARMATURE_HPP_
+
+#include "ControlledObject.hpp"
+#include "ConstraintSet.hpp"
+#include "kdl/treejnttojacsolver.hpp"
+#include "kdl/treefksolverpos_recursive.hpp"
+#include <vector>
+
+namespace iTaSC {
+
+class Armature: public iTaSC::ControlledObject {
+public:
+    Armature();
+    virtual ~Armature();
+
+	bool addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip=F_identity, const Inertia& M = Inertia::Zero());
+	// general purpose constraint on joint
+	int addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param=NULL, bool _freeParam=false, bool _substep=false);
+	// specific limit constraint on joint
+	int addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max);
+	double getMaxJointChange();
+	double getMaxEndEffectorChange();
+	bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip);
+	bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root);
+
+	virtual bool finalize();
+
+	virtual int addEndEffector(const std::string& name);
+	virtual const Frame& getPose(const unsigned int end_effector);
+	virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback);
+    virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+    virtual void updateControlOutput(const Timestamp& timestamp);
+	virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0);
+	virtual void initCache(Cache *_cache);
+	virtual bool setJointArray(const KDL::JntArray& joints);
+	virtual const KDL::JntArray& getJointArray();
+
+	virtual double getArmLength()
+	{
+		return m_armlength;
+	}
+
+	struct Effector_struct {
+		std::string name;
+		Frame oldpose;
+		Frame pose;
+		Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;}
+	};
+	typedef std::vector<Effector_struct> EffectorList;
+
+	enum ID  {
+		ID_JOINT=1,
+		ID_JOINT_RX=2,
+		ID_JOINT_RY=3,
+		ID_JOINT_RZ=4,
+		ID_JOINT_TX=2,
+		ID_JOINT_TY=3,
+		ID_JOINT_TZ=4,
+	};
+	struct JointConstraint_struct {
+		SegmentMap::const_iterator segment;
+		ConstraintSingleValue value[3];
+		ConstraintValues values[3];
+		ConstraintCallback function;
+		unsigned int v_nr;
+		unsigned int y_nr;	// first coordinate of constraint in Y vector
+		void* param;
+		bool freeParam;
+		bool substep;
+		JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep);
+		~JointConstraint_struct();
+	};
+	typedef std::vector<JointConstraint_struct*> JointConstraintList;	
+
+	struct Joint_struct {
+		KDL::Joint::JointType	type;
+		unsigned short			ndof;
+		bool					useLimit;
+		bool					locked;
+		double					rest;
+		double					min;
+		double					max;
+
+		Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) :
+			type(_type), ndof(_ndof), rest(_rest)  { useLimit=locked=false; min=0.0; max=0.0; }
+	};
+	typedef std::vector<Joint_struct> JointList;
+	
+protected:
+    virtual void updateJacobian();
+
+private:
+	static std::string m_root;
+    Tree m_tree;
+	unsigned int m_njoint;
+	unsigned int m_nconstraint;
+	unsigned int m_noutput;
+	unsigned int m_neffector;
+	bool m_finalized;
+	Cache* m_cache;
+	double *m_buf;
+	int m_qCCh;
+	CacheTS m_qCTs;
+	int m_yCCh;
+#if 0
+	CacheTS m_yCTs;
+#endif
+    JntArray m_qKdl;
+    JntArray m_oldqKdl;
+    JntArray m_newqKdl;
+    JntArray m_qdotKdl;
+    Jacobian* m_jac;
+	double m_armlength;
+
+	KDL::TreeJntToJacSolver* m_jacsolver;
+	KDL::TreeFkSolverPos_recursive* m_fksolver;
+	EffectorList m_effectors;
+	JointConstraintList m_constraints;
+	JointList m_joints;
+
+	void pushQ(CacheTS timestamp);
+	bool popQ(CacheTS timestamp);
+	//void pushConstraints(CacheTS timestamp);
+	//bool popConstraints(CacheTS timestamp);
+
+};
+
+}
+
+#endif /* ARMATURE_HPP_ */

blender-5.2.0/intern/itasc/CMakeLists.txt

@@ -0,0 +1,106 @@
+# SPDX-FileCopyrightText: 2006 Blender Authors
+#
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+remove_strict_flags()
+set(INC
+
+)
+
+set(INC_SYS
+)
+
+set(SRC
+  Armature.cpp
+  Cache.cpp
+  ConstraintSet.cpp
+  ControlledObject.cpp
+  CopyPose.cpp
+  Distance.cpp
+  FixedObject.cpp
+  MovingFrame.cpp
+  Scene.cpp
+  UncontrolledObject.cpp
+  WDLSSolver.cpp
+  WSDLSSolver.cpp
+  WorldObject.cpp
+  eigen_types.cpp
+
+  kdl/chain.cpp
+  kdl/chainfksolverpos_recursive.cpp
+  kdl/chainjnttojacsolver.cpp
+  kdl/frameacc.cpp
+  kdl/frames.cpp
+  kdl/frames_io.cpp
+  kdl/framevel.cpp
+  kdl/inertia.cpp
+  kdl/jacobian.cpp
+  kdl/jntarray.cpp
+  kdl/jntarrayacc.cpp
+  kdl/jntarrayvel.cpp
+  kdl/joint.cpp
+  kdl/kinfam_io.cpp
+  kdl/segment.cpp
+  kdl/tree.cpp
+  kdl/treefksolverpos_recursive.cpp
+  kdl/treejnttojacsolver.cpp
+  kdl/utilities/error_stack.cpp
+  kdl/utilities/utility.cpp
+  kdl/utilities/utility_io.cpp
+
+  Armature.hpp
+  Cache.hpp
+  ConstraintSet.hpp
+  ControlledObject.hpp
+  CopyPose.hpp
+  Distance.hpp
+  FixedObject.hpp
+  MovingFrame.hpp
+  Object.hpp
+  Scene.hpp
+  Solver.hpp
+  UncontrolledObject.hpp
+  WDLSSolver.hpp
+  WSDLSSolver.hpp
+  WorldObject.hpp
+  eigen_types.hpp
+  kdl/chain.hpp
+  kdl/chainfksolver.hpp
+  kdl/chainfksolverpos_recursive.hpp
+  kdl/chainjnttojacsolver.hpp
+  kdl/frameacc.hpp
+  kdl/frames.hpp
+  kdl/frames_io.hpp
+  kdl/framevel.hpp
+  kdl/inertia.hpp
+  kdl/jacobian.hpp
+  kdl/jntarray.hpp
+  kdl/jntarrayacc.hpp
+  kdl/jntarrayvel.hpp
+  kdl/joint.hpp
+  kdl/kinfam_io.hpp
+  kdl/segment.hpp
+  kdl/tree.hpp
+  kdl/treefksolver.hpp
+  kdl/treefksolverpos_recursive.hpp
+  kdl/treejnttojacsolver.hpp
+  kdl/utilities/error.h
+  kdl/utilities/error_stack.h
+  kdl/utilities/kdl-config.h
+  kdl/utilities/rall1d.h
+  kdl/utilities/rall2d.h
+  kdl/utilities/svd_eigen_HH.hpp
+  kdl/utilities/traits.h
+  kdl/utilities/utility.h
+  kdl/utilities/utility_io.h
+
+  kdl/frameacc.inl
+  kdl/frames.inl
+  kdl/framevel.inl
+)
+
+set(LIB
+  PUBLIC bf::dependencies::eigen
+)
+
+blender_add_lib(bf_intern_itasc "${SRC}" "${INC}" "${INC_SYS}" "${LIB}")

blender-5.2.0/intern/itasc/Cache.cpp

@@ -0,0 +1,624 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include <string.h>
+#include <assert.h>
+#include <math.h>
+#include <stdlib.h>
+#include "Cache.hpp"
+
+namespace iTaSC {
+
+CacheEntry::~CacheEntry()
+{
+   for (unsigned int id=0; id < m_count; id++)
+		m_channelArray[id].clear();
+   if (m_channelArray)
+	   free(m_channelArray);
+}
+
+CacheItem *CacheChannel::_findBlock(CacheBuffer *buffer, unsigned short timeOffset, unsigned int *retBlock)
+{
+	// the timestamp is necessarily in this buffer
+	unsigned int lowBlock, highBlock, midBlock;
+	if (timeOffset <= buffer->lookup[0].m_timeOffset) {
+		// special case: the item is in the first block, search from start
+		*retBlock = 0;
+		return &buffer->m_firstItem;
+	}
+	// general case, the item is in the middle of the buffer
+	// before doing a dycotomic search, we will assume that timestamp
+	// are regularly spaced so that we can try to locate the block directly
+	highBlock = buffer->m_lastItemPositionW>>m_positionToBlockShiftW;
+	lowBlock = midBlock = (timeOffset*highBlock)/(buffer->m_lastTimestamp-buffer->m_firstTimestamp);
+	// give some space for security
+	if (lowBlock > 0)
+		lowBlock--;
+	if (timeOffset <= buffer->lookup[lowBlock].m_timeOffset) {
+		// bad guess, but we know this block is a good high block, just use it
+		highBlock = lowBlock;
+		lowBlock = 0;
+	} else {
+		// ok, good guess, now check the high block, give some space
+		if (midBlock < highBlock)
+			midBlock++;
+		if (timeOffset <= buffer->lookup[midBlock].m_timeOffset) {
+			// good guess, keep that block as the high block
+			highBlock = midBlock;
+		}
+	}
+	// the item is in a different block, do a dycotomic search
+	// the timestamp is alway > lowBlock and <= highBlock
+	while (1) {
+		midBlock = (lowBlock+highBlock)/2;
+		if (midBlock == lowBlock) {
+			// low block and high block are contigous, we can start search from the low block
+			break;
+		} else if (timeOffset <= buffer->lookup[midBlock].m_timeOffset) {
+			highBlock = midBlock;
+		} else {
+			lowBlock = midBlock;
+		}
+	}
+	assert (lowBlock != highBlock);
+	*retBlock = highBlock;
+	return CACHE_BLOCK_ITEM_ADDR(this,buffer,lowBlock);
+}
+
+void CacheChannel::clear()
+{
+	CacheBuffer *buffer, *next;
+	for (buffer=m_firstBuffer; buffer != 0; buffer = next) {
+		next = buffer->m_next;
+		free(buffer);
+	}
+	m_firstBuffer = NULL;
+	m_lastBuffer = NULL;
+	if (initItem) {
+		free(initItem);
+		initItem = NULL;
+	}
+}
+
+CacheBuffer* CacheChannel::allocBuffer()
+{
+	CacheBuffer* buffer;
+	if (!m_busy)
+		return NULL;
+	buffer = (CacheBuffer*)malloc(CACHE_BUFFER_HEADER_SIZE+(m_bufferSizeW<<2));
+	if (buffer) {
+		memset(buffer, 0, CACHE_BUFFER_HEADER_SIZE);
+	}
+	return buffer;
+}
+
+CacheItem* CacheChannel::findItemOrLater(unsigned int timestamp, CacheBuffer **rBuffer)
+{
+	CacheBuffer* buffer;
+	CacheItem *item, *limit;
+	if (!m_busy)
+		return NULL;
+	if (timestamp == 0 && initItem) {
+		*rBuffer = NULL;
+		return initItem;
+	}
+	for (buffer=m_firstBuffer; buffer; buffer = buffer->m_next) {
+		if (buffer->m_firstFreePositionW == 0)
+			// buffer is empty, this must be the last and we didn't find the timestamp
+			return NULL;
+		if (timestamp < buffer->m_firstTimestamp) {
+			*rBuffer = buffer;
+			return &buffer->m_firstItem;
+		}
+		if (timestamp <= buffer->m_lastTimestamp) {
+			// the timestamp is necessarily in this buffer
+			unsigned short timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp);
+			unsigned int highBlock;
+			item = _findBlock(buffer, timeOffset, &highBlock);
+			// now we do a linear search until we find a timestamp that is equal or higher
+			// we should normally always find an item but let's put a limit just in case
+			limit = CACHE_BLOCK_ITEM_ADDR(this,buffer,highBlock);
+			while (item<=limit && item->m_timeOffset < timeOffset )
+				item = CACHE_NEXT_ITEM(item);
+			assert(item<=limit);
+			*rBuffer = buffer;
+			return item;
+		}
+		// search in next buffer
+	}
+	return NULL;
+}
+
+CacheItem* CacheChannel::findItemEarlier(unsigned int timestamp, CacheBuffer **rBuffer)
+{
+	CacheBuffer *buffer, *prevBuffer;
+	CacheItem *item, *limit, *prevItem;
+	if (!m_busy)
+		return NULL;
+	if (timestamp == 0)
+		return NULL;
+	for (prevBuffer=NULL, buffer=m_firstBuffer; buffer; prevBuffer = buffer, buffer = buffer->m_next) {
+		if (buffer->m_firstFreePositionW == 0)
+			// buffer is empty, this must be the last and we didn't find the timestamp
+			return NULL;
+		if (timestamp <= buffer->m_firstTimestamp) {
+			if (prevBuffer == NULL) {
+				// no item before, except the initial item
+				*rBuffer = NULL;
+				return initItem;
+			}
+			// the item is necessarily the last one of previous buffer
+			*rBuffer = prevBuffer;
+			return CACHE_ITEM_ADDR(prevBuffer,prevBuffer->m_lastItemPositionW);
+		}
+		if (timestamp <= buffer->m_lastTimestamp) {
+			// the timestamp is necessarily in this buffer
+			unsigned short timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp);
+			unsigned int highBlock;
+			item = _findBlock(buffer, timeOffset, &highBlock);
+			// now we do a linear search until we find a timestamp that is equal or higher
+			// we should normally always find an item but let's put a limit just in case
+			limit = CACHE_BLOCK_ITEM_ADDR(this,buffer,highBlock);
+			prevItem = NULL;
+			while (item<=limit && item->m_timeOffset < timeOffset) {
+				prevItem = item;
+				item = CACHE_NEXT_ITEM(item);
+			}
+			assert(item<=limit && prevItem!=NULL);
+			*rBuffer = buffer;
+			return prevItem;
+		}
+		// search in next buffer
+	}
+	// pass all buffer, the last item is the last item of the last buffer
+	if (prevBuffer == NULL) {
+		// no item before, except the initial item
+		*rBuffer = NULL;
+		return initItem;
+	}
+	// the item is necessarily the last one of previous buffer
+	*rBuffer = prevBuffer;
+	return CACHE_ITEM_ADDR(prevBuffer,prevBuffer->m_lastItemPositionW);
+}
+
+
+Cache::Cache()
+{
+}
+
+Cache::~Cache()
+{
+	CacheMap::iterator it;
+	for (it=m_cache.begin(); it!=m_cache.end(); it=m_cache.begin()) {
+		deleteDevice(it->first);
+	}
+}
+
+int Cache::addChannel(const void *device, const char *name, unsigned int maxItemSize)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+	CacheChannel *channel;
+	unsigned int id;
+
+	if (maxItemSize > 0x3FFF0)
+		return -1;
+
+	if (it == m_cache.end()) {
+		// device does not exist yet, create a new entry
+		entry = new CacheEntry();
+		if (entry == NULL)
+			return -1;
+		if (!m_cache.insert(CacheMap::value_type(device,entry)).second) {
+			delete entry;
+			return -1;
+		}
+	} else {
+		entry = it->second;
+	}
+	// locate a channel with the same name and reuse
+	for (channel=entry->m_channelArray, id=0; id<entry->m_count; id++, channel++) {
+		if (channel->m_busy && !strcmp(name, channel->m_name)) {
+			// make this channel free again
+			deleteChannel(device, id);
+			// there can only be one channel with the same name
+			break;
+		}
+	}
+	for (channel=entry->m_channelArray, id=0; id<entry->m_count; id++, channel++) {
+		// locate a free channel
+		if (!channel->m_busy)
+			break;
+	}
+	if (id == entry->m_count) {
+		// no channel free, create new channels
+		int newcount = entry->m_count + CACHE_CHANNEL_EXTEND_SIZE;
+		channel = (CacheChannel*)realloc(entry->m_channelArray, newcount*sizeof(CacheChannel));
+		if (channel == NULL)
+			return -1;
+		entry->m_channelArray = channel;
+		memset(&entry->m_channelArray[entry->m_count], 0, CACHE_CHANNEL_EXTEND_SIZE*sizeof(CacheChannel));
+		entry->m_count = newcount;
+		channel = &entry->m_channelArray[id];
+	}
+	// compute the optimal buffer size
+	// The buffer size must be selected so that
+	// - it does not contain more than 1630 items (=1s of cache assuming 25 items per second)
+	// - it contains at least one item
+	// - it's not bigger than 256kb and preferably around 32kb
+	// - it a multiple of 4
+	unsigned int bufSize = 1630*(maxItemSize+4);
+	if (bufSize >= CACHE_DEFAULT_BUFFER_SIZE)
+		bufSize = CACHE_DEFAULT_BUFFER_SIZE;
+	if (bufSize < maxItemSize+16)
+		bufSize = maxItemSize+16;
+	bufSize = (bufSize + 3) & ~0x3;
+	// compute block size and offset bit mask
+	// the block size is computed so that
+	// - it is a power of 2
+	// - there is at least one item per block
+	// - there is no more than CACHE_LOOKUP_TABLE_SIZE blocks per buffer
+	unsigned int blockSize = bufSize/CACHE_LOOKUP_TABLE_SIZE;
+	if (blockSize < maxItemSize+12)
+		blockSize = maxItemSize+12;
+	// find the power of 2 that is immediately larger than blockSize
+	unsigned int m;
+	unsigned int pwr2Size = blockSize;
+	while ((m = (pwr2Size & (pwr2Size-1))) != 0)
+		pwr2Size = m;
+	blockSize = (pwr2Size < blockSize) ? pwr2Size<<1 : pwr2Size;
+	// convert byte size to word size because all positions and size are expressed in 32 bit words
+	blockSize >>= 2;
+	channel->m_blockSizeW = blockSize;
+	channel->m_bufferSizeW = bufSize>>2;
+	channel->m_firstBuffer = NULL;
+	channel->m_lastBuffer = NULL;
+	channel->m_busy = 1;
+	channel->initItem = NULL;
+	channel->m_maxItemSizeB = maxItemSize;
+	strncpy(channel->m_name, name, sizeof(channel->m_name));
+	channel->m_name[sizeof(channel->m_name)-1] = 0;
+	channel->m_positionToOffsetMaskW = (blockSize-1);
+	for (m=0; blockSize!=1; m++, blockSize>>=1);
+	channel->m_positionToBlockShiftW = m;
+	return (int)id;
+}
+
+int Cache::deleteChannel(const void *device, int id)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return -1;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int)entry->m_count || !entry->m_channelArray[id].m_busy)
+		return -1;
+	entry->m_channelArray[id].clear();
+	entry->m_channelArray[id].m_busy = 0;
+	return 0;
+}
+
+int Cache::deleteDevice(const void *device)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return -1;
+	}
+	entry = it->second;
+	delete entry;
+	m_cache.erase(it);
+	return 0;
+}
+
+void Cache::clearCacheFrom(const void *device, CacheTS timestamp)
+{
+	CacheMap::iterator it = (device) ? m_cache.find(device) : m_cache.begin();
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer, *nextBuffer, *prevBuffer;
+	CacheItem *item, *prevItem, *nextItem;
+	unsigned int positionW, block;
+
+	while (it != m_cache.end()) {
+		entry = it->second;
+		for (unsigned int ch=0; ch<entry->m_count; ch++) {
+			channel = &entry->m_channelArray[ch];
+			if (channel->m_busy) {
+				item = channel->findItemOrLater(timestamp, &buffer);
+				if (item ) {
+					if (!buffer) {
+						// this is possible if we return the special timestamp=0 item, delete all buffers
+						channel->clear();
+					} else {
+						// this item and all later items will be removed, clear any later buffer
+						while ((nextBuffer = buffer->m_next) != NULL) {
+							buffer->m_next = nextBuffer->m_next;
+							free(nextBuffer);
+						}
+						positionW = CACHE_ITEM_POSITIONW(buffer,item);
+						if (positionW == 0) {
+							// this item is the first one of the buffer, remove the buffer completely
+							// first find the buffer just before it
+							nextBuffer = channel->m_firstBuffer;
+							prevBuffer = NULL;
+							while (nextBuffer != buffer) {
+								prevBuffer = nextBuffer;
+								nextBuffer = nextBuffer->m_next;
+								// we must quit this loop before reaching the end of the list
+								assert(nextBuffer);
+							}
+							free(buffer);
+							buffer = prevBuffer;
+							if (buffer == NULL)
+								// this was also the first buffer
+								channel->m_firstBuffer = NULL;
+						} else {
+							// removing this item means finding the previous item to make it the last one
+							block = positionW>>channel->m_positionToBlockShiftW;
+							if (block == 0) {
+								// start from first item, we know it is not our item because positionW > 0
+								prevItem = &buffer->m_firstItem;
+							} else {
+								// no need to check the current block, it will point to our item or a later one
+								// but the previous block will be a good start for sure.
+								block--;
+								prevItem = CACHE_BLOCK_ITEM_ADDR(channel,buffer,block);
+							}
+							while ((nextItem = CACHE_NEXT_ITEM(prevItem)) < item)
+								prevItem = nextItem;
+							// we must have found our item
+							assert(nextItem==item);
+							// now set the buffer
+							buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,prevItem);
+							buffer->m_firstFreePositionW = positionW;
+							buffer->m_lastTimestamp = buffer->m_firstTimestamp + prevItem->m_timeOffset;
+							block = buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW;
+							buffer->lookup[block].m_offsetW = buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW;
+							buffer->lookup[block].m_timeOffset = prevItem->m_timeOffset;
+						}
+						// set the channel
+						channel->m_lastBuffer = buffer;
+						if (buffer) {
+							channel->m_lastTimestamp = buffer->m_lastTimestamp;
+							channel->m_lastItemPositionW = buffer->m_lastItemPositionW;
+						}
+					}
+				}
+			}
+		}
+		if (device)
+			break;
+		++it;
+	}
+}
+
+void *Cache::addCacheItem(const void *device, int id, unsigned int timestamp, void *data, unsigned int length)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer, *next;
+	CacheItem *item;
+	unsigned int positionW, sizeW, block;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return NULL;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy)
+		return NULL;
+	channel = &entry->m_channelArray[id];
+	if (length > channel->m_maxItemSizeB)
+		return NULL;
+	if (timestamp == 0) {
+		// initial item, delete all buffers
+		channel->clear();
+		// and create initial item
+		item = NULL;
+		// we will allocate the memory, which is always pointer aligned => compute size
+		// with NULL will give same result.
+		sizeW = CACHE_ITEM_SIZEW(item,length);
+		item = (CacheItem*)calloc(sizeW, 4);
+		item->m_sizeW = sizeW;
+		channel->initItem = item;
+	} else {
+		if (!channel->m_lastBuffer) {
+			// no item in buffer, insert item at first position of first buffer
+			positionW = 0;
+			if ((buffer = channel->m_firstBuffer) == NULL) {
+				buffer = channel->allocBuffer();
+				channel->m_firstBuffer = buffer;
+			}
+		} else if (timestamp > channel->m_lastTimestamp) {
+			// this is the normal case: we are writing past lastest timestamp
+			buffer = channel->m_lastBuffer;
+			positionW = buffer->m_firstFreePositionW;
+		} else if (timestamp == channel->m_lastTimestamp) {
+			// common case, rewriting the last timestamp, just reuse the last position
+			buffer = channel->m_lastBuffer;
+			positionW = channel->m_lastItemPositionW;
+		} else {
+			// general case, write in the middle of the buffer, locate the timestamp
+			// (or the timestamp just after), clear this item and all future items,
+			// and write at that position
+			item = channel->findItemOrLater(timestamp, &buffer);
+			if (item == NULL) {
+				// this should not happen
+				return NULL;
+			}
+			// this item will become the last one of this channel, clear any later buffer
+			while ((next = buffer->m_next) != NULL) {
+				buffer->m_next = next->m_next;
+				free(next);
+			}
+			// no need to update the buffer, this will be done when the item is written
+			positionW = CACHE_ITEM_POSITIONW(buffer,item);
+		}
+		item = CACHE_ITEM_ADDR(buffer,positionW);
+		sizeW = CACHE_ITEM_SIZEW(item,length);
+		// we have positionW pointing where we can put the item
+		// before we do that we have to check if we can:
+		// - enough room
+		// - timestamp not too late
+		if ((positionW+sizeW > channel->m_bufferSizeW) ||
+			(positionW > 0 && timestamp >= buffer->m_firstTimestamp+0x10000)) {
+			// we must allocate a new buffer to store this item
+			// but before we must make sure that the current buffer is consistent
+			if (positionW != buffer->m_firstFreePositionW) {
+				// This means that we were trying to write in the middle of the buffer.
+				// We must set the buffer right with positionW being the last position
+				// and find the item before positionW to make it the last.
+				block = positionW>>channel->m_positionToBlockShiftW;
+				CacheItem *previousItem, *nextItem;
+				if (block == 0) {
+					// start from first item, we know it is not our item because positionW > 0
+					previousItem = &buffer->m_firstItem;
+				} else {
+					// no need to check the current block, it will point to our item or a later one
+					// but the previous block will be a good start for sure.
+					block--;
+					previousItem = CACHE_BLOCK_ITEM_ADDR(channel,buffer,block);
+				}
+				while ((nextItem = CACHE_NEXT_ITEM(previousItem)) < item)
+					previousItem = nextItem;
+				// we must have found our item
+				assert(nextItem==item);
+				// now set the buffer
+				buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,previousItem);
+				buffer->m_firstFreePositionW = positionW;
+				buffer->m_lastTimestamp = buffer->m_firstTimestamp + previousItem->m_timeOffset;
+				block = buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW;
+				buffer->lookup[block].m_offsetW = buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW;
+				buffer->lookup[block].m_timeOffset = previousItem->m_timeOffset;
+				// and also the channel, just in case
+				channel->m_lastBuffer = buffer;
+				channel->m_lastTimestamp = buffer->m_lastTimestamp;
+				channel->m_lastItemPositionW = buffer->m_lastItemPositionW;
+			}
+			// now allocate a new buffer
+			buffer->m_next = channel->allocBuffer();
+			if (buffer->m_next == NULL)
+				return NULL;
+			buffer = buffer->m_next;
+			positionW = 0;
+			item = &buffer->m_firstItem;
+			sizeW = CACHE_ITEM_SIZEW(item,length);
+		}
+		// all check passed, ready to write the item
+		item->m_sizeW = sizeW;
+		if (positionW == 0) {
+			item->m_timeOffset = 0;
+			buffer->m_firstTimestamp = timestamp;
+		} else {
+			item->m_timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp);
+		}
+		buffer->m_lastItemPositionW = positionW;
+		buffer->m_firstFreePositionW = positionW+sizeW;
+		buffer->m_lastTimestamp = timestamp;
+		block = positionW>>channel->m_positionToBlockShiftW;
+		buffer->lookup[block].m_offsetW = positionW&channel->m_positionToOffsetMaskW;
+		buffer->lookup[block].m_timeOffset = item->m_timeOffset;
+		buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,item);
+		buffer->m_firstFreePositionW = buffer->m_lastItemPositionW+item->m_sizeW;
+		channel->m_lastBuffer = buffer;
+		channel->m_lastItemPositionW = positionW;
+		channel->m_lastTimestamp = timestamp;
+	}
+	// now copy the item
+	void *itemData = CACHE_ITEM_DATA_POINTER(item);
+	if (data)
+		memcpy(itemData, data, length);
+	return itemData;
+}
+
+const void *Cache::getPreviousCacheItem(const void *device, int id, unsigned int *timestamp)
+{
+	CacheMap::iterator it;
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer;
+	CacheItem *item;
+
+	if (device) {
+		it = m_cache.find(device);	
+	} else {
+		it = m_cache.begin();
+	}
+	if (it == m_cache.end()) {
+		// device does not exist
+		return NULL;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy)
+		return NULL;
+	channel = &entry->m_channelArray[id];
+	if ((item = channel->findItemEarlier(*timestamp,&buffer)) == NULL)
+		return NULL;
+	*timestamp = (buffer) ? buffer->m_firstTimestamp+item->m_timeOffset : 0;
+	return CACHE_ITEM_DATA_POINTER(item);
+}
+
+const CacheItem *Cache::getCurrentCacheItemInternal(const void *device, int id, CacheTS timestamp)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer;
+	CacheItem *item;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return NULL;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy)
+		return NULL;
+	channel = &entry->m_channelArray[id];
+	if ((item = channel->findItemOrLater(timestamp,&buffer)) == NULL)
+		return NULL;
+	if (buffer && buffer->m_firstTimestamp+item->m_timeOffset != timestamp)
+		return NULL;
+	return item;
+}
+
+const void *Cache::getCurrentCacheItem(const void *device, int channel, unsigned int timestamp)
+{
+	const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp);
+	return (item) ? CACHE_ITEM_DATA_POINTER(item) : NULL;
+}
+
+double *Cache::addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *newdata, unsigned int length, double threshold)
+{
+	const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp);
+	unsigned int sizeW = CACHE_ITEM_SIZEW(item,length*sizeof(double));
+	if (!item || item->m_sizeW != sizeW)
+		return (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double));
+	double *olddata = (double*)CACHE_ITEM_DATA_POINTER(item);
+	if (!length)
+		return olddata;
+	double *ref = olddata;
+	double *v = newdata;
+	unsigned int i;
+	for (i=length; i>0; --i) {
+		if (fabs(*v-*ref) > threshold)
+			break;
+		*ref++ = *v++;
+	}
+	if (i) 
+		olddata = (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double));
+	return olddata;
+}
+
+}

blender-5.2.0/intern/itasc/Cache.hpp

@@ -0,0 +1,228 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef CACHE_HPP_
+#define CACHE_HPP_
+
+#include <map>
+
+namespace iTaSC {
+
+#define CACHE_LOOKUP_TABLE_SIZE				128
+#define CACHE_DEFAULT_BUFFER_SIZE			32768
+#define CACHE_CHANNEL_EXTEND_SIZE			10
+#define CACHE_MAX_ITEM_SIZE					0x3FFF0
+
+/* macro to get the alignment gap after an item header */
+#define CACHE_ITEM_GAPB(item)				(unsigned int)(((size_t)item+sizeof(CacheItem))&(sizeof(void*)-1))
+/* macro to get item data position, item=CacheItem pointer */
+#define CACHE_ITEM_DATA_POINTER(item)		(void*)((unsigned char*)item+sizeof(CacheItem)+CACHE_ITEM_GAPB(item))
+/* macro to get item size in 32bit words from item address and length, item=CacheItem pointer */
+#define CACHE_ITEM_SIZEW(item,length)		(unsigned int)((sizeof(CacheItem)+CACHE_ITEM_GAPB(item)+(((length)+3)&~0x3))>>2)
+/* macto to move from one item to the next, item=CacheItem pointer, updated by the macro */
+#define CACHE_NEXT_ITEM(item)				((item)+(item)->m_sizeW)
+#define CACHE_BLOCK_ITEM_ADDR(chan,buf,block)	(&(buf)->m_firstItem+(((unsigned int)(block)<<chan->m_positionToBlockShiftW)+(buf)->lookup[block].m_offsetW))
+#define CACHE_ITEM_ADDR(buf,pos)			(&(buf)->m_firstItem+(pos))
+#define CACHE_ITEM_POSITIONW(buf,item)		(unsigned int)(item-&buf->m_firstItem)
+
+typedef unsigned int CacheTS;
+
+struct Timestamp 
+{
+	double realTimestamp;
+	double realTimestep;
+	CacheTS cacheTimestamp;
+	unsigned int numstep:8;
+	unsigned int substep:1;
+	unsigned int reiterate:1;
+	unsigned int cache:1;
+	unsigned int update:1;
+	unsigned int interpolate:1;
+	unsigned int dummy:19;
+
+	Timestamp() { memset(this, 0, sizeof(Timestamp)); }
+};
+
+/* utility function to return second timestamp to millisecond */
+inline void setCacheTimestamp(Timestamp& timestamp)
+{
+	if (timestamp.realTimestamp < 0.0 || timestamp.realTimestamp > 4294967.295)
+		timestamp.cacheTimestamp = 0;
+	else
+		timestamp.cacheTimestamp = (CacheTS)(timestamp.realTimestamp*1000.0+0.5);
+}
+
+
+/*
+class Cache:
+Top level class, only one instance of this class should exists.
+A device (=constraint, object) uses this class to create a cache entry for its data.
+A cache entry is divided into cache channels, each providing a separate buffer for cache items.
+The cache channels must be declared by the devices before they can be used.
+The device must specify the largest cache item (limited to 256Kb) so that the cache
+buffer can be organized optimally.
+Cache channels are identified by small number (starting from 0) allocated by the cache system.
+Cache items are inserted into cache channels ordered by timestamp. Writing is always done
+at the end of the cache buffer: writing an item automatically clears all items with 
+higher timestamp.
+A cache item is an array of bytes provided by the device; the format of the cache item is left 
+to the device. 
+The device can retrieve a cache item associated with a certain timestamp. The cache system
+returns a pointer that points directly in the cache buffer to avoid unnecessary copy. 
+The pointer is guaranteed to be pointer aligned so that direct mapping to C structure is possible 
+(=32 bit aligned on 32 systems and 64 bit aligned on 64 bits system).
+
+Timestamp = rounded time in millisecond.
+*/
+
+struct CacheEntry;
+struct CacheBuffer;
+struct CacheItem;
+struct CacheChannel;
+
+class Cache 
+{
+private:
+	/* map between device and cache entry.
+	   Dynamically updated when more devices create cache channels */
+	typedef std::map<const void *, struct CacheEntry*> CacheMap;
+	CacheMap  m_cache;
+	const CacheItem *getCurrentCacheItemInternal(const void *device, int channel, CacheTS timestamp);
+   
+public:
+	Cache();
+	~Cache();
+	/* add a cache channel, maxItemSize must be < 256k.
+	   name : channel name, truncated at 31 characters
+	   msxItemSize : maximum size of item in bytes, items of larger size will be rejected
+	   return value >= 0: channel id, -1: error */
+	int addChannel(const void *device, const char *name, unsigned int maxItemSize);
+
+	/* delete a cache channel (and all associated buffers and items) */
+	int deleteChannel(const void *device, int channel);
+	/* delete all channels of a device and remove the device from the map */
+	int deleteDevice(const void *device);
+	/* removes all cache items, leaving the special item at timestamp=0. 
+	   if device=NULL, apply to all devices. */
+	void clearCacheFrom(const void *device, CacheTS timestamp);
+
+	/* add a new cache item
+	   channel: the cache channel (as returned by AddChannel
+	   data, length: the cache item and length in bytes
+	                 If data is NULL, the memory is allocated in the cache but not writen
+	   return: error: NULL, success: pointer to item in cache */
+	void *addCacheItem(const void *device, int channel, CacheTS timestamp, void *data, unsigned int length);
+
+	/* specialized function to add a vector of double in the cache
+	   It will first check if a vector exist already in the cache for the same timestamp
+	   and compared the cached vector with the new values. 
+	   If all values are within threshold, the vector is updated but the cache is not deleted
+	   for the future timestamps. */
+	double *addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold);
+
+	/* returns the cache item with timestamp that is just before the given timestamp.
+	   returns the data pointer or NULL if there is no cache item before timestamp.
+	   On return, timestamp is updated with the actual timestamp of the item being returned.
+	   Note that the length of the item is not returned, it is up to the device to organize
+	   the data so that length can be retrieved from the data if needed.
+	   Device can NULL, it will then just look the first channel available, useful to 
+	   test the status of the cache. */
+	const void *getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp);
+
+	/* returns the cache item with the timestamp that is exactly equal to the given timestamp
+	   If there is no cache item for this timestamp, returns NULL.*/
+	const void *getCurrentCacheItem(const void *device, int channel, CacheTS timestamp);
+
+};
+
+/* the following structures are not internal use only, they should not be used directly */
+
+struct CacheEntry 
+{
+	CacheChannel *m_channelArray;		// array of channels, automatically resized if more channels are created
+	unsigned int m_count;				// number of channel in channelArray
+	CacheEntry() : m_channelArray(NULL), m_count(0) {}
+	~CacheEntry();
+};
+
+struct CacheChannel
+{
+	CacheItem* initItem;				// item corresponding to timestamp=0
+	struct CacheBuffer *m_firstBuffer;	// first buffer of list
+	struct CacheBuffer *m_lastBuffer;		// last buffer of list to which an item was written
+	char m_name[32];						// channel name
+	unsigned char m_busy;					// =0 if channel is free, !=0 when channel is in use
+	unsigned char m_positionToBlockShiftW;	// number of bits to shift a position in word to get the block number
+	unsigned short m_positionToOffsetMaskW;   // bit mask to apply on a position in word to get offset in a block
+	unsigned int m_maxItemSizeB;			// maximum item size in bytes
+	unsigned int m_bufferSizeW;			// size of item buffer in word to allocate when a new buffer must be created
+	unsigned int m_blockSizeW;			// block size in words of the lookup table
+	unsigned int m_lastTimestamp;			// timestamp of the last item that was written
+	unsigned int m_lastItemPositionW;		// position in words in lastBuffer of the last item written
+	void clear();
+	CacheBuffer* allocBuffer();
+	CacheItem* findItemOrLater(unsigned int timestamp, CacheBuffer **rBuffer);
+	CacheItem* findItemEarlier(unsigned int timestamp, CacheBuffer **rBuffer);
+	// Internal function: finds an item in a buffer that is < timeOffset
+	// timeOffset must be a valid offset for the buffer and the buffer must not be empty
+	// on return highBlock contains the block with items above or equal to timeOffset
+	CacheItem *_findBlock(CacheBuffer *buffer, unsigned short timeOffset, unsigned int *highBlock);
+};
+
+struct CacheBlock {
+	unsigned short m_timeOffset;		// timestamp relative to m_firstTimestamp
+	unsigned short m_offsetW;			// position in words of item relative to start of block
+};
+
+/* CacheItem is the header of each item in the buffer, must be 32bit
+   Items are always 32 bits aligned and size is the number of 32 bit words until the
+   next item header, including an eventual pre and post padding gap for pointer alignment */
+struct CacheItem
+{
+	unsigned short m_timeOffset;		// timestamp relative to m_firstTimestamp
+	unsigned short m_sizeW;			// size of item in 32 bit words
+	// item data follows header immediately or after a gap if position is not pointer aligned
+};
+
+// Buffer header
+// Defined in a macro to avoid sizeof() potential problem.
+//	next				for linked list. = NULL for last buffer
+//  m_firstTimestamp		timestamp of first item in this buffer
+//  m_lastTimestamp		timestamp of last item in this buffer
+//						m_lastTimestamp must be < m_firstTimestamp+65536
+//  m_lastItemPositionW	position in word of last item written
+//  m_firstFreePositionW	position in word where a new item can be written, 0 if buffer is empty
+//  lookup				lookup table for fast access to item by timestamp
+//						The buffer is divided in blocks of 2**n bytes with n chosen so that 
+//						there are no more than CACHE_LOOKUP_TABLE_SIZE blocks and that each 
+//						block will contain at least one item. 
+//						Each element of the lookup table gives the timestamp and offset 
+//						of the last cache item occupying (=starting in) the corresponding block.
+#define CACHE_HEADER \
+	struct CacheBuffer *m_next;		\
+	unsigned int m_firstTimestamp;	\
+	unsigned int m_lastTimestamp;		\
+									\
+	unsigned int m_lastItemPositionW;	\
+	unsigned int m_firstFreePositionW;\
+	struct CacheBlock lookup[CACHE_LOOKUP_TABLE_SIZE]
+
+struct CacheBufferHeader {
+	CACHE_HEADER;
+};
+#define CACHE_BUFFER_HEADER_SIZE	(sizeof(struct CacheBufferHeader))
+struct CacheBuffer
+{
+	CACHE_HEADER;
+	struct CacheItem m_firstItem;			// the address of this field marks the start of the buffer
+};
+
+
+}
+
+#endif /* CACHE_HPP_ */

blender-5.2.0/intern/itasc/ConstraintSet.cpp

@@ -0,0 +1,174 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "ConstraintSet.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+
+namespace iTaSC {
+
+ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations):
+    m_nc(_nc),
+    m_Cf(e_zero_matrix(m_nc,6)),
+    m_Wy(e_scalar_vector(m_nc,1.0)),
+    m_y(m_nc),m_ydot(e_zero_vector(m_nc)),m_chi(e_zero_vector(6)),
+    m_S(6),m_temp(6),m_tdelta(6),
+    m_Jf(e_identity_matrix(6,6)),
+    m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)),
+    m_Jf_inv(e_zero_matrix(6,6)),
+	m_internalPose(F_identity), m_externalPose(F_identity),
+	m_constraintCallback(NULL), m_constraintParam(NULL), 
+	m_toggle(false),m_substep(false),
+    m_threshold(accuracy),m_maxIter(maximum_iterations)
+{
+	m_maxDeltaChi = e_scalar(0.52);
+}
+
+ConstraintSet::ConstraintSet():
+    m_nc(0), 
+	m_internalPose(F_identity), m_externalPose(F_identity),
+	m_constraintCallback(NULL), m_constraintParam(NULL), 
+	m_toggle(false),m_substep(false),
+	m_threshold(0.0),m_maxIter(0)
+{
+	m_maxDeltaChi = e_scalar(0.52);
+}
+
+void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations)
+{
+    m_nc = _nc;
+    m_Jf = e_identity_matrix(6,6);
+    m_Cf = e_zero_matrix(m_nc,6);
+    m_U = e_identity_matrix(6,6);
+	m_V = e_identity_matrix(6,6);
+	m_B = e_zero_matrix(6,6);
+    m_Jf_inv = e_zero_matrix(6,6),
+    m_Wy = e_scalar_vector(m_nc,1.0),
+    m_chi = e_zero_vector(6);
+    m_chidot = e_zero_vector(6);
+	m_y = e_zero_vector(m_nc);
+	m_ydot = e_zero_vector(m_nc);
+	m_S = e_zero_vector(6);
+	m_temp = e_zero_vector(6);
+	m_tdelta = e_zero_vector(6);
+    m_threshold = accuracy;
+	m_maxIter = maximum_iterations;
+}
+
+ConstraintSet::~ConstraintSet() {
+
+}
+
+void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
+{
+    m_chi+=m_chidot*timestamp.realTimestep;
+	m_externalPose = _external_pose;
+
+    //update the internal pose and Jf
+    updateJacobian();
+	//check if loop is already closed, if not update the pose and Jf
+    unsigned int iter=0;
+    while(iter<5&&!closeLoop())
+        iter++;
+}
+
+double ConstraintSet::getMaxTimestep(double& timestep)
+{
+	e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
+	if (timestep*maxChidot > m_maxDeltaChi) {
+		timestep = m_maxDeltaChi/maxChidot;
+	}
+	return timestep;
+}
+
+bool ConstraintSet::initialise(Frame& init_pose){
+    m_externalPose=init_pose;
+	// get current Jf
+    updateJacobian();
+
+    unsigned int iter=0;
+    while(iter<m_maxIter&&!closeLoop()){
+        iter++;
+    }
+    if (iter<m_maxIter)
+        return true;
+    else
+        return false;
+}
+
+bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
+{
+	ConstraintValues values;
+	ConstraintSingleValue value;
+	values.values = &value;
+	values.number = 0;
+	values.action = action;
+	values.id = id;
+	value.action = action;
+	value.id = id;
+	switch (action) {
+	case ACT_NONE:
+		return true;
+	case ACT_VALUE:
+		value.yd = data;
+		values.number = 1;
+		break;
+	case ACT_VELOCITY:
+		value.yddot = data;
+		values.number = 1;
+		break;
+	case ACT_TOLERANCE:
+		values.tolerance = data;
+		break;
+	case ACT_FEEDBACK:
+		values.feedback = data;
+		break;
+	case ACT_ALPHA:
+		values.alpha = data;
+		break;
+	default:
+		assert(action==ACT_NONE);
+		break;
+	}
+	return setControlParameters(&values, 1, timestep);
+}
+
+bool ConstraintSet::closeLoop(){
+    //Invert Jf
+    //TODO: svd_boost_Macie has problems if Jf contains zero-rows
+    //toggle=!toggle;
+    //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
+	int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp);
+	if(ret<0)
+		return false;
+
+	// the reference point and frame of the jacobian is the base frame
+	// m_externalPose-m_internalPose is the twist to extend the end effector
+	// to get the required pose => change the reference point to the base frame
+	Twist twist_delta(diff(m_internalPose,m_externalPose));
+	twist_delta=twist_delta.RefPoint(-m_internalPose.p);
+	for(unsigned int i=0;i<6;i++)
+		m_tdelta(i)=twist_delta(i);
+	//TODO: use damping in constraintset inversion?
+	for(unsigned int i=0;i<6;i++) {
+		if(m_S(i)<m_threshold){
+			m_B.row(i).setConstant(0.0);
+		} else {
+			m_B.row(i) = m_U.col(i)/m_S(i);
+		}
+	}
+
+	m_Jf_inv.noalias()=m_V*m_B;
+
+	m_chi.noalias()+=m_Jf_inv*m_tdelta;
+	updateJacobian();
+	// m_externalPose-m_internalPose in end effector frame
+	// this is just to compare the pose, a different formula would work too
+	return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold);
+
+}
+}

blender-5.2.0/intern/itasc/ConstraintSet.hpp

@@ -0,0 +1,116 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef CONSTRAINTSET_HPP_
+#define CONSTRAINTSET_HPP_
+
+#include "kdl/frames.hpp"
+#include "eigen_types.hpp"
+#include "Cache.hpp"
+#include <vector>
+
+namespace iTaSC {
+
+enum ConstraintAction {
+	ACT_NONE=		0,
+	ACT_VALUE=		1,
+	ACT_VELOCITY=	2,
+	ACT_TOLERANCE=	4,
+	ACT_FEEDBACK=	8,
+	ACT_ALPHA=		16
+};
+
+struct ConstraintSingleValue {
+	unsigned int id;	// identifier of constraint value, depends on constraint
+	unsigned int action;// action performed, compbination of ACT_..., set on return
+	const double y;		// actual constraint value
+	const double ydot;	// actual constraint velocity
+	double yd;			// current desired constraint value, changed on return
+	double yddot;		// current desired constraint velocity, changed on return
+	ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {}
+};
+
+struct ConstraintValues {
+	unsigned int id;	// identifier of group of constraint values, depend on constraint
+	unsigned short number;		// number of constraints in list
+	unsigned short action;		// action performed, ACT_..., set on return
+	double alpha;		// constraint activation coefficient, should be [0..1]
+	double tolerance;	// current desired tolerance on constraint, same unit than yd, changed on return
+	double feedback;	// current desired feedback on error, in 1/sec, changed on return
+	struct ConstraintSingleValue* values;
+	ConstraintValues(): id(0), number(0), action(0), values(NULL) {}
+};
+
+class ConstraintSet;
+typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
+
+class ConstraintSet {
+protected:
+    unsigned int m_nc;
+	e_scalar m_maxDeltaChi;
+	e_matrix m_Cf;
+    e_vector m_Wy,m_y,m_ydot;
+	e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta;
+    e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv;
+	KDL::Frame m_internalPose,m_externalPose;
+    ConstraintCallback m_constraintCallback;
+    void* m_constraintParam;
+	void* m_poseParam;
+    bool m_toggle;
+	bool m_substep;
+    double m_threshold;
+    unsigned int m_maxIter;
+
+	friend class Scene;
+	virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp);
+    virtual void updateKinematics(const Timestamp& timestamp)=0;
+    virtual void pushCache(const Timestamp& timestamp)=0;
+    virtual void updateJacobian()=0;
+    virtual void updateControlOutput(const Timestamp& timestamp)=0;
+	virtual void initCache(Cache *_cache) = 0;
+	virtual bool initialise(KDL::Frame& init_pose);
+	virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
+    virtual bool closeLoop();
+	virtual double getMaxTimestep(double& timestep);
+
+
+public:
+    ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations);
+    ConstraintSet();
+    virtual ~ConstraintSet();
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	virtual bool registerCallback(ConstraintCallback _function, void* _param)
+	{
+		m_constraintCallback = _function;
+		m_constraintParam = _param;
+		return true;
+	}
+
+    virtual const e_vector& getControlOutput()const{return m_ydot;};
+	virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0;
+	virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0;
+	bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0);
+
+    virtual const e_matrix6& getJf() const{return m_Jf;};
+	virtual const KDL::Frame& getPose() const{return m_internalPose;};
+    virtual const e_matrix& getCf() const{return m_Cf;};
+
+    virtual const e_vector& getWy() const {return m_Wy;};
+    virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;};
+    virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
+
+    virtual unsigned int getNrOfConstraints(){return m_nc;};
+	void substep(bool _substep) {m_substep=_substep;}
+	bool substep() {return m_substep;}
+};
+
+}
+
+#endif /* CONSTRAINTSET_HPP_ */

blender-5.2.0/intern/itasc/ControlledObject.cpp

@@ -0,0 +1,62 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "ControlledObject.hpp"
+
+
+namespace iTaSC {
+ControlledObject::ControlledObject():
+    Object(Controlled),m_nq(0),m_nc(0),m_nee(0)
+{
+	// max joint variable = 0.52 radian or 0.52 meter in one timestep
+	m_maxDeltaQ = e_scalar(0.52);
+}
+
+void ControlledObject::initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee)
+{
+	assert(_nee >= 1);
+	m_nq = _nq;
+	m_nc = _nc;
+	m_nee = _nee;
+	if (m_nq > 0) {
+		m_Wq =  e_identity_matrix(m_nq,m_nq);
+		m_qdot = e_zero_vector(m_nq);
+	}
+	if (m_nc > 0) {
+		m_Wy = e_scalar_vector(m_nc,1.0);
+		m_ydot = e_zero_vector(m_nc);
+	}
+	if (m_nc > 0 && m_nq > 0)
+		m_Cq = e_zero_matrix(m_nc,m_nq);
+	// clear all Jacobian if any
+	m_JqArray.clear();
+	// reserve one more to have a zero matrix handy
+	if (m_nq > 0)
+		m_JqArray.resize(m_nee+1, e_zero_matrix(6,m_nq));
+}
+
+ControlledObject::~ControlledObject() {}
+
+
+
+const e_matrix& ControlledObject::getJq(unsigned int ee) const
+{
+	assert(m_nq > 0);
+	return m_JqArray[(ee>m_nee)?m_nee:ee];
+}
+
+double ControlledObject::getMaxTimestep(double& timestep)
+{
+	e_scalar maxQdot = m_qdot.array().abs().maxCoeff();
+	if (timestep*maxQdot > m_maxDeltaQ) {
+		timestep = m_maxDeltaQ/maxQdot;
+	}
+	return timestep;
+}
+
+}

blender-5.2.0/intern/itasc/ControlledObject.hpp

@@ -0,0 +1,71 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef CONTROLLEDOBJECT_HPP_
+#define CONTROLLEDOBJECT_HPP_
+
+#include "kdl/frames.hpp"
+#include "eigen_types.hpp"
+
+#include "Object.hpp"
+#include "ConstraintSet.hpp"
+#include <vector>
+
+namespace iTaSC {
+
+#define CONSTRAINT_ID_ALL	((unsigned int)-1)
+
+class ControlledObject : public Object {
+protected:
+	e_scalar m_maxDeltaQ;
+    unsigned int m_nq,m_nc,m_nee;
+    e_matrix m_Wq,m_Cq;
+    e_vector m_Wy,m_ydot,m_qdot;
+	std::vector<e_matrix> m_JqArray;
+public:
+    ControlledObject();
+    virtual ~ControlledObject();
+
+	class JointLockCallback {
+	public:
+		JointLockCallback() {}
+		virtual ~JointLockCallback() {}
+
+		// lock a joint, no need to update output
+		virtual void lockJoint(unsigned int q_nr, unsigned int ndof) = 0;
+		// lock a joint and update output in view of reiteration
+		virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot) = 0;
+	};
+
+	virtual void initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee);
+
+	// returns true when a joint has been locked via the callback and the solver must run again
+	virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback) = 0;
+	virtual void updateControlOutput(const Timestamp& timestamp)=0;
+    virtual void setJointVelocity(const e_vector qdot_in){m_qdot = qdot_in;};
+	virtual double getMaxTimestep(double& timestep);
+	virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, e_scalar value, double timestep=0.0)=0;
+
+    virtual const e_vector& getControlOutput() const{return m_ydot;}
+
+	virtual const e_matrix& getJq(unsigned int ee) const;
+
+    virtual const e_matrix& getCq() const{return m_Cq;};
+
+    virtual e_matrix& getWq() {return m_Wq;};
+    virtual void setWq(const e_matrix& Wq_in){m_Wq = Wq_in;};
+
+    virtual const e_vector& getWy() const {return m_Wy;};
+
+    virtual unsigned int getNrOfCoordinates(){return m_nq;};
+    virtual unsigned int getNrOfConstraints(){return m_nc;};
+};
+
+}
+
+#endif /* CONTROLLEDOBJECT_HPP_ */

blender-5.2.0/intern/itasc/CopyPose.cpp

@@ -0,0 +1,481 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "CopyPose.hpp"
+#include "kdl/kinfam_io.hpp"
+#include <math.h>
+#include <string.h>
+
+namespace iTaSC
+{
+
+const unsigned int maxPoseCacheSize = (2*(3+3*2));
+CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations):
+    ConstraintSet(),
+	m_cache(NULL),
+    m_poseCCh(-1),m_poseCTs(0)
+{
+	m_maxerror = armlength/2.0;
+	m_outputControl = (control_output & CTL_ALL);
+	unsigned int _nc = nBitsOn(m_outputControl);
+	if (!_nc) 
+		return;
+	// reset the constraint set
+	reset(_nc, accuracy, maximum_iterations);
+	_nc = 0;
+	m_nvalues = 0;
+	int nrot = 0, npos = 0;
+	int nposCache = 0, nrotCache = 0;
+	m_outputDynamic = (dynamic_output & m_outputControl);
+	memset(m_values, 0, sizeof(m_values));
+	memset(m_posData, 0, sizeof(m_posData));
+	memset(m_rotData, 0, sizeof(m_rotData));
+	memset(&m_rot, 0, sizeof(m_rot));
+	memset(&m_pos, 0, sizeof(m_pos));
+	if (m_outputControl & CTL_POSITION) {
+		m_pos.alpha = 1.0;		
+		m_pos.K = 20.0;		
+		m_pos.tolerance = 0.05;	
+		m_values[m_nvalues].alpha = m_pos.alpha;
+		m_values[m_nvalues].feedback = m_pos.K;
+		m_values[m_nvalues].tolerance = m_pos.tolerance;
+		m_values[m_nvalues].id = ID_POSITION;
+		if (m_outputControl & CTL_POSITIONX) {
+		    m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
+			m_Cf(_nc++,0)=1.0;
+			m_posData[npos++].id = ID_POSITIONX;
+			if (m_outputDynamic & CTL_POSITIONX)
+				nposCache++;
+		} 
+		if (m_outputControl & CTL_POSITIONY) {
+		    m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
+			m_Cf(_nc++,1)=1.0;
+			m_posData[npos++].id = ID_POSITIONY;
+			if (m_outputDynamic & CTL_POSITIONY)
+				nposCache++;
+		}
+		if (m_outputControl & CTL_POSITIONZ) {
+		    m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
+			m_Cf(_nc++,2)=1.0;
+			m_posData[npos++].id = ID_POSITIONZ;
+			if (m_outputDynamic & CTL_POSITIONZ)
+				nposCache++;
+		}
+		m_values[m_nvalues].number = npos;
+		m_values[m_nvalues++].values = m_posData;
+		m_pos.firsty = 0;
+		m_pos.ny = npos;
+	}
+	if (m_outputControl & CTL_ROTATION) {
+		m_rot.alpha = 1.0;		
+		m_rot.K = 20.0;		
+		m_rot.tolerance = 0.05;	
+		m_values[m_nvalues].alpha = m_rot.alpha;
+		m_values[m_nvalues].feedback = m_rot.K;
+		m_values[m_nvalues].tolerance = m_rot.tolerance;
+		m_values[m_nvalues].id = ID_ROTATION;
+		if (m_outputControl & CTL_ROTATIONX) {
+		    m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
+			m_Cf(_nc++,3)=1.0;
+			m_rotData[nrot++].id = ID_ROTATIONX;
+			if (m_outputDynamic & CTL_ROTATIONX)
+				nrotCache++;
+		}
+		if (m_outputControl & CTL_ROTATIONY) {
+		    m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
+			m_Cf(_nc++,4)=1.0;
+			m_rotData[nrot++].id = ID_ROTATIONY;
+			if (m_outputDynamic & CTL_ROTATIONY)
+				nrotCache++;
+		}
+		if (m_outputControl & CTL_ROTATIONZ) {
+		    m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
+			m_Cf(_nc++,5)=1.0;
+			m_rotData[nrot++].id = ID_ROTATIONZ;
+			if (m_outputDynamic & CTL_ROTATIONZ)
+				nrotCache++;
+		}
+		m_values[m_nvalues].number = nrot;
+		m_values[m_nvalues++].values = m_rotData;
+		m_rot.firsty = npos;
+		m_rot.ny = nrot;
+	}
+	assert(_nc == m_nc);
+    m_Jf=e_identity_matrix(6,6);
+	m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
+}
+
+CopyPose::~CopyPose()
+{
+}
+
+bool CopyPose::initialise(Frame& init_pose)
+{
+    m_externalPose = m_internalPose = init_pose;
+	updateJacobian();
+    return true;
+}
+
+void CopyPose::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
+{
+	m_internalPose = m_externalPose = _external_pose;
+	updateJacobian();
+}
+
+void CopyPose::initCache(Cache *_cache)
+{
+    m_cache = _cache;
+    m_poseCCh = -1;
+    if (m_cache) {
+        // create one channel for the coordinates
+        m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize*sizeof(double));
+        // don't save initial value, it will be recomputed from external pose
+        //pushPose(0);
+    }
+}
+
+double* CopyPose::pushValues(double* item, ControlState* _state, unsigned int mask)
+{
+	ControlState::ControlValue* _yval;
+	int i;
+
+	*item++ = _state->alpha;
+	*item++ = _state->K;
+	*item++ = _state->tolerance;
+
+	for (i=0, _yval=_state->output; i<_state->ny; mask<<=1) {
+		if (m_outputControl & mask) {
+			if (m_outputDynamic & mask) {
+				*item++ = _yval->yd;
+				*item++ = _yval->yddot;
+			}
+			_yval++;
+			i++;
+		}
+	}
+	return item;
+}
+
+void CopyPose::pushPose(CacheTS timestamp)
+{
+    if (m_poseCCh >= 0) {
+		if (m_poseCacheSize) {
+			double buf[maxPoseCacheSize];
+			double *item = buf;
+			if (m_outputDynamic & CTL_POSITION)
+				item = pushValues(item, &m_pos, CTL_POSITIONX);
+			if (m_outputDynamic & CTL_ROTATION)
+				item = pushValues(item, &m_rot, CTL_ROTATIONX);
+			m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, m_poseCacheSize, KDL::epsilon);
+		} else
+			m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, NULL, 0, KDL::epsilon);
+		m_poseCTs = timestamp;
+    }
+}
+
+double* CopyPose::restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask)
+{
+	ConstraintSingleValue* _data;
+	ControlState::ControlValue* _yval;
+	int i, j;
+
+	_values->alpha = _state->alpha = *item++;
+	_values->feedback = _state->K = *item++;
+	_values->tolerance = _state->tolerance = *item++;
+
+	for (i=_state->firsty, j=i+_state->ny, _yval=_state->output, _data=_values->values; i<j; mask<<=1) {
+		if (m_outputControl & mask) {
+			m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
+			if (m_outputDynamic & mask) {
+				_data->yd = _yval->yd = *item++;
+				_data->yddot = _yval->yddot = *item++;
+			}
+			_data++;
+			_yval++;
+			i++;
+		}
+	}
+	return item;
+}
+
+bool CopyPose::popPose(CacheTS timestamp)
+{
+	bool found = false;
+    if (m_poseCCh >= 0) {
+        double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
+		if (item) {
+			found = true;
+			if (timestamp != m_poseCTs) {
+				int i=0;
+				if (m_outputControl & CTL_POSITION) {
+					if (m_outputDynamic & CTL_POSITION) {
+						item = restoreValues(item, &m_values[i], &m_pos, CTL_POSITIONX);
+					}
+					i++;
+				}
+				if (m_outputControl & CTL_ROTATION) {
+					if (m_outputDynamic & CTL_ROTATION) {
+						item = restoreValues(item, &m_values[i], &m_rot, CTL_ROTATIONX);
+					}
+					i++;
+				}
+				m_poseCTs = timestamp;
+				item = NULL;
+			}
+        }
+    }
+    return found;
+}
+
+void CopyPose::interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp)
+{
+	ControlState::ControlValue* _yval;
+	int i;
+
+	for (i=0, _yval=_state->output; i<_state->ny; mask <<= 1) {
+		if (m_outputControl & mask) {
+			if (m_outputDynamic & mask) {
+				if (timestamp.substep && timestamp.interpolate) {
+					_yval->yd += _yval->yddot*timestamp.realTimestep;
+				} else {
+					_yval->yd = _yval->nextyd;
+					_yval->yddot = _yval->nextyddot;
+				}
+			}
+			i++;
+			_yval++;
+		}
+	}
+}
+
+void CopyPose::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache) {
+        pushPose(timestamp.cacheTimestamp);
+	}
+}
+
+void CopyPose::updateKinematics(const Timestamp& timestamp)
+{
+	if (timestamp.interpolate) {
+		if (m_outputDynamic & CTL_POSITION)
+			interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
+		if (m_outputDynamic & CTL_ROTATION)
+			interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp);
+	}
+	pushCache(timestamp);
+}
+
+void CopyPose::updateJacobian()
+{
+    //Jacobian is always identity at the start of the constraint chain
+	//instead of going through complicated jacobian operation, implemented direct formula
+	//m_Jf(1,3) = m_internalPose.p.z();
+	//m_Jf(2,3) = -m_internalPose.p.y();
+	//m_Jf(0,4) = -m_internalPose.p.z();
+	//m_Jf(2,4) = m_internalPose.p.x();
+	//m_Jf(0,5) = m_internalPose.p.y();
+	//m_Jf(1,5) = -m_internalPose.p.x();
+}
+
+void CopyPose::updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep)
+{
+	unsigned int id = (mask == CTL_ROTATIONX) ? ID_ROTATIONX : ID_POSITIONX;
+	ControlState::ControlValue* _yval;
+	ConstraintSingleValue* _data;
+	int i, j, k;
+    int action = 0;
+
+    if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
+        _state->alpha = _values->alpha;
+        action |= ACT_ALPHA;
+    }
+    if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
+        _state->tolerance = _values->tolerance;
+        action |= ACT_TOLERANCE;
+    }
+    if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
+        _state->K = _values->feedback;
+        action |= ACT_FEEDBACK;
+    }
+	for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j; mask <<= 1, id++) {
+		if (m_outputControl & mask) {
+			if (action)
+				m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
+			// check if this controlled output is provided
+			for (k=0, _data=_values->values; k<_values->number; k++, _data++) {
+				if (_data->id == id) {
+					switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
+					case 0:
+						// no indication, keep current values
+						break;
+					case ACT_VELOCITY:
+						// only the velocity is given estimate the new value by integration
+						_data->yd = _yval->yd+_data->yddot*timestep;
+						// walkthrough
+					case ACT_VALUE:
+						_yval->nextyd = _data->yd;
+						// if the user sets the value, we assume future velocity is zero
+						// (until the user changes the value again)
+						_yval->nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
+						if (timestep>0.0) {
+							_yval->yddot = (_data->yd-_yval->yd)/timestep;
+						} else {
+							// allow the user to change target instantenously when this function
+							// if called from setControlParameter with timestep = 0
+							_yval->yd = _yval->nextyd;
+							_yval->yddot = _yval->nextyddot;
+						}
+						break;
+					case (ACT_VALUE|ACT_VELOCITY):
+						// the user should not set the value and velocity at the same time.
+						// In this case, we will assume that he wants to set the future value
+						// and we compute the current value to match the velocity
+						_yval->yd = _data->yd - _data->yddot*timestep;
+						_yval->nextyd = _data->yd;
+						_yval->nextyddot = _data->yddot;
+						if (timestep>0.0) {
+							_yval->yddot = (_data->yd-_yval->yd)/timestep;
+						} else {
+							_yval->yd = _yval->nextyd;
+							_yval->yddot = _yval->nextyddot;
+						}
+						break;
+					}
+				}
+			}
+			_yval++;
+			i++;
+		}
+	}
+}
+
+
+bool CopyPose::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
+{
+	while (_nvalues > 0) {
+		if (_values->id >= ID_POSITION && _values->id <= ID_POSITIONZ && (m_outputControl & CTL_POSITION)) {
+			updateState(_values, &m_pos, CTL_POSITIONX, timestep);
+		} 
+		if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ && (m_outputControl & CTL_ROTATION)) {
+			updateState(_values, &m_rot, CTL_ROTATIONX, timestep);
+		}
+		_values++;
+		_nvalues--;
+	}
+    return true;
+}
+
+void CopyPose::updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask)
+{
+	ConstraintSingleValue* _data;
+	ControlState::ControlValue* _yval;
+	int i, j;
+
+	_values->action = 0;
+
+	for (i=_state->firsty, j=0, _yval=_state->output, _data=_values->values; j<3; j++, mask<<=1) {
+		if (m_outputControl & mask) {
+			*(double*)&_data->y = vel(j);
+			*(double*)&_data->ydot = m_ydot(i);
+			_data->yd = _yval->yd;
+			_data->yddot = _yval->yddot;
+			_data->action = 0;
+			i++;
+			_data++;
+			_yval++;
+		}
+	}
+}
+
+void CopyPose::updateOutput(Vector& vel, ControlState* _state, unsigned int mask)
+{
+	ControlState::ControlValue* _yval;
+	int i, j;
+	double coef=1.0;
+	if (mask & CTL_POSITION) {
+		// put a limit on position error
+		double len=0.0;
+		for (j=0, _yval=_state->output; j<3; j++) {
+			if (m_outputControl & (mask<<j)) {
+				len += KDL::sqr(_yval->yd-vel(j));
+				_yval++;
+			}
+		}
+		len = KDL::sqrt(len);
+		if (len > m_maxerror)
+			coef = m_maxerror/len;
+	}
+	for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) {
+		if (m_outputControl & (mask<<j)) {
+			m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd-vel(j));
+			_yval++;
+			i++;
+		}
+	}
+}
+
+void CopyPose::updateControlOutput(const Timestamp& timestamp)
+{
+    //IMO this should be done, no idea if it is enough (wrt Distance impl)
+	Twist y = diff(F_identity, m_internalPose);
+	bool found = true;
+	if (!timestamp.substep) {
+		if (!timestamp.reiterate) {
+			found = popPose(timestamp.cacheTimestamp);
+		}
+	}
+	if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
+		// initialize first callback the application to get the current values
+		int i=0;
+		if (m_outputControl & CTL_POSITION) {
+			updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
+		}
+		if (m_outputControl & CTL_ROTATION) {
+			updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
+		}
+		if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
+			setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0);
+		}
+	}
+	if (m_outputControl & CTL_POSITION) {
+		updateOutput(y.vel, &m_pos, CTL_POSITIONX);
+	}
+	if (m_outputControl & CTL_ROTATION) {
+		updateOutput(y.rot, &m_rot, CTL_ROTATIONX);
+	}
+}
+
+const ConstraintValues* CopyPose::getControlParameters(unsigned int* _nvalues)
+{
+	Twist y = diff(m_internalPose,F_identity);
+	int i=0;
+	if (m_outputControl & CTL_POSITION) {
+		updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
+	}
+	if (m_outputControl & CTL_ROTATION) {
+		updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
+	}
+	if (_nvalues)
+		*_nvalues=m_nvalues; 
+	return m_values; 
+}
+
+double CopyPose::getMaxTimestep(double& timestep)
+{
+	// CopyPose should not have any limit on linear velocity: 
+	// in case the target is out of reach, this can be very high.
+	// We will simply limit on rotation
+	e_scalar maxChidot = m_chidot.block(3,0,3,1).array().abs().maxCoeff();
+	if (timestep*maxChidot > m_maxDeltaChi) {
+		timestep = m_maxDeltaChi/maxChidot;
+	}
+	return timestep;
+}
+
+}

blender-5.2.0/intern/itasc/CopyPose.hpp

@@ -0,0 +1,100 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef COPYPOSE_H_
+#define COPYPOSE_H_
+
+#include "ConstraintSet.hpp"
+namespace iTaSC{
+
+using namespace KDL;
+
+class CopyPose: public iTaSC::ConstraintSet
+{
+protected:
+    virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+    virtual void updateJacobian();
+    virtual bool initialise(Frame& init_pose);
+    virtual void initCache(Cache *_cache);
+    virtual void updateControlOutput(const Timestamp& timestamp);
+	virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
+	virtual double getMaxTimestep(double& timestep);
+
+public:
+    enum ID {		// constraint ID in callback and setControlParameter
+		ID_POSITION=0,
+        ID_POSITIONX=1,
+        ID_POSITIONY=2,
+        ID_POSITIONZ=3,
+		ID_ROTATION=4,
+        ID_ROTATIONX=5,
+        ID_ROTATIONY=6,
+        ID_ROTATIONZ=7,
+    };
+	enum CTL {		// control ID in constructor to specify which output is constrainted
+		CTL_NONE=0x00,
+        CTL_POSITIONX=0x01,		// the bit order is important: it matches the y output order
+        CTL_POSITIONY=0x02,
+        CTL_POSITIONZ=0x04,
+		CTL_POSITION=0x07,
+        CTL_ROTATIONX=0x08,
+        CTL_ROTATIONY=0x10,
+        CTL_ROTATIONZ=0x20,
+		CTL_ROTATION=0x38,
+		CTL_ALL=0x3F,
+	};
+
+	// use a combination of CTL_.. in control_output to specify which 
+    CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
+    virtual ~CopyPose();
+
+    virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
+    virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
+
+private:
+    struct ConstraintSingleValue m_posData[3];	// index = controlled output in X,Y,Z order
+    struct ConstraintSingleValue m_rotData[3];
+    struct ConstraintValues m_values[2];		// index = group of controlled output, in position, rotation order
+    Cache* m_cache;
+    int m_poseCCh;
+    CacheTS m_poseCTs;
+	unsigned int m_poseCacheSize;
+	unsigned int m_outputDynamic;	// combination of CTL_... determine which variables are dynamically controlled by the application
+	unsigned int m_outputControl;	// combination of CTL_... determine which output are constrained
+	unsigned int m_nvalues;		// number of elements used in m_values[]
+	double m_maxerror;
+
+	struct ControlState {
+		int firsty;			// first y index
+		int ny;				// number of y in output
+		double alpha;
+		double K;
+		double tolerance;
+		struct ControlValue {
+			double yddot;
+			double yd;
+			double nextyd;
+			double nextyddot;
+		} output[3];		// inded numbex = same as m_rotData
+	} m_rot, m_pos;
+
+    void pushPose(CacheTS timestamp);
+    bool popPose(CacheTS timestamp);
+	int nBitsOn(unsigned int v)
+		{ int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
+	double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
+	double* pushValues(double* item, ControlState* _state, unsigned int mask);
+	void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
+	void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
+	void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
+	void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);
+
+};
+}
+#endif /* COPYROTATION_H_ */

blender-5.2.0/intern/itasc/Distance.cpp

@@ -0,0 +1,322 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "Distance.hpp"
+#include "kdl/kinfam_io.hpp"
+#include <math.h>
+#include <string.h>
+
+namespace iTaSC
+{
+// a distance constraint is characterized by 5 values: alpha, tolerance, K, yd, yddot
+static const unsigned int distanceCacheSize = sizeof(double)*5 + sizeof(e_scalar)*6;
+
+Distance::Distance(double armlength, double accuracy, unsigned int maximum_iterations):
+    ConstraintSet(1,accuracy,maximum_iterations),
+    m_chiKdl(6),m_jac(6),m_cache(NULL),
+	m_distCCh(-1),m_distCTs(0)
+{
+    m_chain.addSegment(Segment(Joint(Joint::RotZ)));
+    m_chain.addSegment(Segment(Joint(Joint::RotX)));
+    m_chain.addSegment(Segment(Joint(Joint::TransY)));
+    m_chain.addSegment(Segment(Joint(Joint::RotZ)));
+    m_chain.addSegment(Segment(Joint(Joint::RotY)));
+    m_chain.addSegment(Segment(Joint(Joint::RotX)));
+
+	m_fksolver = new KDL::ChainFkSolverPos_recursive(m_chain);
+	m_jacsolver = new KDL::ChainJntToJacSolver(m_chain);
+    m_Cf(0,2)=1.0;
+	m_alpha = 1.0;
+	m_tolerance = 0.05;
+	m_maxerror = armlength/2.0;
+	m_K = 20.0;
+	m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
+	m_yddot = m_nextyddot = 0.0;
+	m_yd = m_nextyd = KDL::epsilon;
+	memset(&m_data, 0, sizeof(m_data));
+	// initialize the data with normally fixed values
+	m_data.id = ID_DISTANCE;
+	m_values.id = ID_DISTANCE;
+	m_values.number = 1;
+	m_values.alpha = m_alpha;
+	m_values.feedback = m_K;
+	m_values.tolerance = m_tolerance;
+	m_values.values = &m_data;
+}
+
+Distance::~Distance()
+{
+    delete m_fksolver;
+    delete m_jacsolver;
+}
+
+bool Distance::computeChi(Frame& pose)
+{
+	double dist, alpha, beta, gamma;
+	dist = pose.p.Norm();
+	Rotation basis;
+	if (dist < KDL::epsilon) {
+		// distance is almost 0, no need for initial rotation
+		m_chi(0) = 0.0;
+		m_chi(1) = 0.0;
+	} else {
+		// find the XZ angles that bring the Y axis to point to init_pose.p
+		Vector axis(pose.p/dist);
+		beta = 0.0;
+		if (fabs(axis(2)) > 1-KDL::epsilon) {
+			// direction is aligned on Z axis, just rotation on X
+			alpha = 0.0;
+			gamma = KDL::sign(axis(2))*KDL::PI/2;
+		} else {
+			alpha = -KDL::atan2(axis(0), axis(1));
+			gamma = KDL::atan2(axis(2), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(1))));
+		}
+		// rotation after first 2 joints
+		basis = Rotation::EulerZYX(alpha, beta, gamma);
+		m_chi(0) = alpha;
+		m_chi(1) = gamma;
+	}
+	m_chi(2) = dist;
+	basis = basis.Inverse()*pose.M;
+	basis.GetEulerZYX(alpha, beta, gamma);
+	// alpha = rotation on Z
+	// beta = rotation on Y
+	// gamma = rotation on X in that order
+	// it corresponds to the joint order, so just assign
+	m_chi(3) = alpha;
+	m_chi(4) = beta;
+	m_chi(5) = gamma;
+	return true;
+}
+
+bool Distance::initialise(Frame& init_pose)
+{
+	// we will initialize m_chi to values that match the pose
+    m_externalPose=init_pose;
+	computeChi(m_externalPose);
+	// get current Jf and update internal pose
+    updateJacobian();
+	return true;
+}
+
+bool Distance::closeLoop()
+{
+	if (!Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold)){
+		computeChi(m_externalPose);
+		updateJacobian();
+	}
+	return true;
+}
+
+void Distance::initCache(Cache *_cache)
+{
+	m_cache = _cache;
+	m_distCCh = -1;
+	if (m_cache) {
+		// create one channel for the coordinates
+		m_distCCh = m_cache->addChannel(this, "Xf", distanceCacheSize);
+		// save initial constraint in cache position 0
+		pushDist(0);
+	}
+}
+
+void Distance::pushDist(CacheTS timestamp)
+{
+	if (m_distCCh >= 0) {
+		double *item = (double*)m_cache->addCacheItem(this, m_distCCh, timestamp, NULL, distanceCacheSize);
+		if (item) {
+			*item++ = m_K;
+			*item++ = m_tolerance;
+			*item++ = m_yd;
+			*item++ = m_yddot;
+			*item++ = m_alpha;
+			memcpy(item, &m_chi[0], 6*sizeof(e_scalar));
+		}
+		m_distCTs = timestamp;
+	}
+}
+
+bool Distance::popDist(CacheTS timestamp)
+{
+	if (m_distCCh >= 0) {
+		double *item = (double*)m_cache->getPreviousCacheItem(this, m_distCCh, &timestamp);
+		if (item && timestamp != m_distCTs) {
+			m_values.feedback = m_K = *item++;
+			m_values.tolerance = m_tolerance = *item++;
+			m_yd = *item++;
+			m_yddot = *item++;
+			m_values.alpha = m_alpha = *item++;
+			memcpy(&m_chi[0], item, 6*sizeof(e_scalar));
+			m_distCTs = timestamp;
+			m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
+			updateJacobian();
+		}
+		return (item) ? true : false;
+	}
+	return true;
+}
+
+void Distance::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache)
+		pushDist(timestamp.cacheTimestamp);
+}
+
+void Distance::updateKinematics(const Timestamp& timestamp)
+{
+	if (timestamp.interpolate) {
+		//the internal pose and Jf is already up to date (see model_update)
+		//update the desired output based on yddot
+		if (timestamp.substep) {
+			m_yd += m_yddot*timestamp.realTimestep;
+			if (m_yd < KDL::epsilon)
+				m_yd = KDL::epsilon;
+		} else {
+			m_yd = m_nextyd;
+			m_yddot = m_nextyddot;
+		}
+	}
+	pushCache(timestamp);
+}
+
+void Distance::updateJacobian()
+{
+    for(unsigned int i=0;i<6;i++)
+        m_chiKdl[i]=m_chi[i];
+
+    m_fksolver->JntToCart(m_chiKdl,m_internalPose);
+    m_jacsolver->JntToJac(m_chiKdl,m_jac);
+    changeRefPoint(m_jac,-m_internalPose.p,m_jac);
+    for(unsigned int i=0;i<6;i++)
+        for(unsigned int j=0;j<6;j++)
+            m_Jf(i,j)=m_jac(i,j);
+}
+
+bool Distance::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
+{
+	int action = 0;
+	int i;
+	ConstraintSingleValue* _data;
+
+	while (_nvalues > 0) {
+		if (_values->id == ID_DISTANCE) {
+			if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
+				m_alpha = _values->alpha;
+				action |= ACT_ALPHA;
+			}
+			if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
+				m_tolerance = _values->tolerance;
+				action |= ACT_TOLERANCE;
+			}
+			if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
+				m_K = _values->feedback;
+				action |= ACT_FEEDBACK;
+			}
+			for (_data = _values->values, i=0; i<_values->number; i++, _data++) {
+				if (_data->id == ID_DISTANCE) {
+					switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
+					case 0:
+						// no indication, keep current values
+						break;
+					case ACT_VELOCITY:
+						// only the velocity is given estimate the new value by integration
+						_data->yd = m_yd+_data->yddot*timestep;
+						// walkthrough for negative value correction
+					case ACT_VALUE:
+						// only the value is given, estimate the velocity from previous value
+						if (_data->yd < KDL::epsilon)
+							_data->yd = KDL::epsilon;
+						m_nextyd = _data->yd;
+						// if the user sets the value, we assume future velocity is zero
+						// (until the user changes the value again)
+						m_nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
+						if (timestep>0.0) {
+							m_yddot = (_data->yd-m_yd)/timestep;
+						} else {
+							// allow the user to change target instantenously when this function
+							// if called from setControlParameter with timestep = 0
+							m_yddot = m_nextyddot;
+							m_yd = m_nextyd;
+						}
+						break;
+					case (ACT_VALUE|ACT_VELOCITY):
+						// the user should not set the value and velocity at the same time.
+						// In this case, we will assume that he want to set the future value
+						// and we compute the current value to match the velocity
+						if (_data->yd < KDL::epsilon)
+							_data->yd = KDL::epsilon;
+						m_yd = _data->yd - _data->yddot*timestep;
+						if (m_yd < KDL::epsilon)
+							m_yd = KDL::epsilon;
+						m_nextyd = _data->yd;
+						m_nextyddot = _data->yddot;
+						if (timestep>0.0) {
+							m_yddot = (_data->yd-m_yd)/timestep;
+						} else {
+							m_yd = m_nextyd;
+							m_yddot = m_nextyddot;
+						}
+						break;
+					}
+				}
+			}
+		}
+		_nvalues--;
+		_values++;
+	}
+	if (action & (ACT_TOLERANCE|ACT_FEEDBACK|ACT_ALPHA)) {
+		// recompute the weight
+		m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
+	}
+	return true;
+}
+
+const ConstraintValues* Distance::getControlParameters(unsigned int* _nvalues)
+{
+	*(double*)&m_data.y = m_chi(2);
+	*(double*)&m_data.ydot = m_ydot(0);
+	m_data.yd = m_yd;
+	m_data.yddot = m_yddot;
+	m_data.action = 0;
+	m_values.action = 0;
+	if (_nvalues) 
+		*_nvalues=1; 
+	return &m_values; 
+}
+
+void Distance::updateControlOutput(const Timestamp& timestamp)
+{
+	bool cacheAvail = true;
+	if (!timestamp.substep) {
+		if (!timestamp.reiterate)
+			cacheAvail = popDist(timestamp.cacheTimestamp);
+	}
+	if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
+		// initialize first callback the application to get the current values
+		*(double*)&m_data.y = m_chi(2);
+		*(double*)&m_data.ydot = m_ydot(0);
+		m_data.yd = m_yd;
+		m_data.yddot = m_yddot;
+		m_data.action = 0;
+		m_values.action = 0;
+		if ((*m_constraintCallback)(timestamp, &m_values, 1, m_constraintParam)) {
+			setControlParameters(&m_values, 1, timestamp.realTimestep);
+		}
+	}
+	if (!cacheAvail || !timestamp.interpolate) {
+		// first position in cache: set the desired output immediately as we cannot interpolate
+		m_yd = m_nextyd;
+		m_yddot = m_nextyddot;
+	}
+	double error = m_yd-m_chi(2);
+	if (KDL::Norm(error) > m_maxerror)
+		error = KDL::sign(error)*m_maxerror;
+    m_ydot(0)=m_yddot+m_K*error;
+}
+
+}

blender-5.2.0/intern/itasc/Distance.hpp

@@ -0,0 +1,63 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef DISTANCE_HPP_
+#define DISTANCE_HPP_
+
+#include "ConstraintSet.hpp"
+#include "kdl/chain.hpp"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
+
+namespace iTaSC
+{
+
+class Distance: public iTaSC::ConstraintSet
+{
+protected:
+    virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+    virtual void updateJacobian();
+    virtual bool initialise(Frame& init_pose);
+	virtual void initCache(Cache *_cache);
+    virtual void updateControlOutput(const Timestamp& timestamp);
+	virtual bool closeLoop();
+
+public:
+	enum ID {
+		ID_DISTANCE=1,
+	};
+    Distance(double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
+    virtual ~Distance();
+
+	virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
+	virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
+
+private:
+	bool computeChi(Frame& pose);
+    KDL::Chain m_chain;
+    KDL::ChainFkSolverPos_recursive* m_fksolver;
+    KDL::ChainJntToJacSolver* m_jacsolver;
+    KDL::JntArray m_chiKdl;
+    KDL::Jacobian m_jac;
+	struct ConstraintSingleValue m_data;
+	struct ConstraintValues m_values;
+	Cache* m_cache;
+	int m_distCCh;
+	CacheTS m_distCTs;
+	double m_maxerror;
+
+	void pushDist(CacheTS timestamp);
+	bool popDist(CacheTS timestamp);
+
+    double m_alpha,m_yddot,m_yd,m_nextyd,m_nextyddot,m_K,m_tolerance;
+};
+
+}
+
+#endif /* DISTANCE_HPP_ */

blender-5.2.0/intern/itasc/FixedObject.cpp

@@ -0,0 +1,72 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "FixedObject.hpp"
+
+namespace iTaSC{
+
+
+FixedObject::FixedObject():UncontrolledObject(),
+	m_finalized(false), m_nframe(0)
+{
+}
+
+FixedObject::~FixedObject() 
+{
+	m_frameArray.clear();
+}
+
+int FixedObject::addFrame(const std::string& name, const Frame& frame)
+{
+	if (m_finalized)
+		return -1;
+	FrameList::iterator it;
+	unsigned int i;
+	for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
+		if (it->first == name) {
+			// this frame will replace the old frame
+			it->second = frame;
+			return i;
+		}
+	}
+	m_frameArray.push_back(FrameList::value_type(name,frame));
+	return m_nframe++;
+}
+
+int FixedObject::addEndEffector(const std::string& name)
+{
+	// verify that this frame name exist
+	FrameList::iterator it;
+	unsigned int i;
+	for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
+		if (it->first == name) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+bool FixedObject::finalize()
+{
+	if (m_finalized)
+		return true;
+	initialize(0, m_nframe);
+	m_finalized = true;
+	return true;
+}
+
+const Frame& FixedObject::getPose(const unsigned int frameIndex)
+{
+	if (frameIndex < m_nframe) {
+		return m_frameArray[frameIndex].second;
+	} else {
+		return F_identity;
+	}
+}
+
+}

blender-5.2.0/intern/itasc/FixedObject.hpp

@@ -0,0 +1,46 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef FIXEDOBJECT_HPP_
+#define FIXEDOBJECT_HPP_
+
+#include "UncontrolledObject.hpp"
+#include <vector>
+
+
+namespace iTaSC{
+
+class FixedObject: public UncontrolledObject {
+public:
+    FixedObject();
+    virtual ~FixedObject();
+
+	int addFrame(const std::string& name, const Frame& frame);
+
+	virtual void updateCoordinates(const Timestamp& timestamp) {};
+	virtual int addEndEffector(const std::string& name);
+	virtual bool finalize();
+	virtual const Frame& getPose(const unsigned int frameIndex);
+	virtual void updateKinematics(const Timestamp& timestamp) {};
+	virtual void pushCache(const Timestamp& timestamp) {};
+	virtual void initCache(Cache *_cache) {};
+
+protected:
+	virtual void updateJacobian() {}
+private:
+    typedef std::vector<std::pair<std::string, Frame> > FrameList;
+
+	bool m_finalized;
+	unsigned int m_nframe;
+	FrameList m_frameArray;
+
+};
+
+}
+
+#endif /* FIXEDOBJECT_H_ */

blender-5.2.0/intern/itasc/MovingFrame.cpp

@@ -0,0 +1,158 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "MovingFrame.hpp"
+#include <string.h>
+namespace iTaSC{
+
+static const unsigned int frameCacheSize = (sizeof(((Frame*)0)->p.data)+sizeof(((Frame*)0)->M.data))/sizeof(double);
+
+MovingFrame::MovingFrame(const Frame& frame):UncontrolledObject(),
+	m_function(NULL), m_param(NULL), m_velocity(), m_poseCCh(-1), m_poseCTs(0)
+{
+	m_internalPose = m_nextPose = frame;
+	initialize(6, 1);
+	e_matrix& Ju = m_JuArray[0];
+	Ju = e_identity_matrix(6,6);
+}
+
+MovingFrame::~MovingFrame()
+{
+}
+
+bool MovingFrame::finalize()
+{
+	updateJacobian();
+	return true;
+}
+
+void MovingFrame::initCache(Cache *_cache)
+{
+	m_cache = _cache;
+	m_poseCCh = -1;
+	if (m_cache) {
+		m_poseCCh = m_cache->addChannel(this,"pose",frameCacheSize*sizeof(double));
+		// don't store the initial pose, it's causing unnecessary large velocity on the first step
+		//pushInternalFrame(0);
+	}
+}
+
+void MovingFrame::pushInternalFrame(CacheTS timestamp)
+{
+	if (m_poseCCh >= 0) {
+		double buf[frameCacheSize];
+		memcpy(buf, m_internalPose.p.data, sizeof(m_internalPose.p.data));
+		memcpy(&buf[sizeof(m_internalPose.p.data)/sizeof(double)], m_internalPose.M.data, sizeof(m_internalPose.M.data));
+
+		m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, frameCacheSize, KDL::epsilon);
+		m_poseCTs = timestamp;
+	}
+}
+
+// load pose just preceding timestamp
+// return false if no cache position was found
+bool MovingFrame::popInternalFrame(CacheTS timestamp)
+{
+	if (m_poseCCh >= 0) {
+		char *item;
+		item = (char *)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
+		if (item && m_poseCTs != timestamp) {
+			memcpy(m_internalPose.p.data, item, sizeof(m_internalPose.p.data));
+			item += sizeof(m_internalPose.p.data);
+			memcpy(m_internalPose.M.data, item, sizeof(m_internalPose.M.data));
+			m_poseCTs = timestamp;
+			// changing the starting pose, recompute the jacobian
+			updateJacobian();
+		}
+		return (item) ? true : false;
+	}
+	// in case of no cache, there is always a previous position
+	return true;
+}
+
+bool MovingFrame::setFrame(const Frame& frame)
+{
+	m_internalPose = m_nextPose = frame;
+	return true;
+}
+
+bool MovingFrame::setCallback(MovingFrameCallback _function, void* _param)
+{
+	m_function = _function;
+	m_param = _param;
+	return true;
+}
+
+void MovingFrame::updateCoordinates(const Timestamp& timestamp)
+{
+	// don't compute the velocity during substepping, it is assumed constant.
+	if (!timestamp.substep) {
+		bool cacheAvail = true;
+		if (!timestamp.reiterate) {
+			cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
+			if (m_function)
+				(*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
+		}
+		// only compute velocity if we have a previous pose
+		if (cacheAvail && timestamp.interpolate) {
+			unsigned int iXu;
+			m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
+			for (iXu=0; iXu<6; iXu++)
+				m_xudot(iXu) = m_velocity(iXu);
+		} else if (!timestamp.reiterate) {
+			// new position is forced, no velocity as we cannot interpolate
+			m_internalPose = m_nextPose;
+			m_velocity = Twist::Zero();
+			m_xudot = e_zero_vector(6);
+			// recompute the jacobian
+			updateJacobian();
+		}
+	}
+}
+
+void MovingFrame::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache)
+		pushInternalFrame(timestamp.cacheTimestamp);
+}
+
+void MovingFrame::updateKinematics(const Timestamp& timestamp)
+{
+	if (timestamp.interpolate) {
+		if (timestamp.substep) {
+			// during substepping, update the internal pose from velocity information
+			Twist localvel = m_internalPose.M.Inverse(m_velocity);
+			m_internalPose.Integrate(localvel, 1.0/timestamp.realTimestep);
+		} else {
+			m_internalPose = m_nextPose;
+		}
+		// m_internalPose is updated, recompute the jacobian
+		updateJacobian();
+	}
+	pushCache(timestamp);
+}
+
+void MovingFrame::updateJacobian()
+{
+	Twist m_jac;
+	e_matrix& Ju = m_JuArray[0];
+
+    //Jacobian is always identity at position on the object, 
+	//we ust change the reference to the world.
+	//instead of going through complicated jacobian operation, implemented direct formula
+	Ju(1,3) = m_internalPose.p.z();
+	Ju(2,3) = -m_internalPose.p.y();
+	Ju(0,4) = -m_internalPose.p.z();
+	Ju(2,4) = m_internalPose.p.x();
+	Ju(0,5) = m_internalPose.p.y();
+	Ju(1,5) = -m_internalPose.p.x();
+	// remember that this object has moved
+	m_updated = true;
+}
+
+}

blender-5.2.0/intern/itasc/MovingFrame.hpp

@@ -0,0 +1,53 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef MOVINGFRAME_HPP_
+#define MOVINGFRAME_HPP_
+
+#include "UncontrolledObject.hpp"
+#include <vector>
+
+
+namespace iTaSC{
+
+typedef bool (*MovingFrameCallback)(
+        const Timestamp& timestamp,
+        const Frame& _current,
+        Frame& _next,
+        void *param);
+
+class MovingFrame: public UncontrolledObject {
+public:
+    MovingFrame(const Frame& frame=F_identity);
+    virtual ~MovingFrame();
+
+	bool setFrame(const Frame& frame);
+	bool setCallback(MovingFrameCallback _function, void* _param);
+
+	virtual void updateCoordinates(const Timestamp& timestamp);
+	virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+	virtual void initCache(Cache *_cache);
+	virtual bool finalize();
+protected:
+	virtual void updateJacobian();
+
+private:
+	void pushInternalFrame(CacheTS timestamp);
+	bool popInternalFrame(CacheTS timestamp);
+	MovingFrameCallback m_function;
+	void* m_param;
+	Frame m_nextPose;
+	Twist m_velocity;
+	int m_poseCCh;		// cache channel for pose
+	unsigned int m_poseCTs;
+};
+
+}
+
+#endif /* MOVINGFRAME_H_ */

blender-5.2.0/intern/itasc/Object.hpp

@@ -0,0 +1,52 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef OBJECT_HPP_
+#define OBJECT_HPP_
+
+#include "Cache.hpp"
+#include "kdl/frames.hpp"
+#include <string>
+
+namespace iTaSC{
+
+class WorldObject;
+
+class Object {
+public:
+    enum ObjectType {Controlled, UnControlled};
+	static WorldObject world;
+
+private:
+    ObjectType m_type;
+protected:
+	Cache *m_cache;
+	KDL::Frame m_internalPose;
+	bool m_updated;
+    virtual void updateJacobian()=0;
+public:
+    Object(ObjectType _type):m_type(_type), m_cache(NULL), m_internalPose(F_identity), m_updated(false) {};
+    virtual ~Object(){};
+
+	virtual int addEndEffector(const std::string&  /*name*/){return 0;};
+	virtual bool finalize(){return true;};
+	virtual const KDL::Frame& getPose(const unsigned int end_effector=0){
+		(void)end_effector;
+		return m_internalPose;
+	};
+    virtual ObjectType getType(){return m_type;};
+    virtual unsigned int getNrOfCoordinates(){return 0;};
+    virtual void updateKinematics(const Timestamp& /*timestamp*/)=0;
+    virtual void pushCache(const Timestamp& /*timestamp*/)=0;
+	virtual void initCache(Cache * /*cache*/) = 0;
+	bool updated() {return m_updated;};
+	void updated(bool val) {m_updated=val;};
+};
+
+}
+#endif /* OBJECT_HPP_ */

blender-5.2.0/intern/itasc/Scene.cpp

@@ -0,0 +1,645 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "Scene.hpp"
+#include "ControlledObject.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+#include <cstdio>
+
+namespace iTaSC {
+
+class SceneLock : public ControlledObject::JointLockCallback {
+ private:
+  Scene *m_scene;
+  Range m_qrange;
+
+ public:
+  SceneLock(Scene *scene) : m_scene(scene), m_qrange(0, 0)
+  {
+  }
+  virtual ~SceneLock()
+  {
+  }
+
+  void setRange(Range &range)
+  {
+    m_qrange = range;
+  }
+  // lock a joint, no need to update output
+  virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
+  {
+    q_nr += m_qrange.start;
+    project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
+  }
+  // lock a joint and update output in view of reiteration
+  virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double *qdot)
+  {
+    q_nr += m_qrange.start;
+    project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
+    // update the output vector so that the movement of this joint will be
+    // taken into account and we can put the joint back in its initial position
+    // which means that the jacobian doesn't need to be changed
+    for (unsigned int i = 0; i < ndof; ++i, ++q_nr) {
+      m_scene->m_ydot -= m_scene->m_A.col(q_nr) * qdot[i];
+    }
+  }
+};
+
+Scene::Scene()
+    : m_A(),
+      m_B(),
+      m_Atemp(),
+      m_Wq(),
+      m_Jf(),
+      m_Jq(),
+      m_Ju(),
+      m_Cf(),
+      m_Cq(),
+      m_Jf_inv(),
+      m_Vf(),
+      m_Uf(),
+      m_Wy(),
+      m_ydot(),
+      m_qdot(),
+      m_xdot(),
+      m_Sf(),
+      m_tempf(),
+      m_ncTotal(0),
+      m_nqTotal(0),
+      m_nuTotal(0),
+      m_nsets(0),
+      m_solver(NULL),
+      m_cache(NULL)
+{
+  m_minstep = 0.01;
+  m_maxstep = 0.06;
+}
+
+Scene::~Scene()
+{
+  ConstraintMap::iterator constraint_it;
+  while ((constraint_it = constraints.begin()) != constraints.end()) {
+    delete constraint_it->second;
+    constraints.erase(constraint_it);
+  }
+  ObjectMap::iterator object_it;
+  while ((object_it = objects.begin()) != objects.end()) {
+    delete object_it->second;
+    objects.erase(object_it);
+  }
+}
+
+bool Scene::setParam(SceneParam paramId, double value)
+{
+  switch (paramId) {
+    case MIN_TIMESTEP:
+      m_minstep = value;
+      break;
+    case MAX_TIMESTEP:
+      m_maxstep = value;
+      break;
+    default:
+      return false;
+  }
+  return true;
+}
+
+bool Scene::addObject(const std::string &name,
+                      Object *object,
+                      UncontrolledObject *base,
+                      const std::string &baseFrame)
+{
+  // finalize the object before adding
+  if (!object->finalize()) {
+    return false;
+  }
+  // Check if Object is controlled or uncontrolled.
+  if (object->getType() == Object::Controlled) {
+    int baseFrameIndex = base->addEndEffector(baseFrame);
+    if (baseFrameIndex < 0)
+      return false;
+    std::pair<ObjectMap::iterator, bool> result;
+    if (base->getNrOfCoordinates() == 0) {
+      // base is fixed object, no coordinate range
+      result = objects.insert(ObjectMap::value_type(
+          name,
+          new Object_struct(object,
+                            base,
+                            baseFrameIndex,
+                            Range(m_nqTotal, object->getNrOfCoordinates()),
+                            Range(m_ncTotal, ((ControlledObject *)object)->getNrOfConstraints()),
+                            Range(0, 0))));
+    }
+    else {
+      // base is a moving object, must be in list already
+      ObjectMap::iterator base_it;
+      for (base_it = objects.begin(); base_it != objects.end(); base_it++) {
+        if (base_it->second->object == base)
+          break;
+      }
+      if (base_it == objects.end())
+        return false;
+      result = objects.insert(ObjectMap::value_type(
+          name,
+          new Object_struct(object,
+                            base,
+                            baseFrameIndex,
+                            Range(m_nqTotal, object->getNrOfCoordinates()),
+                            Range(m_ncTotal, ((ControlledObject *)object)->getNrOfConstraints()),
+                            base_it->second->coordinaterange)));
+    }
+    if (!result.second) {
+      return false;
+    }
+    m_nqTotal += object->getNrOfCoordinates();
+    m_ncTotal += ((ControlledObject *)object)->getNrOfConstraints();
+    return true;
+  }
+  if (object->getType() == Object::UnControlled) {
+    if ((WorldObject *)base != &Object::world)
+      return false;
+    std::pair<ObjectMap::iterator, bool> result = objects.insert(
+        ObjectMap::value_type(name,
+                              new Object_struct(object,
+                                                base,
+                                                0,
+                                                Range(0, 0),
+                                                Range(0, 0),
+                                                Range(m_nuTotal, object->getNrOfCoordinates()))));
+    if (!result.second)
+      return false;
+    m_nuTotal += object->getNrOfCoordinates();
+    return true;
+  }
+  return false;
+}
+
+bool Scene::addConstraintSet(const std::string &name,
+                             ConstraintSet *task,
+                             const std::string &object1,
+                             const std::string &object2,
+                             const std::string &ee1,
+                             const std::string &ee2)
+{
+  // Check if objects exist:
+  ObjectMap::iterator object1_it = objects.find(object1);
+  ObjectMap::iterator object2_it = objects.find(object2);
+  if (object1_it == objects.end() || object2_it == objects.end()) {
+    return false;
+  }
+  int ee1_index = object1_it->second->object->addEndEffector(ee1);
+  int ee2_index = object2_it->second->object->addEndEffector(ee2);
+  if (ee1_index < 0 || ee2_index < 0) {
+    return false;
+  }
+  std::pair<ConstraintMap::iterator, bool> result = constraints.insert(ConstraintMap::value_type(
+      name,
+      new ConstraintSet_struct(task,
+                               object1_it,
+                               ee1_index,
+                               object2_it,
+                               ee2_index,
+                               Range(m_ncTotal, task->getNrOfConstraints()),
+                               Range(6 * m_nsets, 6))));
+  if (!result.second) {
+    return false;
+  }
+  m_ncTotal += task->getNrOfConstraints();
+  m_nsets += 1;
+  return true;
+}
+
+bool Scene::addSolver(Solver *_solver)
+{
+  if (m_solver == NULL) {
+    m_solver = _solver;
+    return true;
+  }
+  else
+    return false;
+}
+
+bool Scene::addCache(Cache *_cache)
+{
+  if (m_cache == NULL) {
+    m_cache = _cache;
+    return true;
+  }
+  else
+    return false;
+}
+
+bool Scene::initialize()
+{
+
+  // prepare all matrices:
+  if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0) {
+    return false;
+  }
+
+  m_A = e_zero_matrix(m_ncTotal, m_nqTotal);
+  if (m_nuTotal > 0) {
+    m_B = e_zero_matrix(m_ncTotal, m_nuTotal);
+    m_xdot = e_zero_vector(m_nuTotal);
+    m_Ju = e_zero_matrix(6 * m_nsets, m_nuTotal);
+  }
+  m_Atemp = e_zero_matrix(m_ncTotal, 6 * m_nsets);
+  m_ydot = e_zero_vector(m_ncTotal);
+  m_qdot = e_zero_vector(m_nqTotal);
+  m_Wq = e_zero_matrix(m_nqTotal, m_nqTotal);
+  m_Wy = e_zero_vector(m_ncTotal);
+  m_Jq = e_zero_matrix(6 * m_nsets, m_nqTotal);
+  m_Jf = e_zero_matrix(6 * m_nsets, 6 * m_nsets);
+  m_Jf_inv = m_Jf;
+  m_Cf = e_zero_matrix(m_ncTotal, m_Jf.rows());
+  m_Cq = e_zero_matrix(m_ncTotal, m_nqTotal);
+
+  bool result = true;
+  // finalize all objects
+  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+    Object_struct *os = it->second;
+
+    os->object->initCache(m_cache);
+    if (os->constraintrange.count > 0)
+      project(m_Cq,
+              os->constraintrange,
+              os->jointrange) = (((ControlledObject *)(os->object))->getCq());
+  }
+
+  m_ytask.resize(m_ncTotal);
+  bool toggle = true;
+  int count = 0;
+  // Initialize all ConstraintSets:
+  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+    // Calculate the external pose:
+    ConstraintSet_struct *cs = it->second;
+    Frame external_pose;
+    getConstraintPose(cs->task, cs, external_pose);
+    result &= cs->task->initialise(external_pose);
+    cs->task->initCache(m_cache);
+    for (int i = 0; i < cs->constraintrange.count; i++, count++) {
+      m_ytask[count] = toggle;
+    }
+    toggle = !toggle;
+    project(m_Cf, cs->constraintrange, cs->featurerange) = cs->task->getCf();
+  }
+
+  if (m_solver != NULL) {
+    m_solver->init(m_nqTotal, m_ncTotal, m_ytask);
+  }
+  else {
+    return false;
+  }
+
+  return result;
+}
+
+bool Scene::getConstraintPose(ConstraintSet *constraint, void *_param, KDL::Frame &_pose)
+{
+  // function called from constraint when they need to get the external pose
+  ConstraintSet_struct *cs = (ConstraintSet_struct *)_param;
+  // verification, the pointer MUST match
+  assert(constraint == cs->task);
+  Object_struct *ob1 = cs->object1->second;
+  Object_struct *ob2 = cs->object2->second;
+  // Calculate the external pose:
+  _pose =
+      (ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index)).Inverse() *
+      (ob2->base->getPose(ob2->baseFrameIndex) * ob2->object->getPose(cs->ee2index));
+  return true;
+}
+
+bool Scene::update(double timestamp,
+                   double timestep,
+                   unsigned int numsubstep,
+                   bool reiterate,
+                   bool cache,
+                   bool interpolate)
+{
+  // we must have valid timestep and timestamp
+  if (timestamp < KDL::epsilon || timestep < 0.0) {
+    return false;
+  }
+  Timestamp ts;
+  ts.realTimestamp = timestamp;
+  // initially we start with the full timestep to allow velocity estimation over the full interval
+  ts.realTimestep = timestep;
+  setCacheTimestamp(ts);
+  ts.substep = 0;
+  // for reiteration don't load cache
+  // reiteration=additional iteration with same timestamp if application finds the convergence not
+  // good enough
+  ts.reiterate = (reiterate) ? 1 : 0;
+  ts.interpolate = (interpolate) ? 1 : 0;
+  ts.cache = (cache) ? 1 : 0;
+  ts.update = 1;
+  ts.numstep = (numsubstep & 0xFF);
+  bool autosubstep = (numsubstep == 0) ? true : false;
+  if (numsubstep < 1) {
+    numsubstep = 1;
+  }
+  double timesubstep = timestep / numsubstep;
+  double timeleft = timestep;
+
+  if (timeleft == 0.0) {
+    // this special case correspond to a request to cache data
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      it->second->object->pushCache(ts);
+    }
+    // Update the Constraints
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      it->second->task->pushCache(ts);
+    }
+    return true;
+  }
+
+  // double maxqdot; // UNUSED
+  e_scalar nlcoef;
+  SceneLock lockCallback(this);
+  Frame external_pose;
+  bool locked;
+
+  // initially we keep timestep unchanged so that update function compute the velocity over
+  while (numsubstep > 0) {
+    // get objects
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      Object_struct *os = it->second;
+      if (os->object->getType() == Object::Controlled) {
+        ((ControlledObject *)(os->object))->updateControlOutput(ts);
+        if (os->constraintrange.count > 0) {
+          project(m_ydot,
+                  os->constraintrange) = ((ControlledObject *)(os->object))->getControlOutput();
+          project(m_Wy, os->constraintrange) = ((ControlledObject *)(os->object))->getWy();
+          // project(m_Cq,os->constraintrange,os->jointrange) =
+          // (((ControlledObject*)(os->object))->getCq());
+        }
+        if (os->jointrange.count > 0) {
+          project(
+              m_Wq, os->jointrange, os->jointrange) = ((ControlledObject *)(os->object))->getWq();
+        }
+      }
+      if (os->object->getType() == Object::UnControlled &&
+          ((UncontrolledObject *)os->object)->getNrOfCoordinates() != 0) {
+        ((UncontrolledObject *)(os->object))->updateCoordinates(ts);
+        if (!ts.substep) {
+          // velocity of uncontrolled object remains constant during substepping
+          project(m_xdot, os->coordinaterange) = ((UncontrolledObject *)(os->object))->getXudot();
+        }
+      }
+    }
+
+    // get new Constraints values
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      ConstraintSet_struct *cs = it->second;
+      Object_struct *ob1 = cs->object1->second;
+      Object_struct *ob2 = cs->object2->second;
+
+      if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() ||
+          ob2->object->updated()) {
+        // the object from which the constraint depends have changed position
+        // recompute the constraint pose
+        getConstraintPose(cs->task, cs, external_pose);
+        cs->task->initialise(external_pose);
+      }
+      cs->task->updateControlOutput(ts);
+      project(m_ydot, cs->constraintrange) = cs->task->getControlOutput();
+      if (!ts.substep || cs->task->substep()) {
+        project(m_Wy, cs->constraintrange) = (cs->task)->getWy();
+        // project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
+      }
+
+      project(m_Jf, cs->featurerange, cs->featurerange) = cs->task->getJf();
+      // std::cout << "Jf = " << Jf << std::endl;
+      // Transform the reference frame of this jacobian to the world reference frame
+      Eigen::Block<e_matrix> Jf_part = project(m_Jf, cs->featurerange, cs->featurerange);
+      changeBase(Jf_part,
+                 ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index));
+      // std::cout << "Jf_w = " << Jf << std::endl;
+
+      // calculate the inverse of Jf
+      KDL::svd_eigen_HH(
+          project(m_Jf, cs->featurerange, cs->featurerange), m_Uf, m_Sf, m_Vf, m_tempf);
+      for (unsigned int i = 0; i < 6; ++i)
+        if (m_Sf(i) < KDL::epsilon)
+          m_Uf.col(i).setConstant(0.0);
+        else
+          m_Uf.col(i) *= (1 / m_Sf(i));
+      project(m_Jf_inv, cs->featurerange, cs->featurerange).noalias() = m_Vf * m_Uf.transpose();
+
+      // Get the robotjacobian associated with this constraintset
+      // Each jacobian is expressed in robot base frame => convert to world reference
+      // and negate second robot because it is taken reversed when closing the loop:
+      if (ob1->object->getType() == Object::Controlled) {
+        project(m_Jq,
+                cs->featurerange,
+                ob1->jointrange) = (((ControlledObject *)(ob1->object))->getJq(cs->ee1index));
+        // Transform the reference frame of this jacobian to the world reference frame:
+        Eigen::Block<e_matrix> Jq_part = project(m_Jq, cs->featurerange, ob1->jointrange);
+        changeBase(Jq_part, ob1->base->getPose(ob1->baseFrameIndex));
+        // if the base of this object is moving, get the Ju part
+        if (ob1->base->getNrOfCoordinates() != 0) {
+          // Ju is already computed for world reference frame
+          project(m_Ju, cs->featurerange, ob1->coordinaterange) = ob1->base->getJu(
+              ob1->baseFrameIndex);
+        }
+      }
+      else if (ob1->object->getType() == Object::UnControlled &&
+               ((UncontrolledObject *)ob1->object)->getNrOfCoordinates() != 0) {
+        // object1 is uncontrolled moving object
+        project(m_Ju,
+                cs->featurerange,
+                ob1->coordinaterange) = ((UncontrolledObject *)ob1->object)->getJu(cs->ee1index);
+      }
+      if (ob2->object->getType() == Object::Controlled) {
+        // Get the robotjacobian associated with this constraintset
+        // process a special case where object2 and object1 are equal but using different end
+        // effector
+        if (ob1->object == ob2->object) {
+          // we must create a temporary matrix
+          e_matrix JqTemp(((ControlledObject *)(ob2->object))->getJq(cs->ee2index));
+          // Transform the reference frame of this jacobian to the world reference frame:
+          changeBase(JqTemp, ob2->base->getPose(ob2->baseFrameIndex));
+          // subtract in place
+          project(m_Jq, cs->featurerange, ob2->jointrange) -= JqTemp;
+        }
+        else {
+          project(m_Jq, cs->featurerange, ob2->jointrange) = -(
+              ((ControlledObject *)(ob2->object))->getJq(cs->ee2index));
+          // Transform the reference frame of this jacobian to the world reference frame:
+          Eigen::Block<e_matrix> Jq_part = project(m_Jq, cs->featurerange, ob2->jointrange);
+          changeBase(Jq_part, ob2->base->getPose(ob2->baseFrameIndex));
+        }
+        if (ob2->base->getNrOfCoordinates() != 0) {
+          // if base is the same as first object or first object base,
+          // that portion of m_Ju has been set already => subtract inplace
+          if (ob2->base == ob1->base || ob2->base == ob1->object) {
+            project(m_Ju, cs->featurerange, ob2->coordinaterange) -= ob2->base->getJu(
+                ob2->baseFrameIndex);
+          }
+          else {
+            project(m_Ju, cs->featurerange, ob2->coordinaterange) = -ob2->base->getJu(
+                ob2->baseFrameIndex);
+          }
+        }
+      }
+      else if (ob2->object->getType() == Object::UnControlled &&
+               ((UncontrolledObject *)ob2->object)->getNrOfCoordinates() != 0) {
+        if (ob2->object == ob1->base || ob2->object == ob1->object) {
+          project(m_Ju, cs->featurerange, ob2->coordinaterange) -=
+              ((UncontrolledObject *)ob2->object)->getJu(cs->ee2index);
+        }
+        else {
+          project(m_Ju, cs->featurerange, ob2->coordinaterange) =
+              -((UncontrolledObject *)ob2->object)->getJu(cs->ee2index);
+        }
+      }
+    }
+
+    // Calculate A
+    m_Atemp.noalias() = m_Cf * m_Jf_inv;
+    m_A.noalias() = m_Cq - (m_Atemp * m_Jq);
+    if (m_nuTotal > 0) {
+      m_B.noalias() = m_Atemp * m_Ju;
+      m_ydot.noalias() += m_B * m_xdot;
+    }
+
+    // Call the solver with A, Wq, Wy, ydot to solver qdot:
+    if (!m_solver->solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
+      // this should never happen
+      return false;
+    // send result to the objects
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      Object_struct *os = it->second;
+      if (os->object->getType() == Object::Controlled)
+        ((ControlledObject *)(os->object))->setJointVelocity(project(m_qdot, os->jointrange));
+    }
+    // compute the constraint velocity
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      ConstraintSet_struct *cs = it->second;
+      Object_struct *ob1 = cs->object1->second;
+      Object_struct *ob2 = cs->object2->second;
+      // Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
+      e_vector6 external_vel = e_zero_vector(6);
+      if (ob1->jointrange.count > 0)
+        external_vel.noalias() += (project(m_Jq, cs->featurerange, ob1->jointrange) *
+                                   project(m_qdot, ob1->jointrange));
+      if (ob2->jointrange.count > 0)
+        external_vel.noalias() += (project(m_Jq, cs->featurerange, ob2->jointrange) *
+                                   project(m_qdot, ob2->jointrange));
+      if (ob1->coordinaterange.count > 0)
+        external_vel.noalias() += (project(m_Ju, cs->featurerange, ob1->coordinaterange) *
+                                   project(m_xdot, ob1->coordinaterange));
+      if (ob2->coordinaterange.count > 0)
+        external_vel.noalias() += (project(m_Ju, cs->featurerange, ob2->coordinaterange) *
+                                   project(m_xdot, ob2->coordinaterange));
+      // the twist caused by the constraint must be opposite because of the closed loop
+      // estimate the velocity of the joints using the inverse jacobian
+      e_vector6 estimated_chidot = project(m_Jf_inv, cs->featurerange, cs->featurerange) *
+                                   (-external_vel);
+      cs->task->setJointVelocity(estimated_chidot);
+    }
+
+    if (autosubstep) {
+      // automatic computing of substep based on maximum joint change
+      // and joint limit gain variation
+      // We will pass the joint velocity to each object and they will recommend a maximum timestep
+      timesubstep = timeleft;
+      // get armature max joint velocity to estimate the maximum duration of integration
+      // maxqdot = m_qdot.cwise().abs().maxCoeff(); // UNUSED
+      double maxsubstep = nlcoef * m_maxstep;
+      if (maxsubstep < m_minstep)
+        maxsubstep = m_minstep;
+      if (timesubstep > maxsubstep)
+        timesubstep = maxsubstep;
+      for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+        Object_struct *os = it->second;
+        if (os->object->getType() == Object::Controlled)
+          ((ControlledObject *)(os->object))->getMaxTimestep(timesubstep);
+      }
+      for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+        ConstraintSet_struct *cs = it->second;
+        cs->task->getMaxTimestep(timesubstep);
+      }
+      // use substep that are even dividers of timestep for more regularity
+      maxsubstep = 2.0 * floor(timestep / 2.0 / timesubstep - 0.66666);
+      timesubstep = (maxsubstep < 0.0) ? timestep : timestep / (2.0 + maxsubstep);
+      if (timesubstep >= timeleft - (m_minstep / 2.0)) {
+        timesubstep = timeleft;
+        numsubstep = 1;
+        timeleft = 0.;
+      }
+      else {
+        numsubstep = 2;
+        timeleft -= timesubstep;
+      }
+    }
+    if (numsubstep > 1) {
+      ts.substep = 1;
+    }
+    else {
+      // set substep to false for last iteration so that controlled output
+      // can be updated in updateKinematics() and model_update)() before next call to
+      // Secne::update()
+      ts.substep = 0;
+    }
+    // change timestep so that integration is done correctly
+    ts.realTimestep = timesubstep;
+
+    do {
+      ObjectMap::iterator it;
+      Object_struct *os;
+      locked = false;
+      for (it = objects.begin(); it != objects.end(); ++it) {
+        os = it->second;
+        if (os->object->getType() == Object::Controlled) {
+          lockCallback.setRange(os->jointrange);
+          if (((ControlledObject *)os->object)->updateJoint(ts, lockCallback)) {
+            // this means one of the joint was locked and we must rerun
+            // the solver to update the remaining joints
+            locked = true;
+            break;
+          }
+        }
+      }
+      if (locked) {
+        // Some rows of m_Wq have been cleared so that the corresponding joint will not move
+        if (!m_solver->solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
+          // this should never happen
+          return false;
+
+        // send result to the objects
+        for (it = objects.begin(); it != objects.end(); ++it) {
+          os = it->second;
+          if (os->object->getType() == Object::Controlled)
+            ((ControlledObject *)(os->object))->setJointVelocity(project(m_qdot, os->jointrange));
+        }
+      }
+    } while (locked);
+
+    // Update the Objects
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      it->second->object->updateKinematics(ts);
+      // mark this object not updated since the constraint will be updated anyway
+      // this flag is only useful to detect external updates
+      it->second->object->updated(false);
+    }
+    // Update the Constraints
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      ConstraintSet_struct *cs = it->second;
+      // Calculate the external pose:
+      getConstraintPose(cs->task, cs, external_pose);
+      cs->task->modelUpdate(external_pose, ts);
+      // update the constraint output and cache
+      cs->task->updateKinematics(ts);
+    }
+    numsubstep--;
+  }
+  return true;
+}
+
+}  // namespace iTaSC

blender-5.2.0/intern/itasc/Scene.hpp

@@ -0,0 +1,105 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef SCENE_HPP_
+#define SCENE_HPP_
+
+#include "eigen_types.hpp"
+
+#include "WorldObject.hpp"
+#include "ConstraintSet.hpp"
+#include "Solver.hpp"
+
+#include <map>
+
+namespace iTaSC {
+
+class SceneLock;
+
+class Scene {
+	friend class SceneLock;	
+public:
+	enum SceneParam {
+		MIN_TIMESTEP	= 0,
+		MAX_TIMESTEP,
+
+		COUNT
+	};
+
+
+	Scene();
+    virtual ~Scene();
+
+	bool addObject(const std::string& name, Object* object, UncontrolledObject* base=&Object::world, const std::string& baseFrame="");
+	bool addConstraintSet(const std::string& name, ConstraintSet* task,const std::string& object1,const std::string& object2,const std::string& ee1="",const std::string& ee2="");
+    bool addSolver(Solver* _solver);
+    bool addCache(Cache* _cache);
+    bool initialize();
+    bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true);
+	bool setParam(SceneParam paramId, double value);
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+private:
+    e_matrix m_A,m_B,m_Atemp,m_Wq,m_Jf,m_Jq,m_Ju,m_Cf,m_Cq,m_Jf_inv;
+	e_matrix6 m_Vf,m_Uf;
+    e_vector m_Wy,m_ydot,m_qdot,m_xdot;
+	e_vector6 m_Sf,m_tempf;
+	double m_minstep;
+	double m_maxstep;
+    unsigned int m_ncTotal,m_nqTotal,m_nuTotal,m_nsets;
+	std::vector<bool> m_ytask;
+
+    Solver* m_solver;
+	Cache* m_cache;
+
+
+    struct Object_struct{
+        Object* object;
+        UncontrolledObject* base;
+		unsigned int baseFrameIndex;
+        Range constraintrange;
+        Range jointrange;
+		Range coordinaterange;	// Xu range of base when object is controlled
+										// Xu range of object when object is uncontrolled 
+
+        Object_struct(Object* _object,UncontrolledObject* _base,unsigned int _baseFrameIndex,Range nq_range,Range nc_range,Range nu_range):
+            object(_object),base(_base),baseFrameIndex(_baseFrameIndex),constraintrange(nc_range),jointrange(nq_range),coordinaterange(nu_range)
+            {};
+    };
+    typedef std::map<std::string,Object_struct*> ObjectMap;
+
+    struct ConstraintSet_struct{
+        ConstraintSet* task;
+        ObjectMap::iterator object1;
+        ObjectMap::iterator object2;
+        Range constraintrange;
+        Range featurerange;
+		unsigned int ee1index;
+		unsigned int ee2index;
+        ConstraintSet_struct(ConstraintSet* _task,
+			ObjectMap::iterator _object1,unsigned int _ee1index,
+			ObjectMap::iterator _object2,unsigned int _ee2index,
+			Range nc_range,Range coord_range):
+				task(_task),
+				object1(_object1),object2(_object2),
+				constraintrange(nc_range),featurerange(coord_range),
+				ee1index(_ee1index), ee2index(_ee2index)
+            {};
+    };
+    typedef std::map<std::string,ConstraintSet_struct*> ConstraintMap;
+
+    ObjectMap objects;
+    ConstraintMap constraints;
+
+	static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose);
+};
+
+}
+
+#endif /* SCENE_HPP_ */

blender-5.2.0/intern/itasc/Solver.hpp

@@ -0,0 +1,34 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef SOLVER_HPP_
+#define SOLVER_HPP_
+
+#include <vector>
+#include "eigen_types.hpp"
+
+namespace iTaSC{
+
+class Solver{
+public:
+	enum SolverParam {
+		DLS_QMAX = 0,
+		DLS_LAMBDA_MAX,
+		DLS_EPSILON
+	};
+    virtual ~Solver(){};
+
+	// gc = grouping of constraint output , 
+	//      size of vector = nc, alternance of true / false to indicate the grouping of output
+	virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)=0;
+    virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)=0;
+	virtual void setParam(SolverParam param, double value)=0;
+};
+
+}
+#endif /* SOLVER_HPP_ */

blender-5.2.0/intern/itasc/UncontrolledObject.cpp

@@ -0,0 +1,44 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "UncontrolledObject.hpp"
+
+namespace iTaSC{
+
+UncontrolledObject::UncontrolledObject():Object(UnControlled),
+	m_nu(0), m_nf(0), m_xudot()
+{
+}
+
+UncontrolledObject::~UncontrolledObject() 
+{
+}
+
+void UncontrolledObject::initialize(unsigned int _nu, unsigned int _nf)
+{
+	assert (_nf >= 1);
+	m_nu = _nu;
+	m_nf = _nf;
+	if (_nu > 0)
+		m_xudot = e_zero_vector(_nu);
+	// clear all Jacobian if any
+	m_JuArray.clear();
+	// reserve one more to have an zero matrix handy
+	if (m_nu > 0)
+		m_JuArray.resize(m_nf+1, e_zero_matrix(6,m_nu));
+}
+
+const e_matrix& UncontrolledObject::getJu(unsigned int frameIndex) const
+{
+	assert (m_nu > 0);
+	return m_JuArray[(frameIndex>m_nf)?m_nf:frameIndex];
+}
+
+
+
+}

blender-5.2.0/intern/itasc/UncontrolledObject.hpp

@@ -0,0 +1,38 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef UNCONTROLLEDOBJECT_HPP_
+#define UNCONTROLLEDOBJECT_HPP_
+
+#include "eigen_types.hpp"
+
+#include "Object.hpp"
+namespace iTaSC{
+
+class UncontrolledObject: public Object {
+protected:
+	unsigned int m_nu, m_nf;
+	e_vector m_xudot;
+	std::vector<e_matrix> m_JuArray;
+
+public:
+    UncontrolledObject();
+    virtual ~UncontrolledObject();
+
+	virtual void initialize(unsigned int _nu, unsigned int _nf);
+	virtual const e_matrix& getJu(unsigned int frameIndex) const;
+    virtual const e_vector& getXudot() const {return m_xudot;}
+	virtual void updateCoordinates(const Timestamp& timestamp)=0;
+    virtual unsigned int getNrOfCoordinates(){return m_nu;};
+    virtual unsigned int getNrOfFrames(){return m_nf;};
+
+};
+
+}
+
+#endif /* UNCONTROLLEDOBJECT_H_ */

blender-5.2.0/intern/itasc/WDLSSolver.cpp

@@ -0,0 +1,102 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "WDLSSolver.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+
+namespace iTaSC {
+
+WDLSSolver::WDLSSolver() : m_lambda(0.5), m_epsilon(0.1) 
+{
+	// maximum joint velocity
+	m_qmax = 50.0;
+}
+
+WDLSSolver::~WDLSSolver() {
+}
+
+bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)
+{
+	m_ns = std::min(nc,nq);
+    m_AWq = e_zero_matrix(nc,nq);
+    m_WyAWq = e_zero_matrix(nc,nq);
+    m_WyAWqt = e_zero_matrix(nq,nc);
+	m_S = e_zero_vector(std::max(nc,nq));
+	m_Wy_ydot = e_zero_vector(nc);
+	if (nq > nc) {
+		m_transpose = true;
+	    m_temp = e_zero_vector(nc);
+	    m_U = e_zero_matrix(nc,nc);
+		m_V = e_zero_matrix(nq,nc);
+	    m_WqV = e_zero_matrix(nq,nc);
+	} else {
+		m_transpose = false;
+	    m_temp = e_zero_vector(nq);
+	    m_U = e_zero_matrix(nc,nq);
+		m_V = e_zero_matrix(nq,nq);
+	    m_WqV = e_zero_matrix(nq,nq);
+	}
+    return true;
+}
+
+bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
+{
+	double alpha, vmax, norm;
+	// Create the Weighted jacobian
+    m_AWq = A*Wq;
+	for (int i=0; i<Wy.size(); i++)
+		m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
+
+    // Compute the SVD of the weighted jacobian
+	int ret;
+	if (m_transpose) {
+		m_WyAWqt = m_WyAWq.transpose();
+		ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
+	} else {
+		ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+	}
+    if(ret<0)
+        return false;
+
+    m_WqV.noalias() = Wq*m_V;
+
+    //Wy*ydot
+	m_Wy_ydot = Wy.array() * ydot.array();
+    //S^-1*U'*Wy*ydot
+	e_scalar maxDeltaS = e_scalar(0.0);
+	e_scalar prevS = e_scalar(0.0);
+	e_scalar maxS = e_scalar(1.0);
+	e_scalar S, lambda;
+	qdot.setZero();
+	for(int i=0;i<m_ns;++i) {
+		S = m_S(i);
+		if (S <= KDL::epsilon)
+			break;
+		if (i > 0 && (prevS-S) > maxDeltaS) {
+			maxDeltaS = (prevS-S);
+			maxS = prevS;
+		}
+		lambda = (S < m_epsilon) ? (e_scalar(1.0)-KDL::sqr(S/m_epsilon))*m_lambda*m_lambda : e_scalar(0.0);
+		alpha = m_U.col(i).dot(m_Wy_ydot)*S/(S*S+lambda);
+		vmax = m_WqV.col(i).array().abs().maxCoeff();
+		norm = fabs(alpha*vmax);
+		if (norm > m_qmax) {
+			qdot += m_WqV.col(i)*(alpha*m_qmax/norm);
+		} else {
+			qdot += m_WqV.col(i)*alpha;
+		}
+		prevS = S;
+	}
+	if (maxDeltaS == e_scalar(0.0))
+		nlcoef = e_scalar(KDL::epsilon);
+	else
+		nlcoef = (maxS-maxDeltaS)/maxS;
+    return true;
+}
+
+}

blender-5.2.0/intern/itasc/WDLSSolver.hpp

@@ -0,0 +1,49 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef WDLSSOLVER_HPP_
+#define WDLSSOLVER_HPP_
+
+#include "Solver.hpp"
+
+namespace iTaSC {
+
+class WDLSSolver: public iTaSC::Solver {
+private:
+    e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV;
+    e_vector m_S,m_temp,m_Wy_ydot;
+    double m_lambda;
+    double m_epsilon;
+	double m_qmax;
+	int m_ns;
+	bool m_transpose;
+public:
+    WDLSSolver();
+    virtual ~WDLSSolver();
+
+    virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc);
+    virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
+	virtual void setParam(SolverParam param, double value)
+	{
+		switch (param) {
+		case DLS_QMAX:
+			m_qmax = value;
+			break;
+		case DLS_LAMBDA_MAX:
+			m_lambda = value;
+			break;
+		case DLS_EPSILON:
+			m_epsilon = value;
+			break;
+		}	
+	}
+};
+
+}
+
+#endif /* WDLSSOLVER_HPP_ */

blender-5.2.0/intern/itasc/WSDLSSolver.cpp

@@ -0,0 +1,139 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "WSDLSSolver.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+#include <cstdio>
+
+namespace iTaSC {
+
+WSDLSSolver::WSDLSSolver() :
+	m_ns(0), m_nc(0), m_nq(0)
+
+{
+	// default maximum speed: 50 rad/s
+	m_qmax = 50.0;
+}
+
+WSDLSSolver::~WSDLSSolver() {
+}
+
+bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc)
+{
+	if (_nc == 0 || _nq == 0 || gc.size() != _nc)
+		return false;
+	m_nc = _nc;
+	m_nq = _nq;
+	m_ns = std::min(m_nc,m_nq);
+    m_AWq = e_zero_matrix(m_nc,m_nq);
+    m_WyAWq = e_zero_matrix(m_nc,m_nq);
+    m_WyAWqt = e_zero_matrix(m_nq,m_nc);
+	m_S = e_zero_vector(std::max(m_nc,m_nq));
+	m_Wy_ydot = e_zero_vector(m_nc);
+	m_ytask = gc;
+	if (m_nq > m_nc) {
+		m_transpose = true;
+	    m_temp = e_zero_vector(m_nc);
+	    m_U = e_zero_matrix(m_nc,m_nc);
+		m_V = e_zero_matrix(m_nq,m_nc);
+	    m_WqV = e_zero_matrix(m_nq,m_nc);
+	} else {
+		m_transpose = false;
+	    m_temp = e_zero_vector(m_nq);
+	    m_U = e_zero_matrix(m_nc,m_nq);
+		m_V = e_zero_matrix(m_nq,m_nq);
+	    m_WqV = e_zero_matrix(m_nq,m_nq);
+	}
+    return true;
+}
+
+bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
+{
+	unsigned int i, j, l;
+	e_scalar N, M;
+
+	// Create the Weighted jacobian
+	m_AWq.noalias() = A*Wq;
+	for (i=0; i<m_nc; i++)
+		m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
+
+    // Compute the SVD of the weighted jacobian
+	int ret;
+	if (m_transpose) {
+		m_WyAWqt = m_WyAWq.transpose();
+		ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
+	} else {
+		ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+	}
+	if(ret<0)
+		return false;
+
+	m_Wy_ydot = Wy.array() * ydot.array();
+	m_WqV.noalias() = Wq*m_V;
+	qdot.setZero();
+	e_scalar maxDeltaS = e_scalar(0.0);
+	e_scalar prevS = e_scalar(0.0);
+	e_scalar maxS = e_scalar(1.0);
+	for(i=0;i<m_ns;++i) {
+		e_scalar norm, mag, alpha, _qmax, Sinv, vmax, damp;
+		e_scalar S = m_S(i);
+		bool prev;
+		if (S < KDL::epsilon)
+			break;
+		Sinv = e_scalar(1.)/S;
+		if (i > 0) {
+			if ((prevS-S) > maxDeltaS) {
+				maxDeltaS = (prevS-S);
+				maxS = prevS;
+			}
+		}
+		N = M = e_scalar(0.);
+		for (l=0, prev=m_ytask[0], norm=e_scalar(0.); l<m_nc; l++) {
+			if (prev == m_ytask[l]) {
+				norm += m_U(l,i)*m_U(l,i);
+			} else {
+				N += std::sqrt(norm);
+				norm = m_U(l,i)*m_U(l,i);
+			}
+			prev = m_ytask[l];
+		}
+		N += std::sqrt(norm);
+		for (j=0; j<m_nq; j++) {
+			for (l=0, prev=m_ytask[0], norm=e_scalar(0.), mag=e_scalar(0.); l<m_nc; l++) {
+				if (prev == m_ytask[l]) {
+					norm += m_WyAWq(l,j)*m_WyAWq(l,j);
+				} else {
+					mag += std::sqrt(norm);
+					norm = m_WyAWq(l,j)*m_WyAWq(l,j);
+				}
+				prev = m_ytask[l];
+			}
+			mag += std::sqrt(norm);
+			M += fabs(m_V(j,i))*mag;
+		}
+		M *= Sinv;
+		alpha = m_U.col(i).dot(m_Wy_ydot);
+		_qmax = (N < M) ? m_qmax*N/M : m_qmax;
+		vmax = m_WqV.col(i).array().abs().maxCoeff();
+		norm = fabs(Sinv*alpha*vmax);
+		if (norm > _qmax) {
+			damp = Sinv*alpha*_qmax/norm;
+		} else {
+			damp = Sinv*alpha;
+		}
+		qdot += m_WqV.col(i)*damp;
+		prevS = S;
+	}
+	if (maxDeltaS == e_scalar(0.0))
+		nlcoef = e_scalar(KDL::epsilon);
+	else
+		nlcoef = (maxS-maxDeltaS)/maxS;
+    return true;
+}
+
+}

blender-5.2.0/intern/itasc/WSDLSSolver.hpp

@@ -0,0 +1,44 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef WSDLSSOLVER_HPP_
+#define WSDLSSOLVER_HPP_
+
+#include "Solver.hpp"
+
+namespace iTaSC {
+
+class WSDLSSolver: public iTaSC::Solver {
+private:
+    e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV;
+    e_vector m_S,m_temp,m_Wy_ydot;
+	std::vector<bool> m_ytask;
+	e_scalar m_qmax;
+	unsigned int m_ns, m_nc, m_nq;
+	bool m_transpose;
+public:
+    WSDLSSolver();
+    virtual ~WSDLSSolver();
+
+    virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc);
+    virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
+	virtual void setParam(SolverParam param, double value)
+	{
+		switch (param) {
+		case DLS_QMAX:
+			m_qmax = value;
+			break;
+		default:
+			break;
+		}
+	}
+};
+
+}
+
+#endif /* WSDLSSOLVER_HPP_ */

blender-5.2.0/intern/itasc/WorldObject.cpp

@@ -0,0 +1,27 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "WorldObject.hpp"
+
+namespace iTaSC{
+
+/* special singleton to be used as base for uncontrolled object */
+WorldObject Object::world;
+
+WorldObject::WorldObject():UncontrolledObject()
+{
+	initialize(0,1);
+	m_internalPose = Frame::Identity();
+}
+
+WorldObject::~WorldObject() 
+{
+}
+
+
+}

blender-5.2.0/intern/itasc/WorldObject.hpp

@@ -0,0 +1,31 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef WORLDOBJECT_HPP_
+#define WORLDOBJECT_HPP_
+
+#include "UncontrolledObject.hpp"
+namespace iTaSC{
+
+class WorldObject: public UncontrolledObject {
+public:
+    WorldObject();
+    virtual ~WorldObject();
+
+	virtual void updateCoordinates(const Timestamp & /*timestamp*/){};
+	virtual void updateKinematics(const Timestamp & /*timestamp*/){};
+	virtual void pushCache(const Timestamp & /*timestamp*/){};
+	virtual void initCache(Cache * /*cache*/){};
+protected:
+	virtual void updateJacobian() {}
+
+};
+
+}
+
+#endif /* WORLDOBJECT_H_ */

blender-5.2.0/intern/itasc/eigen_types.cpp

@@ -0,0 +1,13 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "eigen_types.hpp"
+
+const KDL::Frame iTaSC::F_identity(Rotation::Identity(),Vector::Zero());
+
+

blender-5.2.0/intern/itasc/eigen_types.hpp

@@ -0,0 +1,85 @@
+/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#ifndef EIGEN_TYPES_HPP_
+#define EIGEN_TYPES_HPP_
+
+#include <Eigen/Core>
+#include "kdl/frames.hpp"
+#include "kdl/tree.hpp"
+#include "kdl/chain.hpp"
+#include "kdl/jacobian.hpp"
+#include "kdl/jntarray.hpp"
+
+
+namespace iTaSC{
+
+using KDL::Twist;
+using KDL::Frame;
+using KDL::Joint;
+using KDL::Inertia;
+using KDL::SegmentMap;
+using KDL::Tree;
+using KDL::JntArray;
+using KDL::Jacobian;
+using KDL::Segment;
+using KDL::Rotation;
+using KDL::Vector;
+using KDL::Vector2;
+using KDL::Chain;
+
+extern const Frame F_identity;
+
+#define e_scalar double
+#define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>
+#define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero
+#define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>
+#define e_matrix6 Eigen::Matrix<e_scalar,6,6>
+#define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity
+#define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant
+#define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero
+#define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random
+#define e_vector6 Eigen::Matrix<e_scalar,6,1>
+#define e_vector3 Eigen::Matrix<e_scalar,3,1>
+
+class Range {
+public:
+	int start;
+	int count;
+	Range(int _start, int _count) { start = _start; count=_count; }
+	Range(const Range& other) { start=other.start; count=other.count; }
+};
+
+template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r)
+{
+	return Eigen::Block<MatrixType>(m,r.start,0,r.count,1);
+}
+
+template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c)
+{
+	return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count);
+}
+
+template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {
+
+    if (J.rows() != 6)
+        return -1;
+    for (int j = 0; j < J.cols(); ++j) {
+		typename Derived::ColXpr Jj = J.col(j);
+		Twist arg;
+        for(unsigned int i=0;i<6;++i)
+            arg(i)=Jj[i];
+		Twist tmp(T*arg);
+        for(unsigned int i=0;i<6;++i)
+            Jj[i]=e_scalar(tmp(i));
+    }
+    return 0;
+}
+
+}
+#endif /* UBLAS_TYPES_HPP_ */

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