doxygen: intern/iksolver tagged. Leaving out TNT, since we have it in multiple locations.
[blender.git] / intern / iksolver / intern / IK_QTask.cpp
1 /*
2  * $Id$
3  * ***** BEGIN GPL 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.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software Foundation,
17  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
20  * All rights reserved.
21  *
22  * The Original Code is: all of this file.
23  *
24  * Original Author: Laurence
25  * Contributor(s): Brecht
26  *
27  * ***** END GPL LICENSE BLOCK *****
28  */
29
30 /** \file iksolver/intern/IK_QTask.cpp
31  *  \ingroup iksolver
32  */
33
34
35 #include "IK_QTask.h"
36
37 // IK_QTask
38
39 IK_QTask::IK_QTask(
40         int size,
41         bool primary,
42         bool active,
43         const IK_QSegment *segment
44 ) :
45         m_size(size), m_primary(primary), m_active(active), m_segment(segment),
46         m_weight(1.0)
47 {
48 }
49
50 // IK_QPositionTask
51
52 IK_QPositionTask::IK_QPositionTask(
53         bool primary,
54         const IK_QSegment *segment,
55         const MT_Vector3& goal
56 ) :
57         IK_QTask(3, primary, true, segment), m_goal(goal)
58 {
59         // computing clamping length
60         int num;
61         const IK_QSegment *seg;
62
63         m_clamp_length = 0.0;
64         num = 0;
65
66         for (seg = m_segment; seg; seg = seg->Parent()) {
67                 m_clamp_length += seg->MaxExtension();
68                 num++;
69         }
70
71         m_clamp_length /= 2*num;
72 }
73
74 void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
75 {
76         // compute beta
77         const MT_Vector3& pos = m_segment->GlobalEnd();
78
79         MT_Vector3 d_pos = m_goal - pos;
80         MT_Scalar length = d_pos.length();
81
82         if (length > m_clamp_length)
83                 d_pos = (m_clamp_length/length)*d_pos;
84         
85         jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
86
87         // compute derivatives
88         int i;
89         const IK_QSegment *seg;
90
91         for (seg = m_segment; seg; seg = seg->Parent()) {
92                 MT_Vector3 p = seg->GlobalStart() - pos;
93
94                 for (i = 0; i < seg->NumberOfDoF(); i++) {
95                         MT_Vector3 axis = seg->Axis(i)*m_weight;
96
97                         if (seg->Translational())
98                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis);
99                         else {
100                                 MT_Vector3 pa = p.cross(axis);
101                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, pa);
102                         }
103                 }
104         }
105 }
106
107 MT_Scalar IK_QPositionTask::Distance() const
108 {
109         const MT_Vector3& pos = m_segment->GlobalEnd();
110         MT_Vector3 d_pos = m_goal - pos;
111         return d_pos.length();
112 }
113
114 // IK_QOrientationTask
115
116 IK_QOrientationTask::IK_QOrientationTask(
117         bool primary,
118         const IK_QSegment *segment,
119         const MT_Matrix3x3& goal
120 ) :
121         IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
122 {
123 }
124
125 void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
126 {
127         // compute betas
128         const MT_Matrix3x3& rot = m_segment->GlobalTransform().getBasis();
129
130         MT_Matrix3x3 d_rotm = m_goal*rot.transposed();
131         d_rotm.transpose();
132
133         MT_Vector3 d_rot;
134         d_rot = -0.5*MT_Vector3(d_rotm[2][1] - d_rotm[1][2],
135                                 d_rotm[0][2] - d_rotm[2][0],
136                                 d_rotm[1][0] - d_rotm[0][1]);
137
138         m_distance = d_rot.length();
139
140         jacobian.SetBetas(m_id, m_size, m_weight*d_rot);
141
142         // compute derivatives
143         int i;
144         const IK_QSegment *seg;
145
146         for (seg = m_segment; seg; seg = seg->Parent())
147                 for (i = 0; i < seg->NumberOfDoF(); i++) {
148
149                         if (seg->Translational())
150                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, MT_Vector3(0, 0, 0));
151                         else {
152                                 MT_Vector3 axis = seg->Axis(i)*m_weight;
153                                 jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis);
154                         }
155                 }
156 }
157
158 // IK_QCenterOfMassTask
159 // Note: implementation not finished!
160
161 IK_QCenterOfMassTask::IK_QCenterOfMassTask(
162         bool primary,
163         const IK_QSegment *segment,
164         const MT_Vector3& goal_center
165 ) :
166         IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
167 {
168         m_total_mass_inv = ComputeTotalMass(m_segment);
169         if (!MT_fuzzyZero(m_total_mass_inv))
170                 m_total_mass_inv = 1.0/m_total_mass_inv;
171 }
172
173 MT_Scalar IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
174 {
175         MT_Scalar mass = /*seg->Mass()*/ 1.0;
176
177         const IK_QSegment *seg;
178         for (seg = segment->Child(); seg; seg = seg->Sibling())
179                 mass += ComputeTotalMass(seg);
180         
181         return mass;
182 }
183
184 MT_Vector3 IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
185 {
186         MT_Vector3 center = /*seg->Mass()**/segment->GlobalStart();
187
188         const IK_QSegment *seg;
189         for (seg = segment->Child(); seg; seg = seg->Sibling())
190                 center += ComputeCenter(seg);
191         
192         return center;
193 }
194
195 void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& center, const IK_QSegment *segment)
196 {
197         int i;
198         MT_Vector3 p = center - segment->GlobalStart();
199
200         for (i = 0; i < segment->NumberOfDoF(); i++) {
201                 MT_Vector3 axis = segment->Axis(i)*m_weight;
202                 axis *= /*segment->Mass()**/m_total_mass_inv;
203                 
204                 if (segment->Translational())
205                         jacobian.SetDerivatives(m_id, segment->DoFId()+i, axis);
206                 else {
207                         MT_Vector3 pa = axis.cross(p);
208                         jacobian.SetDerivatives(m_id, segment->DoFId()+i, pa);
209                 }
210         }
211         
212         const IK_QSegment *seg;
213         for (seg = segment->Child(); seg; seg = seg->Sibling())
214                 JacobianSegment(jacobian, center, seg);
215 }
216
217 void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian)
218 {
219         MT_Vector3 center = ComputeCenter(m_segment)*m_total_mass_inv;
220
221         // compute beta
222         MT_Vector3 d_pos = m_goal_center - center;
223
224         m_distance = d_pos.length();
225
226 #if 0
227         if (m_distance > m_clamp_length)
228                 d_pos = (m_clamp_length/m_distance)*d_pos;
229 #endif
230         
231         jacobian.SetBetas(m_id, m_size, m_weight*d_pos);
232
233         // compute derivatives
234         JacobianSegment(jacobian, center, m_segment);
235 }
236
237 MT_Scalar IK_QCenterOfMassTask::Distance() const
238 {
239         return m_distance;
240 }
241