mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Add 3D geometry classes (#4175)
Also clean up 2D geometry documentation.
This commit is contained in:
Tyler Veness
@@ -9,51 +9,47 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(Pose2dTest, TransformBy) {
|
||||
const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
|
||||
const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
|
||||
const Pose2d initial{1_m, 2_m, Rotation2d{45_deg}};
|
||||
const Transform2d transform{Translation2d{5_m, 0_m}, Rotation2d{5_deg}};
|
||||
|
||||
const auto transformed = initial + transform;
|
||||
|
||||
EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
|
||||
EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
|
||||
EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
|
||||
EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
|
||||
EXPECT_DOUBLE_EQ(50.0, transformed.Rotation().Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Pose2dTest, RelativeTo) {
|
||||
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
|
||||
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
|
||||
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
|
||||
const Pose2d final{5_m, 5_m, Rotation2d{45.0_deg}};
|
||||
|
||||
const auto finalRelativeToInitial = final.RelativeTo(initial);
|
||||
|
||||
EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
|
||||
kEpsilon);
|
||||
EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
|
||||
kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
|
||||
EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
|
||||
EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Pose2dTest, Equality) {
|
||||
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
|
||||
const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
|
||||
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
|
||||
const Pose2d b{0_m, 5_m, Rotation2d{43_deg}};
|
||||
EXPECT_TRUE(a == b);
|
||||
}
|
||||
|
||||
TEST(Pose2dTest, Inequality) {
|
||||
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
|
||||
const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
|
||||
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
|
||||
const Pose2d b{0_m, 5_ft, Rotation2d{43_deg}};
|
||||
EXPECT_TRUE(a != b);
|
||||
}
|
||||
|
||||
TEST(Pose2dTest, Minus) {
|
||||
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
|
||||
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
|
||||
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
|
||||
const Pose2d final{5_m, 5_m, Rotation2d{45_deg}};
|
||||
|
||||
const auto transform = final - initial;
|
||||
|
||||
EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
|
||||
EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
|
||||
EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
|
||||
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
|
||||
}
|
||||
|
||||
74
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
Normal file
74
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
Normal file
@@ -0,0 +1,74 @@
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "frc/geometry/Pose3d.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TEST(Pose3dTest, TransformBy) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45.0_deg}};
|
||||
const Transform3d transform{Translation3d{5_m, 0_m, 0_m},
|
||||
Rotation3d{zAxis, 5_deg}};
|
||||
|
||||
const auto transformed = initial + transform;
|
||||
|
||||
EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
|
||||
EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
|
||||
EXPECT_DOUBLE_EQ(transformed.Rotation().Z().value(),
|
||||
units::radian_t{50_deg}.value());
|
||||
}
|
||||
|
||||
TEST(Pose3dTest, RelativeTo) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
|
||||
const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
|
||||
|
||||
const auto finalRelativeToInitial = final.RelativeTo(initial);
|
||||
|
||||
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Y().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Z().value());
|
||||
}
|
||||
|
||||
TEST(Pose3dTest, Equality) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
|
||||
const Pose3d b{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
|
||||
EXPECT_TRUE(a == b);
|
||||
}
|
||||
|
||||
TEST(Pose3dTest, Inequality) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
|
||||
const Pose3d b{0_m, 5_ft, 0_m, Rotation3d{zAxis, 43_deg}};
|
||||
EXPECT_TRUE(a != b);
|
||||
}
|
||||
|
||||
TEST(Pose3dTest, Minus) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
|
||||
const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
|
||||
|
||||
const auto transform = final - initial;
|
||||
|
||||
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, transform.Y().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Z().value());
|
||||
}
|
||||
|
||||
TEST(Pose3dTest, ToPose2d) {
|
||||
Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}};
|
||||
Pose2d expected{1_m, 2_m, 40_deg};
|
||||
|
||||
EXPECT_EQ(expected, pose.ToPose2d());
|
||||
}
|
||||
82
wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
Normal file
82
wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
Normal file
@@ -0,0 +1,82 @@
|
||||
// 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.
|
||||
|
||||
#include <wpi/numbers>
|
||||
|
||||
#include "frc/geometry/Quaternion.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "units/angle.h"
|
||||
#include "units/math.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TEST(QuaternionTest, Init) {
|
||||
// Identity
|
||||
Quaternion q1;
|
||||
EXPECT_DOUBLE_EQ(1.0, q1.W());
|
||||
EXPECT_DOUBLE_EQ(0.0, q1.X());
|
||||
EXPECT_DOUBLE_EQ(0.0, q1.Y());
|
||||
EXPECT_DOUBLE_EQ(0.0, q1.Z());
|
||||
|
||||
// Normalized
|
||||
Quaternion q2{0.5, 0.5, 0.5, 0.5};
|
||||
EXPECT_DOUBLE_EQ(0.5, q2.W());
|
||||
EXPECT_DOUBLE_EQ(0.5, q2.X());
|
||||
EXPECT_DOUBLE_EQ(0.5, q2.Y());
|
||||
EXPECT_DOUBLE_EQ(0.5, q2.Z());
|
||||
|
||||
// Unnormalized
|
||||
Quaternion q3{0.75, 0.3, 0.4, 0.5};
|
||||
EXPECT_DOUBLE_EQ(0.75, q3.W());
|
||||
EXPECT_DOUBLE_EQ(0.3, q3.X());
|
||||
EXPECT_DOUBLE_EQ(0.4, q3.Y());
|
||||
EXPECT_DOUBLE_EQ(0.5, q3.Z());
|
||||
|
||||
q3 = q3.Normalize();
|
||||
double norm = std::sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
|
||||
EXPECT_DOUBLE_EQ(0.75 / norm, q3.W());
|
||||
EXPECT_DOUBLE_EQ(0.3 / norm, q3.X());
|
||||
EXPECT_DOUBLE_EQ(0.4 / norm, q3.Y());
|
||||
EXPECT_DOUBLE_EQ(0.5 / norm, q3.Z());
|
||||
EXPECT_DOUBLE_EQ(1.0, q3.W() * q3.W() + q3.X() * q3.X() + q3.Y() * q3.Y() +
|
||||
q3.Z() * q3.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Multiply) {
|
||||
// 90° CCW rotations around each axis
|
||||
double c = units::math::cos(90_deg / 2.0);
|
||||
double s = units::math::sin(90_deg / 2.0);
|
||||
Quaternion xRot{c, s, 0.0, 0.0};
|
||||
Quaternion yRot{c, 0.0, s, 0.0};
|
||||
Quaternion zRot{c, 0.0, 0.0, s};
|
||||
|
||||
// 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
|
||||
// produce a 90° CCW Y rotation
|
||||
auto expected = yRot;
|
||||
auto actual = zRot * yRot * xRot;
|
||||
EXPECT_NEAR(expected.W(), actual.W(), 1e-9);
|
||||
EXPECT_NEAR(expected.X(), actual.X(), 1e-9);
|
||||
EXPECT_NEAR(expected.Y(), actual.Y(), 1e-9);
|
||||
EXPECT_NEAR(expected.Z(), actual.Z(), 1e-9);
|
||||
|
||||
// Identity
|
||||
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
|
||||
0.48507125007266594};
|
||||
actual = q * q.Inverse();
|
||||
EXPECT_DOUBLE_EQ(1.0, actual.W());
|
||||
EXPECT_DOUBLE_EQ(0.0, actual.X());
|
||||
EXPECT_DOUBLE_EQ(0.0, actual.Y());
|
||||
EXPECT_DOUBLE_EQ(0.0, actual.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Inverse) {
|
||||
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
|
||||
0.48507125007266594};
|
||||
auto inv = q.Inverse();
|
||||
|
||||
EXPECT_DOUBLE_EQ(q.W(), inv.W());
|
||||
EXPECT_DOUBLE_EQ(-q.X(), inv.X());
|
||||
EXPECT_DOUBLE_EQ(-q.Y(), inv.Y());
|
||||
EXPECT_DOUBLE_EQ(-q.Z(), inv.Z());
|
||||
}
|
||||
@@ -11,58 +11,56 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(Rotation2dTest, RadiansToDegrees) {
|
||||
const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)};
|
||||
const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)};
|
||||
const Rotation2d rot1{units::radian_t{wpi::numbers::pi / 3.0}};
|
||||
const Rotation2d rot2{units::radian_t{wpi::numbers::pi / 4.0}};
|
||||
|
||||
EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon);
|
||||
EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(60.0, rot1.Degrees().value());
|
||||
EXPECT_DOUBLE_EQ(45.0, rot2.Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Rotation2dTest, DegreesToRadians) {
|
||||
const auto rot1 = Rotation2d(45.0_deg);
|
||||
const auto rot2 = Rotation2d(30.0_deg);
|
||||
const auto rot1 = Rotation2d{45_deg};
|
||||
const auto rot2 = Rotation2d{30_deg};
|
||||
|
||||
EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon);
|
||||
EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot1.Radians().value());
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 6.0, rot2.Radians().value());
|
||||
}
|
||||
|
||||
TEST(Rotation2dTest, RotateByFromZero) {
|
||||
const Rotation2d zero;
|
||||
auto sum = zero + Rotation2d(90.0_deg);
|
||||
auto rotated = zero + Rotation2d(90_deg);
|
||||
|
||||
EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon);
|
||||
EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 2.0, rotated.Radians().value());
|
||||
EXPECT_DOUBLE_EQ(90.0, rotated.Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Rotation2dTest, RotateByNonZero) {
|
||||
auto rot = Rotation2d(90.0_deg);
|
||||
rot = rot + Rotation2d(30.0_deg);
|
||||
auto rot = Rotation2d{90_deg};
|
||||
rot = rot + Rotation2d{30_deg};
|
||||
|
||||
EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Rotation2dTest, Minus) {
|
||||
const auto rot1 = Rotation2d(70.0_deg);
|
||||
const auto rot2 = Rotation2d(30.0_deg);
|
||||
const auto rot1 = Rotation2d{70_deg};
|
||||
const auto rot2 = Rotation2d{30_deg};
|
||||
|
||||
EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(40.0, (rot1 - rot2).Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Rotation2dTest, Equality) {
|
||||
const auto rot1 = Rotation2d(43_deg);
|
||||
const auto rot2 = Rotation2d(43_deg);
|
||||
auto rot1 = Rotation2d{43_deg};
|
||||
auto rot2 = Rotation2d{43_deg};
|
||||
EXPECT_EQ(rot1, rot2);
|
||||
|
||||
const auto rot3 = Rotation2d(-180_deg);
|
||||
const auto rot4 = Rotation2d(180_deg);
|
||||
EXPECT_EQ(rot3, rot4);
|
||||
rot1 = Rotation2d{-180_deg};
|
||||
rot2 = Rotation2d{180_deg};
|
||||
EXPECT_EQ(rot1, rot2);
|
||||
}
|
||||
|
||||
TEST(Rotation2dTest, Inequality) {
|
||||
const auto rot1 = Rotation2d(43_deg);
|
||||
const auto rot2 = Rotation2d(43.5_deg);
|
||||
const auto rot1 = Rotation2d{43_deg};
|
||||
const auto rot2 = Rotation2d{43.5_deg};
|
||||
EXPECT_NE(rot1, rot2);
|
||||
}
|
||||
|
||||
246
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
Normal file
246
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
Normal file
@@ -0,0 +1,246 @@
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <wpi/MathExtras.h>
|
||||
#include <wpi/numbers>
|
||||
|
||||
#include "frc/geometry/Rotation3d.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TEST(Rotation3dTest, Init) {
|
||||
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
|
||||
const Rotation3d rot1{xAxis, units::radian_t{wpi::numbers::pi / 3}};
|
||||
const Rotation3d rot2{units::radian_t{wpi::numbers::pi / 3}, 0_rad, 0_rad};
|
||||
EXPECT_EQ(rot1, rot2);
|
||||
|
||||
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
|
||||
const Rotation3d rot3{yAxis, units::radian_t{wpi::numbers::pi / 3}};
|
||||
const Rotation3d rot4{0_rad, units::radian_t{wpi::numbers::pi / 3}, 0_rad};
|
||||
EXPECT_EQ(rot3, rot4);
|
||||
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
const Rotation3d rot5{zAxis, units::radian_t{wpi::numbers::pi / 3}};
|
||||
const Rotation3d rot6{0_rad, 0_rad, units::radian_t{wpi::numbers::pi / 3}};
|
||||
EXPECT_EQ(rot5, rot6);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RadiansToDegrees) {
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Rotation3d rot1{zAxis, units::radian_t{wpi::numbers::pi / 3}};
|
||||
EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
|
||||
EXPECT_DOUBLE_EQ(units::radian_t{60_deg}.value(), rot1.Z().value());
|
||||
|
||||
const Rotation3d rot2{zAxis, units::radian_t{wpi::numbers::pi / 4}};
|
||||
EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
|
||||
EXPECT_DOUBLE_EQ(units::radian_t{45_deg}.value(), rot2.Z().value());
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, DegreesToRadians) {
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const auto rot1 = Rotation3d{zAxis, 45_deg};
|
||||
EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot1.Z().value());
|
||||
|
||||
const auto rot2 = Rotation3d{zAxis, 30_deg};
|
||||
EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 6.0, rot2.Z().value());
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RotationLoop) {
|
||||
Rotation3d rot;
|
||||
|
||||
rot = rot + Rotation3d{90_deg, 0_deg, 0_deg};
|
||||
Rotation3d expected{90_deg, 0_deg, 0_deg};
|
||||
EXPECT_EQ(expected, rot);
|
||||
|
||||
rot = rot + Rotation3d{0_deg, 90_deg, 0_deg};
|
||||
expected = Rotation3d{
|
||||
{1.0 / std::sqrt(3), 1.0 / std::sqrt(3), -1.0 / std::sqrt(3)}, 120_deg};
|
||||
EXPECT_EQ(expected, rot);
|
||||
|
||||
rot = rot + Rotation3d{0_deg, 0_deg, 90_deg};
|
||||
expected = Rotation3d{0_deg, 90_deg, 0_deg};
|
||||
EXPECT_EQ(expected, rot);
|
||||
|
||||
rot = rot + Rotation3d{0_deg, -90_deg, 0_deg};
|
||||
EXPECT_EQ(Rotation3d{}, rot);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RotateByFromZeroX) {
|
||||
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
|
||||
|
||||
const Rotation3d zero;
|
||||
auto rotated = zero + Rotation3d{xAxis, 90_deg};
|
||||
|
||||
Rotation3d expected{xAxis, 90_deg};
|
||||
EXPECT_EQ(expected, rotated);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RotateByFromZeroY) {
|
||||
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
|
||||
|
||||
const Rotation3d zero;
|
||||
auto rotated = zero + Rotation3d{yAxis, 90_deg};
|
||||
|
||||
Rotation3d expected{yAxis, 90_deg};
|
||||
EXPECT_EQ(expected, rotated);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RotateByFromZeroZ) {
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Rotation3d zero;
|
||||
auto rotated = zero + Rotation3d{zAxis, 90_deg};
|
||||
|
||||
Rotation3d expected{zAxis, 90_deg};
|
||||
EXPECT_EQ(expected, rotated);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RotateByNonZeroX) {
|
||||
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
|
||||
|
||||
auto rot = Rotation3d{xAxis, 90_deg};
|
||||
rot = rot + Rotation3d{xAxis, 30_deg};
|
||||
|
||||
Rotation3d expected{xAxis, 120_deg};
|
||||
EXPECT_EQ(expected, rot);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RotateByNonZeroY) {
|
||||
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
|
||||
|
||||
auto rot = Rotation3d{yAxis, 90_deg};
|
||||
rot = rot + Rotation3d{yAxis, 30_deg};
|
||||
|
||||
Rotation3d expected{yAxis, 120_deg};
|
||||
EXPECT_EQ(expected, rot);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, RotateByNonZeroZ) {
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
auto rot = Rotation3d{zAxis, 90_deg};
|
||||
rot = rot + Rotation3d{zAxis, 30_deg};
|
||||
|
||||
Rotation3d expected{zAxis, 120_deg};
|
||||
EXPECT_EQ(expected, rot);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, Minus) {
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const auto rot1 = Rotation3d{zAxis, 70_deg};
|
||||
const auto rot2 = Rotation3d{zAxis, 30_deg};
|
||||
|
||||
EXPECT_DOUBLE_EQ(40.0, units::degree_t{(rot1 - rot2).Z()}.value());
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, AxisAngle) {
|
||||
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
|
||||
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
Rotation3d rot1{xAxis, 90_deg};
|
||||
EXPECT_EQ(xAxis, rot1.Axis());
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 2.0, rot1.Angle().value());
|
||||
|
||||
Rotation3d rot2{yAxis, 45_deg};
|
||||
EXPECT_EQ(yAxis, rot2.Axis());
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot2.Angle().value());
|
||||
|
||||
Rotation3d rot3{zAxis, 60_deg};
|
||||
EXPECT_EQ(zAxis, rot3.Axis());
|
||||
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 3.0, rot3.Angle().value());
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, ToRotation2d) {
|
||||
Rotation3d rotation{20_deg, 30_deg, 40_deg};
|
||||
Rotation2d expected{40_deg};
|
||||
|
||||
EXPECT_EQ(expected, rotation.ToRotation2d());
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, Equality) {
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const auto rot1 = Rotation3d{zAxis, 43_deg};
|
||||
const auto rot2 = Rotation3d{zAxis, 43_deg};
|
||||
EXPECT_EQ(rot1, rot2);
|
||||
|
||||
const auto rot3 = Rotation3d{zAxis, -180_deg};
|
||||
const auto rot4 = Rotation3d{zAxis, 180_deg};
|
||||
EXPECT_EQ(rot3, rot4);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, Inequality) {
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const auto rot1 = Rotation3d{zAxis, 43_deg};
|
||||
const auto rot2 = Rotation3d{zAxis, 43.5_deg};
|
||||
EXPECT_NE(rot1, rot2);
|
||||
}
|
||||
|
||||
TEST(Rotation3dTest, Interpolate) {
|
||||
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
|
||||
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
|
||||
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
// 50 + (70 - 50) * 0.5 = 60
|
||||
auto rot1 = Rotation3d{xAxis, 50_deg};
|
||||
auto rot2 = Rotation3d{xAxis, 70_deg};
|
||||
auto interpolated = wpi::Lerp(rot1, rot2, 0.5);
|
||||
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.X()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
|
||||
|
||||
// -160 minus half distance between 170 and -160 (15) = -175
|
||||
rot1 = Rotation3d{xAxis, 170_deg};
|
||||
rot2 = Rotation3d{xAxis, -160_deg};
|
||||
interpolated = wpi::Lerp(rot1, rot2, 0.5);
|
||||
EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.X()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
|
||||
|
||||
// 50 + (70 - 50) * 0.5 = 60
|
||||
rot1 = Rotation3d{yAxis, 50_deg};
|
||||
rot2 = Rotation3d{yAxis, 70_deg};
|
||||
interpolated = wpi::Lerp(rot1, rot2, 0.5);
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
|
||||
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Y()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
|
||||
|
||||
// -160 plus half distance between 170 and -160 (165) = 5
|
||||
rot1 = Rotation3d{yAxis, 170_deg};
|
||||
rot2 = Rotation3d{yAxis, -160_deg};
|
||||
interpolated = wpi::Lerp(rot1, rot2, 0.5);
|
||||
EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.X()}.value());
|
||||
EXPECT_DOUBLE_EQ(-5.0, units::degree_t{interpolated.Y()}.value());
|
||||
EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.Z()}.value());
|
||||
|
||||
// 50 + (70 - 50) * 0.5 = 60
|
||||
rot1 = Rotation3d{zAxis, 50_deg};
|
||||
rot2 = Rotation3d{zAxis, 70_deg};
|
||||
interpolated = wpi::Lerp(rot1, rot2, 0.5);
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
|
||||
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Z()}.value());
|
||||
|
||||
// -160 minus half distance between 170 and -160 (15) = -175
|
||||
rot1 = Rotation3d{zAxis, 170_deg};
|
||||
rot2 = Rotation3d{zAxis, -160_deg};
|
||||
interpolated = wpi::Lerp(rot1, rot2, 0.5);
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
|
||||
EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value());
|
||||
}
|
||||
@@ -12,8 +12,6 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(Transform2dTest, Inverse) {
|
||||
const Pose2d initial{1_m, 2_m, 45_deg};
|
||||
const Transform2d transform{{5_m, 0_m}, 5_deg};
|
||||
@@ -21,10 +19,10 @@ TEST(Transform2dTest, Inverse) {
|
||||
auto transformed = initial + transform;
|
||||
auto untransformed = transformed + transform.Inverse();
|
||||
|
||||
EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon);
|
||||
EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon);
|
||||
EXPECT_NEAR(initial.Rotation().Degrees().value(),
|
||||
untransformed.Rotation().Degrees().value(), kEpsilon);
|
||||
EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9);
|
||||
EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value());
|
||||
EXPECT_DOUBLE_EQ(initial.Rotation().Degrees().value(),
|
||||
untransformed.Rotation().Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Transform2dTest, Composition) {
|
||||
@@ -35,10 +33,10 @@ TEST(Transform2dTest, Composition) {
|
||||
auto transformedSeparate = initial + transform1 + transform2;
|
||||
auto transformedCombined = initial + (transform1 + transform2);
|
||||
|
||||
EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(),
|
||||
kEpsilon);
|
||||
EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(),
|
||||
kEpsilon);
|
||||
EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(),
|
||||
transformedCombined.Rotation().Degrees().value(), kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(transformedSeparate.X().value(),
|
||||
transformedCombined.X().value());
|
||||
EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(),
|
||||
transformedCombined.Y().value());
|
||||
EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Degrees().value(),
|
||||
transformedCombined.Rotation().Degrees().value());
|
||||
}
|
||||
|
||||
49
wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
Normal file
49
wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
Normal file
@@ -0,0 +1,49 @@
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "frc/geometry/Pose3d.h"
|
||||
#include "frc/geometry/Rotation3d.h"
|
||||
#include "frc/geometry/Transform3d.h"
|
||||
#include "frc/geometry/Translation3d.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TEST(Transform3dTest, Inverse) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}};
|
||||
const Transform3d transform{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}};
|
||||
|
||||
auto transformed = initial + transform;
|
||||
auto untransformed = transformed + transform.Inverse();
|
||||
|
||||
EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9);
|
||||
EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value());
|
||||
EXPECT_DOUBLE_EQ(initial.Z().value(), untransformed.Z().value());
|
||||
EXPECT_DOUBLE_EQ(initial.Rotation().Z().value(),
|
||||
untransformed.Rotation().Z().value());
|
||||
}
|
||||
|
||||
TEST(Transform3dTest, Composition) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}};
|
||||
const Transform3d transform1{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}};
|
||||
const Transform3d transform2{{0_m, 2_m, 0_m}, Rotation3d{zAxis, 5_deg}};
|
||||
|
||||
auto transformedSeparate = initial + transform1 + transform2;
|
||||
auto transformedCombined = initial + (transform1 + transform2);
|
||||
|
||||
EXPECT_DOUBLE_EQ(transformedSeparate.X().value(),
|
||||
transformedCombined.X().value());
|
||||
EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(),
|
||||
transformedCombined.Y().value());
|
||||
EXPECT_DOUBLE_EQ(transformedSeparate.Z().value(),
|
||||
transformedCombined.Z().value());
|
||||
EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Z().value(),
|
||||
transformedCombined.Rotation().Z().value());
|
||||
}
|
||||
@@ -9,69 +9,67 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(Translation2dTest, Sum) {
|
||||
const Translation2d one{1.0_m, 3.0_m};
|
||||
const Translation2d two{2.0_m, 5.0_m};
|
||||
const Translation2d one{1_m, 3_m};
|
||||
const Translation2d two{2_m, 5_m};
|
||||
|
||||
const auto sum = one + two;
|
||||
|
||||
EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon);
|
||||
EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(3.0, sum.X().value());
|
||||
EXPECT_DOUBLE_EQ(8.0, sum.Y().value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, Difference) {
|
||||
const Translation2d one{1.0_m, 3.0_m};
|
||||
const Translation2d two{2.0_m, 5.0_m};
|
||||
const Translation2d one{1_m, 3_m};
|
||||
const Translation2d two{2_m, 5_m};
|
||||
|
||||
const auto difference = one - two;
|
||||
|
||||
EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
|
||||
EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(-1.0, difference.X().value());
|
||||
EXPECT_DOUBLE_EQ(-2.0, difference.Y().value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, RotateBy) {
|
||||
const Translation2d another{3.0_m, 0.0_m};
|
||||
const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
|
||||
const Translation2d another{3_m, 0_m};
|
||||
const auto rotated = another.RotateBy(Rotation2d(90_deg));
|
||||
|
||||
EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon);
|
||||
EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
|
||||
EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, Multiplication) {
|
||||
const Translation2d original{3.0_m, 5.0_m};
|
||||
const Translation2d original{3_m, 5_m};
|
||||
const auto mult = original * 3;
|
||||
|
||||
EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
|
||||
EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(9.0, mult.X().value());
|
||||
EXPECT_DOUBLE_EQ(15.0, mult.Y().value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, Division) {
|
||||
const Translation2d original{3.0_m, 5.0_m};
|
||||
const Translation2d original{3_m, 5_m};
|
||||
const auto div = original / 2;
|
||||
|
||||
EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
|
||||
EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(1.5, div.X().value());
|
||||
EXPECT_DOUBLE_EQ(2.5, div.Y().value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, Norm) {
|
||||
const Translation2d one{3.0_m, 5.0_m};
|
||||
EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon);
|
||||
const Translation2d one{3_m, 5_m};
|
||||
EXPECT_DOUBLE_EQ(std::hypot(3.0, 5.0), one.Norm().value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, Distance) {
|
||||
const Translation2d one{1_m, 1_m};
|
||||
const Translation2d two{6_m, 6_m};
|
||||
EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), one.Distance(two).value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, UnaryMinus) {
|
||||
const Translation2d original{-4.5_m, 7_m};
|
||||
const auto inverted = -original;
|
||||
|
||||
EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
|
||||
EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(4.5, inverted.X().value());
|
||||
EXPECT_DOUBLE_EQ(-7.0, inverted.Y().value());
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, Equality) {
|
||||
@@ -87,11 +85,11 @@ TEST(Translation2dTest, Inequality) {
|
||||
}
|
||||
|
||||
TEST(Translation2dTest, PolarConstructor) {
|
||||
Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
|
||||
EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
|
||||
Translation2d one{std::sqrt(2) * 1_m, Rotation2d{45_deg}};
|
||||
EXPECT_DOUBLE_EQ(1.0, one.X().value());
|
||||
EXPECT_DOUBLE_EQ(1.0, one.Y().value());
|
||||
|
||||
Translation2d two{2_m, Rotation2d(60_deg)};
|
||||
EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
|
||||
Translation2d two{2_m, Rotation2d{60_deg}};
|
||||
EXPECT_DOUBLE_EQ(1.0, two.X().value());
|
||||
EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
|
||||
}
|
||||
|
||||
128
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
Normal file
128
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
Normal file
@@ -0,0 +1,128 @@
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "frc/geometry/Translation3d.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(Translation3dTest, Sum) {
|
||||
const Translation3d one{1_m, 3_m, 5_m};
|
||||
const Translation3d two{2_m, 5_m, 8_m};
|
||||
|
||||
const auto sum = one + two;
|
||||
|
||||
EXPECT_NEAR(3.0, sum.X().value(), kEpsilon);
|
||||
EXPECT_NEAR(8.0, sum.Y().value(), kEpsilon);
|
||||
EXPECT_NEAR(13.0, sum.Z().value(), kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, Difference) {
|
||||
const Translation3d one{1_m, 3_m, 5_m};
|
||||
const Translation3d two{2_m, 5_m, 8_m};
|
||||
|
||||
const auto difference = one - two;
|
||||
|
||||
EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
|
||||
EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
|
||||
EXPECT_NEAR(difference.Z().value(), -3.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, RotateBy) {
|
||||
Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
|
||||
Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Translation3d translation{1_m, 2_m, 3_m};
|
||||
|
||||
const auto rotated1 = translation.RotateBy(Rotation3d{xAxis, 90_deg});
|
||||
EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(rotated1.Y().value(), -3.0, kEpsilon);
|
||||
EXPECT_NEAR(rotated1.Z().value(), 2.0, kEpsilon);
|
||||
|
||||
const auto rotated2 = translation.RotateBy(Rotation3d{yAxis, 90_deg});
|
||||
EXPECT_NEAR(rotated2.X().value(), 3.0, kEpsilon);
|
||||
EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
|
||||
EXPECT_NEAR(rotated2.Z().value(), -1.0, kEpsilon);
|
||||
|
||||
const auto rotated3 = translation.RotateBy(Rotation3d{zAxis, 90_deg});
|
||||
EXPECT_NEAR(rotated3.X().value(), -2.0, kEpsilon);
|
||||
EXPECT_NEAR(rotated3.Y().value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, ToTranslation2d) {
|
||||
Translation3d translation{1_m, 2_m, 3_m};
|
||||
Translation2d expected{1_m, 2_m};
|
||||
|
||||
EXPECT_EQ(expected, translation.ToTranslation2d());
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, Multiplication) {
|
||||
const Translation3d original{3_m, 5_m, 7_m};
|
||||
const auto mult = original * 3;
|
||||
|
||||
EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
|
||||
EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
|
||||
EXPECT_NEAR(mult.Z().value(), 21.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, Division) {
|
||||
const Translation3d original{3_m, 5_m, 7_m};
|
||||
const auto div = original / 2;
|
||||
|
||||
EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
|
||||
EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
|
||||
EXPECT_NEAR(div.Z().value(), 3.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, Norm) {
|
||||
const Translation3d one{3_m, 5_m, 7_m};
|
||||
EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5, 7), kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, Distance) {
|
||||
const Translation3d one{1_m, 1_m, 1_m};
|
||||
const Translation3d two{6_m, 6_m, 6_m};
|
||||
EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(3), kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, UnaryMinus) {
|
||||
const Translation3d original{-4.5_m, 7_m, 9_m};
|
||||
const auto inverted = -original;
|
||||
|
||||
EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
|
||||
EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
|
||||
EXPECT_NEAR(inverted.Z().value(), -9, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, Equality) {
|
||||
const Translation3d one{9_m, 5.5_m, 3.5_m};
|
||||
const Translation3d two{9_m, 5.5_m, 3.5_m};
|
||||
EXPECT_TRUE(one == two);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, Inequality) {
|
||||
const Translation3d one{9_m, 5.5_m, 3.5_m};
|
||||
const Translation3d two{9_m, 5.7_m, 3.5_m};
|
||||
EXPECT_TRUE(one != two);
|
||||
}
|
||||
|
||||
TEST(Translation3dTest, PolarConstructor) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
Translation3d one{std::sqrt(2) * 1_m, Rotation3d(zAxis, 45_deg)};
|
||||
EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(one.Z().value(), 0.0, kEpsilon);
|
||||
|
||||
Translation3d two{2_m, Rotation3d(zAxis, 60_deg)};
|
||||
EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
|
||||
EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
|
||||
}
|
||||
@@ -11,55 +11,57 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(Twist2dTest, Straight) {
|
||||
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
|
||||
const Twist2d straight{5_m, 0_m, 0_rad};
|
||||
const auto straightPose = Pose2d().Exp(straight);
|
||||
|
||||
EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(5.0, straightPose.X().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, straightPose.Y().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, straightPose.Rotation().Radians().value());
|
||||
}
|
||||
|
||||
TEST(Twist2dTest, QuarterCircle) {
|
||||
const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m,
|
||||
units::radian_t(wpi::numbers::pi / 2.0)};
|
||||
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
|
||||
const Twist2d quarterCircle{5_m / 2.0 * wpi::numbers::pi, 0_m,
|
||||
units::radian_t{wpi::numbers::pi / 2.0}};
|
||||
const auto quarterCirclePose = Pose2d{}.Exp(quarterCircle);
|
||||
|
||||
EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.X().value());
|
||||
EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.Y().value());
|
||||
EXPECT_DOUBLE_EQ(90.0, quarterCirclePose.Rotation().Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Twist2dTest, DiagonalNoDtheta) {
|
||||
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
|
||||
const auto diagonalPose = Pose2d().Exp(diagonal);
|
||||
const Twist2d diagonal{2_m, 2_m, 0_deg};
|
||||
const auto diagonalPose = Pose2d{}.Exp(diagonal);
|
||||
|
||||
EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
|
||||
EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
|
||||
EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
|
||||
EXPECT_DOUBLE_EQ(2.0, diagonalPose.X().value());
|
||||
EXPECT_DOUBLE_EQ(2.0, diagonalPose.Y().value());
|
||||
EXPECT_DOUBLE_EQ(0.0, diagonalPose.Rotation().Degrees().value());
|
||||
}
|
||||
|
||||
TEST(Twist2dTest, Equality) {
|
||||
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
|
||||
const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
|
||||
const Twist2d one{5_m, 1_m, 3_rad};
|
||||
const Twist2d two{5_m, 1_m, 3_rad};
|
||||
EXPECT_TRUE(one == two);
|
||||
}
|
||||
|
||||
TEST(Twist2dTest, Inequality) {
|
||||
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
|
||||
const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
|
||||
const Twist2d one{5_m, 1_m, 3_rad};
|
||||
const Twist2d two{5_m, 1.2_m, 3_rad};
|
||||
EXPECT_TRUE(one != two);
|
||||
}
|
||||
|
||||
TEST(Twist2dTest, Pose2dLog) {
|
||||
const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
|
||||
const Pose2d start{};
|
||||
const Pose2d end{5_m, 5_m, Rotation2d{90_deg}};
|
||||
const Pose2d start;
|
||||
|
||||
const auto twist = start.Log(end);
|
||||
|
||||
EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
|
||||
EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
|
||||
Twist2d expected{units::meter_t{5.0 / 2.0 * wpi::numbers::pi}, 0_m,
|
||||
units::radian_t{wpi::numbers::pi / 2.0}};
|
||||
EXPECT_EQ(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
const auto reapplied = start.Exp(twist);
|
||||
EXPECT_EQ(end, reapplied);
|
||||
}
|
||||
|
||||
118
wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
Normal file
118
wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
Normal file
@@ -0,0 +1,118 @@
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <wpi/numbers>
|
||||
|
||||
#include "frc/geometry/Pose3d.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TEST(Twist3dTest, StraightX) {
|
||||
const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad};
|
||||
const auto straightPose = Pose3d().Exp(straight);
|
||||
|
||||
Pose3d expected{5_m, 0_m, 0_m, Rotation3d{}};
|
||||
EXPECT_EQ(expected, straightPose);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, StraightY) {
|
||||
const Twist3d straight{0_m, 5_m, 0_m, 0_rad, 0_rad, 0_rad};
|
||||
const auto straightPose = Pose3d().Exp(straight);
|
||||
|
||||
Pose3d expected{0_m, 5_m, 0_m, Rotation3d{}};
|
||||
EXPECT_EQ(expected, straightPose);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, StraightZ) {
|
||||
const Twist3d straight{0_m, 0_m, 5_m, 0_rad, 0_rad, 0_rad};
|
||||
const auto straightPose = Pose3d().Exp(straight);
|
||||
|
||||
Pose3d expected{0_m, 0_m, 5_m, Rotation3d{}};
|
||||
EXPECT_EQ(expected, straightPose);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, QuarterCircle) {
|
||||
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
|
||||
|
||||
const Twist3d quarterCircle{
|
||||
5_m / 2.0 * wpi::numbers::pi, 0_m, 0_m, 0_rad, 0_rad,
|
||||
units::radian_t(wpi::numbers::pi / 2.0)};
|
||||
const auto quarterCirclePose = Pose3d().Exp(quarterCircle);
|
||||
|
||||
Pose3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
|
||||
EXPECT_EQ(expected, quarterCirclePose);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, DiagonalNoDtheta) {
|
||||
const Twist3d diagonal{2_m, 2_m, 0_m, 0_rad, 0_rad, 0_rad};
|
||||
const auto diagonalPose = Pose3d().Exp(diagonal);
|
||||
|
||||
Pose3d expected{2_m, 2_m, 0_m, Rotation3d{}};
|
||||
EXPECT_EQ(expected, diagonalPose);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, Equality) {
|
||||
const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
|
||||
const Twist3d two{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
|
||||
EXPECT_TRUE(one == two);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, Inequality) {
|
||||
const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
|
||||
const Twist3d two{5_m, 1.2_m, 0_m, 0_rad, 0_rad, 3_rad};
|
||||
EXPECT_TRUE(one != two);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, Pose3dLogX) {
|
||||
const Pose3d end{0_m, 5_m, 5_m, Rotation3d{90_deg, 0_deg, 0_deg}};
|
||||
const Pose3d start;
|
||||
|
||||
const auto twist = start.Log(end);
|
||||
|
||||
Twist3d expected{0_m, units::meter_t{5.0 / 2.0 * wpi::numbers::pi},
|
||||
0_m, 90_deg,
|
||||
0_deg, 0_deg};
|
||||
EXPECT_EQ(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
const auto reapplied = start.Exp(twist);
|
||||
EXPECT_EQ(end, reapplied);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, Pose3dLogY) {
|
||||
const Pose3d end{5_m, 0_m, 5_m, Rotation3d{0_deg, 90_deg, 0_deg}};
|
||||
const Pose3d start;
|
||||
|
||||
const auto twist = start.Log(end);
|
||||
|
||||
Twist3d expected{0_m, 0_m, units::meter_t{5.0 / 2.0 * wpi::numbers::pi},
|
||||
0_deg, 90_deg, 0_deg};
|
||||
EXPECT_EQ(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
const auto reapplied = start.Exp(twist);
|
||||
EXPECT_EQ(end, reapplied);
|
||||
}
|
||||
|
||||
TEST(Twist3dTest, Pose3dLogZ) {
|
||||
const Pose3d end{5_m, 5_m, 0_m, Rotation3d{0_deg, 0_deg, 90_deg}};
|
||||
const Pose3d start;
|
||||
|
||||
const auto twist = start.Log(end);
|
||||
|
||||
Twist3d expected{units::meter_t{5.0 / 2.0 * wpi::numbers::pi},
|
||||
0_m,
|
||||
0_m,
|
||||
0_deg,
|
||||
0_deg,
|
||||
90_deg};
|
||||
EXPECT_EQ(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
const auto reapplied = start.Exp(twist);
|
||||
EXPECT_EQ(end, reapplied);
|
||||
}
|
||||
Reference in New Issue
Block a user