style cleanup
[blender.git] / intern / iksolver / intern / IK_QJacobianSolver.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_QJacobianSolver.cpp
30  *  \ingroup iksolver
31  */
32
33
34 #include <stdio.h>
35 #include "IK_QJacobianSolver.h"
36 #include "MT_Quaternion.h"
37
38 //#include "analyze.h"
39 IK_QJacobianSolver::IK_QJacobianSolver()
40 {
41         m_poleconstraint = false;
42         m_getpoleangle = false;
43         m_rootmatrix.setIdentity();
44 }
45
46 MT_Scalar IK_QJacobianSolver::ComputeScale()
47 {
48         std::vector<IK_QSegment *>::iterator seg;
49         MT_Scalar length = 0.0f;
50
51         for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
52                 length += (*seg)->MaxExtension();
53         
54         if (length == 0.0)
55                 return 1.0;
56         else
57                 return 1.0 / length;
58 }
59
60 void IK_QJacobianSolver::Scale(MT_Scalar scale, std::list<IK_QTask *>& tasks)
61 {
62         std::list<IK_QTask *>::iterator task;
63         std::vector<IK_QSegment *>::iterator seg;
64
65         for (task = tasks.begin(); task != tasks.end(); task++)
66                 (*task)->Scale(scale);
67
68         for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
69                 (*seg)->Scale(scale);
70         
71         m_rootmatrix.getOrigin() *= scale;
72         m_goal *= scale;
73         m_polegoal *= scale;
74 }
75
76 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
77 {
78         m_segments.push_back(seg);
79
80         IK_QSegment *child;
81         for (child = seg->Child(); child; child = child->Sibling())
82                 AddSegmentList(child);
83 }
84
85 bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks)
86 {
87         m_segments.clear();
88         AddSegmentList(root);
89
90         // assign each segment a unique id for the jacobian
91         std::vector<IK_QSegment *>::iterator seg;
92         int num_dof = 0;
93
94         for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
95                 (*seg)->SetDoFId(num_dof);
96                 num_dof += (*seg)->NumberOfDoF();
97         }
98
99         if (num_dof == 0)
100                 return false;
101
102         // compute task id's and assing weights to task
103         int primary_size = 0, primary = 0;
104         int secondary_size = 0, secondary = 0;
105         MT_Scalar primary_weight = 0.0, secondary_weight = 0.0;
106         std::list<IK_QTask *>::iterator task;
107
108         for (task = tasks.begin(); task != tasks.end(); task++) {
109                 IK_QTask *qtask = *task;
110
111                 if (qtask->Primary()) {
112                         qtask->SetId(primary_size);
113                         primary_size += qtask->Size();
114                         primary_weight += qtask->Weight();
115                         primary++;
116                 }
117                 else {
118                         qtask->SetId(secondary_size);
119                         secondary_size += qtask->Size();
120                         secondary_weight += qtask->Weight();
121                         secondary++;
122                 }
123         }
124
125         if (primary_size == 0 || MT_fuzzyZero(primary_weight))
126                 return false;
127
128         m_secondary_enabled = (secondary > 0);
129         
130         // rescale weights of tasks to sum up to 1
131         MT_Scalar primary_rescale = 1.0 / primary_weight;
132         MT_Scalar secondary_rescale;
133         if (MT_fuzzyZero(secondary_weight))
134                 secondary_rescale = 0.0;
135         else
136                 secondary_rescale = 1.0 / secondary_weight;
137         
138         for (task = tasks.begin(); task != tasks.end(); task++) {
139                 IK_QTask *qtask = *task;
140
141                 if (qtask->Primary())
142                         qtask->SetWeight(qtask->Weight() * primary_rescale);
143                 else
144                         qtask->SetWeight(qtask->Weight() * secondary_rescale);
145         }
146
147         // set matrix sizes
148         m_jacobian.ArmMatrices(num_dof, primary_size);
149         if (secondary > 0)
150                 m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
151
152         // set dof weights
153         int i;
154
155         for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
156                 for (i = 0; i < (*seg)->NumberOfDoF(); i++)
157                         m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
158
159         return true;
160 }
161
162 void IK_QJacobianSolver::SetPoleVectorConstraint(IK_QSegment *tip, MT_Vector3& goal, MT_Vector3& polegoal, float poleangle, bool getangle)
163 {
164         m_poleconstraint = true;
165         m_poletip = tip;
166         m_goal = goal;
167         m_polegoal = polegoal;
168         m_poleangle = (getangle) ? 0.0f : poleangle;
169         m_getpoleangle = getangle;
170 }
171
172 static MT_Scalar safe_acos(MT_Scalar f)
173 {
174         // acos that does not return NaN with rounding errors
175         if (f <= -1.0) return MT_PI;
176         else if (f >= 1.0) return 0.0;
177         else return acos(f);
178 }
179
180 static MT_Vector3 normalize(const MT_Vector3& v)
181 {
182         // a sane normalize function that doesn't give (1, 0, 0) in case
183         // of a zero length vector, like MT_Vector3.normalize
184         MT_Scalar len = v.length();
185         return MT_fuzzyZero(len) ?  MT_Vector3(0, 0, 0) : v / len;
186 }
187
188 static float angle(const MT_Vector3& v1, const MT_Vector3& v2)
189 {
190         return safe_acos(v1.dot(v2));
191 }
192
193 void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *>& tasks)
194 {
195         // this function will be called before and after solving. calling it before
196         // solving gives predictable solutions by rotating towards the solution,
197         // and calling it afterwards ensures the solution is exact.
198
199         if (!m_poleconstraint)
200                 return;
201         
202         // disable pole vector constraint in case of multiple position tasks
203         std::list<IK_QTask *>::iterator task;
204         int positiontasks = 0;
205
206         for (task = tasks.begin(); task != tasks.end(); task++)
207                 if ((*task)->PositionTask())
208                         positiontasks++;
209         
210         if (positiontasks >= 2) {
211                 m_poleconstraint = false;
212                 return;
213         }
214
215         // get positions and rotations
216         root->UpdateTransform(m_rootmatrix);
217
218         const MT_Vector3 rootpos = root->GlobalStart();
219         const MT_Vector3 endpos = m_poletip->GlobalEnd();
220         const MT_Matrix3x3& rootbasis = root->GlobalTransform().getBasis();
221
222         // construct "lookat" matrices (like gluLookAt), based on a direction and
223         // an up vector, with the direction going from the root to the end effector
224         // and the up vector going from the root to the pole constraint position.
225         MT_Vector3 dir = normalize(endpos - rootpos);
226         MT_Vector3 rootx = rootbasis.getColumn(0);
227         MT_Vector3 rootz = rootbasis.getColumn(2);
228         MT_Vector3 up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle);
229
230         // in post, don't rotate towards the goal but only correct the pole up
231         MT_Vector3 poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
232         MT_Vector3 poleup = normalize(m_polegoal - rootpos);
233
234         MT_Matrix3x3 mat, polemat;
235
236         mat[0] = normalize(MT_cross(dir, up));
237         mat[1] = MT_cross(mat[0], dir);
238         mat[2] = -dir;
239
240         polemat[0] = normalize(MT_cross(poledir, poleup));
241         polemat[1] = MT_cross(polemat[0], poledir);
242         polemat[2] = -poledir;
243
244         if (m_getpoleangle) {
245                 // we compute the pole angle that to rotate towards the target
246                 m_poleangle = angle(mat[1], polemat[1]);
247
248                 if (rootz.dot(mat[1] * cos(m_poleangle) + mat[0] * sin(m_poleangle)) > 0.0)
249                         m_poleangle = -m_poleangle;
250
251                 // solve again, with the pole angle we just computed
252                 m_getpoleangle = false;
253                 ConstrainPoleVector(root, tasks);
254         }
255         else {
256                 // now we set as root matrix the difference between the current and
257                 // desired rotation based on the pole vector constraint. we use
258                 // transpose instead of inverse because we have orthogonal matrices
259                 // anyway, and in case of a singular matrix we don't get NaN's.
260                 MT_Transform trans(MT_Point3(0, 0, 0), polemat.transposed() * mat);
261                 m_rootmatrix = trans * m_rootmatrix;
262         }
263 }
264
265 bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
266 {
267         // assing each segment a unique id for the jacobian
268         std::vector<IK_QSegment *>::iterator seg;
269         IK_QSegment *qseg, *minseg = NULL;
270         MT_Scalar minabsdelta = 1e10, absdelta;
271         MT_Vector3 delta, mindelta;
272         bool locked = false, clamp[3];
273         int i, mindof = 0;
274
275         // here we check if any angle limits were violated. angles whose clamped
276         // position is the same as it was before, are locked immediate. of the
277         // other violation angles the most violating angle is rememberd
278         for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
279                 qseg = *seg;
280                 if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
281                         for (i = 0; i < qseg->NumberOfDoF(); i++) {
282                                 if (clamp[i] && !qseg->Locked(i)) {
283                                         absdelta = MT_abs(delta[i]);
284
285                                         if (absdelta < MT_EPSILON) {
286                                                 qseg->Lock(i, m_jacobian, delta);
287                                                 locked = true;
288                                         }
289                                         else if (absdelta < minabsdelta) {
290                                                 minabsdelta = absdelta;
291                                                 mindelta = delta;
292                                                 minseg = qseg;
293                                                 mindof = i;
294                                         }
295                                 }
296                         }
297                 }
298         }
299
300         // lock most violating angle
301         if (minseg) {
302                 minseg->Lock(mindof, m_jacobian, mindelta);
303                 locked = true;
304
305                 if (minabsdelta > norm)
306                         norm = minabsdelta;
307         }
308
309         if (locked == false)
310                 // no locking done, last inner iteration, apply the angles
311                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
312                         (*seg)->UnLock();
313                         (*seg)->UpdateAngleApply();
314                 }
315         
316         // signal if another inner iteration is needed
317         return locked;
318 }
319
320 bool IK_QJacobianSolver::Solve(
321     IK_QSegment *root,
322     std::list<IK_QTask *> tasks,
323     const MT_Scalar,
324     const int max_iterations
325     )
326 {
327         float scale = ComputeScale();
328         bool solved = false;
329         //double dt = analyze_time();
330
331         Scale(scale, tasks);
332
333         ConstrainPoleVector(root, tasks);
334
335         root->UpdateTransform(m_rootmatrix);
336
337         // iterate
338         for (int iterations = 0; iterations < max_iterations; iterations++) {
339                 // update transform
340                 root->UpdateTransform(m_rootmatrix);
341
342                 std::list<IK_QTask *>::iterator task;
343
344                 // compute jacobian
345                 for (task = tasks.begin(); task != tasks.end(); task++) {
346                         if ((*task)->Primary())
347                                 (*task)->ComputeJacobian(m_jacobian);
348                         else
349                                 (*task)->ComputeJacobian(m_jacobian_sub);
350                 }
351
352                 MT_Scalar norm = 0.0;
353
354                 do {
355                         // invert jacobian
356                         try {
357                                 m_jacobian.Invert();
358                                 if (m_secondary_enabled)
359                                         m_jacobian.SubTask(m_jacobian_sub);
360                         }
361                         catch (...) {
362                                 fprintf(stderr, "IK Exception\n");
363                                 return false;
364                         }
365
366                         // update angles and check limits
367                 } while (UpdateAngles(norm));
368
369                 // unlock segments again after locking in clamping loop
370                 std::vector<IK_QSegment *>::iterator seg;
371                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
372                         (*seg)->UnLock();
373
374                 // compute angle update norm
375                 MT_Scalar maxnorm = m_jacobian.AngleUpdateNorm();
376                 if (maxnorm > norm)
377                         norm = maxnorm;
378
379                 // check for convergence
380                 if (norm < 1e-3) {
381                         solved = true;
382                         break;
383                 }
384         }
385
386         if (m_poleconstraint)
387                 root->PrependBasis(m_rootmatrix.getBasis());
388
389         Scale(1.0f / scale, tasks);
390
391         //analyze_add_run(max_iterations, analyze_time()-dt);
392
393         return solved;
394 }
395