bmesh: maintain active face when dissolving.
[blender.git] / intern / itasc / ConstraintSet.hpp
1 /*
2  * ConstraintSet.hpp
3  *
4  *  Created on: Jan 5, 2009
5  *      Author: rubensmits
6  */
7
8 #ifndef CONSTRAINTSET_HPP_
9 #define CONSTRAINTSET_HPP_
10
11 #include "kdl/frames.hpp"
12 #include "eigen_types.hpp"
13 #include "Cache.hpp"
14 #include <vector>
15
16 namespace iTaSC {
17
18 enum ConstraintAction {
19         ACT_NONE=               0,
20         ACT_VALUE=              1,
21         ACT_VELOCITY=   2,
22         ACT_TOLERANCE=  4,
23         ACT_FEEDBACK=   8,
24         ACT_ALPHA=              16
25 };
26
27 struct ConstraintSingleValue {
28         unsigned int id;        // identifier of constraint value, depends on constraint
29         unsigned int action;// action performed, compbination of ACT_..., set on return
30         const double y;         // actual constraint value
31         const double ydot;      // actual constraint velocity
32         double yd;                      // current desired constraint value, changed on return
33         double yddot;           // current desired constraint velocity, changed on return
34         ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {}
35 };
36
37 struct ConstraintValues {
38         unsigned int id;        // identifier of group of constraint values, depend on constraint
39         unsigned short number;          // number of constraints in list
40         unsigned short action;          // action performed, ACT_..., set on return
41         double alpha;           // constraint activation coefficient, should be [0..1]
42         double tolerance;       // current desired tolerance on constraint, same unit than yd, changed on return
43         double feedback;        // current desired feedback on error, in 1/sec, changed on return
44         struct ConstraintSingleValue* values;
45         ConstraintValues(): id(0), number(0), action(0), values(NULL) {}
46 };
47
48 class ConstraintSet;
49 typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
50
51 class ConstraintSet {
52 protected:
53     unsigned int m_nc;
54         e_scalar m_maxDeltaChi;
55         e_matrix m_Cf;
56     e_vector m_Wy,m_y,m_ydot;
57         e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta;
58     e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv;
59         KDL::Frame m_internalPose,m_externalPose;
60     ConstraintCallback m_constraintCallback;
61     void* m_constraintParam;
62         void* m_poseParam;
63     bool m_toggle;
64         bool m_substep;
65     double m_threshold;
66     unsigned int m_maxIter;
67
68         friend class Scene;
69         virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp);
70     virtual void updateKinematics(const Timestamp& timestamp)=0;
71     virtual void pushCache(const Timestamp& timestamp)=0;
72     virtual void updateJacobian()=0;
73     virtual void updateControlOutput(const Timestamp& timestamp)=0;
74         virtual void initCache(Cache *_cache) = 0;
75         virtual bool initialise(KDL::Frame& init_pose);
76         virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
77     virtual bool closeLoop();
78         virtual double getMaxTimestep(double& timestep);
79
80
81 public:
82     ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations);
83     ConstraintSet();
84     virtual ~ConstraintSet();
85
86         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
87
88         virtual bool registerCallback(ConstraintCallback _function, void* _param)
89         {
90                 m_constraintCallback = _function;
91                 m_constraintParam = _param;
92                 return true;
93         }
94
95     virtual const e_vector& getControlOutput()const{return m_ydot;};
96         virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0;
97         virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0;
98         bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0);
99
100     virtual const e_matrix6& getJf() const{return m_Jf;};
101         virtual const KDL::Frame& getPose() const{return m_internalPose;};
102     virtual const e_matrix& getCf() const{return m_Cf;};
103
104     virtual const e_vector& getWy() const {return m_Wy;};
105     virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;};
106     virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
107
108     virtual unsigned int getNrOfConstraints(){return m_nc;};
109         void substep(bool _substep) {m_substep=_substep;}
110         bool substep() {return m_substep;}
111 };
112
113 }
114
115 #endif /* CONSTRAINTSET_HPP_ */