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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-06-13 22:26:09 -07:00
parent fbee476fd2 05c080328b
commit 3355383fe9
13 changed files with 1031 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java
View File

@@ -13,7 +13,9 @@ import javax.lang.model.element.ExecutableElement;
import javax.lang.model.element.Modifier;
import javax.lang.model.element.TypeElement;
import javax.lang.model.element.VariableElement;
import javax.lang.model.type.TypeKind;
import javax.lang.model.type.TypeMirror;
import javax.lang.model.type.TypeVariable;
/**
* Handles logging of fields or methods. An element that passes the {@link #isLoggable(Element)}
@@ -126,11 +128,15 @@ public abstract class ElementHandler {
}
private static String fieldAccess(VariableElement field) {
if (field.getModifiers().contains(Modifier.PRIVATE)) {
if (!field.getModifiers().contains(Modifier.PUBLIC)) {
// ((com.example.Foo) $fooField.get(object))
// Extra parentheses so cast evaluates before appended methods
// (e.g. when appending .getAsDouble())
return "((" + field.asType() + ") $" + field.getSimpleName() + ".get(object))";
TypeMirror type = field.asType();
if (type.getKind() == TypeKind.TYPEVAR) {
type = ((TypeVariable) type).getUpperBound();
}
return "((" + type.toString() + ") $" + field.getSimpleName() + ".get(object))";
} else {
// object.fooField
return "object." + field.getSimpleName();

105
epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java
View File

@@ -114,42 +114,10 @@ public class LoggerGenerator {
if (config == null) {
config = m_defaultConfig;
}
boolean requireExplicitOptIn = config.strategy() == Logged.Strategy.OPT_IN;
Predicate<Element> notSkipped = LoggerGenerator::isNotSkipped;
Predicate<Element> optedIn =
e -> !requireExplicitOptIn || e.getAnnotation(Logged.class) != null;
List<VariableElement> fieldsToLog;
if (Objects.equals(clazz.getSuperclass().toString(), "java.lang.Record")) {
// Do not log record members - just use the accessor methods
fieldsToLog = List.of();
} else {
fieldsToLog =
clazz.getEnclosedElements().stream()
.filter(e -> e instanceof VariableElement)
.map(e -> (VariableElement) e)
.filter(notSkipped)
.filter(optedIn)
.filter(e -> !e.getModifiers().contains(Modifier.STATIC))
.filter(this::isLoggable)
.toList();
}
List<ExecutableElement> methodsToLog =
clazz.getEnclosedElements().stream()
.filter(e -> e instanceof ExecutableElement)
.map(e -> (ExecutableElement) e)
.filter(notSkipped)
.filter(optedIn)
.filter(e -> !e.getModifiers().contains(Modifier.STATIC))
.filter(e -> e.getModifiers().contains(Modifier.PUBLIC))
.filter(e -> e.getParameters().isEmpty())
.filter(e -> e.getReceiverType() != null)
.filter(kIsBuiltInJavaMethod.negate())
.filter(this::isLoggable)
.filter(e -> !isSimpleGetterMethodForLoggedField(e, fieldsToLog))
.toList();
List<VariableElement> fieldsToLog = new ArrayList<>();
List<ExecutableElement> methodsToLog = new ArrayList<>();
collectLoggables(clazz, fieldsToLog, methodsToLog);
// Validate no name collisions
Map<String, List<Element>> usedNames =
@@ -216,9 +184,9 @@ public class LoggerGenerator {
var loggerFile = m_processingEnv.getFiler().createSourceFile(loggerClassName);
var privateFields =
loggableFields.stream().filter(e -> e.getModifiers().contains(Modifier.PRIVATE)).toList();
boolean requiresVarHandles = !privateFields.isEmpty();
var varHandleFields =
loggableFields.stream().filter(e -> !e.getModifiers().contains(Modifier.PUBLIC)).toList();
boolean requiresVarHandles = !varHandleFields.isEmpty();
try (var out = new PrintWriter(loggerFile.openWriter())) {
if (packageName != null) {
@@ -246,10 +214,10 @@ public class LoggerGenerator {
+ "> {");
if (requiresVarHandles) {
for (var privateField : privateFields) {
for (var varHandleField : varHandleFields) {
// This field needs a VarHandle to access.
// Cache it in the class to avoid lookups
out.println(" private static final VarHandle $" + privateField.getSimpleName() + ";");
out.println(" private static final VarHandle $" + varHandleField.getSimpleName() + ";");
}
out.println();
@@ -262,8 +230,8 @@ public class LoggerGenerator {
+ classReference
+ ", MethodHandles.lookup());");
for (var privateField : privateFields) {
var fieldName = privateField.getSimpleName();
for (var varHandleField : varHandleFields) {
var fieldName = varHandleField.getSimpleName();
out.println(
" $"
+ fieldName
@@ -272,7 +240,7 @@ public class LoggerGenerator {
+ ", \""
+ fieldName
+ "\", "
+ m_processingEnv.getTypeUtils().erasure(privateField.asType())
+ m_processingEnv.getTypeUtils().erasure(varHandleField.asType())
+ ".class);");
}
@@ -347,6 +315,57 @@ public class LoggerGenerator {
}
}
private void collectLoggables(
TypeElement clazz, List<VariableElement> fields, List<ExecutableElement> methods) {
var config = clazz.getAnnotation(Logged.class);
if (config == null) {
config = m_defaultConfig;
}
boolean requireExplicitOptIn = config.strategy() == Logged.Strategy.OPT_IN;
Predicate<Element> notSkipped = LoggerGenerator::isNotSkipped;
Predicate<Element> optedIn =
e -> !requireExplicitOptIn || e.getAnnotation(Logged.class) != null;
List<VariableElement> classFields;
if (Objects.equals(clazz.getSuperclass().toString(), "java.lang.Record")) {
// Do not log record members - just use the accessor methods
classFields = List.of();
} else {
classFields =
clazz.getEnclosedElements().stream()
.filter(e -> e instanceof VariableElement)
.map(e -> (VariableElement) e)
.filter(notSkipped)
.filter(optedIn)
.filter(e -> !e.getModifiers().contains(Modifier.STATIC))
.filter(this::isLoggable)
.toList();
}
fields.addAll(classFields);
methods.addAll(
clazz.getEnclosedElements().stream()
.filter(e -> e instanceof ExecutableElement)
.map(e -> (ExecutableElement) e)
.filter(notSkipped)
.filter(optedIn)
.filter(e -> !e.getModifiers().contains(Modifier.STATIC))
.filter(e -> e.getModifiers().contains(Modifier.PUBLIC))
.filter(e -> e.getParameters().isEmpty())
.filter(e -> e.getReceiverType() != null)
.filter(kIsBuiltInJavaMethod.negate())
.filter(this::isLoggable)
.filter(e -> !isSimpleGetterMethodForLoggedField(e, classFields))
.toList());
TypeElement superclass =
(TypeElement) m_processingEnv.getTypeUtils().asElement(clazz.getSuperclass());
if (superclass != null) {
collectLoggables(superclass, fields, methods);
}
}
private boolean isLoggable(Element element) {
return m_handlers.stream().anyMatch(h -> h.isLoggable(element));
}

654
epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java
View File

File diff suppressed because it is too large Load Diff

19
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -21,6 +21,9 @@ import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.Objects;
/** Represents a 3D pose containing translational and rotational elements. */
@@ -396,6 +399,22 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
}
/**
* Returns the nearest Pose3d from a list of poses. If two or more poses in the list have the same
* distance from this pose, return the one with the closest rotation component.
*
* @param poses The list of poses to find the nearest.
* @return The nearest Pose3d from the list.
*/
public Pose3d nearest(List<Pose3d> poses) {
return Collections.min(
poses,
Comparator.comparing(
(Pose3d other) -> this.getTranslation().getDistance(other.getTranslation()))
.thenComparing(
(Pose3d other) -> this.getRotation().minus(other.getRotation()).getAngle()));
}
@Override
public String toString() {
return String.format("Pose3d(%s, %s)", m_translation, m_rotation);

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -20,6 +20,9 @@ import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.Objects;
/**
@@ -296,6 +299,16 @@ public class Translation3d
return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
}
/**
* Returns the nearest Translation3d from a collection of translations.
*
* @param translations The collection of translations to find the nearest.
* @return The nearest Translation3d from the collection.
*/
public Translation3d nearest(List<Translation3d> translations) {
return Collections.min(translations, Comparator.comparing(this::getDistance));
}
@Override
public String toString() {
return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);

12
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -242,7 +242,11 @@ class WPILIB_DLLEXPORT Pose2d {
}
/**
* Returns the nearest Pose2d from a collection of poses
* Returns the nearest Pose2d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/
@@ -264,7 +268,11 @@ class WPILIB_DLLEXPORT Pose2d {
}
/**
* Returns the nearest Pose2d from a collection of poses
* Returns the nearest Pose2d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/

53
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -4,6 +4,9 @@
#pragma once
#include <algorithm>
#include <initializer_list>
#include <span>
#include <stdexcept>
#include <type_traits>
#include <utility>
@@ -270,6 +273,56 @@ class WPILIB_DLLEXPORT Pose3d {
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
}
/**
* Returns the nearest Pose3d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose3d from the collection.
*/
constexpr Pose3d Nearest(std::span<const Pose3d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& b) {
auto aDistance = this->Translation().Distance(a.Translation());
auto bDistance = this->Translation().Distance(b.Translation());
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
}
return aDistance < bDistance;
});
}
/**
* Returns the nearest Pose3d from a collection of poses.
*
* If two or more poses in the collection have the same distance from this
* pose, return the one with the closest rotation component.
*
* @param poses The collection of poses.
* @return The nearest Pose3d from the collection.
*/
constexpr Pose3d Nearest(std::initializer_list<Pose3d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& b) {
auto aDistance = this->Translation().Distance(a.Translation());
auto bDistance = this->Translation().Distance(b.Translation());
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
}
return aDistance < bDistance;
});
}
private:
Translation3d m_translation;
Rotation3d m_rotation;

