Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add rotation matrix constructor to Rotation3d (#4413)

Browse Source
This commit is contained in:
Tyler Veness
2022-09-17 00:17:30 -07:00
committed by GitHub
parent 9730032866
commit ab1baf4832
9 changed files with 208 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
View File

@@ -7,7 +7,11 @@ 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 static org.junit.jupiter.api.Assertions.assertThrows;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Test;
@@ -36,6 +40,32 @@ class Rotation3dTest {
assertEquals(rot5, rot6);
}
@Test
void testInitRotationMatrix() {
// No rotation
final var R1 = Matrix.eye(Nat.N3());
final var rot1 = new Rotation3d(R1);
assertEquals(new Rotation3d(), rot1);
// 90 degree CCW rotation around z-axis
final var R2 = new Matrix<>(Nat.N3(), Nat.N3());
R2.assignBlock(0, 0, VecBuilder.fill(0.0, 1.0, 0.0));
R2.assignBlock(0, 1, VecBuilder.fill(-1.0, 0.0, 0.0));
R2.assignBlock(0, 2, VecBuilder.fill(0.0, 0.0, 1.0));
final var rot2 = new Rotation3d(R2);
final var expected2 = new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0));
assertEquals(expected2, rot2);
// Matrix that isn't orthogonal
final var R3 =
new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3));
// Matrix that's orthogonal but not special orthogonal
final var R4 = Matrix.eye(Nat.N3()).times(2.0);
assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R4));
}
@Test
void testInitTwoVector() {
@SuppressWarnings("LocalVariableName")