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