Kindr
Kinematics and Dynamics for Robotics
RotationDiffBase.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 
29 #pragma once
30 
31 
32 #include "kindr/common/common.hpp"
34 
35 namespace kindr {
36 
38 namespace internal {
39 
44 template<typename Left_, typename Right_>
45 class RotationDiffAdditionTraits {
46  public:
47 // inline static LeftAndRight_ add(const LeftAndRight_& lhs, const LeftAndRight_& rhs);
48 // inline static LeftAndRight_ subtract(const LeftAndRight_& lhs, const LeftAndRight_& rhs);
49 };
50 
55 template<typename Dest_, typename Source_, typename Rotation_>
56 class RotationDiffConversionTraits {
57  public:
58  // inline static Dest_ convert(const Rotation_&, const Source_& );
59 };
60 
61 
62 } // namespace internal
63 
73 template<typename Derived_>
75  public:
76 
81  RotationDiffBase() = default;
82 
87  RotationDiffBase(const Derived_&) = delete; // on purpose!!
88 
93  operator Derived_& () {
94  return static_cast<Derived_&>(*this);
95  }
96 
101  operator const Derived_& () const {
102  return static_cast<const Derived_&>(*this);
103  }
104 
109  const Derived_& derived() const {
110  return static_cast<const Derived_&>(*this);
111  }
112 
113 
117  Derived_& setZero();
118 
119 
120 
124  template<typename OtherDerived_>
125  Derived_ operator +(const RotationDiffBase<OtherDerived_>& other) const {
126  return internal::RotationDiffAdditionTraits<RotationDiffBase<Derived_>, RotationDiffBase<OtherDerived_>>::add(this->derived(), other.derived()); // todo: 1. ok? 2. may be optimized
127  }
128 
132  template<typename OtherDerived_>
133  Derived_ operator -(const RotationDiffBase<OtherDerived_>& other) const {
134  return internal::RotationDiffAdditionTraits<RotationDiffBase<Derived_>, RotationDiffBase<OtherDerived_>>::subtract(this->derived(), other.derived()); // todo: 1. ok? 2. may be optimized
135  }
136 
140  template<typename OtherDerived_>
141  Derived_& operator +=(const RotationDiffBase<OtherDerived_>& other);
142 
146  template<typename OtherDerived_>
147  Derived_& operator -=(const RotationDiffBase<OtherDerived_>& other);
148 
149 };
150 
151 
152 
153 namespace internal {
154 
155 template<typename Left_, typename Right_>
156 class RotationDiffAdditionTraits<RotationDiffBase<Left_>, RotationDiffBase<Right_>> {
157  public:
158  inline static Left_ add(const RotationDiffBase<Left_>& lhs, const RotationDiffBase<Right_>& rhs) {
160  (LocalAngularVelocity<typename Left_::Scalar>(lhs.derived())).toImplementation() +
161  (LocalAngularVelocity<typename Right_::Scalar>(rhs.derived())).toImplementation()
162  ));
163  }
164  inline static Left_ subtract(const RotationDiffBase<Left_>& lhs, const RotationDiffBase<Right_>& rhs) {
166  ( LocalAngularVelocity<typename Left_::Scalar>(lhs.derived())).toImplementation() -
167  ( LocalAngularVelocity<typename Right_::Scalar>(rhs.derived())).toImplementation()
168  ));
169  }
170 };
171 
172 
173 template<typename LeftAndRight_>
174 class RotationDiffAdditionTraits<RotationDiffBase<LeftAndRight_>, RotationDiffBase<LeftAndRight_>> {
175  public:
180  inline static LeftAndRight_ add(const RotationDiffBase<LeftAndRight_>& lhs, const RotationDiffBase<LeftAndRight_>& rhs) {
181  return LeftAndRight_(typename LeftAndRight_::Implementation(lhs.derived().toImplementation() + rhs.derived().toImplementation()));
182  }
187  inline static LeftAndRight_ subtract(const RotationDiffBase<LeftAndRight_>& lhs, const RotationDiffBase<LeftAndRight_>& rhs) {
188  return LeftAndRight_(typename LeftAndRight_::Implementation(lhs.derived().toImplementation() - rhs.derived().toImplementation()));
189  }
190 };
191 
192 
193 
194 
195 } // namespace internal
196 } // namespace kindr
197 
198 
const Derived_ & derived() const
Gets the derived time derivative of a rotation. (only for advanced users)
Interface for an angular velocity.