Blender  V3.3
IK_Solver.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 "../extern/IK_solver.h"
9 
10 #include "IK_QJacobianSolver.h"
11 #include "IK_QSegment.h"
12 #include "IK_QTask.h"
13 
14 #include <list>
15 using namespace std;
16 
17 class IK_QSolver {
18  public:
19  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
20  IK_QSolver() : root(NULL)
21  {
22  }
23 
26  std::list<IK_QTask *> tasks;
27 };
28 
29 // FIXME: locks still result in small "residual" changes to the locked axes...
30 static IK_QSegment *CreateSegment(int flag, bool translate)
31 {
32  int ndof = 0;
33  ndof += (flag & IK_XDOF) ? 1 : 0;
34  ndof += (flag & IK_YDOF) ? 1 : 0;
35  ndof += (flag & IK_ZDOF) ? 1 : 0;
36 
37  IK_QSegment *seg;
38 
39  if (ndof == 0)
40  return NULL;
41  else if (ndof == 1) {
42  int axis;
43 
44  if (flag & IK_XDOF)
45  axis = 0;
46  else if (flag & IK_YDOF)
47  axis = 1;
48  else
49  axis = 2;
50 
51  if (translate)
52  seg = new IK_QTranslateSegment(axis);
53  else
54  seg = new IK_QRevoluteSegment(axis);
55  }
56  else if (ndof == 2) {
57  int axis1, axis2;
58 
59  if (flag & IK_XDOF) {
60  axis1 = 0;
61  axis2 = (flag & IK_YDOF) ? 1 : 2;
62  }
63  else {
64  axis1 = 1;
65  axis2 = 2;
66  }
67 
68  if (translate)
69  seg = new IK_QTranslateSegment(axis1, axis2);
70  else {
71  if (axis1 + axis2 == 2)
72  seg = new IK_QSwingSegment();
73  else
74  seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2);
75  }
76  }
77  else {
78  if (translate)
79  seg = new IK_QTranslateSegment();
80  else
81  seg = new IK_QSphericalSegment();
82  }
83 
84  return seg;
85 }
86 
88 {
89  IK_QSegment *rot = CreateSegment(flag, false);
90  IK_QSegment *trans = CreateSegment(flag >> 3, true);
91 
92  IK_QSegment *seg;
93 
94  if (rot == NULL && trans == NULL)
95  seg = new IK_QNullSegment();
96  else if (rot == NULL)
97  seg = trans;
98  else {
99  seg = rot;
100 
101  // make it seem from the interface as if the rotation and translation
102  // segment are one
103  if (trans) {
104  seg->SetComposite(trans);
105  trans->SetParent(seg);
106  }
107  }
108 
109  return seg;
110 }
111 
113 {
114  IK_QSegment *qseg = (IK_QSegment *)seg;
115 
116  if (qseg->Composite())
117  delete qseg->Composite();
118  delete qseg;
119 }
120 
121 void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
122 {
123  IK_QSegment *qseg = (IK_QSegment *)seg;
124  IK_QSegment *qparent = (IK_QSegment *)parent;
125 
126  if (qparent && qparent->Composite())
127  qseg->SetParent(qparent->Composite());
128  else
129  qseg->SetParent(qparent);
130 }
131 
133  IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
134 {
135  IK_QSegment *qseg = (IK_QSegment *)seg;
136 
137  Vector3d mstart(start[0], start[1], start[2]);
138  // convert from blender column major
139  Matrix3d mbasis = CreateMatrix(basis[0][0],
140  basis[1][0],
141  basis[2][0],
142  basis[0][1],
143  basis[1][1],
144  basis[2][1],
145  basis[0][2],
146  basis[1][2],
147  basis[2][2]);
148  Matrix3d mrest = CreateMatrix(rest[0][0],
149  rest[1][0],
150  rest[2][0],
151  rest[0][1],
152  rest[1][1],
153  rest[2][1],
154  rest[0][2],
155  rest[1][2],
156  rest[2][2]);
157  double mlength(length);
158 
159  if (qseg->Composite()) {
160  Vector3d cstart(0, 0, 0);
161  Matrix3d cbasis;
162  cbasis.setIdentity();
163 
164  qseg->SetTransform(mstart, mrest, mbasis, 0.0);
165  qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
166  }
167  else
168  qseg->SetTransform(mstart, mrest, mbasis, mlength);
169 }
170 
171 void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
172 {
173  IK_QSegment *qseg = (IK_QSegment *)seg;
174 
175  if (axis >= IK_TRANS_X) {
176  if (!qseg->Translational()) {
177  if (qseg->Composite() && qseg->Composite()->Translational())
178  qseg = qseg->Composite();
179  else
180  return;
181  }
182 
183  if (axis == IK_TRANS_X)
184  axis = IK_X;
185  else if (axis == IK_TRANS_Y)
186  axis = IK_Y;
187  else
188  axis = IK_Z;
189  }
190 
191  qseg->SetLimit(axis, lmin, lmax);
192 }
193 
194 void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
195 {
196  if (stiffness < 0.0f)
197  return;
198 
199  if (stiffness > (1.0 - IK_STRETCH_STIFF_EPS))
200  stiffness = (1.0 - IK_STRETCH_STIFF_EPS);
201 
202  IK_QSegment *qseg = (IK_QSegment *)seg;
203  double weight = 1.0f - stiffness;
204 
205  if (axis >= IK_TRANS_X) {
206  if (!qseg->Translational()) {
207  if (qseg->Composite() && qseg->Composite()->Translational())
208  qseg = qseg->Composite();
209  else
210  return;
211  }
212 
213  if (axis == IK_TRANS_X)
214  axis = IK_X;
215  else if (axis == IK_TRANS_Y)
216  axis = IK_Y;
217  else
218  axis = IK_Z;
219  }
220 
221  qseg->SetWeight(axis, weight);
222 }
223 
224 void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
225 {
226  IK_QSegment *qseg = (IK_QSegment *)seg;
227 
228  if (qseg->Translational() && qseg->Composite())
229  qseg = qseg->Composite();
230 
231  const Matrix3d &change = qseg->BasisChange();
232 
233  // convert to blender column major
234  basis_change[0][0] = (float)change(0, 0);
235  basis_change[1][0] = (float)change(0, 1);
236  basis_change[2][0] = (float)change(0, 2);
237  basis_change[0][1] = (float)change(1, 0);
238  basis_change[1][1] = (float)change(1, 1);
239  basis_change[2][1] = (float)change(1, 2);
240  basis_change[0][2] = (float)change(2, 0);
241  basis_change[1][2] = (float)change(2, 1);
242  basis_change[2][2] = (float)change(2, 2);
243 }
244 
245 void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
246 {
247  IK_QSegment *qseg = (IK_QSegment *)seg;
248 
249  if (!qseg->Translational() && qseg->Composite())
250  qseg = qseg->Composite();
251 
252  const Vector3d &change = qseg->TranslationChange();
253 
254  translation_change[0] = (float)change[0];
255  translation_change[1] = (float)change[1];
256  translation_change[2] = (float)change[2];
257 }
258 
260 {
261  if (root == NULL)
262  return NULL;
263 
264  IK_QSolver *solver = new IK_QSolver();
265  solver->root = (IK_QSegment *)root;
266 
267  return (IK_Solver *)solver;
268 }
269 
271 {
272  if (solver == NULL)
273  return;
274 
275  IK_QSolver *qsolver = (IK_QSolver *)solver;
276  std::list<IK_QTask *> &tasks = qsolver->tasks;
277  std::list<IK_QTask *>::iterator task;
278 
279  for (task = tasks.begin(); task != tasks.end(); task++)
280  delete (*task);
281 
282  delete qsolver;
283 }
284 
285 void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
286 {
287  if (solver == NULL || tip == NULL)
288  return;
289 
290  IK_QSolver *qsolver = (IK_QSolver *)solver;
291  IK_QSegment *qtip = (IK_QSegment *)tip;
292 
293  // in case of composite segment the second segment is the tip
294  if (qtip->Composite())
295  qtip = qtip->Composite();
296 
297  Vector3d pos(goal[0], goal[1], goal[2]);
298 
299  IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
300  ee->SetWeight(weight);
301  qsolver->tasks.push_back(ee);
302 }
303 
304 void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
305 {
306  if (solver == NULL || tip == NULL)
307  return;
308 
309  IK_QSolver *qsolver = (IK_QSolver *)solver;
310  IK_QSegment *qtip = (IK_QSegment *)tip;
311 
312  // in case of composite segment the second segment is the tip
313  if (qtip->Composite())
314  qtip = qtip->Composite();
315 
316  // convert from blender column major
317  Matrix3d rot = CreateMatrix(goal[0][0],
318  goal[1][0],
319  goal[2][0],
320  goal[0][1],
321  goal[1][1],
322  goal[2][1],
323  goal[0][2],
324  goal[1][2],
325  goal[2][2]);
326 
327  IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
328  orient->SetWeight(weight);
329  qsolver->tasks.push_back(orient);
330 }
331 
333  IK_Segment *tip,
334  float goal[3],
335  float polegoal[3],
336  float poleangle,
337  int getangle)
338 {
339  if (solver == NULL || tip == NULL)
340  return;
341 
342  IK_QSolver *qsolver = (IK_QSolver *)solver;
343  IK_QSegment *qtip = (IK_QSegment *)tip;
344 
345  // in case of composite segment the second segment is the tip
346  if (qtip->Composite())
347  qtip = qtip->Composite();
348 
349  Vector3d qgoal(goal[0], goal[1], goal[2]);
350  Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]);
351 
352  qsolver->solver.SetPoleVectorConstraint(qtip, qgoal, qpolegoal, poleangle, getangle);
353 }
354 
356 {
357  if (solver == NULL)
358  return 0.0f;
359 
360  IK_QSolver *qsolver = (IK_QSolver *)solver;
361 
362  return qsolver->solver.GetPoleAngle();
363 }
364 
365 #if 0
366 static void IK_SolverAddCenterOfMass(IK_Solver *solver,
367  IK_Segment *root,
368  float goal[3],
369  float weight)
370 {
371  if (solver == NULL || root == NULL)
372  return;
373 
374  IK_QSolver *qsolver = (IK_QSolver *)solver;
375  IK_QSegment *qroot = (IK_QSegment *)root;
376 
377  // convert from blender column major
378  Vector3d center(goal);
379 
380  IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
381  com->SetWeight(weight);
382  qsolver->tasks.push_back(com);
383 }
384 #endif
385 
386 int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
387 {
388  if (solver == NULL)
389  return 0;
390 
391  IK_QSolver *qsolver = (IK_QSolver *)solver;
392 
393  IK_QSegment *root = qsolver->root;
394  IK_QJacobianSolver &jacobian = qsolver->solver;
395  std::list<IK_QTask *> &tasks = qsolver->tasks;
396  double tol = tolerance;
397 
398  if (!jacobian.Setup(root, tasks))
399  return 0;
400 
401  bool result = jacobian.Solve(root, tasks, tol, max_iterations);
402 
403  return ((result) ? 1 : 0);
404 }
typedef float(TangentPoint)[2]
NSNotificationCenter * center
static Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz)
Definition: IK_Math.h:33
IK_Solver * IK_CreateSolver(IK_Segment *root)
Definition: IK_Solver.cpp:259
float IK_SolverGetPoleAngle(IK_Solver *solver)
Definition: IK_Solver.cpp:355
IK_Segment * IK_CreateSegment(int flag)
Definition: IK_Solver.cpp:87
static IK_QSegment * CreateSegment(int flag, bool translate)
Definition: IK_Solver.cpp:30
void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle)
Definition: IK_Solver.cpp:332
void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
Definition: IK_Solver.cpp:121
void IK_FreeSolver(IK_Solver *solver)
Definition: IK_Solver.cpp:270
void IK_FreeSegment(IK_Segment *seg)
Definition: IK_Solver.cpp:112
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
Definition: IK_Solver.cpp:304
void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
Definition: IK_Solver.cpp:171
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
Definition: IK_Solver.cpp:285
int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
Definition: IK_Solver.cpp:386
void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
Definition: IK_Solver.cpp:224
void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
Definition: IK_Solver.cpp:194
void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
Definition: IK_Solver.cpp:245
void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
Definition: IK_Solver.cpp:132
void IK_Segment
Definition: IK_solver.h:79
#define IK_STRETCH_STIFF_EPS
Definition: IK_solver.h:144
void IK_Solver
Definition: IK_solver.h:124
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)
IK_QSegment * Composite() const
Definition: IK_QSegment.h:71
bool Translational() const
Definition: IK_QSegment.h:121
virtual void SetWeight(int, double)
Definition: IK_QSegment.h:168
void SetComposite(IK_QSegment *seg)
Definition: IK_QSegment.cpp:98
Matrix3d BasisChange() const
Definition: IK_QSegment.cpp:63
void SetParent(IK_QSegment *parent)
Definition: IK_QSegment.cpp:82
virtual void SetLimit(int, double, double)
Definition: IK_QSegment.h:163
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
IK_QJacobianSolver solver
Definition: IK_Solver.cpp:24
IK_QSegment * root
Definition: IK_Solver.cpp:25
std::list< IK_QTask * > tasks
Definition: IK_Solver.cpp:26
EIGEN_MAKE_ALIGNED_OPERATOR_NEW IK_QSolver()
Definition: IK_Solver.cpp:20
void SetWeight(double weight)
Definition: IK_QTask.h:51
#define rot(x, k)
uint pos
@ IK_ZDOF
@ IK_XDOF
@ IK_YDOF
IK_SegmentAxis
@ IK_X
@ IK_TRANS_X
@ IK_Y
@ IK_TRANS_Y
@ IK_Z
struct blender::compositor::@179::@181 task
T length(const vec_base< T, Size > &a)