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