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