style cleanup
[blender.git] / intern / iksolver / intern / MT_ExpMap.h
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Original author: Laurence
24  * Contributor(s): Brecht
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 /** \file iksolver/intern/MT_ExpMap.h
30  *  \ingroup iksolver
31  */
32
33
34 #ifndef MT_ExpMap_H
35 #define MT_ExpMap_H
36
37 #include <MT_assert.h>
38
39 #include "MT_Vector3.h"
40 #include "MT_Quaternion.h"
41 #include "MT_Matrix4x4.h"
42
43 const MT_Scalar MT_EXPMAP_MINANGLE (1e-7);
44
45 /**
46  * MT_ExpMap an exponential map parameterization of rotations
47  * in 3D. This implementation is derived from the paper 
48  * "F. Sebastian Grassia. Practical parameterization of 
49  * rotations using the exponential map. Journal of Graphics Tools,
50  *  3(3):29-48, 1998" Please go to http://www.acm.org/jgt/papers/Grassia98/
51  * for a thorough description of the theory and sample code used 
52  * to derive this class. 
53  *
54  * Basic overview of why this class is used.
55  * In an IK system we need to paramterize the joint angles in some
56  * way. Typically 2 parameterizations are used.
57  * - Euler Angles
58  * These suffer from singularities in the parameterization known
59  * as gimbal lock. They also do not interpolate well. For every
60  * set of euler angles there is exactly 1 corresponding 3d rotation.
61  * - Quaternions.
62  * Great for interpolating. Only unit quaternions are valid rotations
63  * means that in a differential ik solver we often stray outside of
64  * this manifold into invalid rotations. Means we have to do a lot
65  * of nasty normalizations all the time. Does not suffer from 
66  * gimbal lock problems. More expensive to compute partial derivatives
67  * as there are 4 of them.
68  * 
69  * So exponential map is similar to a quaternion axis/angle 
70  * representation but we store the angle as the length of the
71  * axis. So require only 3 parameters. Means that all exponential
72  * maps are valid rotations. Suffers from gimbal lock. But it's
73  * possible to detect when gimbal lock is near and reparameterize
74  * away from it. Also nice for interpolating.
75  * Exponential maps are share some of the useful properties of
76  * euler and quaternion parameterizations. And are very useful
77  * for differential IK solvers.
78  */
79
80 class MT_ExpMap {
81 public:
82
83         /**
84          * Default constructor
85          * @warning there is no initialization in the
86          * default constructor
87          */ 
88
89     MT_ExpMap() {}
90     MT_ExpMap(const MT_Vector3& v) : m_v(v) { angleUpdated(); }
91
92     MT_ExpMap(const float v[3]) : m_v(v) { angleUpdated(); }
93     MT_ExpMap(const double v[3]) : m_v(v) { angleUpdated(); }
94
95     MT_ExpMap(MT_Scalar x, MT_Scalar y, MT_Scalar z) :
96         m_v(x, y, z) { angleUpdated(); }
97
98         /** 
99          * Construct an exponential map from a quaternion
100          */
101
102         MT_ExpMap(
103                 const MT_Quaternion &q
104         ) {
105                 setRotation(q);
106         }
107
108         /** 
109          * Accessors
110          * Decided not to inherit from MT_Vector3 but rather
111          * this class contains an MT_Vector3. This is because
112          * it is very dangerous to use MT_Vector3 functions
113          * on this class and some of them have no direct meaning.
114          */
115
116         const 
117                 MT_Vector3 &
118         vector(
119         ) const {
120                 return m_v;
121         }
122
123         /** 
124          * Set the exponential map from a quaternion
125          */
126
127                 void
128         setRotation(
129                 const MT_Quaternion &q
130         );
131
132         /** 
133          * Convert from an exponential map to a quaternion
134          * representation
135          */     
136         
137                 const MT_Quaternion&
138         getRotation(
139         ) const;
140
141         /** 
142          * Convert the exponential map to a 3x3 matrix
143          */
144
145                 MT_Matrix3x3
146         getMatrix(
147         ) const; 
148         
149         /** 
150          * Update (and reparameterize) the expontial map
151          * @param dv delta update values.
152          */
153
154                 void
155         update(
156                 const MT_Vector3& dv
157         );
158
159         /**
160          * Compute the partial derivatives of the exponential
161          * map  (dR/de - where R is a 4x4 matrix formed 
162          * from the map) and return them as a 4x4 matrix
163          */
164
165                 void
166         partialDerivatives(
167                 MT_Matrix3x3& dRdx,
168                 MT_Matrix3x3& dRdy,
169                 MT_Matrix3x3& dRdz
170         ) const ;
171         
172 private :
173
174         // m_v contains the exponential map, the other variables are
175         // cached for efficiency
176         
177         MT_Vector3 m_v;
178         MT_Scalar m_theta, m_sinp;
179         MT_Quaternion m_q;
180
181         // private methods
182
183         // Compute partial derivatives dR (3x3 rotation matrix) / dVi (EM vector)
184         // given the partial derivative dQ (Quaternion) / dVi (ith element of EM vector)
185
186                 void
187         compute_dRdVi(
188                 const MT_Quaternion &dQdV,
189                 MT_Matrix3x3 & dRdVi
190         ) const;
191
192         // compute partial derivatives dQ/dVi
193
194                 void
195         compute_dQdVi(
196                 MT_Quaternion *dQdX
197         ) const ; 
198
199         // reparametrize away from singularity
200
201                 void
202         reParametrize(
203         );
204
205         // (re-)compute cached variables
206
207                 void
208         angleUpdated(
209         );
210 };
211
212 #endif
213