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(MT_Scalar 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(MT_Scalar 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