114 lines
3.6 KiB
C++
114 lines
3.6 KiB
C++
//==============================================================================
|
|
/*
|
|
ROTGEN - Runtime Overlay for Eigen
|
|
Copyright : CODE RECKONS
|
|
SPDX-License-Identifier: BSL-1.0
|
|
*/
|
|
//==============================================================================
|
|
|
|
#include <rotgen/rotgen.hpp>
|
|
|
|
#include "unit/common/references.hpp"
|
|
#include "unit/tests.hpp"
|
|
#include <Eigen/Eigen>
|
|
|
|
TTS_CASE_TPL("Test quaternion::angleAxis",
|
|
float,
|
|
double)<typename Scalar>(tts::type<Scalar>)
|
|
{
|
|
// Eigen
|
|
Eigen::Quaternion<Scalar> eigen_result;
|
|
eigen_result =
|
|
Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
// Rotgen
|
|
rotgen::quaternion<Scalar> rotgen_result;
|
|
rotgen_result =
|
|
rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
TTS_EQUAL(rotgen_result.w(), eigen_result.w());
|
|
TTS_EQUAL(rotgen_result.x(), eigen_result.x());
|
|
TTS_EQUAL(rotgen_result.y(), eigen_result.y());
|
|
TTS_EQUAL(rotgen_result.z(), eigen_result.z());
|
|
};
|
|
|
|
TTS_CASE_TPL("Test quaternion::operator* with rotgen::matrix",
|
|
float,
|
|
double)<typename Scalar>(tts::type<Scalar>)
|
|
{
|
|
// Eigen
|
|
Eigen::Quaternion<Scalar> eigen_quat;
|
|
eigen_quat =
|
|
Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
Eigen::Matrix<Scalar, 3, 1> eigen_other{3, 2, 1};
|
|
auto eigen_result = eigen_quat * eigen_other;
|
|
|
|
// Rotgen
|
|
rotgen::quaternion<Scalar> rotgen_quat;
|
|
rotgen_quat =
|
|
rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
rotgen::matrix<Scalar, 3, 1> rotgen_other{3, 2, 1};
|
|
auto rotgen_result = rotgen_quat * rotgen_other;
|
|
|
|
for (auto i = 0; i < eigen_result.size(); i++)
|
|
TTS_ULP_EQUAL(rotgen_result(i), eigen_result(i), 2);
|
|
};
|
|
|
|
TTS_CASE_TPL("Test quaternion::operator* with rotgen::map",
|
|
float,
|
|
double)<typename Scalar>(tts::type<Scalar>)
|
|
{
|
|
Scalar data[] = {3, 2, 1};
|
|
|
|
// Eigen
|
|
Eigen::Quaternion<Scalar> eigen_quat;
|
|
eigen_quat =
|
|
Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
Eigen::Map<Eigen::Matrix<Scalar, 3, 1>> eigen_other{data};
|
|
auto eigen_result = eigen_quat * eigen_other;
|
|
|
|
// Rotgen
|
|
rotgen::quaternion<Scalar> rotgen_quat;
|
|
rotgen_quat =
|
|
rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
rotgen::map<rotgen::matrix<Scalar, 3, 1>> rotgen_other{data};
|
|
auto rotgen_result = rotgen_quat * rotgen_other;
|
|
|
|
for (auto i = 0; i < eigen_result.size(); i++)
|
|
TTS_ULP_EQUAL(rotgen_result(i), eigen_result(i), 2);
|
|
};
|
|
|
|
TTS_CASE_TPL("Test quaternion::operator* with rotgen::block",
|
|
float,
|
|
double)<typename Scalar>(tts::type<Scalar>)
|
|
{
|
|
|
|
// Eigen
|
|
Eigen::Quaternion<Scalar> eigen_quat;
|
|
eigen_quat =
|
|
Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
Eigen::Matrix<Scalar, 16, 16> eigen_original_mat;
|
|
rotgen::tests::prepare(16, 16, rotgen::tests::default_init_function,
|
|
eigen_original_mat);
|
|
auto eigen_other = eigen_original_mat.template block<3, 1>(0, 0);
|
|
auto eigen_result = eigen_quat * eigen_other;
|
|
|
|
// Rotgen
|
|
rotgen::quaternion<Scalar> rotgen_quat;
|
|
rotgen_quat =
|
|
rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});
|
|
|
|
rotgen::matrix<Scalar, 16, 16> rotgen_original_mat;
|
|
rotgen::tests::prepare(16, 16, rotgen::tests::default_init_function,
|
|
rotgen_original_mat);
|
|
auto rotgen_other = rotgen::extract<3, 1>(rotgen_original_mat, 0, 0);
|
|
auto rotgen_result = rotgen_quat * rotgen_other;
|
|
|
|
for (auto i = 0; i < eigen_result.size(); i++)
|
|
TTS_ULP_EQUAL(rotgen_result(i), eigen_result(i), 2);
|
|
};
|