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