2 * ***** BEGIN GPL LICENSE BLOCK *****
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.
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.
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.
18 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19 * All rights reserved.
21 * The Original Code is: all of this file.
23 * Original author: Laurence
24 * Contributor(s): Brecht
26 * ***** END GPL LICENSE BLOCK *****
29 /** \file iksolver/intern/IK_QSegment.h
34 #ifndef __IK_QSEGMENT_H__
35 #define __IK_QSEGMENT_H__
37 #include "MT_Vector3.h"
38 #include "MT_Transform.h"
39 #include "MT_Matrix4x4.h"
40 #include "IK_QJacobian.h"
45 * An IK_Qsegment encodes information about a segments
46 * local coordinate system.
48 * These segments always point along the y-axis.
50 * Here we define the local coordinates of a joint as
52 * translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0)
53 * We use the standard moto column ordered matrices. You can read
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
68 virtual ~IK_QSegment();
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
76 const MT_Vector3& start,
77 const MT_Matrix3x3& rest_basis,
78 const MT_Matrix3x3& basis,
79 const MT_Scalar length
82 // tree structure access
83 void SetParent(IK_QSegment *parent);
85 IK_QSegment *Child() const
88 IK_QSegment *Sibling() const
91 IK_QSegment *Parent() const
94 // for combining two joints into one from the interface
95 void SetComposite(IK_QSegment *seg);
97 IK_QSegment *Composite() const
98 { return m_composite; }
100 // number of degrees of freedom
101 int NumberOfDoF() const
102 { return m_num_DoF; }
104 // unique id for this segment, for identification in the jacobian
108 void SetDoFId(int dof_id)
109 { m_DoF_id = dof_id; }
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; }
115 // the change in rotation and translation w.r.t. the rest pose
116 MT_Matrix3x3 BasisChange() const;
117 MT_Vector3 TranslationChange() const;
119 // the start and end of the segment
120 const MT_Point3 &GlobalStart() const
121 { return m_global_start; }
123 const MT_Point3 &GlobalEnd() const
124 { return m_global_transform.getOrigin(); }
126 // the global transformation at the end of the segment
127 const MT_Transform &GlobalTransform() const
128 { return m_global_transform; }
130 // is a translational segment?
131 bool Translational() const
132 { return m_translational; }
134 // locking (during inner clamping loop)
135 bool Locked(int dof) const
136 { return m_locked[dof]; }
139 { m_locked[0] = m_locked[1] = m_locked[2] = false; }
141 // per dof joint weighting
142 MT_Scalar Weight(int dof) const
143 { return m_weight[dof]; }
145 void ScaleWeight(int dof, MT_Scalar scale)
146 { m_weight[dof] *= scale; }
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);
152 // get axis from rotation matrix for derivative computation
153 virtual MT_Vector3 Axis(int dof) const=0;
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;
161 virtual void SetLimit(int, MT_Scalar, MT_Scalar) {}
163 // set joint weights (per axis)
164 virtual void SetWeight(int, MT_Scalar) {}
166 virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; }
168 // functions needed for pole vector constraint
169 void PrependBasis(const MT_Matrix3x3& mat);
173 virtual void Scale(MT_Scalar scale);
177 // num_DoF: number of degrees of freedom
178 IK_QSegment(int num_DoF, bool translational);
180 // remove child as a child of this segment
181 void RemoveChild(IK_QSegment *child);
183 // tree structure variables
184 IK_QSegment *m_parent;
185 IK_QSegment *m_child;
186 IK_QSegment *m_sibling;
187 IK_QSegment *m_composite;
190 // start * rest_basis * basis * translation
192 MT_Matrix3x3 m_rest_basis;
193 MT_Matrix3x3 m_basis;
194 MT_Vector3 m_translation;
197 MT_Matrix3x3 m_orig_basis;
198 MT_Vector3 m_orig_translation;
200 // maximum extension of this segment
201 MT_Scalar m_max_extension;
203 // accumulated transformations starting from root
204 MT_Point3 m_global_start;
205 MT_Transform m_global_transform;
207 // number degrees of freedom, (first) id of this segments DOF's
208 int m_num_DoF, m_DoF_id;
211 bool m_translational;
212 MT_Scalar m_weight[3];
215 class IK_QSphericalSegment : public IK_QSegment
218 IK_QSphericalSegment();
220 MT_Vector3 Axis(int dof) const;
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();
226 bool ComputeClampRotation(MT_Vector3& clamp);
228 void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
229 void SetWeight(int axis, MT_Scalar weight);
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;
239 class IK_QNullSegment : public IK_QSegment
244 bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*) { return false; }
245 void UpdateAngleApply() {}
247 MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
248 void SetBasis(const MT_Matrix3x3&) { m_basis.setIdentity(); }
251 class IK_QRevoluteSegment : public IK_QSegment
254 // axis: the axis of the DoF, in range 0..2
255 IK_QRevoluteSegment(int axis);
257 MT_Vector3 Axis(int dof) const;
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();
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);
269 MT_Scalar m_angle, m_new_angle;
271 MT_Scalar m_min, m_max;
274 class IK_QSwingSegment : public IK_QSegment
277 // XZ DOF, uses one direct rotation
280 MT_Vector3 Axis(int dof) const;
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();
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);
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;
297 class IK_QElbowSegment : public IK_QSegment
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);
304 MT_Vector3 Axis(int dof) const;
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();
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);
317 MT_Scalar m_twist, m_angle, m_new_twist, m_new_angle;
318 MT_Scalar m_cos_twist, m_sin_twist;
320 bool m_limit, m_limit_twist;
321 MT_Scalar m_min, m_max, m_min_twist, m_max_twist;
324 class IK_QTranslateSegment : public IK_QSegment
327 // 1DOF, 2DOF or 3DOF translational segments
328 IK_QTranslateSegment(int axis1);
329 IK_QTranslateSegment(int axis1, int axis2);
330 IK_QTranslateSegment();
332 MT_Vector3 Axis(int dof) const;
334 bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
335 void Lock(int, IK_QJacobian&, MT_Vector3&);
336 void UpdateAngleApply();
338 void SetWeight(int axis, MT_Scalar weight);
339 void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
341 void Scale(MT_Scalar scale);
345 bool m_axis_enabled[3], m_limit[3];
346 MT_Vector3 m_new_translation;
347 MT_Scalar m_min[3], m_max[3];