NumCpp  2.10.1
A Templatized Header Only C++ Implementation of the Python NumPy Library
rodriguesRotation.hpp
Go to the documentation of this file.
1 #pragma once
30 
31 #include <cmath>
32 
33 #include "NumCpp/NdArray.hpp"
34 #include "NumCpp/Vector/Vec3.hpp"
35 
36 namespace nc::rotations
37 {
38  //============================================================================
39  // Method Description:
49  inline Vec3 rodriguesRotation(const Vec3& k, double theta, const Vec3& v) noexcept
50  {
51  const auto kUnit = k.normalize();
52 
53  const auto vCosTheta = v * std::cos(theta);
54 
55  auto kCrossV = kUnit.cross(v);
56  kCrossV *= std::sin(theta);
57 
58  const auto kDotV = kUnit.dot(v);
59  auto kkDotV = kUnit * kDotV;
60  kkDotV *= 1 - std::cos(theta);
61 
62  auto vec = vCosTheta + kCrossV;
63  vec += kkDotV;
64 
65  return vec;
66  }
67 
68  //============================================================================
69  // Method Description:
79  template<typename dtype>
81  {
82  return rodriguesRotation(Vec3(k), theta, Vec3(v)).toNdArray();
83  }
84 } // namespace nc::rotations
Holds a 3D vector.
Definition: Vec3.hpp:50
NdArray< double > toNdArray() const
Definition: Vec3.hpp:324
Vec3 normalize() const noexcept
Definition: Vec3.hpp:276
Definition: DCM.hpp:39
Vec3 rodriguesRotation(const Vec3 &k, double theta, const Vec3 &v) noexcept
Definition: rodriguesRotation.hpp:49
auto sin(dtype inValue) noexcept
Definition: sin.hpp:49
auto cos(dtype inValue) noexcept
Definition: cos.hpp:49