4bd3d28ce69aef7163ec36b41faf7d0d069f4cde
[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 #include <iostream>
38 using namespace std;
39
40 void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
41 {
42         m_segments.push_back(seg);
43
44         IK_QSegment *child;
45         for (child = seg->Child(); child; child = child->Sibling())
46                 AddSegmentList(child);
47 }
48
49 bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask*>& tasks)
50 {
51         m_segments.clear();
52         AddSegmentList(root);
53
54         // assing each segment a unique id for the jacobian
55         std::vector<IK_QSegment*>::iterator seg;
56         int num_dof = 0;
57
58         for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
59                 (*seg)->SetDoFId(num_dof);
60                 num_dof += (*seg)->NumberOfDoF();
61         }
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         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         if (minseg) {
156                 minseg->Lock(mindof, m_jacobian, mindelta);
157                 locked = true;
158
159                 if (minabsdelta > norm)
160                         norm = minabsdelta;
161         }
162
163         if (locked == false)
164                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
165                         (*seg)->UnLock();
166                         (*seg)->UpdateAngleApply();
167                 }
168         
169         return locked;
170 }
171
172 bool IK_QJacobianSolver::Solve(
173         IK_QSegment *root,
174         std::list<IK_QTask*> tasks,
175         MT_Scalar,
176         int max_iterations
177 )
178 {
179         //double dt = analyze_time();
180
181         if (!Setup(root, tasks))
182                 return false;
183
184         // iterate
185         for (int iterations = 0; iterations < max_iterations; iterations++) {
186                 // update transform
187                 root->UpdateTransform(MT_Transform::Identity());
188
189                 std::list<IK_QTask*>::iterator task;
190
191                 //bool done = true;
192
193                 // compute jacobian
194                 for (task = tasks.begin(); task != tasks.end(); task++) {
195                         if ((*task)->Primary())
196                                 (*task)->ComputeJacobian(m_jacobian);
197                         else
198                                 (*task)->ComputeJacobian(m_jacobian_sub);
199
200                         //printf("#> %f\n", (*task)->Distance());
201                         //if ((*task)->Distance() > 1e-4)
202                         //      done = false;
203                 }
204
205                 /*if (done) {
206                         //analyze_add_run(iterations, analyze_time()-dt);
207                         return true;
208                 }*/
209
210                 MT_Scalar norm = 0.0;
211
212                 do {
213                         // invert jacobian
214                         try {
215                                 m_jacobian.Invert();
216                                 if (m_secondary_enabled)
217                                         m_jacobian.SubTask(m_jacobian_sub);
218                         }
219                         catch (...) {
220                                 printf("IK Exception\n");
221                                 return false;
222                         }
223
224                         // update angles and check limits
225                 } while (UpdateAngles(norm));
226
227                 // unlock segments again after locking in clamping loop
228                 std::vector<IK_QSegment*>::iterator seg;
229                 for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
230                         (*seg)->UnLock();
231
232                 // compute angle update norm
233                 MT_Scalar maxnorm = m_jacobian.AngleUpdateNorm();
234                 if (maxnorm > norm)
235                         norm = maxnorm;
236
237                 // check for convergence
238                 if (norm < 1e-3) {
239                         //analyze_add_run(iterations, analyze_time()-dt);
240                         return true;
241                 }
242         }
243
244         //analyze_add_run(max_iterations, analyze_time()-dt);
245         return false;
246 }
247