Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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

2
.github/workflows/cmake.yml vendored
View File

@@ -91,7 +91,7 @@ jobs:
vcpkgGitCommitId: 78b61582c9e093fda56a01ebb654be15a0033897 # HEAD on 2023-08-6
- name: configure
run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=ON -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=OFF -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
env:
SCCACHE_GHA_ENABLED: "true"

1
vcpkg.json
View File

@@ -3,7 +3,6 @@
"version-string": "latest",
"dependencies": [
"opencv",
"eigen3",
"fmt",
"libuv",
"protobuf"

48
wpimath/src/main/java/edu/wpi/first/math/Vector.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
import org.ejml.simple.SimpleMatrix;
@@ -48,6 +49,16 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
super(other);
}
/**
* Returns an element of the vector at a specified row.
*
* @param row The row that the element is located at.
* @return An element of the vector.
*/
public double get(int row) {
return get(row, 0);
}
@Override
public Vector<R> times(double value) {
return new Vector<>(this.m_storage.scale(value));
@@ -105,12 +116,39 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
* @return The norm.
*/
public double norm() {
double squaredNorm = 0.0;
return Math.sqrt(dot(this));
}
for (int i = 0; i < getNumRows(); ++i) {
squaredNorm += get(i, 0) * get(i, 0);
}
/**
* Returns the unit vector parallel with this vector.
*
* @return The unit vector.
*/
public Vector<R> unit() {
return div(norm());
}
return Math.sqrt(squaredNorm);
/**
* Returns the projection of this vector along another.
*
* @param other The vector to project along.
* @return The projection.
*/
public Vector<R> projection(Vector<R> other) {
return other.times(dot(other)).div(other.dot(other));
}
/**
* Returns the cross product of 3 dimensional vectors a and b.
*
* @param a The vector to cross with b.
* @param b The vector to cross with a.
* @return The cross product of a and b.
*/
public static Vector<N3> cross(Vector<N3> a, Vector<N3> b) {
return VecBuilder.fill(
a.get(1) * b.get(2) - a.get(2) * b.get(1),
a.get(2) * b.get(0) - a.get(0) * b.get(2),
a.get(0) * b.get(1) - a.get(1) * b.get(0));
}
}

