RotationVectorToMatrix.hpp

Go to the documentation of this file.

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
#pragma once
 
#include <Eigen/Core>
 
#include "wpi/math/linalg/ct_matrix.hpp"
 
namespace wpi::math::detail {
 
constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d& rotation) {
  return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
                     {rotation(2), 0.0, -rotation(0)},
                     {-rotation(1), rotation(0), 0.0}};
}
 
constexpr Eigen::Matrix3d RotationVectorToMatrix(
    const Eigen::Vector3d& rotation) {
  return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
                         {rotation(2), 0.0, -rotation(0)},
                         {-rotation(1), rotation(0), 0.0}};
}
 
}  // namespace wpi::math::detail