18
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -238,10 +238,11 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr Translation2d Nearest(
std::span<const Translation2d> translations) const {
return *std::min_element(translations.begin(), translations.end(),
[this](Translation2d a, Translation2d b) {
return this->Distance(a) < this->Distance(b);
});
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation2d& a, const Translation2d& b) {
return this->Distance(a) < this->Distance(b);
});
}
/**
@@ -251,10 +252,11 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr Translation2d Nearest(
std::initializer_list<Translation2d> translations) const {
return *std::min_element(translations.begin(), translations.end(),
[this](Translation2d a, Translation2d b) {
return this->Distance(a) < this->Distance(b);
});
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation2d& a, const Translation2d& b) {
return this->Distance(a) < this->Distance(b);
});
}
private:

32
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -4,6 +4,10 @@
#pragma once
#include <algorithm>
#include <initializer_list>
#include <span>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -244,6 +248,34 @@ class WPILIB_DLLEXPORT Translation3d {
units::math::abs(m_z - other.m_z) < 1E-9_m;
}
/**
* Returns the nearest Translation3d from a collection of translations
* @param translations The collection of translations.
* @return The nearest Translation3d from the collection.
*/
constexpr Translation3d Nearest(
std::span<const Translation3d> translations) const {
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation3d& a, const Translation3d& b) {
return this->Distance(a) < this->Distance(b);
});
}
/**
* Returns the nearest Translation3d from a collection of translations
* @param translations The collection of translations.
* @return The nearest Translation3d from the collection.
*/
constexpr Translation3d Nearest(
std::initializer_list<Translation3d> translations) const {
return *std::min_element(
translations.begin(), translations.end(),
[this](const Translation3d& a, const Translation3d& b) {
return this->Distance(a) < this->Distance(b);
});
}
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;

