Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Quaternion.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of the Technische Universitaet Muenchen nor the names of
14 // its contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef _RL_MATH_QUATERNION_H_
31 #define _RL_MATH_QUATERNION_H_
32 
33 #define EIGEN_MATRIXBASE_PLUGIN <rl/math/MatrixBaseAddons.h>
34 
35 #include <cassert>
36 #include <Eigen/Geometry>
37 
38 #include "Real.h"
39 
40 namespace rl
41 {
42  namespace math
43  {
44  typedef ::Eigen::Quaternion< Real > Quaternion;
45 
46  namespace quaternion
47  {
48  inline
49  void
50  fromUniform(const Real& rand0, const Real& rand1, const Real& rand2, Quaternion& q)
51  {
52  assert(rand0 >= 0.0f);
53  assert(rand0 <= 1.0f);
54  assert(rand1 >= 0.0f);
55  assert(rand1 <= 1.0f);
56  assert(rand2 >= 0.0f);
57  assert(rand2 <= 1.0f);
58 
59  Real sigma1 = ::std::sqrt(1.0f - rand0);
60  Real sigma2 = ::std::sqrt(rand0);
61  Real theta1 = 2.0f * static_cast< Real >(M_PI) * rand1;
62  Real theta2 = 2.0f * static_cast< Real >(M_PI) * rand2;
63 
64  q = Quaternion(
65  ::std::sin(theta1) * sigma1,
66  ::std::cos(theta1) * sigma1,
67  ::std::sin(theta2) * sigma2,
68  ::std::cos(theta2) * sigma2
69  );
70 
71  q.normalize();
72  }
73  }
74  }
75 }
76 
77 #endif // _RL_MATH_QUATERNION_H_