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