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