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