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