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