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