Bugfix; rotation limits for < 3 DOF bones were using wrong reference
[blender.git] / intern / iksolver / intern / IK_QSegment.h
1 /**
2  * $Id$
3  * ***** BEGIN GPL/BL DUAL 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. The Blender
9  * Foundation also sells licenses for use in proprietary software under
10  * the Blender License.  See http://www.blender.org/BL/ for information
11  * about this.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software Foundation,
20  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21  *
22  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
23  * All rights reserved.
24  *
25  * The Original Code is: all of this file.
26  *
27  * Original author: Laurence
28  * Contributor(s): Brecht
29  *
30  * ***** END GPL/BL DUAL LICENSE BLOCK *****
31  */
32
33 #ifndef NAN_INCLUDED_IK_QSegment_h
34 #define NAN_INCLUDED_IK_QSegment_h
35
36 #include "MT_Vector3.h"
37 #include "MT_Transform.h"
38 #include "MT_Matrix4x4.h"
39 #include "IK_QJacobian.h"
40 #include "MEM_SmartPtr.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         // number of degrees of freedom
95         int NumberOfDoF() const
96         { return m_num_DoF; }
97
98         // unique id for this segment, for identification in the jacobian
99         int DoFId() const
100         { return m_DoF_id; }
101
102         void SetDoFId(int dof_id)
103         { m_DoF_id = dof_id; }
104
105         // the max distance of the end of this bone from the local origin.
106         const MT_Scalar MaxExtension() const
107         { return m_max_extension; }
108
109         // the change in rotation and translation w.r.t. the rest pose
110         MT_Matrix3x3 BasisChange() const;
111         MT_Vector3 TranslationChange() const;
112
113         // the start and end of the segment
114         const MT_Point3 &GlobalStart() const
115         { return m_global_start; }
116
117         const MT_Point3 &GlobalEnd() const
118         { return m_global_transform.getOrigin(); }
119
120         // the global transformation at the end of the segment
121         const MT_Transform &GlobalTransform() const
122         { return m_global_transform; }
123
124         // is a translational segment?
125         bool Translational() const
126         { return m_translational; }
127
128         // locking (during inner clamping loop)
129         bool Locked(int dof) const
130         { return m_locked[dof]; }
131
132         void UnLock()
133         { m_locked[0] = m_locked[1] = m_locked[2] = false; }
134
135         // per dof joint weighting
136         MT_Scalar Weight(int dof) const
137         { return m_weight[dof]; }
138
139         void ScaleWeight(int dof, MT_Scalar scale)
140         { m_weight[dof] *= scale; }
141
142         // recursively update the global coordinates of this segment, 'global'
143         // is the global transformation from the parent segment
144         void UpdateTransform(const MT_Transform &global);
145
146         // get axis from rotation matrix for derivative computation
147         virtual MT_Vector3 Axis(int dof) const=0;
148
149         // update the angles using the dTheta's computed using the jacobian matrix
150         virtual bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*)=0;
151         virtual void Lock(int, IK_QJacobian&, MT_Vector3&) {}
152         virtual void UpdateAngleApply()=0;
153
154         // set joint limits
155         virtual void SetLimit(int, MT_Scalar, MT_Scalar) {};
156
157         // set joint weights (per axis)
158         virtual void SetWeight(int, MT_Scalar) {};
159
160         virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; }
161
162 protected:
163
164         // num_DoF: number of degrees of freedom
165         IK_QSegment(int num_DoF, bool translational);
166
167         // remove child as a child of this segment
168         void RemoveChild(IK_QSegment *child);
169
170         // tree structure variables
171         IK_QSegment *m_parent;
172         IK_QSegment *m_child;
173         IK_QSegment *m_sibling;
174
175         // full transform = 
176         // start * rest_basis * basis * translation
177         MT_Vector3 m_start;
178         MT_Matrix3x3 m_rest_basis;
179         MT_Matrix3x3 m_basis;
180         MT_Vector3 m_translation;
181
182         // original basis
183         MT_Matrix3x3 m_orig_basis;
184         MT_Vector3 m_orig_translation;
185
186         // maximum extension of this segment
187         MT_Scalar m_max_extension;
188
189         // accumulated transformations starting from root
190         MT_Point3 m_global_start;
191         MT_Transform m_global_transform;
192
193         // number degrees of freedom, (first) id of this segments DOF's
194         int m_num_DoF, m_DoF_id;
195
196         bool m_locked[3];
197         bool m_translational;
198         MT_Scalar m_weight[3];
199 };
200
201 class IK_QSphericalSegment : public IK_QSegment
202 {
203 public:
204         IK_QSphericalSegment();
205
206         MT_Vector3 Axis(int dof) const;
207
208         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
209         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
210         void UpdateAngleApply();
211
212         bool ComputeClampRotation(MT_Vector3& clamp);
213
214         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
215         void SetWeight(int axis, MT_Scalar weight);
216
217 private:
218         MT_Matrix3x3 m_new_basis;
219         bool m_limit_x, m_limit_y, m_limit_z;
220         MT_Scalar m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
221         MT_Scalar m_locked_ax, m_locked_ay, m_locked_az;
222 };
223
224 class IK_QNullSegment : public IK_QSegment
225 {
226 public:
227         IK_QNullSegment();
228
229         bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*) { return false; }
230         void UpdateAngleApply() {}
231
232         MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
233         void SetBasis(const MT_Matrix3x3&) { m_basis.setIdentity(); }
234 };
235
236 class IK_QRevoluteSegment : public IK_QSegment
237 {
238 public:
239         // axis: the axis of the DoF, in range 0..2
240         IK_QRevoluteSegment(int axis);
241
242         MT_Vector3 Axis(int dof) const;
243
244         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
245         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
246         void UpdateAngleApply();
247
248         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
249         void SetWeight(int axis, MT_Scalar weight);
250         void SetBasis(const MT_Matrix3x3& basis);
251
252 private:
253         int m_axis;
254         MT_Scalar m_angle, m_new_angle;
255         MT_Scalar m_limit;
256         MT_Scalar m_min, m_max;
257 };
258
259 class IK_QSwingSegment : public IK_QSegment
260 {
261 public:
262         // XZ DOF, uses one direct rotation
263         IK_QSwingSegment();
264
265         MT_Vector3 Axis(int dof) const;
266
267         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
268         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
269         void UpdateAngleApply();
270
271         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
272         void SetWeight(int axis, MT_Scalar weight);
273         void SetBasis(const MT_Matrix3x3& basis);
274
275 private:
276         MT_Matrix3x3 m_new_basis;
277         bool m_limit_x, m_limit_z;
278         MT_Scalar m_max_x, m_max_z, m_offset_x, m_offset_z;
279 };
280
281 class IK_QElbowSegment : public IK_QSegment
282 {
283 public:
284         // XY or ZY DOF, uses two sequential rotations: first rotate around
285         // X or Z, then rotate around Y (twist)
286         IK_QElbowSegment(int axis);
287
288         MT_Vector3 Axis(int dof) const;
289
290         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
291         void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
292         void UpdateAngleApply();
293
294         void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
295         void SetWeight(int axis, MT_Scalar weight);
296         void SetBasis(const MT_Matrix3x3& basis);
297
298 private:
299         int m_axis;
300
301         MT_Scalar m_twist, m_angle, m_new_twist, m_new_angle;
302         MT_Scalar m_cos_twist, m_sin_twist;
303
304         bool m_limit, m_limit_twist;
305         MT_Scalar m_min, m_max, m_min_twist, m_max_twist;
306 };
307
308 class IK_QTranslateSegment : public IK_QSegment
309 {
310 public:
311         // Revolute, 2DOF or 3DOF translational segments
312         IK_QTranslateSegment(int axis1);
313         IK_QTranslateSegment(int axis1, int axis2);
314         IK_QTranslateSegment();
315
316         MT_Vector3 Axis(int dof) const;
317
318         bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
319         void Lock(int, IK_QJacobian&, MT_Vector3&) {};
320         void UpdateAngleApply();
321
322         void SetWeight(int axis, MT_Scalar weight);
323
324 private:
325         int m_axis[3];
326         bool m_axis_enabled[3];
327         MT_Vector3 m_new_translation;
328 };
329
330 #endif
331