Cycles / Math:
[blender.git] / intern / itasc / Armature.cpp
index 46d114e6c3f1fe860ef60eb4b7a91b31186f01e0..e6f5fda406620a02f078ef44a6250996a0a1fa44 100644 (file)
@@ -1,4 +1,7 @@
-/* $Id$
+/** \file itasc/Armature.cpp
+ *  \ingroup itasc
+ */
+/*
  * Armature.cpp
  *
  *  Created on: Feb 3, 2009
@@ -29,7 +32,9 @@ Armature::Armature():
        m_qCCh(-1),
        m_qCTs(0),
        m_yCCh(-1),
+#if 0
        m_yCTs(0),
+#endif
        m_qKdl(),
        m_oldqKdl(),
        m_newqKdl(),
@@ -155,7 +160,7 @@ 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_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, m_qKdl(0), m_qKdl.rows(), KDL::epsilon);
                m_qCTs = timestamp;
        }
 }
@@ -165,10 +170,10 @@ bool Armature::popQ(CacheTS timestamp)
 {
        if (m_qCCh >= 0) {
                double* item;
-               item = (double*)m_cache->getPreviousCacheItem(this, m_qCCh, &timestamp);
+               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(q));
+                       double* q = m_qKdl(0);
+                       memcpy(q, item, m_qKdl.rows()*sizeof(double));
                        m_qCTs = timestamp;
                        // changing the joint => recompute the jacobian
                        updateJacobian();
@@ -252,7 +257,7 @@ bool Armature::getSegment(const std::string& name, const unsigned int q_size, co
        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);
+               (&q)[dof] = m_qKdl[sit->second.q_nr+dof];
        }
        return true;
 }
@@ -264,7 +269,7 @@ double Armature::getMaxJointChange()
        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));
+               double joint = fabs(m_oldqKdl[i]-m_qKdl[i]);
                if (maxJoint < joint)
                        maxJoint = joint;
        }
@@ -316,7 +321,7 @@ int Armature::addConstraint(const std::string& segment_name, ConstraintCallback
                        return iConstraint;
                }
        }
-       if (m_finalized)  {
+       if (m_finalized) {
                if (_freeParam && _param)
                        free(_param);
                return -1;
@@ -366,11 +371,13 @@ int Armature::addEndEffector(const std::string& name)
        return m_neffector++;
 }
 
-void Armature::finalize()
+bool Armature::finalize()
 {
        unsigned int i, j, c;
        if (m_finalized)
-               return;
+               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];
@@ -387,7 +394,7 @@ void Armature::finalize()
        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;
+               m_newqKdl[i] = m_oldqKdl[i] = m_qKdl[i] = m_joints[i].rest;
        }
        updateJacobian();
        // estimate the maximum size of the robot arms
@@ -407,6 +414,7 @@ void Armature::finalize()
        if (m_armlength < KDL::epsilon)
                m_armlength = KDL::epsilon;
        m_finalized = true;
+       return true;
 }
 
 void Armature::pushCache(const Timestamp& timestamp)
@@ -441,15 +449,15 @@ bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callba
        // 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* 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)
-               m_qdotKdl(q_nr)=m_qdot(q_nr);
+               qdot[q_nr]=m_qdot[q_nr];
 
        for (q_nr=0; q_nr<m_nq; ) {
                Joint_struct* joint = &m_joints[q_nr];
@@ -618,7 +626,7 @@ void Armature::updateKinematics(const Timestamp& timestamp){
                return;
 
        // the new joint value have been computed already, just copy
-       memcpy(&m_qKdl(0), &m_newqKdl(0), sizeof(double)*m_qKdl.rows());
+       memcpy(m_qKdl(0), m_newqKdl(0), sizeof(double)*m_qKdl.rows());
        pushCache(timestamp);
        updateJacobian();
        // here update the desired output.
@@ -671,7 +679,7 @@ void Armature::updateControlOutput(const Timestamp& timestamp)
 
        if (!timestamp.substep) {
                // save previous joint state for getMaxJointChange()
-               memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows());
+               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;
                }
@@ -690,8 +698,8 @@ void Armature::updateControlOutput(const Timestamp& timestamp)
                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);
+                       *(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);