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