67
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -324,4 +324,71 @@ class Pose3dTest {
() -> assertFalse(((Double) twist.rz).isNaN()));
}
}
@Test
void testNearest() {
var origin = Pose3d.kZero;
// Distance sort
// poses are in order of closest to farthest away from the origin at various positions in 3D
// space.
final var pose1 = new Pose3d(1, 0, 0, Rotation3d.kZero);
final var pose2 = new Pose3d(0, 2, 0, Rotation3d.kZero);
final var pose3 = new Pose3d(0, 0, 3, Rotation3d.kZero);
final var pose4 = new Pose3d(2, 2, 2, Rotation3d.kZero);
final var pose5 = new Pose3d(3, 3, 3, Rotation3d.kZero);
assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3)));
// Rotation component sort (when distance is the same)
// Use the same translation to avoid distance differences
final var translation = new Translation3d(1, 0, 0);
final var poseA = new Pose3d(translation, Rotation3d.kZero); // No rotation
final var poseB = new Pose3d(translation, new Rotation3d(Math.toRadians(30), 0, 0));
final var poseC = new Pose3d(translation, new Rotation3d(0, Math.toRadians(45), 0));
final var poseD = new Pose3d(translation, new Rotation3d(0, 0, Math.toRadians(90)));
final var poseE = new Pose3d(translation, new Rotation3d(Math.toRadians(180), 0, 0));
assertEquals(
poseA, new Pose3d(0, 0, 0, Rotation3d.kZero).nearest(List.of(poseA, poseB, poseD)));
assertEquals(
poseB,
new Pose3d(0, 0, 0, new Rotation3d(Math.toRadians(25), 0, 0))
.nearest(List.of(poseB, poseC, poseD)));
assertEquals(
poseC,
new Pose3d(0, 0, 0, new Rotation3d(0, Math.toRadians(50), 0))
.nearest(List.of(poseB, poseC, poseD)));
assertEquals(
poseD,
new Pose3d(0, 0, 0, new Rotation3d(0, 0, Math.toRadians(85)))
.nearest(List.of(poseA, poseC, poseD)));
assertEquals(
poseE,
new Pose3d(0, 0, 0, new Rotation3d(Math.toRadians(170), 0, 0))
.nearest(List.of(poseA, poseD, poseE)));
// Test with complex 3D rotations (combining roll, pitch, yaw)
final var complexPose1 =
new Pose3d(
translation,
new Rotation3d(Math.toRadians(45), Math.toRadians(30), Math.toRadians(60)));
final var complexPose2 =
new Pose3d(
translation,
new Rotation3d(Math.toRadians(90), Math.toRadians(45), Math.toRadians(90)));
final var complexPose3 =
new Pose3d(
translation,
new Rotation3d(Math.toRadians(10), Math.toRadians(15), Math.toRadians(20)));
assertEquals(
complexPose3,
new Pose3d(
0, 0, 0, new Rotation3d(Math.toRadians(5), Math.toRadians(10), Math.toRadians(15)))
.nearest(List.of(complexPose1, complexPose2, complexPose3)));
}
}

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

