Bugfix #24296: AutoIK interactive chain length adjustment feature is missing
[blender-staging.git] / intern / iksolver / intern / IK_Solver.cpp
1 /**
2  * $Id$
3  * ***** BEGIN GPL 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.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software Foundation,
17  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
20  * All rights reserved.
21  *
22  * The Original Code is: all of this file.
23  *
24  * Original Author: Laurence
25  * Contributor(s): Brecht
26  *
27  * ***** END GPL LICENSE BLOCK *****
28  */
29
30 #include "../extern/IK_solver.h"
31
32 #include "IK_QJacobianSolver.h"
33 #include "IK_QSegment.h"
34 #include "IK_QTask.h"
35
36 #include <list>
37 using namespace std;
38
39 class IK_QSolver {
40 public:
41         IK_QSolver() : root(NULL) {};
42
43         IK_QJacobianSolver solver;
44         IK_QSegment *root;
45         std::list<IK_QTask*> tasks;
46 };
47
48 // FIXME: locks still result in small "residual" changes to the locked axes...
49 IK_QSegment *CreateSegment(int flag, bool translate)
50 {
51         int ndof = 0;
52         ndof += (flag & IK_XDOF)? 1: 0;
53         ndof += (flag & IK_YDOF)? 1: 0;
54         ndof += (flag & IK_ZDOF)? 1: 0;
55
56         IK_QSegment *seg;
57
58         if (ndof == 0)
59                 return NULL;
60         else if (ndof == 1) {
61                 int axis;
62
63                 if (flag & IK_XDOF) axis = 0;
64                 else if (flag & IK_YDOF) axis = 1;
65                 else 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(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
149 {
150         IK_QSegment *qseg = (IK_QSegment*)seg;
151
152         MT_Vector3 mstart(start);
153         // convert from blender column major to moto row major
154         MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
155                             basis[0][1], basis[1][1], basis[2][1],
156                             basis[0][2], basis[1][2], basis[2][2]);
157         MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
158                            rest[0][1], rest[1][1], rest[2][1],
159                            rest[0][2], rest[1][2], rest[2][2]);
160         MT_Scalar mlength(length);
161
162         if (qseg->Composite()) {
163                 MT_Vector3 cstart(0, 0, 0);
164                 MT_Matrix3x3 cbasis;
165                 cbasis.setIdentity();
166                 
167                 qseg->SetTransform(mstart, mrest, mbasis, 0.0);
168                 qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
169         }
170         else
171                 qseg->SetTransform(mstart, mrest, mbasis, mlength);
172 }
173
174 void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
175 {
176         IK_QSegment *qseg = (IK_QSegment*)seg;
177
178         if (axis >= IK_TRANS_X) {
179                 if(!qseg->Translational()) {
180                         if(qseg->Composite() && qseg->Composite()->Translational())
181                                 qseg = qseg->Composite();
182                         else
183                                 return;
184                 }
185
186                 if(axis == IK_TRANS_X) axis = IK_X;
187                 else if(axis == IK_TRANS_Y) axis = IK_Y;
188                 else 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.0)
197                 return;
198         
199         if (stiffness > 0.999)
200                 stiffness = 0.999;
201
202         IK_QSegment *qseg = (IK_QSegment*)seg;
203         MT_Scalar weight = 1.0-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) axis = IK_X;
214                 else if(axis == IK_TRANS_Y) axis = IK_Y;
215                 else axis = IK_Z;
216         }
217
218         qseg->SetWeight(axis, weight);
219 }
220
221 void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
222 {
223         IK_QSegment *qseg = (IK_QSegment*)seg;
224         const MT_Matrix3x3& change = qseg->BasisChange();
225
226         if (qseg->Translational() && qseg->Composite())
227                 qseg = qseg->Composite();
228
229         // convert from moto row major to blender column major
230         basis_change[0][0] = (float)change[0][0];
231         basis_change[1][0] = (float)change[0][1];
232         basis_change[2][0] = (float)change[0][2];
233         basis_change[0][1] = (float)change[1][0];
234         basis_change[1][1] = (float)change[1][1];
235         basis_change[2][1] = (float)change[1][2];
236         basis_change[0][2] = (float)change[2][0];
237         basis_change[1][2] = (float)change[2][1];
238         basis_change[2][2] = (float)change[2][2];
239 }
240
241 void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
242 {
243         IK_QSegment *qseg = (IK_QSegment*)seg;
244
245         if (!qseg->Translational() && qseg->Composite())
246                 qseg = qseg->Composite();
247         
248         const MT_Vector3& change = qseg->TranslationChange();
249
250         translation_change[0] = (float)change[0];
251         translation_change[1] = (float)change[1];
252         translation_change[2] = (float)change[2];
253 }
254
255 IK_Solver *IK_CreateSolver(IK_Segment *root)
256 {
257         if (root == NULL)
258                 return NULL;
259         
260         IK_QSolver *solver = new IK_QSolver();
261         solver->root = (IK_QSegment*)root;
262
263         return (IK_Solver*)solver;
264 }
265
266 void IK_FreeSolver(IK_Solver *solver)
267 {
268         if (solver == NULL)
269                 return;
270
271         IK_QSolver *qsolver = (IK_QSolver*)solver;
272         std::list<IK_QTask*>& tasks = qsolver->tasks;
273         std::list<IK_QTask*>::iterator task;
274
275         for (task = tasks.begin(); task != tasks.end(); task++)
276                 delete (*task);
277         
278         delete qsolver;
279 }
280
281 void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
282 {
283         if (solver == NULL || tip == NULL)
284                 return;
285
286         IK_QSolver *qsolver = (IK_QSolver*)solver;
287         IK_QSegment *qtip = (IK_QSegment*)tip;
288
289         if (qtip->Composite())
290                 qtip = qtip->Composite();
291
292         MT_Vector3 pos(goal);
293
294         IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
295         ee->SetWeight(weight);
296         qsolver->tasks.push_back(ee);
297 }
298
299 void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
300 {
301         if (solver == NULL || tip == NULL)
302                 return;
303
304         IK_QSolver *qsolver = (IK_QSolver*)solver;
305         IK_QSegment *qtip = (IK_QSegment*)tip;
306
307         if (qtip->Composite())
308                 qtip = qtip->Composite();
309
310         // convert from blender column major to moto row major
311         MT_Matrix3x3 rot(goal[0][0], goal[1][0], goal[2][0],
312                          goal[0][1], goal[1][1], goal[2][1],
313                          goal[0][2], goal[1][2], goal[2][2]);
314
315         IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
316         orient->SetWeight(weight);
317         qsolver->tasks.push_back(orient);
318 }
319
320 void IK_SolverSetPoleVectorConstraint(IK_Solver *solver, IK_Segment *tip, float goal[3], float polegoal[3], float poleangle, int getangle)
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         MT_Vector3 qgoal(goal);
329         MT_Vector3 qpolegoal(polegoal);
330
331         qsolver->solver.SetPoleVectorConstraint(
332                 qtip, qgoal, qpolegoal, poleangle, getangle);
333 }
334
335 float IK_SolverGetPoleAngle(IK_Solver *solver)
336 {
337         if (solver == NULL)
338                 return 0.0f;
339
340         IK_QSolver *qsolver = (IK_QSolver*)solver;
341
342         return qsolver->solver.GetPoleAngle();
343 }
344
345 void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
346 {
347         if (solver == NULL || root == NULL)
348                 return;
349
350         IK_QSolver *qsolver = (IK_QSolver*)solver;
351         IK_QSegment *qroot = (IK_QSegment*)root;
352
353         // convert from blender column major to moto row major
354         MT_Vector3 center(goal);
355
356         IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
357         com->SetWeight(weight);
358         qsolver->tasks.push_back(com);
359 }
360
361 int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
362 {
363         if (solver == NULL)
364                 return 0;
365
366         IK_QSolver *qsolver = (IK_QSolver*)solver;
367
368         IK_QSegment *root = qsolver->root;
369         IK_QJacobianSolver& jacobian = qsolver->solver;
370         std::list<IK_QTask*>& tasks = qsolver->tasks;
371         MT_Scalar tol = tolerance;
372
373         if(!jacobian.Setup(root, tasks))
374                 return 0;
375
376         bool result = jacobian.Solve(root, tasks, tol, max_iterations);
377
378         return ((result)? 1: 0);
379 }
380