Patch to change license to GPL only, from GSR.
[blender-staging.git] / intern / iksolver / intern / MT_ExpMap.h
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 #ifndef MT_ExpMap_H
31 #define MT_ExpMap_H
32
33 #include <MT_assert.h>
34
35 #include "MT_Vector3.h"
36 #include "MT_Quaternion.h"
37 #include "MT_Matrix4x4.h"
38
39 const MT_Scalar MT_EXPMAP_MINANGLE (1e-7);
40
41 /**
42  * MT_ExpMap an exponential map parameterization of rotations
43  * in 3D. This implementation is derived from the paper 
44  * "F. Sebastian Grassia. Practical parameterization of 
45  * rotations using the exponential map. Journal of Graphics Tools,
46  *  3(3):29-48, 1998" Please go to http://www.acm.org/jgt/papers/Grassia98/
47  * for a thorough description of the theory and sample code used 
48  * to derive this class. 
49  *
50  * Basic overview of why this class is used.
51  * In an IK system we need to paramterize the joint angles in some
52  * way. Typically 2 parameterizations are used.
53  * - Euler Angles
54  * These suffer from singularities in the parameterization known
55  * as gimbal lock. They also do not interpolate well. For every
56  * set of euler angles there is exactly 1 corresponding 3d rotation.
57  * - Quaternions.
58  * Great for interpolating. Only unit quaternions are valid rotations
59  * means that in a differential ik solver we often stray outside of
60  * this manifold into invalid rotations. Means we have to do a lot
61  * of nasty normalizations all the time. Does not suffer from 
62  * gimbal lock problems. More expensive to compute partial derivatives
63  * as there are 4 of them.
64  * 
65  * So exponential map is similar to a quaternion axis/angle 
66  * representation but we store the angle as the length of the
67  * axis. So require only 3 parameters. Means that all exponential
68  * maps are valid rotations. Suffers from gimbal lock. But it's
69  * possible to detect when gimbal lock is near and reparameterize
70  * away from it. Also nice for interpolating.
71  * Exponential maps are share some of the useful properties of
72  * euler and quaternion parameterizations. And are very useful
73  * for differential IK solvers.
74  */
75
76 class MT_ExpMap {
77 public:
78
79         /**
80          * Default constructor
81          * @warning there is no initialization in the
82          * default constructor
83          */ 
84
85     MT_ExpMap() {}
86     MT_ExpMap(const MT_Vector3& v) : m_v(v) { angleUpdated(); }
87
88     MT_ExpMap(const float v[3]) : m_v(v) { angleUpdated(); }
89     MT_ExpMap(const double v[3]) : m_v(v) { angleUpdated(); }
90
91     MT_ExpMap(MT_Scalar x, MT_Scalar y, MT_Scalar z) :
92         m_v(x, y, z) { angleUpdated(); }
93
94         /** 
95          * Construct an exponential map from a quaternion
96          */
97
98         MT_ExpMap(
99                 const MT_Quaternion &q
100         ) {
101                 setRotation(q);
102         };
103
104         /** 
105          * Accessors
106          * Decided not to inherit from MT_Vector3 but rather
107          * this class contains an MT_Vector3. This is because
108          * it is very dangerous to use MT_Vector3 functions
109          * on this class and some of them have no direct meaning.
110          */
111
112         const 
113                 MT_Vector3 &
114         vector(
115         ) const {
116                 return m_v;
117         };
118
119         /** 
120          * Set the exponential map from a quaternion
121          */
122
123                 void
124         setRotation(
125                 const MT_Quaternion &q
126         );
127
128         /** 
129          * Convert from an exponential map to a quaternion
130          * representation
131          */     
132         
133                 const MT_Quaternion&
134         getRotation(
135         ) const;
136
137         /** 
138          * Convert the exponential map to a 3x3 matrix
139          */
140
141                 MT_Matrix3x3
142         getMatrix(
143         ) const; 
144         
145         /** 
146          * Update (and reparameterize) the expontial map
147          * @param dv delta update values.
148          */
149
150                 void
151         update(
152                 const MT_Vector3& dv
153         );
154
155         /**
156          * Compute the partial derivatives of the exponential
157          * map  (dR/de - where R is a 4x4 matrix formed 
158          * from the map) and return them as a 4x4 matrix
159          */
160
161                 void
162         partialDerivatives(
163                 MT_Matrix3x3& dRdx,
164                 MT_Matrix3x3& dRdy,
165                 MT_Matrix3x3& dRdz
166         ) const ;
167         
168 private :
169
170         // m_v contains the exponential map, the other variables are
171         // cached for efficiency
172         
173         MT_Vector3 m_v;
174         MT_Scalar m_theta, m_sinp;
175         MT_Quaternion m_q;
176
177         // private methods
178
179         // Compute partial derivatives dR (3x3 rotation matrix) / dVi (EM vector)
180         // given the partial derivative dQ (Quaternion) / dVi (ith element of EM vector)
181
182                 void
183         compute_dRdVi(
184                 const MT_Quaternion &dQdV,
185                 MT_Matrix3x3 & dRdVi
186         ) const;
187
188         // compute partial derivatives dQ/dVi
189
190                 void
191         compute_dQdVi(
192                 MT_Quaternion *dQdX
193         ) const ; 
194
195         // reparametrize away from singularity
196
197                 void
198         reParametrize(
199         );
200
201         // (re-)compute cached variables
202
203                 void
204         angleUpdated(
205         );
206 };
207
208 #endif
209