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