mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
[wpimath] Add 3D geometry classes (#4175)
Also clean up 2D geometry documentation.
This commit is contained in:
Tyler Veness
@@ -21,9 +21,9 @@ class Pose2dTest {
|
||||
var transformed = initial.plus(transformation);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
|
||||
() -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
|
||||
() -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon));
|
||||
() -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
|
||||
() -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
|
||||
() -> assertEquals(50.0, transformed.getRotation().getDegrees(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -34,9 +34,9 @@ class Pose2dTest {
|
||||
var finalRelativeToInitial = last.relativeTo(initial);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
|
||||
() -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
|
||||
() -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon));
|
||||
() -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
|
||||
() -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
|
||||
() -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -61,8 +61,8 @@ class Pose2dTest {
|
||||
final var transform = last.minus(initial);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
|
||||
() -> assertEquals(transform.getY(), 0.0, kEpsilon),
|
||||
() -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon));
|
||||
() -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
|
||||
() -> assertEquals(0.0, transform.getY(), kEpsilon),
|
||||
() -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,106 @@
|
||||
// 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.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class Pose3dTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testTransformBy() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var initial =
|
||||
new Pose3d(
|
||||
new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
var transformation =
|
||||
new Transform3d(
|
||||
new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
|
||||
|
||||
var transformed = initial.plus(transformation);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
|
||||
() -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
|
||||
() ->
|
||||
assertEquals(Units.degreesToRadians(50.0), transformed.getRotation().getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRelativeTo() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
|
||||
var finalRelativeToInitial = last.relativeTo(initial);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
|
||||
() -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
|
||||
() -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testEquality() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
|
||||
var two = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
|
||||
assertEquals(one, two);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInequality() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
|
||||
var two = new Pose3d(0.0, 1.524, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
|
||||
assertNotEquals(one, two);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMinus() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
|
||||
final var transform = last.minus(initial);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
|
||||
() -> assertEquals(0.0, transform.getY(), kEpsilon),
|
||||
() -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testToPose2d() {
|
||||
var pose =
|
||||
new Pose3d(
|
||||
1.0,
|
||||
2.0,
|
||||
3.0,
|
||||
new Rotation3d(
|
||||
Units.degreesToRadians(20.0),
|
||||
Units.degreesToRadians(30.0),
|
||||
Units.degreesToRadians(40.0)));
|
||||
var expected = new Pose2d(1.0, 2.0, new Rotation2d(Units.degreesToRadians(40.0)));
|
||||
|
||||
assertEquals(expected, pose.toPose2d());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
// 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.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class QuaternionTest {
|
||||
@Test
|
||||
void testInit() {
|
||||
// Identity
|
||||
var q1 = new Quaternion();
|
||||
assertEquals(1.0, q1.getW());
|
||||
assertEquals(0.0, q1.getX());
|
||||
assertEquals(0.0, q1.getY());
|
||||
assertEquals(0.0, q1.getZ());
|
||||
|
||||
// Normalized
|
||||
var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5);
|
||||
assertEquals(0.5, q2.getW());
|
||||
assertEquals(0.5, q2.getX());
|
||||
assertEquals(0.5, q2.getY());
|
||||
assertEquals(0.5, q2.getZ());
|
||||
|
||||
// Unnormalized
|
||||
var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5);
|
||||
assertEquals(0.75, q3.getW());
|
||||
assertEquals(0.3, q3.getX());
|
||||
assertEquals(0.4, q3.getY());
|
||||
assertEquals(0.5, q3.getZ());
|
||||
|
||||
q3 = q3.normalize();
|
||||
double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
|
||||
assertEquals(0.75 / norm, q3.getW());
|
||||
assertEquals(0.3 / norm, q3.getX());
|
||||
assertEquals(0.4 / norm, q3.getY());
|
||||
assertEquals(0.5 / norm, q3.getZ());
|
||||
assertEquals(
|
||||
1.0,
|
||||
q3.getW() * q3.getW()
|
||||
+ q3.getX() * q3.getX()
|
||||
+ q3.getY() * q3.getY()
|
||||
+ q3.getZ() * q3.getZ());
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testTimes() {
|
||||
// 90° CCW rotations around each axis
|
||||
double c = Math.cos(Units.degreesToRadians(90.0) / 2.0);
|
||||
double s = Math.sin(Units.degreesToRadians(90.0) / 2.0);
|
||||
var xRot = new Quaternion(c, s, 0.0, 0.0);
|
||||
var yRot = new Quaternion(c, 0.0, s, 0.0);
|
||||
var zRot = new Quaternion(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
|
||||
var expected = yRot;
|
||||
var actual = zRot.times(yRot).times(xRot);
|
||||
assertEquals(expected.getW(), actual.getW(), 1e-9);
|
||||
assertEquals(expected.getX(), actual.getX(), 1e-9);
|
||||
assertEquals(expected.getY(), actual.getY(), 1e-9);
|
||||
assertEquals(expected.getZ(), actual.getZ(), 1e-9);
|
||||
|
||||
// Identity
|
||||
var q =
|
||||
new Quaternion(
|
||||
0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594);
|
||||
actual = q.times(q.inverse());
|
||||
assertEquals(1.0, actual.getW());
|
||||
assertEquals(0.0, actual.getX());
|
||||
assertEquals(0.0, actual.getY());
|
||||
assertEquals(0.0, actual.getZ());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInverse() {
|
||||
var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
|
||||
var inv = q.inverse();
|
||||
|
||||
assertEquals(q.getW(), inv.getW());
|
||||
assertEquals(-q.getX(), inv.getX());
|
||||
assertEquals(-q.getY(), inv.getY());
|
||||
assertEquals(-q.getZ(), inv.getZ());
|
||||
}
|
||||
}
|
||||
@@ -19,8 +19,8 @@ class Rotation2dTest {
|
||||
var rot2 = Rotation2d.fromRadians(Math.PI / 4);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(rot1.getDegrees(), 60.0, kEpsilon),
|
||||
() -> assertEquals(rot2.getDegrees(), 45.0, kEpsilon));
|
||||
() -> assertEquals(60.0, rot1.getDegrees(), kEpsilon),
|
||||
() -> assertEquals(45.0, rot2.getDegrees(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -29,8 +29,8 @@ class Rotation2dTest {
|
||||
var rot2 = Rotation2d.fromDegrees(30.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(rot1.getRadians(), Math.PI / 4, kEpsilon),
|
||||
() -> assertEquals(rot2.getRadians(), Math.PI / 6, kEpsilon));
|
||||
() -> assertEquals(Math.PI / 4.0, rot1.getRadians(), kEpsilon),
|
||||
() -> assertEquals(Math.PI / 6.0, rot2.getRadians(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -39,8 +39,8 @@ class Rotation2dTest {
|
||||
var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
|
||||
() -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon));
|
||||
() -> assertEquals(Math.PI / 2.0, rotated.getRadians(), kEpsilon),
|
||||
() -> assertEquals(90.0, rotated.getDegrees(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -48,7 +48,7 @@ class Rotation2dTest {
|
||||
var rot = Rotation2d.fromDegrees(90.0);
|
||||
rot = rot.plus(Rotation2d.fromDegrees(30.0));
|
||||
|
||||
assertEquals(rot.getDegrees(), 120.0, kEpsilon);
|
||||
assertEquals(120.0, rot.getDegrees(), kEpsilon);
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -56,7 +56,7 @@ class Rotation2dTest {
|
||||
var rot1 = Rotation2d.fromDegrees(70.0);
|
||||
var rot2 = Rotation2d.fromDegrees(30.0);
|
||||
|
||||
assertEquals(rot1.minus(rot2).getDegrees(), 40.0, kEpsilon);
|
||||
assertEquals(40.0, rot1.minus(rot2).getDegrees(), kEpsilon);
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -65,9 +65,9 @@ class Rotation2dTest {
|
||||
var rot2 = Rotation2d.fromDegrees(43.0);
|
||||
assertEquals(rot1, rot2);
|
||||
|
||||
var rot3 = Rotation2d.fromDegrees(-180.0);
|
||||
var rot4 = Rotation2d.fromDegrees(180.0);
|
||||
assertEquals(rot3, rot4);
|
||||
rot1 = Rotation2d.fromDegrees(-180.0);
|
||||
rot2 = Rotation2d.fromDegrees(180.0);
|
||||
assertEquals(rot1, rot2);
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -83,12 +83,12 @@ class Rotation2dTest {
|
||||
var rot1 = Rotation2d.fromDegrees(50);
|
||||
var rot2 = Rotation2d.fromDegrees(70);
|
||||
var interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(60.0, interpolated.getDegrees(), 1e-2);
|
||||
assertEquals(60.0, interpolated.getDegrees(), kEpsilon);
|
||||
|
||||
// -160 minus half distance between 170 and -160 (15) = -175
|
||||
var rot3 = Rotation2d.fromDegrees(170);
|
||||
var rot4 = Rotation2d.fromDegrees(-160);
|
||||
interpolated = rot3.interpolate(rot4, 0.5);
|
||||
rot1 = Rotation2d.fromDegrees(170);
|
||||
rot2 = Rotation2d.fromDegrees(-160);
|
||||
interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(-175.0, interpolated.getDegrees());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,293 @@
|
||||
// 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.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class Rotation3dTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testInit() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
|
||||
final var rot1 = new Rotation3d(xAxis, Math.PI / 3);
|
||||
final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0);
|
||||
assertEquals(rot1, rot2);
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
|
||||
final var rot3 = new Rotation3d(yAxis, Math.PI / 3);
|
||||
final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0);
|
||||
assertEquals(rot3, rot4);
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
final var rot5 = new Rotation3d(zAxis, Math.PI / 3);
|
||||
final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3);
|
||||
assertEquals(rot5, rot6);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRadiansToDegrees() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var rot1 = new Rotation3d(zAxis, Math.PI / 3);
|
||||
assertAll(
|
||||
() -> assertEquals(Units.degreesToRadians(0.0), rot1.getX(), kEpsilon),
|
||||
() -> assertEquals(Units.degreesToRadians(0.0), rot1.getY(), kEpsilon),
|
||||
() -> assertEquals(Units.degreesToRadians(60.0), rot1.getZ(), kEpsilon));
|
||||
|
||||
var rot2 = new Rotation3d(zAxis, Math.PI / 4);
|
||||
assertAll(
|
||||
() -> assertEquals(Units.degreesToRadians(0.0), rot2.getX(), kEpsilon),
|
||||
() -> assertEquals(Units.degreesToRadians(0.0), rot2.getY(), kEpsilon),
|
||||
() -> assertEquals(Units.degreesToRadians(45.0), rot2.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRadiansAndDegrees() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(45.0));
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, rot1.getX(), kEpsilon),
|
||||
() -> assertEquals(0.0, rot1.getY(), kEpsilon),
|
||||
() -> assertEquals(Math.PI / 4.0, rot1.getZ(), kEpsilon));
|
||||
|
||||
var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, rot2.getX(), kEpsilon),
|
||||
() -> assertEquals(0.0, rot2.getY(), kEpsilon),
|
||||
() -> assertEquals(Math.PI / 6.0, rot2.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testRotationLoop() {
|
||||
var rot = new Rotation3d();
|
||||
|
||||
rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
|
||||
var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0);
|
||||
assertEquals(expected, rot);
|
||||
|
||||
rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
|
||||
expected =
|
||||
new Rotation3d(
|
||||
VecBuilder.fill(1.0 / Math.sqrt(3), 1.0 / Math.sqrt(3), -1.0 / Math.sqrt(3)),
|
||||
Units.degreesToRadians(120.0));
|
||||
assertEquals(expected, rot);
|
||||
|
||||
rot = rot.plus(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
|
||||
expected = new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0);
|
||||
assertEquals(expected, rot);
|
||||
|
||||
rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
|
||||
assertEquals(new Rotation3d(), rot);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testRotateByFromZeroX() {
|
||||
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
|
||||
|
||||
final var zero = new Rotation3d();
|
||||
var rotated = zero.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
|
||||
|
||||
var expected = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
|
||||
assertEquals(expected, rotated);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testRotateByFromZeroY() {
|
||||
final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
|
||||
|
||||
final var zero = new Rotation3d();
|
||||
var rotated = zero.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
|
||||
|
||||
var expected = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
|
||||
assertEquals(expected, rotated);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testRotateByFromZeroZ() {
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
final var zero = new Rotation3d();
|
||||
var rotated = zero.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
|
||||
|
||||
var expected = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
|
||||
assertEquals(expected, rotated);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRotateByNonZeroX() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
|
||||
|
||||
var rot = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
|
||||
rot = rot.plus(new Rotation3d(xAxis, Units.degreesToRadians(30.0)));
|
||||
|
||||
var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0));
|
||||
assertEquals(expected, rot);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRotateByNonZeroY() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
|
||||
|
||||
var rot = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
|
||||
rot = rot.plus(new Rotation3d(yAxis, Units.degreesToRadians(30.0)));
|
||||
|
||||
var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0));
|
||||
assertEquals(expected, rot);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRotateByNonZeroZ() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var rot = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
|
||||
rot = rot.plus(new Rotation3d(zAxis, Units.degreesToRadians(30.0)));
|
||||
|
||||
var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0));
|
||||
assertEquals(expected, rot);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMinus() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(70.0));
|
||||
var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
|
||||
|
||||
assertEquals(rot1.minus(rot2).getZ(), Units.degreesToRadians(40.0), kEpsilon);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testEquality() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
|
||||
var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
|
||||
assertEquals(rot1, rot2);
|
||||
|
||||
rot1 = new Rotation3d(zAxis, Units.degreesToRadians(-180.0));
|
||||
rot2 = new Rotation3d(zAxis, Units.degreesToRadians(180.0));
|
||||
assertEquals(rot1, rot2);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testAxisAngle() {
|
||||
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
|
||||
final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
|
||||
assertEquals(xAxis, rot1.getAxis());
|
||||
assertEquals(Math.PI / 2.0, rot1.getAngle(), 1e-9);
|
||||
|
||||
var rot2 = new Rotation3d(yAxis, Units.degreesToRadians(45.0));
|
||||
assertEquals(yAxis, rot2.getAxis());
|
||||
assertEquals(Math.PI / 4.0, rot2.getAngle(), 1e-9);
|
||||
|
||||
var rot3 = new Rotation3d(zAxis, Units.degreesToRadians(60.0));
|
||||
assertEquals(zAxis, rot3.getAxis());
|
||||
assertEquals(Math.PI / 3.0, rot3.getAngle(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testToRotation2d() {
|
||||
var rotation =
|
||||
new Rotation3d(
|
||||
Units.degreesToRadians(20.0),
|
||||
Units.degreesToRadians(30.0),
|
||||
Units.degreesToRadians(40.0));
|
||||
var expected = new Rotation2d(Units.degreesToRadians(40.0));
|
||||
|
||||
assertEquals(expected, rotation.toRotation2d());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInequality() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
|
||||
var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.5));
|
||||
assertNotEquals(rot1, rot2);
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testInterpolate() {
|
||||
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
|
||||
final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
|
||||
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
// 50 + (70 - 50) * 0.5 = 60
|
||||
var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(50));
|
||||
var rot2 = new Rotation3d(xAxis, Units.degreesToRadians(70));
|
||||
var interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(Units.degreesToRadians(60.0), interpolated.getX(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
|
||||
|
||||
// -160 minus half distance between 170 and -160 (15) = -175
|
||||
rot1 = new Rotation3d(xAxis, Units.degreesToRadians(170));
|
||||
rot2 = new Rotation3d(xAxis, Units.degreesToRadians(-160));
|
||||
interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(Units.degreesToRadians(-175.0), interpolated.getX());
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getZ());
|
||||
|
||||
// 50 + (70 - 50) * 0.5 = 60
|
||||
rot1 = new Rotation3d(yAxis, Units.degreesToRadians(50));
|
||||
rot2 = new Rotation3d(yAxis, Units.degreesToRadians(70));
|
||||
interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(60.0), interpolated.getY(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
|
||||
|
||||
// -160 minus half distance between 170 and -160 (165) = 5
|
||||
rot1 = new Rotation3d(yAxis, Units.degreesToRadians(170));
|
||||
rot2 = new Rotation3d(yAxis, Units.degreesToRadians(-160));
|
||||
interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(Units.degreesToRadians(180.0), interpolated.getX(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(-5.0), interpolated.getY(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(180.0), interpolated.getZ(), kEpsilon);
|
||||
|
||||
// 50 + (70 - 50) * 0.5 = 60
|
||||
rot1 = new Rotation3d(zAxis, Units.degreesToRadians(50));
|
||||
rot2 = new Rotation3d(zAxis, Units.degreesToRadians(70));
|
||||
interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(60.0), interpolated.getZ(), kEpsilon);
|
||||
|
||||
// -160 minus half distance between 170 and -160 (15) = -175
|
||||
rot1 = new Rotation3d(zAxis, Units.degreesToRadians(170));
|
||||
rot2 = new Rotation3d(zAxis, Units.degreesToRadians(-160));
|
||||
interpolated = rot1.interpolate(rot2, 0.5);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
|
||||
assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
// 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.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class Transform3dTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testInverse() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var initial =
|
||||
new Pose3d(
|
||||
new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
var transform =
|
||||
new Transform3d(
|
||||
new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
|
||||
|
||||
var transformed = initial.plus(transform);
|
||||
var untransformed = transformed.plus(transform.inverse());
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon),
|
||||
() -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon),
|
||||
() -> assertEquals(initial.getZ(), untransformed.getZ(), kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
initial.getRotation().getZ(), untransformed.getRotation().getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testComposition() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var initial =
|
||||
new Pose3d(
|
||||
new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
var transform1 =
|
||||
new Transform3d(
|
||||
new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
|
||||
var transform2 =
|
||||
new Transform3d(
|
||||
new Translation3d(0.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
|
||||
|
||||
var transformedSeparate = initial.plus(transform1).plus(transform2);
|
||||
var transformedCombined = initial.plus(transform1.plus(transform2));
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon),
|
||||
() -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon),
|
||||
() -> assertEquals(transformedSeparate.getZ(), transformedCombined.getZ(), kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
transformedSeparate.getRotation().getZ(),
|
||||
transformedCombined.getRotation().getZ(),
|
||||
kEpsilon));
|
||||
}
|
||||
}
|
||||
@@ -21,8 +21,8 @@ class Translation2dTest {
|
||||
var sum = one.plus(two);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(sum.getX(), 3.0, kEpsilon),
|
||||
() -> assertEquals(sum.getY(), 8.0, kEpsilon));
|
||||
() -> assertEquals(3.0, sum.getX(), kEpsilon),
|
||||
() -> assertEquals(8.0, sum.getY(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -33,8 +33,8 @@ class Translation2dTest {
|
||||
var difference = one.minus(two);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(difference.getX(), -1.0, kEpsilon),
|
||||
() -> assertEquals(difference.getY(), -2.0, kEpsilon));
|
||||
() -> assertEquals(-1.0, difference.getX(), kEpsilon),
|
||||
() -> assertEquals(-2.0, difference.getY(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -43,8 +43,8 @@ class Translation2dTest {
|
||||
var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(rotated.getX(), 0.0, kEpsilon),
|
||||
() -> assertEquals(rotated.getY(), 3.0, kEpsilon));
|
||||
() -> assertEquals(0.0, rotated.getX(), kEpsilon),
|
||||
() -> assertEquals(3.0, rotated.getY(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -53,8 +53,8 @@ class Translation2dTest {
|
||||
var mult = original.times(3);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(mult.getX(), 9.0, kEpsilon),
|
||||
() -> assertEquals(mult.getY(), 15.0, kEpsilon));
|
||||
() -> assertEquals(9.0, mult.getX(), kEpsilon),
|
||||
() -> assertEquals(15.0, mult.getY(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -63,21 +63,21 @@ class Translation2dTest {
|
||||
var div = original.div(2);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(div.getX(), 1.5, kEpsilon),
|
||||
() -> assertEquals(div.getY(), 2.5, kEpsilon));
|
||||
() -> assertEquals(1.5, div.getX(), kEpsilon),
|
||||
() -> assertEquals(2.5, div.getY(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testNorm() {
|
||||
var one = new Translation2d(3.0, 5.0);
|
||||
assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
|
||||
assertEquals(Math.hypot(3.0, 5.0), one.getNorm(), kEpsilon);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDistance() {
|
||||
var one = new Translation2d(1, 1);
|
||||
var two = new Translation2d(6, 6);
|
||||
assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
|
||||
assertEquals(5.0 * Math.sqrt(2.0), one.getDistance(two), kEpsilon);
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -86,8 +86,8 @@ class Translation2dTest {
|
||||
var inverted = original.unaryMinus();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(inverted.getX(), 4.5, kEpsilon),
|
||||
() -> assertEquals(inverted.getY(), -7, kEpsilon));
|
||||
() -> assertEquals(4.5, inverted.getX(), kEpsilon),
|
||||
() -> assertEquals(-7.0, inverted.getY(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -109,9 +109,9 @@ class Translation2dTest {
|
||||
var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
|
||||
var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
|
||||
assertAll(
|
||||
() -> assertEquals(one.getX(), 1.0, kEpsilon),
|
||||
() -> assertEquals(one.getY(), 1.0, kEpsilon),
|
||||
() -> assertEquals(two.getX(), 1.0, kEpsilon),
|
||||
() -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon));
|
||||
() -> assertEquals(1.0, one.getX(), kEpsilon),
|
||||
() -> assertEquals(1.0, one.getY(), kEpsilon),
|
||||
() -> assertEquals(1.0, two.getX(), kEpsilon),
|
||||
() -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,155 @@
|
||||
// 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.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class Translation3dTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testSum() {
|
||||
var one = new Translation3d(1.0, 3.0, 5.0);
|
||||
var two = new Translation3d(2.0, 5.0, 8.0);
|
||||
|
||||
var sum = one.plus(two);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, sum.getX(), kEpsilon),
|
||||
() -> assertEquals(8.0, sum.getY(), kEpsilon),
|
||||
() -> assertEquals(13.0, sum.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDifference() {
|
||||
var one = new Translation3d(1.0, 3.0, 5.0);
|
||||
var two = new Translation3d(2.0, 5.0, 8.0);
|
||||
|
||||
var difference = one.minus(two);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-1.0, difference.getX(), kEpsilon),
|
||||
() -> assertEquals(-2.0, difference.getY(), kEpsilon),
|
||||
() -> assertEquals(-3.0, difference.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
@Test
|
||||
void testRotateBy() {
|
||||
var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
|
||||
var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var translation = new Translation3d(1.0, 2.0, 3.0);
|
||||
|
||||
var rotated1 = translation.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, rotated1.getX(), kEpsilon),
|
||||
() -> assertEquals(-3.0, rotated1.getY(), kEpsilon),
|
||||
() -> assertEquals(2.0, rotated1.getZ(), kEpsilon));
|
||||
|
||||
var rotated2 = translation.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, rotated2.getX(), kEpsilon),
|
||||
() -> assertEquals(2.0, rotated2.getY(), kEpsilon),
|
||||
() -> assertEquals(-1.0, rotated2.getZ(), kEpsilon));
|
||||
|
||||
var rotated3 = translation.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
|
||||
assertAll(
|
||||
() -> assertEquals(-2.0, rotated3.getX(), kEpsilon),
|
||||
() -> assertEquals(1.0, rotated3.getY(), kEpsilon),
|
||||
() -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testToTranslation2d() {
|
||||
var translation = new Translation3d(1.0, 2.0, 3.0);
|
||||
var expected = new Translation2d(1.0, 2.0);
|
||||
|
||||
assertEquals(expected, translation.toTranslation2d());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMultiplication() {
|
||||
var original = new Translation3d(3.0, 5.0, 7.0);
|
||||
var mult = original.times(3);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(9.0, mult.getX(), kEpsilon),
|
||||
() -> assertEquals(15.0, mult.getY(), kEpsilon),
|
||||
() -> assertEquals(21.0, mult.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDivision() {
|
||||
var original = new Translation3d(3.0, 5.0, 7.0);
|
||||
var div = original.div(2);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.5, div.getX(), kEpsilon),
|
||||
() -> assertEquals(2.5, div.getY(), kEpsilon),
|
||||
() -> assertEquals(3.5, div.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testNorm() {
|
||||
var one = new Translation3d(3.0, 5.0, 7.0);
|
||||
assertEquals(Math.sqrt(83.0), one.getNorm(), kEpsilon);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDistance() {
|
||||
var one = new Translation3d(1.0, 1.0, 1.0);
|
||||
var two = new Translation3d(6.0, 6.0, 6.0);
|
||||
assertEquals(5.0 * Math.sqrt(3.0), one.getDistance(two), kEpsilon);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testUnaryMinus() {
|
||||
var original = new Translation3d(-4.5, 7.0, 9.0);
|
||||
var inverted = original.unaryMinus();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(4.5, inverted.getX(), kEpsilon),
|
||||
() -> assertEquals(-7.0, inverted.getY(), kEpsilon),
|
||||
() -> assertEquals(-9.0, inverted.getZ(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testEquality() {
|
||||
var one = new Translation3d(9, 5.5, 3.5);
|
||||
var two = new Translation3d(9, 5.5, 3.5);
|
||||
assertEquals(one, two);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInequality() {
|
||||
var one = new Translation3d(9, 5.5, 3.5);
|
||||
var two = new Translation3d(9, 5.7, 3.5);
|
||||
assertNotEquals(one, two);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPolarConstructor() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var one = new Translation3d(Math.sqrt(2), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
|
||||
var two = new Translation3d(2, new Rotation3d(zAxis, Units.degreesToRadians(60.0)));
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, one.getX(), kEpsilon),
|
||||
() -> assertEquals(1.0, one.getY(), kEpsilon),
|
||||
() -> assertEquals(0.0, one.getZ(), kEpsilon),
|
||||
() -> assertEquals(1.0, two.getX(), kEpsilon),
|
||||
() -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon),
|
||||
() -> assertEquals(0.0, two.getZ(), kEpsilon));
|
||||
}
|
||||
}
|
||||
@@ -4,35 +4,28 @@
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class Twist2dTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testStraightLineTwist() {
|
||||
void testStraight() {
|
||||
var straight = new Twist2d(5.0, 0.0, 0.0);
|
||||
var straightPose = new Pose2d().exp(straight);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
|
||||
() -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
|
||||
() -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon));
|
||||
var expected = new Pose2d(5.0, 0.0, new Rotation2d());
|
||||
assertEquals(expected, straightPose);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testQuarterCirleTwist() {
|
||||
void testQuarterCirle() {
|
||||
var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
|
||||
var quarterCirclePose = new Pose2d().exp(quarterCircle);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
|
||||
() -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
|
||||
() -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon));
|
||||
var expected = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
|
||||
assertEquals(expected, quarterCirclePose);
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -40,10 +33,8 @@ class Twist2dTest {
|
||||
var diagonal = new Twist2d(2.0, 2.0, 0.0);
|
||||
var diagonalPose = new Pose2d().exp(diagonal);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
|
||||
() -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
|
||||
() -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon));
|
||||
var expected = new Pose2d(2.0, 2.0, new Rotation2d());
|
||||
assertEquals(expected, diagonalPose);
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -67,9 +58,11 @@ class Twist2dTest {
|
||||
|
||||
final var twist = start.log(end);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
|
||||
() -> assertEquals(twist.dy, 0.0, kEpsilon),
|
||||
() -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon));
|
||||
var expected = new Twist2d(5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0);
|
||||
assertEquals(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
final var reapplied = start.exp(twist);
|
||||
assertEquals(end, reapplied);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,125 @@
|
||||
// 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.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class Twist3dTest {
|
||||
@Test
|
||||
void testStraightX() {
|
||||
var straight = new Twist3d(5.0, 0.0, 0.0, 0.0, 0.0, 0.0);
|
||||
var straightPose = new Pose3d().exp(straight);
|
||||
|
||||
var expected = new Pose3d(5.0, 0.0, 0.0, new Rotation3d());
|
||||
assertEquals(expected, straightPose);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testStraightY() {
|
||||
var straight = new Twist3d(0.0, 5.0, 0.0, 0.0, 0.0, 0.0);
|
||||
var straightPose = new Pose3d().exp(straight);
|
||||
|
||||
var expected = new Pose3d(0.0, 5.0, 0.0, new Rotation3d());
|
||||
assertEquals(expected, straightPose);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testStraightZ() {
|
||||
var straight = new Twist3d(0.0, 0.0, 5.0, 0.0, 0.0, 0.0);
|
||||
var straightPose = new Pose3d().exp(straight);
|
||||
|
||||
var expected = new Pose3d(0.0, 0.0, 5.0, new Rotation3d());
|
||||
assertEquals(expected, straightPose);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testQuarterCirle() {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
|
||||
|
||||
var quarterCircle = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
|
||||
var quarterCirclePose = new Pose3d().exp(quarterCircle);
|
||||
|
||||
var expected = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
|
||||
assertEquals(expected, quarterCirclePose);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDiagonalNoDtheta() {
|
||||
var diagonal = new Twist3d(2.0, 2.0, 0.0, 0.0, 0.0, 0.0);
|
||||
var diagonalPose = new Pose3d().exp(diagonal);
|
||||
|
||||
var expected = new Pose3d(2.0, 2.0, 0.0, new Rotation3d());
|
||||
assertEquals(expected, diagonalPose);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testEquality() {
|
||||
var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
|
||||
var two = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
|
||||
assertEquals(one, two);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInequality() {
|
||||
var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
|
||||
var two = new Twist3d(5, 1.2, 0, 0.0, 0.0, 3.0);
|
||||
assertNotEquals(one, two);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPose3dLogX() {
|
||||
final var start = new Pose3d();
|
||||
final var end =
|
||||
new Pose3d(0.0, 5.0, 5.0, new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
|
||||
|
||||
final var twist = start.log(end);
|
||||
|
||||
var expected =
|
||||
new Twist3d(0.0, 5.0 / 2.0 * Math.PI, 0.0, Units.degreesToRadians(90.0), 0.0, 0.0);
|
||||
assertEquals(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
final var reapplied = start.exp(twist);
|
||||
assertEquals(end, reapplied);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPose3dLogY() {
|
||||
final var start = new Pose3d();
|
||||
final var end =
|
||||
new Pose3d(5.0, 0.0, 5.0, new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
|
||||
|
||||
final var twist = start.log(end);
|
||||
|
||||
var expected = new Twist3d(0.0, 0.0, 5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0, 0.0);
|
||||
assertEquals(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
final var reapplied = start.exp(twist);
|
||||
assertEquals(end, reapplied);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPose3dLogZ() {
|
||||
final var start = new Pose3d();
|
||||
final var end =
|
||||
new Pose3d(5.0, 5.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
|
||||
|
||||
final var twist = start.log(end);
|
||||
|
||||
var expected = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
|
||||
assertEquals(expected, twist);
|
||||
|
||||
// Make sure computed twist gives back original end pose
|
||||
final var reapplied = start.exp(twist);
|
||||
assertEquals(end, reapplied);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user