Kindr
Kinematics and Dynamics for Robotics
QuaternionBase.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
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  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 #pragma once
29 
30 #include "kindr/common/common.hpp"
31 
32 namespace kindr {
33 
35 namespace quat_internal {
36 
38 template<typename Dest_, typename Source_>
40  // Dest_ convert(const Source_& );
41 };
42 
46 template<typename Left_, typename Right_>
48  public:
49  inline static typename Left_::Implementation mult(const Left_& lhs, const Right_& rhs){
50  return typename Left_::Implementation(lhs.toImplementation() * rhs.toImplementation());
51  }
52 };
53 
55 template<typename Quaternion_>
57  public:
58  inline static bool isEqual(const Quaternion_& a, const Quaternion_& b){
59  return (a.w() == b.w() &&
60  a.x() == b.x() &&
61  a.y() == b.y() &&
62  a.z() == b.z());
63  }
64 };
65 
66 } // namespace internal
67 
75 template<typename Derived_>
77  public:
81  Derived_& invert();
82 
86  Derived_ inverted() const;
87 
91  Derived_& conjugate();
92 
96  Derived_ conjugated() const;
97 
102  operator Derived_& () {
103  return static_cast<Derived_&>(*this);
104  }
105 
110  operator const Derived_& () const {
111  return static_cast<const Derived_&>(*this);
112  }
117  Derived_& derived() {
118  return static_cast<Derived_&>(*this);
119  }
124  const Derived_& derived() const {
125  return static_cast<const Derived_&>(*this);
126  }
127 
132  template<typename OtherDerived_>
133  Derived_ operator *(const QuaternionBase<OtherDerived_>& other) const {
134  return Derived_(quat_internal::MultiplicationTraits<Derived_, OtherDerived_>::mult(this->derived(), other.derived()));
135  }
136 
137 // template<typename OtherDerived_>
138 // Derived_ operator *(const QuaternionBase<OtherDerived_>& other) const {
139 // return quat_internal::MultiplicationTraits<Derived_, OtherDerived_>::mult(this->derived(), static_cast<Derived_>(other));
140 // }
141 
146  template<typename OtherDerived_>
147  bool operator ==(const QuaternionBase<OtherDerived_>& other) const {
148  return quat_internal::ComparisonTraits<Derived_>::isEqual(this->derived(), static_cast<Derived_>(other)); // cast to Quaternion
149  }
150 
154  friend std::ostream& operator << (std::ostream& out, const QuaternionBase<Derived_>& quat) {
155  out << quat.derived().w() << " " << quat.derived().x() << " " << quat.derived().y() << " " << quat.derived().z();
156  return out;
157  }
158 };
159 
160 
161 
168 template<typename Derived_>
169 class UnitQuaternionBase : public QuaternionBase<Derived_> {
170  public:
172 
176  Derived_ inverted() const {
177  return Base::derived().conjugated();
178  }
179 
183  Derived_& invert() {
184  return Base::derived().conjugate();
185  }
189  Derived_ conjugated() const;
190 
194  Derived_& conjugate();
195 
200  template<typename OtherDerived_>
201  Derived_ operator *(const UnitQuaternionBase<OtherDerived_>& other) const {
202  return Derived_(quat_internal::MultiplicationTraits<Derived_, OtherDerived_>::mult(this->derived(), other.derived()));
203  }
204 
209  template<typename OtherDerived_>
210  OtherDerived_ operator *(const QuaternionBase<OtherDerived_>& other) const {
211  return OtherDerived_(quat_internal::MultiplicationTraits<Derived_, OtherDerived_>::mult(this->derived(), other.derived()));
212  }
213 };
214 
215 
216 } // namespace rm
const Derived_ & derived() const
gets the derived quaternion (only for advanced users)
Derived_ & invert()
inverts the unit quaternion using the conjugate
Derived_ inverted() const
gets the inverse of the unit quaternion using the conjugate
Vector< PhysicalType_, PrimType_, Dimension_ > operator*(PrimTypeFactor_ factor, const Vector< PhysicalType_, PrimType_, Dimension_ > &vector)
Multiplies a vector with a scalar.
Definition: Vector.hpp:578
Conversion trait to implement conversions between different types.
Base class that defines the interface of a unit quaternion This class defines a generic interface for...
Base class that defines the interface of a quaternion This class defines a generic interface for a qu...
QuaternionBase< Derived_ > Base
static bool isEqual(const Quaternion_ &a, const Quaternion_ &b)
Derived_ & derived()
gets the derived quaternion (only for advanced users)
static Left_::Implementation mult(const Left_ &lhs, const Right_ &rhs)
Comparison trait to implement to compare two quaternions.
Multiplication trait to implement quaternion multiplication.