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