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