Initial revision
[blender.git] / intern / iksolver / intern / IK_QChain.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  * Contributor(s): none yet.
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 /**
33
34  * $Id$
35  * Copyright (C) 2001 NaN Technologies B.V.
36  *
37  * @author Laurence
38  */
39
40 #include "IK_QChain.h"
41
42 using namespace std;
43
44 IK_QChain::
45 IK_QChain(
46 )
47 {
48         // nothing to do;
49 };
50
51 const 
52         vector<IK_QSegment> &
53 IK_QChain::
54 Segments(
55 ) const {
56         return m_segments;
57 };
58
59         vector<IK_QSegment> &
60 IK_QChain::
61 Segments(
62 ){
63         return m_segments;
64 };      
65
66         void
67 IK_QChain::
68 UpdateGlobalTransformations(
69 ){
70
71         // now iterate through the segment list
72         // compute their local transformations if needed
73                 
74         // assign their global transformations
75         // (relative to chain origin)
76
77         vector<IK_QSegment>::const_iterator s_end = m_segments.end();
78         vector<IK_QSegment>::iterator s_it = m_segments.begin();
79
80         MT_Transform global;
81         global.setIdentity();
82
83         for (; s_it != s_end; ++s_it) {
84                 global = s_it->UpdateGlobal(global);
85         }
86
87         // we also need to compute the accumulated local transforms
88         // for each segment
89
90         MT_Transform acc_local;
91         acc_local.setIdentity();
92
93         vector<IK_QSegment>::reverse_iterator s_rit = m_segments.rbegin();
94         vector<IK_QSegment>::reverse_iterator s_rend = m_segments.rend();
95
96         for (; s_rit != s_rend; ++s_rit) {
97                 acc_local = s_rit->UpdateAccumulatedLocal(acc_local);
98         }               
99
100         // compute the position of the end effector and it's pose
101
102         const MT_Transform &last_t = m_segments.back().GlobalTransform();
103         m_end_effector = last_t.getOrigin();
104
105 #if 0
106         
107         // The end pose is not currently used. 
108
109         MT_Matrix3x3 last_basis = last_t.getBasis();
110         last_basis.transpose();
111         MT_Vector3 m_end_pose = last_basis[1];
112         
113 #endif
114         
115 };
116         
117 const 
118         TNT::Matrix<MT_Scalar> &
119 IK_QChain::
120 Jacobian(
121 ) const  {
122         return m_jacobian;
123 } ;
124  
125
126 const 
127         TNT::Matrix<MT_Scalar> &
128 IK_QChain::
129 TransposedJacobian(
130 ) const {
131         return m_t_jacobian;
132 };
133
134         void
135 IK_QChain::
136 ComputeJacobian(
137 ){
138         // let's assume that the chain's global transfomations
139         // have already been computed.
140         
141         int dof = DoF();
142
143         int num_segs = m_segments.size();
144         vector<IK_QSegment>::const_iterator segs = m_segments.begin();
145
146         m_t_jacobian.newsize(dof,3);
147         m_jacobian.newsize(3,dof);
148
149         // compute the transposed jacobian first
150
151         int n;
152         int i = 0;
153
154         for (n= 0; n < num_segs; n++) {
155 #if 0
156
157                 // For euler angle computation we can use a slightly quicker method.
158
159                 const MT_Matrix3x3 &basis = segs[n].GlobalTransform().getBasis();
160                 const MT_Vector3 &origin  = segs[n].GlobalSegmentStart();
161                                 
162                 const MT_Vector3 p = origin-m_end_effector;
163
164                 const MT_Vector3 x_axis(1,0,0);
165                 const MT_Vector3 y_axis(0,1,0);
166                 const MT_Vector3 z_axis(0,0,1);
167                 
168                 MT_Vector3 a = basis * x_axis;
169                 MT_Vector3 pca = p.cross(a);
170
171                 m_t_jacobian(n*3 + 1,1) = pca.x();
172                 m_t_jacobian(n*3 + 1,2) = pca.y();
173                 m_t_jacobian(n*3 + 1,3) = pca.z();
174
175                 a = basis * y_axis;
176                 pca = p.cross(a);
177
178                 m_t_jacobian(n*3 + 2,1) = pca.x();
179                 m_t_jacobian(n*3 + 2,2) = pca.y();
180                 m_t_jacobian(n*3 + 2,3) = pca.z();
181
182                 a = basis * z_axis;
183                 pca = p.cross(a);
184
185                 m_t_jacobian(n*3 + 3,1) = pca.x();
186                 m_t_jacobian(n*3 + 3,2) = pca.y();
187                 m_t_jacobian(n*3 + 3,3) = pca.z();
188 #else
189                 // user slower general jacobian computation method
190
191                 MT_Vector3 j1 = segs[n].ComputeJacobianColumn(0);
192
193                 m_t_jacobian(n*3 + 1,1) = j1.x();
194                 m_t_jacobian(n*3 + 1,2) = j1.y();
195                 m_t_jacobian(n*3 + 1,3) = j1.z();
196
197                 j1 = segs[n].ComputeJacobianColumn(1);
198
199                 m_t_jacobian(n*3 + 2,1) = j1.x();
200                 m_t_jacobian(n*3 + 2,2) = j1.y();
201                 m_t_jacobian(n*3 + 2,3) = j1.z();
202
203                 j1 = segs[n].ComputeJacobianColumn(2);
204
205                 m_t_jacobian(n*3 + 3,1) = j1.x();
206                 m_t_jacobian(n*3 + 3,2) = j1.y();
207                 m_t_jacobian(n*3 + 3,3) = j1.z();
208 #endif
209
210
211
212         }
213
214
215         // get the origina1 jacobain
216
217         TNT::transpose(m_t_jacobian,m_jacobian);
218 };
219
220         MT_Vector3 
221 IK_QChain::
222 EndEffector(
223 ) const {
224         return m_end_effector;
225 };
226
227         MT_Vector3 
228 IK_QChain::
229 EndPose(
230 ) const {
231         return m_end_pose;
232 };
233                 
234
235         int
236 IK_QChain::
237 DoF(
238 ) const {
239         return 3 * m_segments.size();
240 }
241
242 const 
243         MT_Scalar
244 IK_QChain::
245 MaxExtension(
246 ) const {
247
248         vector<IK_QSegment>::const_iterator s_end = m_segments.end();
249         vector<IK_QSegment>::const_iterator s_it = m_segments.begin();
250
251         if (m_segments.size() == 0) return MT_Scalar(0);
252
253         MT_Scalar output = s_it->Length();
254         
255         ++s_it  ;
256         for (; s_it != s_end; ++s_it) {
257                 output += s_it->MaxExtension();
258         }
259         return output;
260 }
261
262         
263
264
265
266
267
268
269
270
271
272
273         
274
275
276
277
278
279
280