Cycles / Math:
[blender.git] / intern / itasc / Armature.cpp
index 2491b0f8c9f47b38366bb16a0076b335290f43d2..e6f5fda406620a02f078ef44a6250996a0a1fa44 100644 (file)
@@ -1,4 +1,7 @@
-/* $Id$
+/** \file itasc/Armature.cpp
+ *  \ingroup itasc
+ */
+/*
  * Armature.cpp
  *
  *  Created on: Feb 3, 2009
@@ -7,8 +10,8 @@
 
 #include "Armature.hpp"
 #include <algorithm>
-#include <malloc.h>
 #include <string.h>
+#include <stdlib.h>
 
 namespace iTaSC {
 
@@ -29,15 +32,17 @@ Armature::Armature():
        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),
-       m_armlength(0.0)
+       m_fksolver(NULL)
 {
 }
 
@@ -117,6 +122,8 @@ Armature::JointConstraint_struct::JointConstraint_struct(SegmentMap::const_itera
                values[1].id = value[1].id = ID_JOINT_RZ;               
                v_nr = 2;
                break;
+       case Joint::None:
+               break;
        }
 }
 
@@ -153,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;
        }
 }
@@ -163,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();
@@ -250,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;
 }
@@ -262,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;
        }
@@ -314,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;
@@ -338,11 +345,6 @@ int Armature::addLimitConstraint(const std::string& segment_name, unsigned int d
        }
        if ((joint.getNDof() == 1 && dof > 0) || (joint.getNDof() == 2 && dof > 1))
                return -1;
-       if (joint.getType() < Joint::TransX || joint.getType() == Joint::Swing) {
-               // for rotation joint, the limit is given in degree, convert to radian
-               _min *= KDL::deg2rad;
-               _max *= KDL::deg2rad;
-       }
        Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof];
        p_joint.min = _min;
        p_joint.max = _max;
@@ -369,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];
@@ -390,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
@@ -410,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)
@@ -444,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];
@@ -621,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.
@@ -674,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;
                }
@@ -693,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);
@@ -732,6 +737,8 @@ bool Armature::setControlParameter(unsigned int constraintId, unsigned int value
                                case ACT_ALPHA:
                                        pConstraint->values[i].alpha = value;
                                        break;
+                               default:
+                                       break;
                                }
                        }
                } else {
@@ -753,6 +760,8 @@ bool Armature::setControlParameter(unsigned int constraintId, unsigned int value
                                        case ACT_ALPHA:
                                                pConstraint->values[i].alpha = value;
                                                break;
+                                       case ACT_NONE:
+                                               break;
                                        }
                                }
                        }