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