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