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