@@ -11,6 +11,7 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.util.Units;
import java.util.List;
import org.junit.jupiter.api.Test;
class Translation3dTest {
@@ -206,4 +207,22 @@ class Translation3dTest {
assertEquals(vec, translation.toVector());
}
@Test
void testNearest() {
var origin = Translation3d.kZero;
// Distance sort
// translations are in order of closest to farthest away from the origin at various positions
// in 3D space.
final var translation1 = new Translation3d(1, 0, 0);
final var translation2 = new Translation3d(0, 2, 0);
final var translation3 = new Translation3d(0, 0, 3);
final var translation4 = new Translation3d(2, 2, 2);
final var translation5 = new Translation3d(3, 3, 3);
assertEquals(translation3, origin.nearest(List.of(translation5, translation3, translation4)));
assertEquals(translation1, origin.nearest(List.of(translation1, translation2, translation3)));
assertEquals(translation2, origin.nearest(List.of(translation4, translation2, translation3)));
}
}

108
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -221,3 +221,111 @@ TEST(Pose3dTest, TwistNaN) {
EXPECT_FALSE(std::isnan(twist.rz.value()));
}
}
TEST(Pose3dTest, Nearest) {
const Pose3d origin{0_m, 0_m, 0_m, Rotation3d{}};
// Distance sort
// poses are in order of closest to farthest away from the origin at
// various positions in 3D space.
const Pose3d pose1{1_m, 0_m, 0_m, Rotation3d{}};
const Pose3d pose2{0_m, 2_m, 0_m, Rotation3d{}};
const Pose3d pose3{0_m, 0_m, 3_m, Rotation3d{}};
const Pose3d pose4{2_m, 2_m, 2_m, Rotation3d{}};
const Pose3d pose5{3_m, 3_m, 3_m, Rotation3d{}};
EXPECT_DOUBLE_EQ(pose3.X().value(),
origin.Nearest({pose5, pose3, pose4}).X().value());
EXPECT_DOUBLE_EQ(pose3.Y().value(),
origin.Nearest({pose5, pose3, pose4}).Y().value());
EXPECT_DOUBLE_EQ(pose3.Z().value(),
origin.Nearest({pose5, pose3, pose4}).Z().value());
EXPECT_DOUBLE_EQ(pose1.X().value(),
origin.Nearest({pose1, pose2, pose3}).X().value());
EXPECT_DOUBLE_EQ(pose1.Y().value(),
origin.Nearest({pose1, pose2, pose3}).Y().value());
EXPECT_DOUBLE_EQ(pose1.Z().value(),
origin.Nearest({pose1, pose2, pose3}).Z().value());
EXPECT_DOUBLE_EQ(pose2.X().value(),
origin.Nearest({pose4, pose2, pose3}).X().value());
EXPECT_DOUBLE_EQ(pose2.Y().value(),
origin.Nearest({pose4, pose2, pose3}).Y().value());
EXPECT_DOUBLE_EQ(pose2.Z().value(),
origin.Nearest({pose4, pose2, pose3}).Z().value());
// Rotation component sort (when distance is the same)
// Use the same translation to avoid distance differences
const Translation3d translation{1_m, 0_m, 0_m};
const Pose3d poseA{translation, Rotation3d{}}; // No rotation
const Pose3d poseB{translation, Rotation3d{30_deg, 0_deg, 0_deg}};
const Pose3d poseC{translation, Rotation3d{0_deg, 45_deg, 0_deg}};
const Pose3d poseD{translation, Rotation3d{0_deg, 0_deg, 90_deg}};
const Pose3d poseE{translation, Rotation3d{180_deg, 0_deg, 0_deg}};
auto result1 =
Pose3d{0_m, 0_m, 0_m, Rotation3d{}}.Nearest({poseA, poseB, poseD});
EXPECT_DOUBLE_EQ(poseA.Rotation().X().value(),
result1.Rotation().X().value());
EXPECT_DOUBLE_EQ(poseA.Rotation().Y().value(),
result1.Rotation().Y().value());
EXPECT_DOUBLE_EQ(poseA.Rotation().Z().value(),
result1.Rotation().Z().value());
auto result2 =
Pose3d{0_m, 0_m, 0_m, Rotation3d{25_deg, 0_deg, 0_deg}}.Nearest(
{poseB, poseC, poseD});
EXPECT_DOUBLE_EQ(poseB.Rotation().X().value(),
result2.Rotation().X().value());
EXPECT_DOUBLE_EQ(poseB.Rotation().Y().value(),
result2.Rotation().Y().value());
EXPECT_DOUBLE_EQ(poseB.Rotation().Z().value(),
result2.Rotation().Z().value());
auto result3 =
Pose3d{0_m, 0_m, 0_m, Rotation3d{0_deg, 50_deg, 0_deg}}.Nearest(
{poseB, poseC, poseD});
EXPECT_DOUBLE_EQ(poseC.Rotation().X().value(),
result3.Rotation().X().value());
EXPECT_DOUBLE_EQ(poseC.Rotation().Y().value(),
result3.Rotation().Y().value());
EXPECT_DOUBLE_EQ(poseC.Rotation().Z().value(),
result3.Rotation().Z().value());
auto result4 =
Pose3d{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 85_deg}}.Nearest(
{poseA, poseC, poseD});
EXPECT_DOUBLE_EQ(poseD.Rotation().X().value(),
result4.Rotation().X().value());
EXPECT_DOUBLE_EQ(poseD.Rotation().Y().value(),
result4.Rotation().Y().value());
EXPECT_DOUBLE_EQ(poseD.Rotation().Z().value(),
result4.Rotation().Z().value());
auto result5 =
Pose3d{0_m, 0_m, 0_m, Rotation3d{170_deg, 0_deg, 0_deg}}.Nearest(
{poseA, poseD, poseE});
EXPECT_DOUBLE_EQ(poseE.Rotation().X().value(),
result5.Rotation().X().value());
EXPECT_DOUBLE_EQ(poseE.Rotation().Y().value(),
result5.Rotation().Y().value());
EXPECT_DOUBLE_EQ(poseE.Rotation().Z().value(),
result5.Rotation().Z().value());
// Test with complex 3D rotations (combining roll, pitch, yaw)
const Pose3d complexPose1{translation, Rotation3d{45_deg, 30_deg, 60_deg}};
const Pose3d complexPose2{translation, Rotation3d{90_deg, 45_deg, 90_deg}};
const Pose3d complexPose3{translation, Rotation3d{10_deg, 15_deg, 20_deg}};
auto complexResult =
Pose3d{0_m, 0_m, 0_m, Rotation3d{5_deg, 10_deg, 15_deg}}.Nearest(
{complexPose1, complexPose2, complexPose3});
EXPECT_DOUBLE_EQ(complexPose3.Rotation().X().value(),
complexResult.Rotation().X().value());
EXPECT_DOUBLE_EQ(complexPose3.Rotation().Y().value(),
complexResult.Rotation().Y().value());
EXPECT_DOUBLE_EQ(complexPose3.Rotation().Z().value(),
complexResult.Rotation().Z().value());
}

