Fix too much memory usage for Cycles attribute map.
[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
36 #include "IK_QJacobianSolver.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 double IK_QJacobianSolver::ComputeScale()
47 {
48         std::vector<IK_QSegment *>::iterator seg;
49         double 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(double 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.translation() *= 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         double 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 || 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         double primary_rescale = 1.0 / primary_weight;
132         double secondary_rescale;
133         if (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, Vector3d& goal, Vector3d& 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 void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *>& tasks)
173 {
174         // this function will be called before and after solving. calling it before
175         // solving gives predictable solutions by rotating towards the solution,
176         // and calling it afterwards ensures the solution is exact.
177
178         if (!m_poleconstraint)
179                 return;
180         
181         // disable pole vector constraint in case of multiple position tasks
182         std::list<IK_QTask *>::iterator task;
183         int positiontasks = 0;
184
185         for (task = tasks.begin(); task != tasks.end(); task++)
186                 if ((*task)->PositionTask())
187                         positiontasks++;
188         
189         if (positiontasks >= 2) {
190                 m_poleconstraint = false;
191                 return;
192         }
193
194         // get positions and rotations
195         root->UpdateTransform(m_rootmatrix);
196
197         const Vector3d rootpos = root->GlobalStart();
198         const Vector3d endpos = m_poletip->GlobalEnd();
199         const Matrix3d& rootbasis = root->GlobalTransform().linear();
200
201         // construct "lookat" matrices (like gluLookAt), based on a direction and
202         // an up vector, with the direction going from the root to the end effector
203         // and the up vector going from the root to the pole constraint position.
204         Vector3d dir = normalize(endpos - rootpos);
205         Vector3d rootx = rootbasis.col(0);
206         Vector3d rootz = rootbasis.col(2);
207         Vector3d up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle);
208
209         // in post, don't rotate towards the goal but only correct the pole up
210         Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
211         Vector3d poleup = normalize(m_polegoal - rootpos);
212
213         Matrix3d mat, polemat;
214
215         mat.row(0) = normalize(dir.cross(up));
216         mat.row(1) = mat.row(0).cross(dir);
217         mat.row(2) = -dir;
218
219         polemat.row(0) = normalize(poledir.cross(poleup));
220         polemat.row(1) = polemat.row(0).cross(poledir);
221         polemat.row(2) = -poledir;
222
223         if (m_getpoleangle) {
224                 // we compute the pole angle that to rotate towards the target
225                 m_poleangle = angle(mat.row(1), polemat.row(1));
226
227                 double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle));
228                 if (dt > 0.0)
229                         m_poleangle = -m_poleangle;
230
231                 // solve again, with the pole angle we just computed
232                 m_getpoleangle = false;
233                 ConstrainPoleVector(root, tasks);
234         }
235         else {
236                 // now we set as root matrix the difference between the current and
237                 // desired rotation based on the pole vector constraint. we use
238                 // transpose instead of inverse because we have orthogonal matrices
239                 // anyway, and in case of a singular matrix we don't get NaN's.
240                 Affine3d trans;
241                 trans.linear() = polemat.transpose() * mat;
242                 trans.translation() = Vector3d(0, 0, 0);
243                 m_rootmatrix = trans * m_rootmatrix;
244         }
245 }
246
247 bool IK_QJacobianSolver::UpdateAngles(double& norm)
248 {
249         // assing each segment a unique id for the jacobian
250         std::vector<IK_QSegment *>::iterator seg;
251         IK_QSegment *qseg, *minseg = NULL;
252         double minabsdelta = 1e10, absdelta;
253         Vector3d delta, mindelta;
254         bool locked = false, clamp[3];
255         int i, mindof = 0;
256
257         // here we check if any angle limits were violated. angles whose clamped
258         // position is the same as it was before, are locked immediate. of the
259         // other violation angles the most violating angle is rememberd
260         for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
261                 qseg = *seg;
262                 if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
263                         for (i = 0; i < qseg->NumberOfDoF(); i++) {
264                                 if (clamp[i] && !qseg->Locked(i)) {
265                                         absdelta = fabs(delta[i]);
266
267                                         if (absdelta < IK_EPSILON) {
268                                                 qseg->Lock(i, m_jacobian, delta);
269                                                 locked = true;
270                                         }
271                                         else if (absdelta < minabsdelta) {
272                                                 minabsdelta = absdelta;
273                                                 mindelta = delta;
274                                                 minseg = qseg;
275                                                 mindof = i;
276                                         }
277                                 }
278                         }
279                 }
280         }
281
282         // lock most violating angle
283         if (minseg) {
284                 minseg->Lock(mindof, m_jacobian, mindelta);
285                 locked = true;
286
287                 if (minabsdelta > norm)
288                         norm = minabsdelta;
289         }
290
291         if (locked == false)
292                 // no locking done, last inner iteration, apply the angles
293                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
294                         (*seg)->UnLock();
295                         (*seg)->UpdateAngleApply();
296                 }
297         
298         // signal if another inner iteration is needed
299         return locked;
300 }
301
302 bool IK_QJacobianSolver::Solve(
303     IK_QSegment *root,
304     std::list<IK_QTask *> tasks,
305     const double,
306     const int max_iterations
307     )
308 {
309         float scale = ComputeScale();
310         bool solved = false;
311         //double dt = analyze_time();
312
313         Scale(scale, tasks);
314
315         ConstrainPoleVector(root, tasks);
316
317         root->UpdateTransform(m_rootmatrix);
318
319         // iterate
320         for (int iterations = 0; iterations < max_iterations; iterations++) {
321                 // update transform
322                 root->UpdateTransform(m_rootmatrix);
323
324                 std::list<IK_QTask *>::iterator task;
325
326                 // compute jacobian
327                 for (task = tasks.begin(); task != tasks.end(); task++) {
328                         if ((*task)->Primary())
329                                 (*task)->ComputeJacobian(m_jacobian);
330                         else
331                                 (*task)->ComputeJacobian(m_jacobian_sub);
332                 }
333
334                 double norm = 0.0;
335
336                 do {
337                         // invert jacobian
338                         try {
339                                 m_jacobian.Invert();
340                                 if (m_secondary_enabled)
341                                         m_jacobian.SubTask(m_jacobian_sub);
342                         }
343                         catch (...) {
344                                 fprintf(stderr, "IK Exception\n");
345                                 return false;
346                         }
347
348                         // update angles and check limits
349                 } while (UpdateAngles(norm));
350
351                 // unlock segments again after locking in clamping loop
352                 std::vector<IK_QSegment *>::iterator seg;
353                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
354                         (*seg)->UnLock();
355
356                 // compute angle update norm
357                 double maxnorm = m_jacobian.AngleUpdateNorm();
358                 if (maxnorm > norm)
359                         norm = maxnorm;
360
361                 // check for convergence
362                 if (norm < 1e-3 && iterations > 10) {
363                         solved = true;
364                         break;
365                 }
366         }
367
368         if (m_poleconstraint)
369                 root->PrependBasis(m_rootmatrix.linear());
370
371         Scale(1.0f / scale, tasks);
372
373         //analyze_add_run(max_iterations, analyze_time()-dt);
374
375         return solved;
376 }
377