a335a94616615a54689634300956142433a4c284
[blender-staging.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 #include <iostream>
41 using namespace std;
42
43 typedef struct {
44         IK_QJacobianSolver solver;
45         IK_QSegment *root;
46         std::list<IK_QTask*> tasks;
47 } IK_QSolver;
48
49 IK_Segment *IK_CreateSegment(int flag)
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                 seg = new IK_QNullSegment();
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 (flag & IK_TRANSLATIONAL)
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 (flag & IK_TRANSLATIONAL)
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 (flag & IK_TRANSLATIONAL)
95                         seg = new IK_QTranslateSegment();
96                 else
97                         seg = new IK_QSphericalSegment();
98         }
99
100         return (IK_Segment*)seg;
101 }
102
103 void IK_FreeSegment(IK_Segment *seg)
104 {
105         IK_QSegment *qseg = (IK_QSegment*)seg;
106
107         delete qseg;
108 }
109
110 void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
111 {
112         IK_QSegment *qseg = (IK_QSegment*)seg;
113
114         qseg->SetParent((IK_QSegment*)parent);
115 }
116
117 void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
118 {
119         IK_QSegment *qseg = (IK_QSegment*)seg;
120
121         MT_Vector3 mstart(start);
122         // convert from blender column major to moto row major
123         MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
124                             basis[0][1], basis[1][1], basis[2][1],
125                             basis[0][2], basis[1][2], basis[2][2]);
126         MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
127                            rest[0][1], rest[1][1], rest[2][1],
128                            rest[0][2], rest[1][2], rest[2][2]);
129         MT_Scalar mlength(length);
130
131         qseg->SetTransform(mstart, mrest, mbasis, mlength);
132 }
133
134 void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
135 {
136         IK_QSegment *qseg = (IK_QSegment*)seg;
137
138         if (axis == IK_X)
139                 qseg->SetLimit(0, lmin, lmax);
140         else if (axis == IK_Y)
141                 qseg->SetLimit(1, lmin, lmax);
142         else if (axis == IK_Z)
143                 qseg->SetLimit(2, lmin, lmax);
144 }
145
146 void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
147 {
148         if (stiffness < 1.0)
149                 return;
150
151         IK_QSegment *qseg = (IK_QSegment*)seg;
152         MT_Scalar weight = 1.0/stiffness;
153
154         if (axis == IK_X)
155                 qseg->SetWeight(0, weight);
156         else if (axis == IK_Y)
157                 qseg->SetWeight(1, weight);
158         else if (axis == IK_Z)
159                 qseg->SetWeight(2, weight);
160 }
161
162 void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
163 {
164         IK_QSegment *qseg = (IK_QSegment*)seg;
165         const MT_Matrix3x3& change = qseg->BasisChange();
166
167         // convert from moto row major to blender column major
168         basis_change[0][0] = (float)change[0][0];
169         basis_change[1][0] = (float)change[0][1];
170         basis_change[2][0] = (float)change[0][2];
171         basis_change[0][1] = (float)change[1][0];
172         basis_change[1][1] = (float)change[1][1];
173         basis_change[2][1] = (float)change[1][2];
174         basis_change[0][2] = (float)change[2][0];
175         basis_change[1][2] = (float)change[2][1];
176         basis_change[2][2] = (float)change[2][2];
177 }
178
179 void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
180 {
181         IK_QSegment *qseg = (IK_QSegment*)seg;
182         const MT_Vector3& change = qseg->TranslationChange();
183
184         translation_change[0] = (float)change[0];
185         translation_change[1] = (float)change[1];
186         translation_change[2] = (float)change[2];
187 }
188
189 IK_Solver *IK_CreateSolver(IK_Segment *root)
190 {
191         if (root == NULL)
192                 return NULL;
193         
194         IK_QSolver *solver = new IK_QSolver();
195         solver->root = (IK_QSegment*)root;
196
197         return (IK_Solver*)solver;
198 }
199
200 void IK_FreeSolver(IK_Solver *solver)
201 {
202         if (solver == NULL)
203                 return;
204
205         IK_QSolver *qsolver = (IK_QSolver*)solver;
206         std::list<IK_QTask*>& tasks = qsolver->tasks;
207         std::list<IK_QTask*>::iterator task;
208
209         for (task = tasks.begin(); task != tasks.end(); task++)
210                 delete (*task);
211         
212         delete qsolver;
213 }
214
215 void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
216 {
217         if (solver == NULL || tip == NULL)
218                 return;
219
220         IK_QSolver *qsolver = (IK_QSolver*)solver;
221         IK_QSegment *qtip = (IK_QSegment*)tip;
222
223         MT_Vector3 pos(goal);
224
225         // qsolver->tasks.empty()
226         IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
227         ee->SetWeight(weight);
228         qsolver->tasks.push_back(ee);
229 }
230
231 void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
232 {
233         if (solver == NULL || tip == NULL)
234                 return;
235
236         IK_QSolver *qsolver = (IK_QSolver*)solver;
237         IK_QSegment *qtip = (IK_QSegment*)tip;
238
239         // convert from blender column major to moto row major
240         MT_Matrix3x3 rot(goal[0][0], goal[1][0], goal[2][0],
241                          goal[0][1], goal[1][1], goal[2][1],
242                          goal[0][2], goal[1][2], goal[2][2]);
243
244         IK_QTask *orient = new IK_QOrientationTask(false, qtip, rot);
245         orient->SetWeight(weight);
246         qsolver->tasks.push_back(orient);
247 }
248
249 void IK_SolverAddCenterOfMass(IK_Solver *solver, IK_Segment *root, float goal[3], float weight)
250 {
251         if (solver == NULL || root == NULL)
252                 return;
253
254         IK_QSolver *qsolver = (IK_QSolver*)solver;
255         IK_QSegment *qroot = (IK_QSegment*)root;
256
257         // convert from blender column major to moto row major
258         MT_Vector3 center(goal);
259
260         IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
261         com->SetWeight(weight);
262         qsolver->tasks.push_back(com);
263 }
264
265 int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
266 {
267         if (solver == NULL)
268                 return 0;
269
270         IK_QSolver *qsolver = (IK_QSolver*)solver;
271
272         IK_QSegment *root = qsolver->root;
273         IK_QJacobianSolver& jacobian = qsolver->solver;
274         std::list<IK_QTask*>& tasks = qsolver->tasks;
275         MT_Scalar tol = tolerance;
276
277         bool result = jacobian.Solve(root, tasks, tol, max_iterations);
278
279         return ((result)? 1: 0);
280 }
281