Merge branch 'blender2.7'
[blender.git] / intern / itasc / MovingFrame.cpp
1 /** \file itasc/MovingFrame.cpp
2  *  \ingroup itasc
3  */
4 /*
5  * MovingFrame.cpp
6  *
7  *  Created on: Feb 10, 2009
8  *      Author: benoitbolsee
9  */
10
11 #include "MovingFrame.hpp"
12 #include <string.h>
13 namespace iTaSC{
14
15 static const unsigned int frameCacheSize = (sizeof(((Frame*)0)->p.data)+sizeof(((Frame*)0)->M.data))/sizeof(double);
16
17 MovingFrame::MovingFrame(const Frame& frame):UncontrolledObject(),
18         m_function(NULL), m_param(NULL), m_velocity(), m_poseCCh(-1), m_poseCTs(0)
19 {
20         m_internalPose = m_nextPose = frame;
21         initialize(6, 1);
22         e_matrix& Ju = m_JuArray[0];
23         Ju = e_identity_matrix(6,6);
24 }
25
26 MovingFrame::~MovingFrame()
27 {
28 }
29
30 bool MovingFrame::finalize()
31 {
32         updateJacobian();
33         return true;
34 }
35
36 void MovingFrame::initCache(Cache *_cache)
37 {
38         m_cache = _cache;
39         m_poseCCh = -1;
40         if (m_cache) {
41                 m_poseCCh = m_cache->addChannel(this,"pose",frameCacheSize*sizeof(double));
42                 // don't store the initial pose, it's causing unnecessary large velocity on the first step
43                 //pushInternalFrame(0);
44         }
45 }
46
47 void MovingFrame::pushInternalFrame(CacheTS timestamp)
48 {
49         if (m_poseCCh >= 0) {
50                 double buf[frameCacheSize];
51                 memcpy(buf, m_internalPose.p.data, sizeof(m_internalPose.p.data));
52                 memcpy(&buf[sizeof(m_internalPose.p.data)/sizeof(double)], m_internalPose.M.data, sizeof(m_internalPose.M.data));
53
54                 m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, frameCacheSize, KDL::epsilon);
55                 m_poseCTs = timestamp;
56         }
57 }
58
59 // load pose just preceding timestamp
60 // return false if no cache position was found
61 bool MovingFrame::popInternalFrame(CacheTS timestamp)
62 {
63         if (m_poseCCh >= 0) {
64                 char *item;
65                 item = (char *)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
66                 if (item && m_poseCTs != timestamp) {
67                         memcpy(m_internalPose.p.data, item, sizeof(m_internalPose.p.data));
68                         item += sizeof(m_internalPose.p.data);
69                         memcpy(m_internalPose.M.data, item, sizeof(m_internalPose.M.data));
70                         m_poseCTs = timestamp;
71                         // changing the starting pose, recompute the jacobian
72                         updateJacobian();
73                 }
74                 return (item) ? true : false;
75         }
76         // in case of no cache, there is always a previous position
77         return true;
78 }
79
80 bool MovingFrame::setFrame(const Frame& frame)
81 {
82         m_internalPose = m_nextPose = frame;
83         return true;
84 }
85
86 bool MovingFrame::setCallback(MovingFrameCallback _function, void* _param)
87 {
88         m_function = _function;
89         m_param = _param;
90         return true;
91 }
92
93 void MovingFrame::updateCoordinates(const Timestamp& timestamp)
94 {
95         // don't compute the velocity during substepping, it is assumed constant.
96         if (!timestamp.substep) {
97                 bool cacheAvail = true;
98                 if (!timestamp.reiterate) {
99                         cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
100                         if (m_function)
101                                 (*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
102                 }
103                 // only compute velocity if we have a previous pose
104                 if (cacheAvail && timestamp.interpolate) {
105                         unsigned int iXu;
106                         m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
107                         for (iXu=0; iXu<6; iXu++)
108                                 m_xudot(iXu) = m_velocity(iXu);
109                 } else if (!timestamp.reiterate) {
110                         // new position is forced, no velocity as we cannot interpolate
111                         m_internalPose = m_nextPose;
112                         m_velocity = Twist::Zero();
113                         m_xudot = e_zero_vector(6);
114                         // recompute the jacobian
115                         updateJacobian();
116                 }
117         }
118 }
119
120 void MovingFrame::pushCache(const Timestamp& timestamp)
121 {
122         if (!timestamp.substep && timestamp.cache)
123                 pushInternalFrame(timestamp.cacheTimestamp);
124 }
125
126 void MovingFrame::updateKinematics(const Timestamp& timestamp)
127 {
128         if (timestamp.interpolate) {
129                 if (timestamp.substep) {
130                         // during substepping, update the internal pose from velocity information
131                         Twist localvel = m_internalPose.M.Inverse(m_velocity);
132                         m_internalPose.Integrate(localvel, 1.0/timestamp.realTimestep);
133                 } else {
134                         m_internalPose = m_nextPose;
135                 }
136                 // m_internalPose is updated, recompute the jacobian
137                 updateJacobian();
138         }
139         pushCache(timestamp);
140 }
141
142 void MovingFrame::updateJacobian()
143 {
144         Twist m_jac;
145         e_matrix& Ju = m_JuArray[0];
146
147     //Jacobian is always identity at position on the object, 
148         //we ust change the reference to the world.
149         //instead of going through complicated jacobian operation, implemented direct formula
150         Ju(1,3) = m_internalPose.p.z();
151         Ju(2,3) = -m_internalPose.p.y();
152         Ju(0,4) = -m_internalPose.p.z();
153         Ju(2,4) = m_internalPose.p.x();
154         Ju(0,5) = m_internalPose.p.y();
155         Ju(1,5) = -m_internalPose.p.x();
156         // remember that this object has moved
157         m_updated = true;
158 }
159
160 }