1 files changed, 99 insertions, 0 deletions
diff --git a/intern/itasc/CopyPose.hpp b/intern/itasc/CopyPose.hpp
new file mode 100644
index 00000000000..3a3f60a9f37
--- /dev/null
+++ b/intern/itasc/CopyPose.hpp
@@ -0,0 +1,99 @@
+/* $Id: CopyPose.hpp 20622 2009-06-04 12:47:59Z ben2610 $
+ * CopyPose.h
+ *
+ *  Created on: Mar 17, 2009
+ *      Author: benoit bolsee
+ */
+
+#ifndef COPYPOSE_H_
+#define COPYPOSE_H_
+
+#include "ConstraintSet.hpp"
+namespace iTaSC{
+
+using namespace KDL;
+
+class CopyPose: public iTaSC::ConstraintSet
+{
+protected:
+    virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+    virtual void updateJacobian();
+    virtual bool initialise(Frame& init_pose);
+    virtual void initCache(Cache *_cache);
+    virtual void updateControlOutput(const Timestamp& timestamp);
+	virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
+	virtual double getMaxTimestep(double& timestep);
+
+public:
+    enum ID {		// constraint ID in callback and setControlParameter
+		ID_POSITION=0,
+        ID_POSITIONX=1,
+        ID_POSITIONY=2,
+        ID_POSITIONZ=3,
+		ID_ROTATION=4,
+        ID_ROTATIONX=5,
+        ID_ROTATIONY=6,
+        ID_ROTATIONZ=7,
+    };
+	enum CTL {		// control ID in constructor to specify which output is constrainted
+		CTL_NONE=0x00,
+        CTL_POSITIONX=0x01,		// the bit order is important: it matches the y output order
+        CTL_POSITIONY=0x02,
+        CTL_POSITIONZ=0x04,
+		CTL_POSITION=0x07,
+        CTL_ROTATIONX=0x08,
+        CTL_ROTATIONY=0x10,
+        CTL_ROTATIONZ=0x20,
+		CTL_ROTATION=0x38,
+		CTL_ALL=0x3F,
+	};
+
+	// use a combination of CTL_.. in control_output to specify which 
+    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);
+    virtual ~CopyPose();
+
+    virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
+    virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
+
+private:
+    struct ConstraintSingleValue m_posData[3];	// index = controlled output in X,Y,Z order
+    struct ConstraintSingleValue m_rotData[3];
+    struct ConstraintValues m_values[2];		// index = group of controlled output, in position, rotation order
+    Cache* m_cache;
+    int m_poseCCh;
+    CacheTS m_poseCTs;
+	unsigned int m_poseCacheSize;
+	unsigned int m_outputDynamic;	// combination of CTL_... determine which variables are dynamically controlled by the application
+	unsigned int m_outputControl;	// combination of CTL_... determine which output are constrained
+	unsigned int m_nvalues;		// number of elements used in m_values[]
+	double m_maxerror;
+
+	struct ControlState {
+		int firsty;			// first y index
+		int ny;				// number of y in output
+		double alpha;
+		double K;
+		double tolerance;
+		struct ControlValue {
+			double yddot;
+			double yd;
+			double nextyd;
+			double nextyddot;
+		} output[3];		// inded numbex = same as m_rotData
+	} m_rot, m_pos;
+
+    void pushPose(CacheTS timestamp);
+    bool popPose(CacheTS timestamp);
+	int nBitsOn(unsigned int v)
+		{ int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
+	double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
+	double* pushValues(double* item, ControlState* _state, unsigned int mask);
+	void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
+	void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
+	void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
+	void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);
+
+};
+}
+#endif /* COPYROTATION_H_ */