bmesh: maintain active face when dissolving.
[blender.git] / intern / itasc / CopyPose.cpp
1 /** \file itasc/CopyPose.cpp
2  *  \ingroup itasc
3  */
4 /*
5  * CopyPose.cpp
6  *
7  *  Created on: Mar 17, 2009
8  *      Author: benoit bolsee
9  */
10
11 #include "CopyPose.hpp"
12 #include "kdl/kinfam_io.hpp"
13 #include <math.h>
14 #include <string.h>
15
16 namespace iTaSC
17 {
18
19 const unsigned int maxPoseCacheSize = (2*(3+3*2));
20 CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations):
21     ConstraintSet(),
22         m_cache(NULL),
23     m_poseCCh(-1),m_poseCTs(0)
24 {
25         m_maxerror = armlength/2.0;
26         m_outputControl = (control_output & CTL_ALL);
27         unsigned int _nc = nBitsOn(m_outputControl);
28         if (!_nc) 
29                 return;
30         // reset the constraint set
31         reset(_nc, accuracy, maximum_iterations);
32         _nc = 0;
33         m_nvalues = 0;
34         int nrot = 0, npos = 0;
35         int nposCache = 0, nrotCache = 0;
36         m_outputDynamic = (dynamic_output & m_outputControl);
37         memset(m_values, 0, sizeof(m_values));
38         memset(m_posData, 0, sizeof(m_posData));
39         memset(m_rotData, 0, sizeof(m_rotData));
40         memset(&m_rot, 0, sizeof(m_rot));
41         memset(&m_pos, 0, sizeof(m_pos));
42         if (m_outputControl & CTL_POSITION) {
43                 m_pos.alpha = 1.0;              
44                 m_pos.K = 20.0;         
45                 m_pos.tolerance = 0.05; 
46                 m_values[m_nvalues].alpha = m_pos.alpha;
47                 m_values[m_nvalues].feedback = m_pos.K;
48                 m_values[m_nvalues].tolerance = m_pos.tolerance;
49                 m_values[m_nvalues].id = ID_POSITION;
50                 if (m_outputControl & CTL_POSITIONX) {
51                     m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
52                         m_Cf(_nc++,0)=1.0;
53                         m_posData[npos++].id = ID_POSITIONX;
54                         if (m_outputDynamic & CTL_POSITIONX)
55                                 nposCache++;
56                 } 
57                 if (m_outputControl & CTL_POSITIONY) {
58                     m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
59                         m_Cf(_nc++,1)=1.0;
60                         m_posData[npos++].id = ID_POSITIONY;
61                         if (m_outputDynamic & CTL_POSITIONY)
62                                 nposCache++;
63                 }
64                 if (m_outputControl & CTL_POSITIONZ) {
65                     m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
66                         m_Cf(_nc++,2)=1.0;
67                         m_posData[npos++].id = ID_POSITIONZ;
68                         if (m_outputDynamic & CTL_POSITIONZ)
69                                 nposCache++;
70                 }
71                 m_values[m_nvalues].number = npos;
72                 m_values[m_nvalues++].values = m_posData;
73                 m_pos.firsty = 0;
74                 m_pos.ny = npos;
75         }
76         if (m_outputControl & CTL_ROTATION) {
77                 m_rot.alpha = 1.0;              
78                 m_rot.K = 20.0;         
79                 m_rot.tolerance = 0.05; 
80                 m_values[m_nvalues].alpha = m_rot.alpha;
81                 m_values[m_nvalues].feedback = m_rot.K;
82                 m_values[m_nvalues].tolerance = m_rot.tolerance;
83                 m_values[m_nvalues].id = ID_ROTATION;
84                 if (m_outputControl & CTL_ROTATIONX) {
85                     m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
86                         m_Cf(_nc++,3)=1.0;
87                         m_rotData[nrot++].id = ID_ROTATIONX;
88                         if (m_outputDynamic & CTL_ROTATIONX)
89                                 nrotCache++;
90                 }
91                 if (m_outputControl & CTL_ROTATIONY) {
92                     m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
93                         m_Cf(_nc++,4)=1.0;
94                         m_rotData[nrot++].id = ID_ROTATIONY;
95                         if (m_outputDynamic & CTL_ROTATIONY)
96                                 nrotCache++;
97                 }
98                 if (m_outputControl & CTL_ROTATIONZ) {
99                     m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
100                         m_Cf(_nc++,5)=1.0;
101                         m_rotData[nrot++].id = ID_ROTATIONZ;
102                         if (m_outputDynamic & CTL_ROTATIONZ)
103                                 nrotCache++;
104                 }
105                 m_values[m_nvalues].number = nrot;
106                 m_values[m_nvalues++].values = m_rotData;
107                 m_rot.firsty = npos;
108                 m_rot.ny = nrot;
109         }
110         assert(_nc == m_nc);
111     m_Jf=e_identity_matrix(6,6);
112         m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
113 }
114
115 CopyPose::~CopyPose()
116 {
117 }
118
119 bool CopyPose::initialise(Frame& init_pose)
120 {
121     m_externalPose = m_internalPose = init_pose;
122         updateJacobian();
123     return true;
124 }
125
126 void CopyPose::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
127 {
128         m_internalPose = m_externalPose = _external_pose;
129         updateJacobian();
130 }
131
132 void CopyPose::initCache(Cache *_cache)
133 {
134     m_cache = _cache;
135     m_poseCCh = -1;
136     if (m_cache) {
137         // create one channel for the coordinates
138         m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize*sizeof(double));
139         // don't save initial value, it will be recomputed from external pose
140         //pushPose(0);
141     }
142 }
143
144 double* CopyPose::pushValues(double* item, ControlState* _state, unsigned int mask)
145 {
146         ControlState::ControlValue* _yval;
147         int i;
148
149         *item++ = _state->alpha;
150         *item++ = _state->K;
151         *item++ = _state->tolerance;
152
153         for (i=0, _yval=_state->output; i<_state->ny; mask<<=1) {
154                 if (m_outputControl & mask) {
155                         if (m_outputDynamic & mask) {
156                                 *item++ = _yval->yd;
157                                 *item++ = _yval->yddot;
158                         }
159                         _yval++;
160                         i++;
161                 }
162         }
163         return item;
164 }
165
166 void CopyPose::pushPose(CacheTS timestamp)
167 {
168     if (m_poseCCh >= 0) {
169                 if (m_poseCacheSize) {
170                         double buf[maxPoseCacheSize];
171                         double *item = buf;
172                         if (m_outputDynamic & CTL_POSITION)
173                                 item = pushValues(item, &m_pos, CTL_POSITIONX);
174                         if (m_outputDynamic & CTL_ROTATION)
175                                 item = pushValues(item, &m_rot, CTL_ROTATIONX);
176                         m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, m_poseCacheSize, KDL::epsilon);
177                 } else
178                         m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, NULL, 0, KDL::epsilon);
179                 m_poseCTs = timestamp;
180     }
181 }
182
183 double* CopyPose::restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask)
184 {
185         ConstraintSingleValue* _data;
186         ControlState::ControlValue* _yval;
187         int i, j;
188
189         _values->alpha = _state->alpha = *item++;
190         _values->feedback = _state->K = *item++;
191         _values->tolerance = _state->tolerance = *item++;
192
193         for (i=_state->firsty, j=i+_state->ny, _yval=_state->output, _data=_values->values; i<j; mask<<=1) {
194                 if (m_outputControl & mask) {
195                         m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
196                         if (m_outputDynamic & mask) {
197                                 _data->yd = _yval->yd = *item++;
198                                 _data->yddot = _yval->yddot = *item++;
199                         }
200                         _data++;
201                         _yval++;
202                         i++;
203                 }
204         }
205         return item;
206 }
207
208 bool CopyPose::popPose(CacheTS timestamp)
209 {
210         bool found = false;
211     if (m_poseCCh >= 0) {
212         double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
213                 if (item) {
214                         found = true;
215                         if (timestamp != m_poseCTs) {
216                                 int i=0;
217                                 if (m_outputControl & CTL_POSITION) {
218                                         if (m_outputDynamic & CTL_POSITION) {
219                                                 item = restoreValues(item, &m_values[i], &m_pos, CTL_POSITIONX);
220                                         }
221                                         i++;
222                                 }
223                                 if (m_outputControl & CTL_ROTATION) {
224                                         if (m_outputDynamic & CTL_ROTATION) {
225                                                 item = restoreValues(item, &m_values[i], &m_rot, CTL_ROTATIONX);
226                                         }
227                                         i++;
228                                 }
229                                 m_poseCTs = timestamp;
230                                 item = NULL;
231                         }
232         }
233     }
234     return found;
235 }
236
237 void CopyPose::interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp)
238 {
239         ControlState::ControlValue* _yval;
240         int i;
241
242         for (i=0, _yval=_state->output; i<_state->ny; mask <<= 1) {
243                 if (m_outputControl & mask) {
244                         if (m_outputDynamic & mask) {
245                                 if (timestamp.substep && timestamp.interpolate) {
246                                         _yval->yd += _yval->yddot*timestamp.realTimestep;
247                                 } else {
248                                         _yval->yd = _yval->nextyd;
249                                         _yval->yddot = _yval->nextyddot;
250                                 }
251                         }
252                         i++;
253                         _yval++;
254                 }
255         }
256 }
257
258 void CopyPose::pushCache(const Timestamp& timestamp)
259 {
260         if (!timestamp.substep && timestamp.cache) {
261         pushPose(timestamp.cacheTimestamp);
262         }
263 }
264
265 void CopyPose::updateKinematics(const Timestamp& timestamp)
266 {
267         if (timestamp.interpolate) {
268                 if (m_outputDynamic & CTL_POSITION)
269                         interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
270                 if (m_outputDynamic & CTL_ROTATION)
271                         interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp);
272         }
273         pushCache(timestamp);
274 }
275
276 void CopyPose::updateJacobian()
277 {
278     //Jacobian is always identity at the start of the constraint chain
279         //instead of going through complicated jacobian operation, implemented direct formula
280         //m_Jf(1,3) = m_internalPose.p.z();
281         //m_Jf(2,3) = -m_internalPose.p.y();
282         //m_Jf(0,4) = -m_internalPose.p.z();
283         //m_Jf(2,4) = m_internalPose.p.x();
284         //m_Jf(0,5) = m_internalPose.p.y();
285         //m_Jf(1,5) = -m_internalPose.p.x();
286 }
287
288 void CopyPose::updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep)
289 {
290         unsigned int id = (mask == CTL_ROTATIONX) ? ID_ROTATIONX : ID_POSITIONX;
291         ControlState::ControlValue* _yval;
292         ConstraintSingleValue* _data;
293         int i, j, k;
294     int action = 0;
295
296     if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
297         _state->alpha = _values->alpha;
298         action |= ACT_ALPHA;
299     }
300     if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
301         _state->tolerance = _values->tolerance;
302         action |= ACT_TOLERANCE;
303     }
304     if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
305         _state->K = _values->feedback;
306         action |= ACT_FEEDBACK;
307     }
308         for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j; mask <<= 1, id++) {
309                 if (m_outputControl & mask) {
310                         if (action)
311                                 m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
312                         // check if this controlled output is provided
313                         for (k=0, _data=_values->values; k<_values->number; k++, _data++) {
314                                 if (_data->id == id) {
315                                         switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
316                                         case 0:
317                                                 // no indication, keep current values
318                                                 break;
319                                         case ACT_VELOCITY:
320                                                 // only the velocity is given estimate the new value by integration
321                                                 _data->yd = _yval->yd+_data->yddot*timestep;
322                                                 // walkthrough
323                                         case ACT_VALUE:
324                                                 _yval->nextyd = _data->yd;
325                                                 // if the user sets the value, we assume future velocity is zero
326                                                 // (until the user changes the value again)
327                                                 _yval->nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
328                                                 if (timestep>0.0) {
329                                                         _yval->yddot = (_data->yd-_yval->yd)/timestep;
330                                                 } else {
331                                                         // allow the user to change target instantenously when this function
332                                                         // if called from setControlParameter with timestep = 0
333                                                         _yval->yd = _yval->nextyd;
334                                                         _yval->yddot = _yval->nextyddot;
335                                                 }
336                                                 break;
337                                         case (ACT_VALUE|ACT_VELOCITY):
338                                                 // the user should not set the value and velocity at the same time.
339                                                 // In this case, we will assume that he wants to set the future value
340                                                 // and we compute the current value to match the velocity
341                                                 _yval->yd = _data->yd - _data->yddot*timestep;
342                                                 _yval->nextyd = _data->yd;
343                                                 _yval->nextyddot = _data->yddot;
344                                                 if (timestep>0.0) {
345                                                         _yval->yddot = (_data->yd-_yval->yd)/timestep;
346                                                 } else {
347                                                         _yval->yd = _yval->nextyd;
348                                                         _yval->yddot = _yval->nextyddot;
349                                                 }
350                                                 break;
351                                         }
352                                 }
353                         }
354                         _yval++;
355                         i++;
356                 }
357         }
358 }
359
360
361 bool CopyPose::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
362 {
363         while (_nvalues > 0) {
364                 if (_values->id >= ID_POSITION && _values->id <= ID_POSITIONZ && (m_outputControl & CTL_POSITION)) {
365                         updateState(_values, &m_pos, CTL_POSITIONX, timestep);
366                 } 
367                 if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ && (m_outputControl & CTL_ROTATION)) {
368                         updateState(_values, &m_rot, CTL_ROTATIONX, timestep);
369                 }
370                 _values++;
371                 _nvalues--;
372         }
373     return true;
374 }
375
376 void CopyPose::updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask)
377 {
378         ConstraintSingleValue* _data;
379         ControlState::ControlValue* _yval;
380         int i, j;
381
382         _values->action = 0;
383
384         for (i=_state->firsty, j=0, _yval=_state->output, _data=_values->values; j<3; j++, mask<<=1) {
385                 if (m_outputControl & mask) {
386                         *(double*)&_data->y = vel(j);
387                         *(double*)&_data->ydot = m_ydot(i);
388                         _data->yd = _yval->yd;
389                         _data->yddot = _yval->yddot;
390                         _data->action = 0;
391                         i++;
392                         _data++;
393                         _yval++;
394                 }
395         }
396 }
397
398 void CopyPose::updateOutput(Vector& vel, ControlState* _state, unsigned int mask)
399 {
400         ControlState::ControlValue* _yval;
401         int i, j;
402         double coef=1.0;
403         if (mask & CTL_POSITION) {
404                 // put a limit on position error
405                 double len=0.0;
406                 for (j=0, _yval=_state->output; j<3; j++) {
407                         if (m_outputControl & (mask<<j)) {
408                                 len += KDL::sqr(_yval->yd-vel(j));
409                                 _yval++;
410                         }
411                 }
412                 len = KDL::sqrt(len);
413                 if (len > m_maxerror)
414                         coef = m_maxerror/len;
415         }
416         for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) {
417                 if (m_outputControl & (mask<<j)) {
418                         m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd-vel(j));
419                         _yval++;
420                         i++;
421                 }
422         }
423 }
424
425 void CopyPose::updateControlOutput(const Timestamp& timestamp)
426 {
427     //IMO this should be done, no idea if it is enough (wrt Distance impl)
428         Twist y = diff(F_identity, m_internalPose);
429         bool found = true;
430         if (!timestamp.substep) {
431                 if (!timestamp.reiterate) {
432                         found = popPose(timestamp.cacheTimestamp);
433                 }
434         }
435         if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
436                 // initialize first callback the application to get the current values
437                 int i=0;
438                 if (m_outputControl & CTL_POSITION) {
439                         updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
440                 }
441                 if (m_outputControl & CTL_ROTATION) {
442                         updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
443                 }
444                 if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
445                         setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0);
446                 }
447         }
448         if (m_outputControl & CTL_POSITION) {
449                 updateOutput(y.vel, &m_pos, CTL_POSITIONX);
450         }
451         if (m_outputControl & CTL_ROTATION) {
452                 updateOutput(y.rot, &m_rot, CTL_ROTATIONX);
453         }
454 }
455
456 const ConstraintValues* CopyPose::getControlParameters(unsigned int* _nvalues)
457 {
458         Twist y = diff(m_internalPose,F_identity);
459         int i=0;
460         if (m_outputControl & CTL_POSITION) {
461                 updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
462         }
463         if (m_outputControl & CTL_ROTATION) {
464                 updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
465         }
466         if (_nvalues)
467                 *_nvalues=m_nvalues; 
468         return m_values; 
469 }
470
471 double CopyPose::getMaxTimestep(double& timestep)
472 {
473         // CopyPose should not have any limit on linear velocity: 
474         // in case the target is out of reach, this can be very high.
475         // We will simply limit on rotation
476         e_scalar maxChidot = m_chidot.block(3,0,3,1).array().abs().maxCoeff();
477         if (timestep*maxChidot > m_maxDeltaChi) {
478                 timestep = m_maxDeltaChi/maxChidot;
479         }
480         return timestep;
481 }
482
483 }