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