Merge branch 'blender2.7'
[blender.git] / intern / itasc / ControlledObject.cpp
1 /** \file itasc/ControlledObject.cpp
2  *  \ingroup itasc
3  */
4 /*
5  * ControlledObject.cpp
6  *
7  *  Created on: Jan 5, 2009
8  *      Author: rubensmits
9  */
10
11 #include "ControlledObject.hpp"
12
13
14 namespace iTaSC {
15 ControlledObject::ControlledObject():
16     Object(Controlled),m_nq(0),m_nc(0),m_nee(0)
17 {
18         // max joint variable = 0.52 radian or 0.52 meter in one timestep
19         m_maxDeltaQ = e_scalar(0.52);
20 }
21
22 void ControlledObject::initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee)
23 {
24         assert(_nee >= 1);
25         m_nq = _nq;
26         m_nc = _nc;
27         m_nee = _nee;
28         if (m_nq > 0) {
29                 m_Wq =  e_identity_matrix(m_nq,m_nq);
30                 m_qdot = e_zero_vector(m_nq);
31         }
32         if (m_nc > 0) {
33                 m_Wy = e_scalar_vector(m_nc,1.0);
34                 m_ydot = e_zero_vector(m_nc);
35         }
36         if (m_nc > 0 && m_nq > 0)
37                 m_Cq = e_zero_matrix(m_nc,m_nq);
38         // clear all Jacobian if any
39         m_JqArray.clear();
40         // reserve one more to have a zero matrix handy
41         if (m_nq > 0)
42                 m_JqArray.resize(m_nee+1, e_zero_matrix(6,m_nq));
43 }
44
45 ControlledObject::~ControlledObject() {}
46
47
48
49 const e_matrix& ControlledObject::getJq(unsigned int ee) const
50 {
51         assert(m_nq > 0);
52         return m_JqArray[(ee>m_nee)?m_nee:ee];
53 }
54
55 double ControlledObject::getMaxTimestep(double& timestep)
56 {
57         e_scalar maxQdot = m_qdot.array().abs().maxCoeff();
58         if (timestep*maxQdot > m_maxDeltaQ) {
59                 timestep = m_maxDeltaQ/maxQdot;
60         }
61         return timestep;
62 }
63
64 }