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