22
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.proto.Translation2dProto;
import edu.wpi.first.math.geometry.struct.Translation2dStruct;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -78,6 +81,16 @@ public class Translation2d
this(x.in(Meters), y.in(Meters));
}
/**
* Constructs a Translation2d from the provided translation vector's X and Y components. The
* values are assumed to be in meters.
*
* @param vector The translation vector to represent.
*/
public Translation2d(Vector<N2> vector) {
this(vector.get(0), vector.get(1));
}
/**
* Calculates the distance between two translations in 2D space.
*
@@ -110,6 +123,15 @@ public class Translation2d
return m_y;
}
/**
* Returns a vector representation of this translation.
*
* @return A Vector representation of this translation.
*/
public Vector<N2> toVector() {
return VecBuilder.fill(m_x, m_y);
}
/**
* Returns the norm, or distance from the origin to the translation.
*

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

@@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.proto.Translation3dProto;
import edu.wpi.first.math.geometry.struct.Translation3dStruct;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -83,6 +86,16 @@ public class Translation3d
this(x.in(Meters), y.in(Meters), z.in(Meters));
}
/**
* Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The
* values are assumed to be in meters.
*
* @param vector The translation vector to represent.
*/
public Translation3d(Vector<N3> vector) {
this(vector.get(0), vector.get(1), vector.get(2));
}
/**
* Calculates the distance between two translations in 3D space.
*
@@ -126,6 +139,15 @@ public class Translation3d
return m_z;
}
/**
* Returns a vector representation of this translation.
*
* @return A Vector representation of this translation.
*/
public Vector<N3> toVector() {
return VecBuilder.fill(m_x, m_y, m_z);
}
/**
* Returns the norm, or distance from the origin to the translation.
*

8
wpimath/src/main/native/cpp/geometry/Pose3d.cpp
View File

@@ -36,15 +36,15 @@ Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
} // namespace
Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation)
: m_translation(x, y, z), m_rotation(std::move(rotation)) {}
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
Pose3d::Pose3d(const Pose2d& pose)
: m_translation(pose.X(), pose.Y(), 0_m),
m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {}
: m_translation{pose.X(), pose.Y(), 0_m},
m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
Pose3d Pose3d::operator+(const Transform3d& other) const {
return TransformBy(other);

4
wpimath/src/main/native/cpp/geometry/Transform3d.cpp
View File

@@ -19,11 +19,11 @@ Transform3d::Transform3d(Pose3d initial, Pose3d final) {
}
Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation)
: m_translation(x, y, z), m_rotation(std::move(rotation)) {}
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
Transform3d Transform3d::Inverse() const {
// We are rotating the difference between the translations

3
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -11,6 +11,9 @@
using namespace frc;
Translation2d::Translation2d(const Eigen::Vector2d& vector)
: m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
units::meter_t Translation2d::Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}

5
wpimath/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -19,6 +19,11 @@ Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
m_z = rectangular.Z();
}
Translation3d::Translation3d(const Eigen::Vector3d& vector)
: m_x{units::meter_t{vector.x()}},
m_y{units::meter_t{vector.y()}},
m_z{units::meter_t{vector.z()}} {}
units::meter_t Translation3d::Distance(const Translation3d& other) const {
return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +

4
wpimath/src/main/native/include/frc/geometry/Pose2d.inc
View File

@@ -13,11 +13,11 @@
namespace frc {
constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
Rotation2d rotation)
: m_translation(x, y), m_rotation(std::move(rotation)) {}
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);

8
wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
View File

@@ -12,9 +12,9 @@
namespace frc {
constexpr Rotation2d::Rotation2d(units::angle_unit auto value)
: m_value(value),
m_cos(gcem::cos(value.template convert<units::radian>().value())),
m_sin(gcem::sin(value.template convert<units::radian>().value())) {}
: m_value{value},
m_cos{gcem::cos(value.template convert<units::radian>().value())},
m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
constexpr Rotation2d::Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
@@ -33,7 +33,7 @@ constexpr Rotation2d Rotation2d::operator-() const {
}
constexpr Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d(m_value * scalar);
return Rotation2d{m_value * scalar};
}
constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {

4
wpimath/src/main/native/include/frc/geometry/Transform2d.inc
View File

@@ -14,11 +14,11 @@ namespace frc {
constexpr Transform2d::Transform2d(Translation2d translation,
Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
Rotation2d rotation)
: m_translation(x, y), m_rotation(std::move(rotation)) {}
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
constexpr Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations

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

@@ -7,6 +7,7 @@
#include <initializer_list>
#include <span>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
/**
* Constructs a Translation2d from the provided translation vector's X and Y
* components. The values are assumed to be in meters.
*
* @param vector The translation vector to represent.
*/
explicit Translation2d(const Eigen::Vector2d& vector);
/**
* Calculates the distance between two translations in 2D space.
*
@@ -73,6 +82,13 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr units::meter_t Y() const { return m_y; }
/**
* Returns a vector representation of this translation.
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector2d ToVector() const;
/**
* Returns the norm, or distance from the origin to the translation.
*

8
wpimath/src/main/native/include/frc/geometry/Translation2d.inc
View File

@@ -10,11 +10,15 @@
namespace frc {
constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x(x), m_y(y) {}
: m_x{x}, m_y{y} {}
constexpr Translation2d::Translation2d(units::meter_t distance,
const Rotation2d& angle)
: m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
: m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
constexpr Eigen::Vector2d Translation2d::ToVector() const {
return Eigen::Vector2d{{m_x.value(), m_y.value()}};
}
constexpr Rotation2d Translation2d::Angle() const {
return Rotation2d{m_x.value(), m_y.value()};

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

@@ -4,6 +4,7 @@
#pragma once
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -47,6 +48,14 @@ class WPILIB_DLLEXPORT Translation3d {
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
/**
* Constructs a Translation3d from the provided translation vector's X, Y, and
* Z components. The values are assumed to be in meters.
*
* @param vector The translation vector to represent.
*/
explicit Translation3d(const Eigen::Vector3d& vector);
/**
* Calculates the distance between two translations in 3D space.
*
@@ -80,6 +89,13 @@ class WPILIB_DLLEXPORT Translation3d {
*/
constexpr units::meter_t Z() const { return m_z; }
/**
* Returns a vector representation of this translation.
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector3d ToVector() const;
/**
* Returns the norm, or distance from the origin to the translation.
*

6
wpimath/src/main/native/include/frc/geometry/Translation3d.inc
View File

@@ -13,12 +13,16 @@ namespace frc {
constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x(x), m_y(y), m_z(z) {}
: m_x{x}, m_y{y}, m_z{z} {}
constexpr Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
constexpr Eigen::Vector3d Translation3d::ToVector() const {
return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
}
constexpr Translation3d Translation3d::operator+(
const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};

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());
}
}

10
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -126,6 +126,16 @@ TEST(Translation2dTest, Nearest) {
translation2.Y().value());
}
TEST(Translation2dTest, ToVector) {
const Eigen::Vector2d vec(1.0, 2.0);
const Translation2d translation{vec};
EXPECT_DOUBLE_EQ(vec[0], translation.X().value());
EXPECT_DOUBLE_EQ(vec[1], translation.Y().value());
EXPECT_TRUE(vec == translation.ToVector());
}
TEST(Translation2dTest, Constexpr) {
constexpr Translation2d defaultCtor;
constexpr Translation2d componentCtor{1_m, 2_m};

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

@@ -128,6 +128,17 @@ TEST(Translation3dTest, PolarConstructor) {
EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
}
TEST(Translation3dTest, ToVector) {
const Eigen::Vector3d vec(1.0, 2.0, 3.0);
const Translation3d translation{vec};
EXPECT_DOUBLE_EQ(vec[0], translation.X().value());
EXPECT_DOUBLE_EQ(vec[1], translation.Y().value());
EXPECT_DOUBLE_EQ(vec[2], translation.Z().value());
EXPECT_TRUE(vec == translation.ToVector());
}
TEST(Translation3dTest, Constexpr) {
constexpr Translation3d defaultCtor;
constexpr Translation3d componentCtor{1_m, 2_m, 3_m};