Blender  V3.3
IK_QJacobianSolver.cpp
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 #include <stdio.h>
9 
10 #include "IK_QJacobianSolver.h"
11 
12 //#include "analyze.h"
14 {
15  m_poleconstraint = false;
16  m_getpoleangle = false;
17  m_rootmatrix.setIdentity();
18 }
19 
20 double IK_QJacobianSolver::ComputeScale()
21 {
22  std::vector<IK_QSegment *>::iterator seg;
23  double length = 0.0f;
24 
25  for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
26  length += (*seg)->MaxExtension();
27 
28  if (length == 0.0)
29  return 1.0;
30  else
31  return 1.0 / length;
32 }
33 
34 void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *> &tasks)
35 {
36  std::list<IK_QTask *>::iterator task;
37  std::vector<IK_QSegment *>::iterator seg;
38 
39  for (task = tasks.begin(); task != tasks.end(); task++)
40  (*task)->Scale(scale);
41 
42  for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
43  (*seg)->Scale(scale);
44 
45  m_rootmatrix.translation() *= scale;
46  m_goal *= scale;
47  m_polegoal *= scale;
48 }
49 
50 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
51 {
52  m_segments.push_back(seg);
53 
54  IK_QSegment *child;
55  for (child = seg->Child(); child; child = child->Sibling())
56  AddSegmentList(child);
57 }
58 
59 bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks)
60 {
61  m_segments.clear();
62  AddSegmentList(root);
63 
64  // assign each segment a unique id for the jacobian
65  std::vector<IK_QSegment *>::iterator seg;
66  int num_dof = 0;
67 
68  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
69  (*seg)->SetDoFId(num_dof);
70  num_dof += (*seg)->NumberOfDoF();
71  }
72 
73  if (num_dof == 0)
74  return false;
75 
76  // compute task ids and assign weights to task
77  int primary_size = 0, primary = 0;
78  int secondary_size = 0, secondary = 0;
79  double primary_weight = 0.0, secondary_weight = 0.0;
80  std::list<IK_QTask *>::iterator task;
81 
82  for (task = tasks.begin(); task != tasks.end(); task++) {
83  IK_QTask *qtask = *task;
84 
85  if (qtask->Primary()) {
86  qtask->SetId(primary_size);
87  primary_size += qtask->Size();
88  primary_weight += qtask->Weight();
89  primary++;
90  }
91  else {
92  qtask->SetId(secondary_size);
93  secondary_size += qtask->Size();
94  secondary_weight += qtask->Weight();
95  secondary++;
96  }
97  }
98 
99  if (primary_size == 0 || FuzzyZero(primary_weight))
100  return false;
101 
102  m_secondary_enabled = (secondary > 0);
103 
104  // rescale weights of tasks to sum up to 1
105  double primary_rescale = 1.0 / primary_weight;
106  double secondary_rescale;
107  if (FuzzyZero(secondary_weight))
108  secondary_rescale = 0.0;
109  else
110  secondary_rescale = 1.0 / secondary_weight;
111 
112  for (task = tasks.begin(); task != tasks.end(); task++) {
113  IK_QTask *qtask = *task;
114 
115  if (qtask->Primary())
116  qtask->SetWeight(qtask->Weight() * primary_rescale);
117  else
118  qtask->SetWeight(qtask->Weight() * secondary_rescale);
119  }
120 
121  // set matrix sizes
122  m_jacobian.ArmMatrices(num_dof, primary_size);
123  if (secondary > 0)
124  m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
125 
126  // set dof weights
127  int i;
128 
129  for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
130  for (i = 0; i < (*seg)->NumberOfDoF(); i++)
131  m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
132 
133  return true;
134 }
135 
137  IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle)
138 {
139  m_poleconstraint = true;
140  m_poletip = tip;
141  m_goal = goal;
142  m_polegoal = polegoal;
143  m_poleangle = (getangle) ? 0.0f : poleangle;
144  m_getpoleangle = getangle;
145 }
146 
147 void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *> &tasks)
148 {
149  // this function will be called before and after solving. calling it before
150  // solving gives predictable solutions by rotating towards the solution,
151  // and calling it afterwards ensures the solution is exact.
152 
153  if (!m_poleconstraint)
154  return;
155 
156  // disable pole vector constraint in case of multiple position tasks
157  std::list<IK_QTask *>::iterator task;
158  int positiontasks = 0;
159 
160  for (task = tasks.begin(); task != tasks.end(); task++)
161  if ((*task)->PositionTask())
162  positiontasks++;
163 
164  if (positiontasks >= 2) {
165  m_poleconstraint = false;
166  return;
167  }
168 
169  // get positions and rotations
170  root->UpdateTransform(m_rootmatrix);
171 
172  const Vector3d rootpos = root->GlobalStart();
173  const Vector3d endpos = m_poletip->GlobalEnd();
174  const Matrix3d &rootbasis = root->GlobalTransform().linear();
175 
176  // construct "lookat" matrices (like gluLookAt), based on a direction and
177  // an up vector, with the direction going from the root to the end effector
178  // and the up vector going from the root to the pole constraint position.
179  Vector3d dir = normalize(endpos - rootpos);
180  Vector3d rootx = rootbasis.col(0);
181  Vector3d rootz = rootbasis.col(2);
182  Vector3d up = rootx * cos(m_poleangle) + rootz * sin(m_poleangle);
183 
184  // in post, don't rotate towards the goal but only correct the pole up
185  Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
186  Vector3d poleup = normalize(m_polegoal - rootpos);
187 
188  Matrix3d mat, polemat;
189 
190  mat.row(0) = normalize(dir.cross(up));
191  mat.row(1) = mat.row(0).cross(dir);
192  mat.row(2) = -dir;
193 
194  polemat.row(0) = normalize(poledir.cross(poleup));
195  polemat.row(1) = polemat.row(0).cross(poledir);
196  polemat.row(2) = -poledir;
197 
198  if (m_getpoleangle) {
199  // we compute the pole angle that to rotate towards the target
200  m_poleangle = angle(mat.row(1), polemat.row(1));
201 
202  double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle));
203  if (dt > 0.0)
204  m_poleangle = -m_poleangle;
205 
206  // solve again, with the pole angle we just computed
207  m_getpoleangle = false;
208  ConstrainPoleVector(root, tasks);
209  }
210  else {
211  // now we set as root matrix the difference between the current and
212  // desired rotation based on the pole vector constraint. we use
213  // transpose instead of inverse because we have orthogonal matrices
214  // anyway, and in case of a singular matrix we don't get NaN's.
215  Affine3d trans;
216  trans.linear() = polemat.transpose() * mat;
217  trans.translation() = Vector3d(0, 0, 0);
218  m_rootmatrix = trans * m_rootmatrix;
219  }
220 }
221 
222 bool IK_QJacobianSolver::UpdateAngles(double &norm)
223 {
224  // assign each segment a unique id for the jacobian
225  std::vector<IK_QSegment *>::iterator seg;
226  IK_QSegment *qseg, *minseg = NULL;
227  double minabsdelta = 1e10, absdelta;
228  Vector3d delta, mindelta;
229  bool locked = false, clamp[3];
230  int i, mindof = 0;
231 
232  // here we check if any angle limits were violated. angles whose clamped
233  // position is the same as it was before, are locked immediate. of the
234  // other violation angles the most violating angle is rememberd
235  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
236  qseg = *seg;
237  if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
238  for (i = 0; i < qseg->NumberOfDoF(); i++) {
239  if (clamp[i] && !qseg->Locked(i)) {
240  absdelta = fabs(delta[i]);
241 
242  if (absdelta < IK_EPSILON) {
243  qseg->Lock(i, m_jacobian, delta);
244  locked = true;
245  }
246  else if (absdelta < minabsdelta) {
247  minabsdelta = absdelta;
248  mindelta = delta;
249  minseg = qseg;
250  mindof = i;
251  }
252  }
253  }
254  }
255  }
256 
257  // lock most violating angle
258  if (minseg) {
259  minseg->Lock(mindof, m_jacobian, mindelta);
260  locked = true;
261 
262  if (minabsdelta > norm)
263  norm = minabsdelta;
264  }
265 
266  if (locked == false)
267  // no locking done, last inner iteration, apply the angles
268  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
269  (*seg)->UnLock();
270  (*seg)->UpdateAngleApply();
271  }
272 
273  // signal if another inner iteration is needed
274  return locked;
275 }
276 
278  std::list<IK_QTask *> tasks,
279  const double,
280  const int max_iterations)
281 {
282  float scale = ComputeScale();
283  bool solved = false;
284  // double dt = analyze_time();
285 
286  Scale(scale, tasks);
287 
288  ConstrainPoleVector(root, tasks);
289 
290  root->UpdateTransform(m_rootmatrix);
291 
292  // iterate
293  for (int iterations = 0; iterations < max_iterations; iterations++) {
294  // update transform
295  root->UpdateTransform(m_rootmatrix);
296 
297  std::list<IK_QTask *>::iterator task;
298 
299  // compute jacobian
300  for (task = tasks.begin(); task != tasks.end(); task++) {
301  if ((*task)->Primary())
302  (*task)->ComputeJacobian(m_jacobian);
303  else
304  (*task)->ComputeJacobian(m_jacobian_sub);
305  }
306 
307  double norm = 0.0;
308 
309  do {
310  // invert jacobian
311  try {
312  m_jacobian.Invert();
313  if (m_secondary_enabled)
314  m_jacobian.SubTask(m_jacobian_sub);
315  }
316  catch (...) {
317  fprintf(stderr, "IK Exception\n");
318  return false;
319  }
320 
321  // update angles and check limits
322  } while (UpdateAngles(norm));
323 
324  // unlock segments again after locking in clamping loop
325  std::vector<IK_QSegment *>::iterator seg;
326  for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
327  (*seg)->UnLock();
328 
329  // compute angle update norm
330  double maxnorm = m_jacobian.AngleUpdateNorm();
331  if (maxnorm > norm)
332  norm = maxnorm;
333 
334  // check for convergence
335  if (norm < 1e-3 && iterations > 10) {
336  solved = true;
337  break;
338  }
339  }
340 
341  if (m_poleconstraint)
342  root->PrependBasis(m_rootmatrix.linear());
343 
344  Scale(1.0f / scale, tasks);
345 
346  // analyze_add_run(max_iterations, analyze_time()-dt);
347 
348  return solved;
349 }
static const double IK_EPSILON
Definition: IK_Math.h:21
static bool FuzzyZero(double x)
Definition: IK_Math.h:23
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition: btVector3.h:356
void SetPoleVectorConstraint(IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle)
bool Solve(IK_QSegment *root, std::list< IK_QTask * > tasks, const double tolerance, const int max_iterations)
bool Setup(IK_QSegment *root, std::list< IK_QTask * > &tasks)
void SetDoFWeight(int dof, double weight)
void SubTask(IK_QJacobian &jacobian)
double AngleUpdateNorm() const
void ArmMatrices(int dof, int task_size)
int NumberOfDoF() const
Definition: IK_QSegment.h:77
const Vector3d GlobalStart() const
Definition: IK_QSegment.h:104
const Affine3d & GlobalTransform() const
Definition: IK_QSegment.h:115
const Vector3d GlobalEnd() const
Definition: IK_QSegment.h:109
virtual void Lock(int, IK_QJacobian &, Vector3d &)
Definition: IK_QSegment.h:157
virtual bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *)=0
IK_QSegment * Child() const
Definition: IK_QSegment.h:53
IK_QSegment * Sibling() const
Definition: IK_QSegment.h:58
bool Locked(int dof) const
Definition: IK_QSegment.h:127
void UpdateTransform(const Affine3d &global)
void PrependBasis(const Matrix3d &mat)
void SetDoFId(int dof_id)
Definition: IK_QSegment.h:88
void SetId(int id)
Definition: IK_QTask.h:26
void SetWeight(double weight)
Definition: IK_QTask.h:51
double Weight() const
Definition: IK_QTask.h:46
bool Primary() const
Definition: IK_QTask.h:36
int Size() const
Definition: IK_QTask.h:31
ccl_device_inline float2 fabs(const float2 &a)
Definition: math_float2.h:222
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:319
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:311
struct blender::compositor::@179::@181 task
T clamp(const T &a, const T &min, const T &max)
T length(const vec_base< T, Size > &a)
vec_base< T, Size > normalize(const vec_base< T, Size > &v)