28
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -186,3 +186,31 @@ TEST(Translation3dTest, Constexpr) {
static_assert(projected.X() == 1_m);
static_assert(projected.Y() == 2_m);
}
TEST(Translation3dTest, Nearest) {
const Translation3d origin{0_m, 0_m, 0_m};
// Distance sort
// translations are in order of closest to farthest away from the origin at
// various positions in 3D space.
const Translation3d translation1{1_m, 0_m, 0_m};
const Translation3d translation2{0_m, 2_m, 0_m};
const Translation3d translation3{0_m, 0_m, 3_m};
const Translation3d translation4{2_m, 2_m, 2_m};
const Translation3d translation5{3_m, 3_m, 3_m};
auto nearest1 = origin.Nearest({translation5, translation3, translation4});
EXPECT_DOUBLE_EQ(nearest1.X().value(), translation3.X().value());
EXPECT_DOUBLE_EQ(nearest1.Y().value(), translation3.Y().value());
EXPECT_DOUBLE_EQ(nearest1.Z().value(), translation3.Z().value());
auto nearest2 = origin.Nearest({translation1, translation2, translation3});
EXPECT_DOUBLE_EQ(nearest2.X().value(), translation1.X().value());
EXPECT_DOUBLE_EQ(nearest2.Y().value(), translation1.Y().value());
EXPECT_DOUBLE_EQ(nearest2.Z().value(), translation1.Z().value());
auto nearest3 = origin.Nearest({translation4, translation2, translation3});
EXPECT_DOUBLE_EQ(nearest3.X().value(), translation2.X().value());
EXPECT_DOUBLE_EQ(nearest3.Y().value(), translation2.Y().value());
EXPECT_DOUBLE_EQ(nearest3.Z().value(), translation2.Z().value());
}