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