Merging r49287 through r49299 from trunk into soc-2011-tomato
[blender.git] / intern / iksolver / intern / IK_QTask.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_QTask.cpp
30  *  \ingroup iksolver
31  */
32
33
34 #include "IK_QTask.h"
35
36 // IK_QTask
37
38 IK_QTask::IK_QTask(
39         int size,
40         bool primary,
41         bool active,
42         const IK_QSegment *segment
43 ) :
44         m_size(size), m_primary(primary), m_active(active), m_segment(segment),
45         m_weight(1.0)
46 {
47 }
48
49 // IK_QPositionTask
50
51 IK_QPositionTask::IK_QPositionTask(
52         bool primary,
53         const IK_QSegment *segment,
54         const MT_Vector3& goal
55 ) :
56         IK_QTask(3, primary, true, segment), m_goal(goal)
57 {
58         // computing clamping length
59         int num;
60         const IK_QSegment *seg;
61
62         m_clamp_length = 0.0;
63         num = 0;
64
65         for (seg = m_segment; seg; seg = seg->Parent()) {
66                 m_clamp_length += seg->MaxExtension();
67                 num++;
68         }
69
70         m_clamp_length /= 2*num;
71 }
72
73 void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
74 {
75         // compute beta
76         const MT_Vector3& pos = m_segment->GlobalEnd();
77
78         MT_Vector3 d_pos = m_goal - pos;
79         MT_Scalar length = d_pos.length();
80
81         if (length > m_clamp_length)
82                 d_pos = (m_clamp_length/length)*d_pos;
83         
84         jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
85
86         // compute derivatives
87         int i;
88         const IK_QSegment *seg;
89
90         for (seg = m_segment; seg; seg = seg->Parent()) {
91                 MT_Vector3 p = seg->GlobalStart() - pos;
92
93                 for (i = 0; i < seg->NumberOfDoF(); i++) {
94                         MT_Vector3 axis = seg->Axis(i)*m_weight;
95
96                         if (seg->Translational())
97                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis, 1e2);
98                         else {
99                                 MT_Vector3 pa = p.cross(axis);
100                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, pa, 1e0);
101                         }
102                 }
103         }
104 }
105
106 MT_Scalar IK_QPositionTask::Distance() const
107 {
108         const MT_Vector3& pos = m_segment->GlobalEnd();
109         MT_Vector3 d_pos = m_goal - pos;
110         return d_pos.length();
111 }
112
113 // IK_QOrientationTask
114
115 IK_QOrientationTask::IK_QOrientationTask(
116         bool primary,
117         const IK_QSegment *segment,
118         const MT_Matrix3x3& goal
119 ) :
120         IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
121 {
122 }
123
124 void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
125 {
126         // compute betas
127         const MT_Matrix3x3& rot = m_segment->GlobalTransform().getBasis();
128
129         MT_Matrix3x3 d_rotm = m_goal*rot.transposed();
130         d_rotm.transpose();
131
132         MT_Vector3 d_rot;
133         d_rot = -0.5*MT_Vector3(d_rotm[2][1] - d_rotm[1][2],
134                                 d_rotm[0][2] - d_rotm[2][0],
135                                 d_rotm[1][0] - d_rotm[0][1]);
136
137         m_distance = d_rot.length();
138
139         jacobian.SetBetas(m_id, m_size, m_weight*d_rot);
140
141         // compute derivatives
142         int i;
143         const IK_QSegment *seg;
144
145         for (seg = m_segment; seg; seg = seg->Parent())
146                 for (i = 0; i < seg->NumberOfDoF(); i++) {
147
148                         if (seg->Translational())
149                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, MT_Vector3(0, 0, 0), 1e2);
150                         else {
151                                 MT_Vector3 axis = seg->Axis(i)*m_weight;
152                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis, 1e0);
153                         }
154                 }
155 }
156
157 // IK_QCenterOfMassTask
158 // Note: implementation not finished!
159
160 IK_QCenterOfMassTask::IK_QCenterOfMassTask(
161         bool primary,
162         const IK_QSegment *segment,
163         const MT_Vector3& goal_center
164 ) :
165         IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
166 {
167         m_total_mass_inv = ComputeTotalMass(m_segment);
168         if (!MT_fuzzyZero(m_total_mass_inv))
169                 m_total_mass_inv = 1.0/m_total_mass_inv;
170 }
171
172 MT_Scalar IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
173 {
174         MT_Scalar mass = /*seg->Mass()*/ 1.0;
175
176         const IK_QSegment *seg;
177         for (seg = segment->Child(); seg; seg = seg->Sibling())
178                 mass += ComputeTotalMass(seg);
179         
180         return mass;
181 }
182
183 MT_Vector3 IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
184 {
185         MT_Vector3 center = /*seg->Mass()**/segment->GlobalStart();
186
187         const IK_QSegment *seg;
188         for (seg = segment->Child(); seg; seg = seg->Sibling())
189                 center += ComputeCenter(seg);
190         
191         return center;
192 }
193
194 void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& center, const IK_QSegment *segment)
195 {
196         int i;
197         MT_Vector3 p = center - segment->GlobalStart();
198
199         for (i = 0; i < segment->NumberOfDoF(); i++) {
200                 MT_Vector3 axis = segment->Axis(i)*m_weight;
201                 axis *= /*segment->Mass()**/m_total_mass_inv;
202                 
203                 if (segment->Translational())
204                         jacobian.SetDerivatives(m_id, segment->DoFId()+i, axis, 1e2);
205                 else {
206                         MT_Vector3 pa = axis.cross(p);
207                         jacobian.SetDerivatives(m_id, segment->DoFId()+i, pa, 1e0);
208                 }
209         }
210         
211         const IK_QSegment *seg;
212         for (seg = segment->Child(); seg; seg = seg->Sibling())
213                 JacobianSegment(jacobian, center, seg);
214 }
215
216 void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian)
217 {
218         MT_Vector3 center = ComputeCenter(m_segment)*m_total_mass_inv;
219
220         // compute beta
221         MT_Vector3 d_pos = m_goal_center - center;
222
223         m_distance = d_pos.length();
224
225 #if 0
226         if (m_distance > m_clamp_length)
227                 d_pos = (m_clamp_length/m_distance)*d_pos;
228 #endif
229         
230         jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
231
232         // compute derivatives
233         JacobianSegment(jacobian, center, m_segment);
234 }
235
236 MT_Scalar IK_QCenterOfMassTask::Distance() const
237 {
238         return m_distance;
239 }
240