doxygen: intern/iksolver tagged. Leaving out TNT, since we have it in multiple locations.
[blender.git] / intern / iksolver / intern / MT_ExpMap.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/MT_ExpMap.cpp
31  *  \ingroup iksolver
32  */
33
34
35 #include "MT_ExpMap.h"
36
37 /** 
38  * Set the exponential map from a quaternion. The quaternion must be non-zero.
39  */
40
41         void
42 MT_ExpMap::
43 setRotation(
44         const MT_Quaternion &q
45 ) {
46         // ok first normalize the quaternion
47         // then compute theta the axis-angle and the normalized axis v
48         // scale v by theta and that's it hopefully!
49
50         m_q = q.normalized();
51         m_v = MT_Vector3(m_q.x(), m_q.y(), m_q.z());
52
53         MT_Scalar cosp = m_q.w();
54         m_sinp = m_v.length();
55         m_v /= m_sinp;
56
57         m_theta = atan2(double(m_sinp),double(cosp));
58         m_v *= m_theta;
59 }
60         
61 /** 
62  * Convert from an exponential map to a quaternion
63  * representation
64  */     
65
66         const MT_Quaternion&
67 MT_ExpMap::
68 getRotation(
69 ) const {
70         return m_q;
71 }
72         
73 /** 
74  * Convert the exponential map to a 3x3 matrix
75  */
76
77         MT_Matrix3x3
78 MT_ExpMap::
79 getMatrix(
80 ) const {
81         return MT_Matrix3x3(m_q);
82 }
83
84 /** 
85  * Update & reparameterizate the exponential map
86  */
87
88         void
89 MT_ExpMap::
90 update(
91         const MT_Vector3& dv
92 ){
93         m_v += dv;
94
95         angleUpdated();
96 }
97
98 /**
99  * Compute the partial derivatives of the exponential
100  * map  (dR/de - where R is a 3x3 rotation matrix formed 
101  * from the map) and return them as a 3x3 matrix
102  */
103
104         void
105 MT_ExpMap::
106 partialDerivatives(
107         MT_Matrix3x3& dRdx,
108         MT_Matrix3x3& dRdy,
109         MT_Matrix3x3& dRdz
110 ) const {
111         
112         MT_Quaternion dQdx[3];
113
114         compute_dQdVi(dQdx);
115
116         compute_dRdVi(dQdx[0], dRdx);
117         compute_dRdVi(dQdx[1], dRdy);
118         compute_dRdVi(dQdx[2], dRdz);
119 }
120
121         void
122 MT_ExpMap::
123 compute_dRdVi(
124         const MT_Quaternion &dQdvi,
125         MT_Matrix3x3 & dRdvi
126 ) const {
127
128         MT_Scalar  prod[9];
129         
130         /* This efficient formulation is arrived at by writing out the
131          * entire chain rule product dRdq * dqdv in terms of 'q' and 
132          * noticing that all the entries are formed from sums of just
133          * nine products of 'q' and 'dqdv' */
134
135         prod[0] = -MT_Scalar(4)*m_q.x()*dQdvi.x();
136         prod[1] = -MT_Scalar(4)*m_q.y()*dQdvi.y();
137         prod[2] = -MT_Scalar(4)*m_q.z()*dQdvi.z();
138         prod[3] = MT_Scalar(2)*(m_q.y()*dQdvi.x() + m_q.x()*dQdvi.y());
139         prod[4] = MT_Scalar(2)*(m_q.w()*dQdvi.z() + m_q.z()*dQdvi.w());
140         prod[5] = MT_Scalar(2)*(m_q.z()*dQdvi.x() + m_q.x()*dQdvi.z());
141         prod[6] = MT_Scalar(2)*(m_q.w()*dQdvi.y() + m_q.y()*dQdvi.w());
142         prod[7] = MT_Scalar(2)*(m_q.z()*dQdvi.y() + m_q.y()*dQdvi.z());
143         prod[8] = MT_Scalar(2)*(m_q.w()*dQdvi.x() + m_q.x()*dQdvi.w());
144
145         /* first row, followed by second and third */
146         dRdvi[0][0] = prod[1] + prod[2];
147         dRdvi[0][1] = prod[3] - prod[4];
148         dRdvi[0][2] = prod[5] + prod[6];
149
150         dRdvi[1][0] = prod[3] + prod[4];
151         dRdvi[1][1] = prod[0] + prod[2];
152         dRdvi[1][2] = prod[7] - prod[8];
153
154         dRdvi[2][0] = prod[5] - prod[6];
155         dRdvi[2][1] = prod[7] + prod[8];
156         dRdvi[2][2] = prod[0] + prod[1];
157 }
158
159 // compute partial derivatives dQ/dVi
160
161         void
162 MT_ExpMap::
163 compute_dQdVi(
164         MT_Quaternion *dQdX
165 ) const {
166
167         /* This is an efficient implementation of the derivatives given
168          * in Appendix A of the paper with common subexpressions factored out */
169
170         MT_Scalar sinc, termCoeff;
171
172         if (m_theta < MT_EXPMAP_MINANGLE) {
173                 sinc = 0.5 - m_theta*m_theta/48.0;
174                 termCoeff = (m_theta*m_theta/40.0 - 1.0)/24.0;
175         }
176         else {
177                 MT_Scalar cosp = m_q.w();
178                 MT_Scalar ang = 1.0/m_theta;
179
180                 sinc = m_sinp*ang;
181                 termCoeff = ang*ang*(0.5*cosp - sinc);
182         }
183
184         for (int i = 0; i < 3; i++) {
185                 MT_Quaternion& dQdx = dQdX[i];
186                 int i2 = (i+1)%3;
187                 int i3 = (i+2)%3;
188
189                 MT_Scalar term = m_v[i]*termCoeff;
190                 
191                 dQdx[i] = term*m_v[i] + sinc;
192                 dQdx[i2] = term*m_v[i2];
193                 dQdx[i3] = term*m_v[i3];
194                 dQdx.w() = -0.5*m_v[i]*sinc;
195         }
196 }
197
198 // reParametize away from singularity, updating
199 // m_v and m_theta
200
201         void
202 MT_ExpMap::
203 reParametrize(
204 ){
205         if (m_theta > MT_PI) {
206                 MT_Scalar scl = m_theta;
207                 if (m_theta > MT_2_PI){ /* first get theta into range 0..2PI */
208                         m_theta = MT_Scalar(fmod(m_theta, MT_2_PI));
209                         scl = m_theta/scl;
210                         m_v *= scl;
211                 }
212                 if (m_theta > MT_PI){
213                         scl = m_theta;
214                         m_theta = MT_2_PI - m_theta;
215                         scl = MT_Scalar(1.0) - MT_2_PI/scl;
216                         m_v *= scl;
217                 }
218         }
219 }
220
221 // compute cached variables
222
223         void
224 MT_ExpMap::
225 angleUpdated(
226 ){
227         m_theta = m_v.length();
228
229         reParametrize();
230
231         // compute quaternion, sinp and cosp
232
233         if (m_theta < MT_EXPMAP_MINANGLE) {
234                 m_sinp = MT_Scalar(0.0);
235
236                 /* Taylor Series for sinc */
237                 MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - m_theta*m_theta/MT_Scalar(48.0));
238                 m_q.x() = temp.x();
239                 m_q.y() = temp.y();
240                 m_q.z() = temp.z();
241                 m_q.w() = MT_Scalar(1.0);
242         } else {
243                 m_sinp = MT_Scalar(sin(.5*m_theta));
244
245                 /* Taylor Series for sinc */
246                 MT_Vector3 temp = m_v * (m_sinp/m_theta);
247                 m_q.x() = temp.x();
248                 m_q.y() = temp.y();
249                 m_q.z() = temp.z();
250                 m_q.w() = MT_Scalar(cos(.5*m_theta));
251         }
252 }
253