8ae836a7eb8ac5715cf202ead0d8df64c00906a4
[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 "IK_QJacobianSolver.h"
34
35 //#include "analyze.h"
36
37 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
38 {
39         m_segments.push_back(seg);
40
41         IK_QSegment *child;
42         for (child = seg->Child(); child; child = child->Sibling())
43                 AddSegmentList(child);
44 }
45
46 bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
47 {
48         m_segments.clear();
49         AddSegmentList(root);
50
51         // assing each segment a unique id for the jacobian
52         std::vector<IK_QSegment*>::iterator seg;
53         int num_dof = 0;
54
55         for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
56                 (*seg)->SetDoFId(num_dof);
57                 num_dof += (*seg)->NumberOfDoF();
58         }
59
60         if (num_dof == 0)
61                 return false;
62
63         // compute task id's and assing weights to task
64         int primary_size = 0, primary = 0;
65         int secondary_size = 0, secondary = 0;
66         MT_Scalar primary_weight = 0.0, secondary_weight = 0.0;
67         std::list<IK_QTask*>::iterator task;
68
69         for (task = tasks.begin(); task != tasks.end(); task++) {
70                 IK_QTask *qtask = *task;
71
72                 if (qtask->Primary()) {
73                         qtask->SetId(primary_size);
74                         primary_size += qtask->Size();
75                         primary_weight += qtask->Weight();
76                         primary++;
77                 }
78                 else {
79                         qtask->SetId(secondary_size);
80                         secondary_size += qtask->Size();
81                         secondary_weight += qtask->Weight();
82                         secondary++;
83                 }
84         }
85
86         if (primary_size == 0 || MT_fuzzyZero(primary_weight))
87                 return false;
88
89         m_secondary_enabled = (secondary > 0);
90         
91         // rescale weights of tasks to sum up to 1
92         MT_Scalar primary_rescale = 1.0/primary_weight;
93         MT_Scalar secondary_rescale;
94         if (MT_fuzzyZero(secondary_weight))
95                 secondary_rescale = 0.0;
96         else
97                 secondary_rescale = 1.0/secondary_weight;
98         
99         for (task = tasks.begin(); task != tasks.end(); task++) {
100                 IK_QTask *qtask = *task;
101
102                 if (qtask->Primary())
103                         qtask->SetWeight(qtask->Weight()*primary_rescale);
104                 else
105                         qtask->SetWeight(qtask->Weight()*secondary_rescale);
106         }
107
108         // set matrix sizes
109         m_jacobian.ArmMatrices(num_dof, primary_size, primary);
110         if (secondary > 0)
111                 m_jacobian_sub.ArmMatrices(num_dof, secondary_size, secondary);
112
113         // set dof weights
114         int i;
115
116         for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
117                 for (i = 0; i < (*seg)->NumberOfDoF(); i++)
118                         m_jacobian.SetDoFWeight((*seg)->DoFId()+i, (*seg)->Weight(i));
119
120         return true;
121 }
122
123 bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
124 {
125         // assing each segment a unique id for the jacobian
126         std::vector<IK_QSegment*>::iterator seg;
127         IK_QSegment *qseg, *minseg = NULL;
128         MT_Scalar minabsdelta = 1e10, absdelta;
129         MT_Vector3 delta, mindelta;
130         bool locked = false, clamp[3];
131         int i, mindof = 0;
132
133         // here we check if any angle limits were violated. angles whose clamped
134         // position is the same as it was before, are locked immediate. of the
135         // other violation angles the most violating angle is rememberd
136         for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
137                 qseg = *seg;
138                 if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
139                         for (i = 0; i < qseg->NumberOfDoF(); i++) {
140                                 if (clamp[i] && !qseg->Locked(i)) {
141                                         absdelta = MT_abs(delta[i]);
142
143                                         if (absdelta < MT_EPSILON) {
144                                                 qseg->Lock(i, m_jacobian, delta);
145                                                 locked = true;
146                                         }
147                                         else if (absdelta < minabsdelta) {
148                                                 minabsdelta = absdelta;
149                                                 mindelta = delta;
150                                                 minseg = qseg;
151                                                 mindof = i;
152                                         }
153                                 }
154                         }
155                 }
156         }
157
158         // lock most violating angle
159         if (minseg) {
160                 minseg->Lock(mindof, m_jacobian, mindelta);
161                 locked = true;
162
163                 if (minabsdelta > norm)
164                         norm = minabsdelta;
165         }
166
167         if (locked == false)
168                 // no locking done, last inner iteration, apply the angles
169                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
170                         (*seg)->UnLock();
171                         (*seg)->UpdateAngleApply();
172                 }
173         
174         // signal if another inner iteration is needed
175         return locked;
176 }
177
178 bool IK_QJacobianSolver::Solve(
179         IK_QSegment *root,
180         std::list<IK_QTask*> tasks,
181         MT_Scalar,
182         int max_iterations
183 )
184 {
185         //double dt = analyze_time();
186
187         if (!Setup(root, tasks))
188                 return false;
189
190         // iterate
191         for (int iterations = 0; iterations < max_iterations; iterations++) {
192                 // update transform
193                 root->UpdateTransform(MT_Transform::Identity());
194
195                 std::list<IK_QTask*>::iterator task;
196
197                 // compute jacobian
198                 for (task = tasks.begin(); task != tasks.end(); task++) {
199                         if ((*task)->Primary())
200                                 (*task)->ComputeJacobian(m_jacobian);
201                         else
202                                 (*task)->ComputeJacobian(m_jacobian_sub);
203                 }
204
205                 MT_Scalar norm = 0.0;
206
207                 do {
208                         // invert jacobian
209                         try {
210                                 m_jacobian.Invert();
211                                 if (m_secondary_enabled)
212                                         m_jacobian.SubTask(m_jacobian_sub);
213                         }
214                         catch (...) {
215                                 printf("IK Exception\n");
216                                 return false;
217                         }
218
219                         // update angles and check limits
220                 } while (UpdateAngles(norm));
221
222                 // unlock segments again after locking in clamping loop
223                 std::vector<IK_QSegment*>::iterator seg;
224                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
225                         (*seg)->UnLock();
226
227                 // compute angle update norm
228                 MT_Scalar maxnorm = m_jacobian.AngleUpdateNorm();
229                 if (maxnorm > norm)
230                         norm = maxnorm;
231
232                 // check for convergence
233                 if (norm < 1e-3) {
234                         //analyze_add_run(iterations, analyze_time()-dt);
235                         return true;
236                 }
237         }
238
239         //analyze_add_run(max_iterations, analyze_time()-dt);
240         return false;
241 }
242