synched with trunk at revision 34793
[blender.git] / intern / iksolver / intern / IK_QSegment.h
1 /**
2  * $Id$
3  * ***** BEGIN GPL LICENSE BLOCK *****
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software Foundation,
17  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
20  * All rights reserved.
21  *
22  * The Original Code is: all of this file.
23  *
24  * Original author: Laurence
25  * Contributor(s): Brecht
26  *
27  * ***** END GPL LICENSE BLOCK *****
28  */
29
30 #ifndef NAN_INCLUDED_IK_QSegment_h
31 #define NAN_INCLUDED_IK_QSegment_h
32
33 #include "MT_Vector3.h"
34 #include "MT_Transform.h"
35 #include "MT_Matrix4x4.h"
36 #include "IK_QJacobian.h"
37 #include "MEM_SmartPtr.h"
38
39 #include <vector>
40
41 /**
42  * An IK_Qsegment encodes information about a segments
43  * local coordinate system.
44  * 
45  * These segments always point along the y-axis.
46  * 
47  * Here we define the local coordinates of a joint as
48  * local_transform = 
49  * translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0)
50  * We use the standard moto column ordered matrices. You can read
51  * this as:
52  * - first translate by (0,length,0)
53  * - multiply by the rotation matrix derived from the current
54  * angle parameterization q.
55  * - multiply by the user defined matrix representing the rest
56  * position of the bone.
57  * - translate by the used defined translation (tr1)
58  * The ordering of these transformations is vital, you must
59  * use exactly the same transformations when displaying the segments
60  */
61
62 class IK_QSegment
63 {
64 public:
65         virtual ~IK_QSegment();
66
67         // start: a user defined translation
68         // rest_basis: a user defined rotation
69         // basis: a user defined rotation
70         // length: length of this segment
71
72         void SetTransform(
73                 const MT_Vector3& start,
74                 const MT_Matrix3x3& rest_basis,
75                 const MT_Matrix3x3& basis,
76                 const MT_Scalar length
77         );
78
79         // tree structure access
80         void SetParent(IK_QSegment *parent);
81
82         IK_QSegment *Child() const
83         { return m_child; }
84
85         IK_QSegment *Sibling() const
86         { return m_sibling; }
87
88         IK_QSegment *Parent() const
89         { return m_parent; }
90
91         // for combining two joints into one from the interface
92         void SetComposite(IK_QSegment *seg);
93         
94         IK_QSegment *Composite() const
95         { return m_composite; }
96
97         // number of degrees of freedom
98         int NumberOfDoF() const
99         { return m_num_DoF; }
100
101         // unique id for this segment, for identification in the jacobian
102         int DoFId() const
103         { return m_DoF_id; }
104
105         void SetDoFId(int dof_id)
106         { m_DoF_id = dof_id; }
107
108         // the max distance of the end of this bone from the local origin.
109         const MT_Scalar MaxExtension() const
110         { return m_max_extension; }
111
112         // the change in rotation and translation w.r.t. the rest pose
113         MT_Matrix3x3 BasisChange() const;
114         MT_Vector3 TranslationChange() const;
115
116         // the start and end of the segment
117         const MT_Point3 &GlobalStart() const
118         { return m_global_start; }
119
120         const MT_Point3 &GlobalEnd() const
121         { return m_global_transform.getOrigin(); }
122
123         // the global transformation at the end of the segment
124         const MT_Transform &GlobalTransform() const
125         { return m_global_transform; }
126
127         // is a translational segment?
128         bool Translational() const
129         { return m_translational; }
130
131         // locking (during inner clamping loop)
132         bool Locked(int dof) const
133         { return m_locked[dof]; }
134
135         void UnLock()
136         { m_locked[0] = m_locked[1] = m_locked[2] = false; }
137
138         // per dof joint weighting
139         MT_Scalar Weight(int dof) const
140         { return m_weight[dof]; }
141
142         void ScaleWeight(int dof, MT_Scalar scale)
143         { m_weight[dof] *= scale; }
144
145         // recursively update the global coordinates of this segment, 'global'
146         // is the global transformation from the parent segment
147         void UpdateTransform(const MT_Transform &global);
148
149         // get axis from rotation matrix for derivative computation
150         virtual MT_Vector3 Axis(int dof) const=0;
151
152         // update the angles using the dTheta's computed using the jacobian matrix
153         virtual bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*)=0;
154         virtual void Lock(int, IK_QJacobian&, MT_Vector3&) {}
155         virtual void UpdateAngleApply()=0;
156
157         // set joint limits
158         virtual void SetLimit(int, MT_Scalar, MT_Scalar) {};
159
160         // set joint weights (per axis)
161         virtual void SetWeight(int, MT_Scalar) {};
162
163         virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; }
164
165         // functions needed for pole vector constraint
166         void PrependBasis(const MT_Matrix3x3& mat);
167         void Reset();
168
169         // scale
170         virtual void Scale(float scale);
171
172 protected:
173
174         // num_DoF: number of degrees of freedom
175         IK_QSegment(int num_DoF, bool translational);
176
177         // remove child as a child of this segment
178         void RemoveChild(IK_QSegment *child);
179
180         // tree structure variables
181         IK_QSegment *m_parent;
182         IK_QSegment *m_child;
183         IK_QSegment *m_sibling;
184         IK_QSegment *m_composite;
185
186         // full transform = 
187         // start * rest_basis * basis * translation
188         MT_Vector3 m_start;
189         MT_Matrix3x3 m_rest_basis;
190         MT_Matrix3x3 m_basis;
191         MT_Vector3 m_translation;
192
193         // original basis
194         MT_Matrix3x3 m_orig_basis;
195         MT_Vector3 m_orig_translation;
196
197         // maximum extension of this segment
198         MT_Scalar m_max_extension;
199
200         // accumulated transformations starting from root
201         MT_Point3 m_global_start;
202         MT_Transform m_global_transform;
203
204         // number degrees of freedom, (first) id of this segments DOF's
205         int m_num_DoF, m_DoF_id;
206
207         bool m_locked[3];
208         bool m_translational;
209         MT_Scalar m_weight[3];
210 };
211
212 class IK_QSphericalSegment : public IK_QSegment
213 {
214 public:
215         IK_QSphericalSegment();
216
217         MT_Vector3 Axis(int dof) const;
218
219         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
220         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
221         void UpdateAngleApply();
222
223         bool ComputeClampRotation(MT_Vector3& clamp);
224
225         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
226         void SetWeight(int axis, MT_Scalar weight);
227
228 private:
229         MT_Matrix3x3 m_new_basis;
230         bool m_limit_x, m_limit_y, m_limit_z;
231         MT_Scalar m_min[2], m_max[2];
232         MT_Scalar m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
233         MT_Scalar m_locked_ax, m_locked_ay, m_locked_az;
234 };
235
236 class IK_QNullSegment : public IK_QSegment
237 {
238 public:
239         IK_QNullSegment();
240
241         bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*) { return false; }
242         void UpdateAngleApply() {}
243
244         MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
245         void SetBasis(const MT_Matrix3x3&) { m_basis.setIdentity(); }
246 };
247
248 class IK_QRevoluteSegment : public IK_QSegment
249 {
250 public:
251         // axis: the axis of the DoF, in range 0..2
252         IK_QRevoluteSegment(int axis);
253
254         MT_Vector3 Axis(int dof) const;
255
256         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
257         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
258         void UpdateAngleApply();
259
260         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
261         void SetWeight(int axis, MT_Scalar weight);
262         void SetBasis(const MT_Matrix3x3& basis);
263
264 private:
265         int m_axis;
266         MT_Scalar m_angle, m_new_angle;
267         bool m_limit;
268         MT_Scalar m_min, m_max;
269 };
270
271 class IK_QSwingSegment : public IK_QSegment
272 {
273 public:
274         // XZ DOF, uses one direct rotation
275         IK_QSwingSegment();
276
277         MT_Vector3 Axis(int dof) const;
278
279         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
280         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
281         void UpdateAngleApply();
282
283         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
284         void SetWeight(int axis, MT_Scalar weight);
285         void SetBasis(const MT_Matrix3x3& basis);
286
287 private:
288         MT_Matrix3x3 m_new_basis;
289         bool m_limit_x, m_limit_z;
290         MT_Scalar m_min[2], m_max[2];
291         MT_Scalar m_max_x, m_max_z, m_offset_x, m_offset_z;
292 };
293
294 class IK_QElbowSegment : public IK_QSegment
295 {
296 public:
297         // XY or ZY DOF, uses two sequential rotations: first rotate around
298         // X or Z, then rotate around Y (twist)
299         IK_QElbowSegment(int axis);
300
301         MT_Vector3 Axis(int dof) const;
302
303         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
304         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
305         void UpdateAngleApply();
306
307         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
308         void SetWeight(int axis, MT_Scalar weight);
309         void SetBasis(const MT_Matrix3x3& basis);
310
311 private:
312         int m_axis;
313
314         MT_Scalar m_twist, m_angle, m_new_twist, m_new_angle;
315         MT_Scalar m_cos_twist, m_sin_twist;
316
317         bool m_limit, m_limit_twist;
318         MT_Scalar m_min, m_max, m_min_twist, m_max_twist;
319 };
320
321 class IK_QTranslateSegment : public IK_QSegment
322 {
323 public:
324         // 1DOF, 2DOF or 3DOF translational segments
325         IK_QTranslateSegment(int axis1);
326         IK_QTranslateSegment(int axis1, int axis2);
327         IK_QTranslateSegment();
328
329         MT_Vector3 Axis(int dof) const;
330
331         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
332         void Lock(int, IK_QJacobian&, MT_Vector3&);
333         void UpdateAngleApply();
334
335         void SetWeight(int axis, MT_Scalar weight);
336         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
337
338         void Scale(float scale);
339
340 private:
341         int m_axis[3];
342         bool m_axis_enabled[3], m_limit[3];
343         MT_Vector3 m_new_translation;
344         MT_Scalar m_min[3], m_max[3];
345 };
346
347 #endif
348