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