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

[wpimath] Add vector projection and geometry vector conversions (#6343)

Browse Source
This commit is contained in:
Asa Paparo
2024-02-10 13:43:58 -05:00
committed by GitHub
parent 3207795d0d
commit 62cba9a4d3
21 changed files with 234 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
View File

@@ -65,4 +65,40 @@ class VectorTest {
assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, 2.0, 3.0).norm());
assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, -2.0, 3.0).norm());
}
@Test
void testVectorUnit() {
assertEquals(VecBuilder.fill(3.0, 4.0).unit(), VecBuilder.fill(3.0 / 5.0, 4.0 / 5.0));
assertEquals(VecBuilder.fill(8.0, 15.0).unit(), VecBuilder.fill(8.0 / 17.0, 15.0 / 17.0));
}
@Test
void testVectorProjection() {
var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
var res1 = vec1.projection(vec2);
assertEquals(res1.get(0), 1.662, 0.01);
assertEquals(res1.get(1), 2.077, 0.01);
assertEquals(res1.get(2), 2.49, 0.01);
var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
var res2 = vec4.projection(vec3);
assertEquals(res2.get(0), 2.29, 0.01);
assertEquals(res2.get(1), -4.57, 0.01);
assertEquals(res2.get(2), 6.86, 0.01);
}
@Test
void testVectorCross() {
var e1 = VecBuilder.fill(1.0, 0.0, 0.0);
var e2 = VecBuilder.fill(0.0, 1.0, 0.0);
assertEquals(Vector.cross(e1, e2), VecBuilder.fill(0.0, 0.0, 1.0));
var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
var vec2 = VecBuilder.fill(3.0, 4.0, 5.0);
assertEquals(Vector.cross(vec1, vec2), VecBuilder.fill(-2.0, 4.0, -2.0));
}
}

12
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
View File

@@ -8,6 +8,7 @@ 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 java.util.List;
import org.junit.jupiter.api.Test;
@@ -131,4 +132,15 @@ class Translation2dTest {
assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
}
@Test
void testToVector() {
var vec = VecBuilder.fill(1.0, 2.0);
var translation = new Translation2d(vec);
assertEquals(vec.get(0), translation.getX());
assertEquals(vec.get(1), translation.getY());
assertEquals(vec, translation.toVector());
}
}

12
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
View File

@@ -160,4 +160,16 @@ class Translation3dTest {
() -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon),
() -> assertEquals(0.0, two.getZ(), kEpsilon));
}
@Test
void testToVector() {
var vec = VecBuilder.fill(1.0, 2.0, 3.0);
var translation = new Translation3d(vec);
assertEquals(vec.get(0), translation.getX());
assertEquals(vec.get(1), translation.getY());
assertEquals(vec.get(2), translation.getZ());
assertEquals(vec, translation.toVector());
}
}