Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / CopyPose.hpp
1 /* $Id: CopyPose.hpp 20622 2009-06-04 12:47:59Z ben2610 $
2  * CopyPose.h
3  *
4  *  Created on: Mar 17, 2009
5  *      Author: benoit bolsee
6  */
7
8 #ifndef COPYPOSE_H_
9 #define COPYPOSE_H_
10
11 #include "ConstraintSet.hpp"
12 namespace iTaSC{
13
14 using namespace KDL;
15
16 class CopyPose: public iTaSC::ConstraintSet
17 {
18 protected:
19     virtual void updateKinematics(const Timestamp& timestamp);
20     virtual void pushCache(const Timestamp& timestamp);
21     virtual void updateJacobian();
22     virtual bool initialise(Frame& init_pose);
23     virtual void initCache(Cache *_cache);
24     virtual void updateControlOutput(const Timestamp& timestamp);
25         virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
26         virtual double getMaxTimestep(double& timestep);
27
28 public:
29     enum ID {           // constraint ID in callback and setControlParameter
30                 ID_POSITION=0,
31         ID_POSITIONX=1,
32         ID_POSITIONY=2,
33         ID_POSITIONZ=3,
34                 ID_ROTATION=4,
35         ID_ROTATIONX=5,
36         ID_ROTATIONY=6,
37         ID_ROTATIONZ=7,
38     };
39         enum CTL {              // control ID in constructor to specify which output is constrainted
40                 CTL_NONE=0x00,
41         CTL_POSITIONX=0x01,             // the bit order is important: it matches the y output order
42         CTL_POSITIONY=0x02,
43         CTL_POSITIONZ=0x04,
44                 CTL_POSITION=0x07,
45         CTL_ROTATIONX=0x08,
46         CTL_ROTATIONY=0x10,
47         CTL_ROTATIONZ=0x20,
48                 CTL_ROTATION=0x38,
49                 CTL_ALL=0x3F,
50         };
51
52         // use a combination of CTL_.. in control_output to specify which 
53     CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
54     virtual ~CopyPose();
55
56     virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
57     virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
58
59 private:
60     struct ConstraintSingleValue m_posData[3];  // index = controlled output in X,Y,Z order
61     struct ConstraintSingleValue m_rotData[3];
62     struct ConstraintValues m_values[2];                // index = group of controlled output, in position, rotation order
63     Cache* m_cache;
64     int m_poseCCh;
65     CacheTS m_poseCTs;
66         unsigned int m_poseCacheSize;
67         unsigned int m_outputDynamic;   // combination of CTL_... determine which variables are dynamically controlled by the application
68         unsigned int m_outputControl;   // combination of CTL_... determine which output are constrained
69         unsigned int m_nvalues;         // number of elements used in m_values[]
70         double m_maxerror;
71
72         struct ControlState {
73                 int firsty;                     // first y index
74                 int ny;                         // number of y in output
75                 double alpha;
76                 double K;
77                 double tolerance;
78                 struct ControlValue {
79                         double yddot;
80                         double yd;
81                         double nextyd;
82                         double nextyddot;
83                 } output[3];            // inded numbex = same as m_rotData
84         } m_rot, m_pos;
85
86     void pushPose(CacheTS timestamp);
87     bool popPose(CacheTS timestamp);
88         int nBitsOn(unsigned int v)
89                 { int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
90         double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
91         double* pushValues(double* item, ControlState* _state, unsigned int mask);
92         void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
93         void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
94         void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
95         void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);
96
97 };
98 }
99 #endif /* COPYROTATION_H_ */