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