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

[wpimath] Add 3D geometry classes (#4175)

Browse Source 
Also clean up 2D geometry documentation.
This commit is contained in:
Tyler Veness
2022-05-06 08:41:23 -07:00
committed by GitHub
parent 708a4bc3bc
commit f20a20f3f1
48 changed files with 4299 additions and 255 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -11,17 +11,14 @@ import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/** Represents a 2d pose containing translational and rotational elements. */
/** Represents a 2D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose2d implements Interpolatable<Pose2d> {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
/**
* Constructs a pose at the origin facing toward the positive X axis. (Translation2d{0, 0} and
* Rotation{0})
*/
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
@@ -42,8 +39,7 @@ public class Pose2d implements Interpolatable<Pose2d> {
}
/**
* Convenience constructors that takes in x and y values directly instead of having to construct a
* Translation2d.
* Constructs a pose with x and y translations instead of a separate Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
@@ -57,8 +53,11 @@ public class Pose2d implements Interpolatable<Pose2d> {
/**
* Transforms the pose by the given transformation and returns the new transformed pose.
*
* <p>The matrix multiplication is as follows [x_new] [cos, -sin, 0][transform.x] [y_new] += [sin,
* cos, 0][transform.y] [t_new] [0, 0, 1][transform.t]
* <pre>
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
* [t_new] [ 0, 0, 1][transform.t]
* </pre>
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
@@ -160,8 +159,8 @@ public class Pose2d implements Interpolatable<Pose2d> {
*
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist2d{0.01, 0.0,
* toRadians(0.5)}
* degrees since the previous pose update, the twist would be Twist2d(0.01, 0.0,
* Units.degreesToRadians(0.5)).
* @return The new pose of the robot.
*/
public Pose2d exp(Twist2d twist) {

326
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java Normal file
View File

@@ -0,0 +1,326 @@
// 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 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.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** Represents a 3D pose containing translational and rotational elements. */
public class Pose3d implements Interpolatable<Pose3d> {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose3d(Translation3d translation, Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs a pose with x, y, and z translations instead of a separate Translation3d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose3d(double x, double y, double z, Rotation3d rotation) {
m_translation = new Translation3d(x, y, z);
m_rotation = rotation;
}
/**
* Transforms the pose by the given transformation and returns the new transformed pose.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose3d plus(Transform3d other) {
return transformBy(other);
}
/**
* Returns the Transform3d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
public Transform3d minus(Pose3d other) {
final var pose = this.relativeTo(other);
return new Transform3d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the pose.
*/
public Translation3d getTranslation() {
return m_translation;
}
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
public double getX() {
return m_translation.getX();
}
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
public double getY() {
return m_translation.getY();
}
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return The rotational component of the pose.
*/
public Rotation3d getRotation() {
return m_rotation;
}
/**
* Transforms the pose by the given transformation and returns the new pose. See + operator for
* the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose3d transformBy(Transform3d other) {
return new Pose3d(
m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
}
/**
* Returns the other pose relative to the current pose.
*
* <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
* get the error between the reference and the current pose.
*
* @param other The pose that is the origin of the new coordinate frame that the current pose will
* be converted into.
* @return The current pose relative to the new origin pose.
*/
public Pose3d relativeTo(Pose3d other) {
var transform = new Transform3d(other, this);
return new Pose3d(transform.getTranslation(), transform.getRotation());
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the previous known field-relative pose with the argument
* being the twist, the user will receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new
* Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
* @return The new pose of the robot.
*/
@SuppressWarnings("LocalVariableName")
public Pose3d exp(Twist3d twist) {
final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz));
final var OmegaSq = Omega.times(Omega);
double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz;
// Get left Jacobian of SO3. See first line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> J;
if (thetaSq < 1E-9 * 1E-9) {
// J = I + 0.5ω
J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
} else {
double theta = Math.sqrt(thetaSq);
// J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
J =
Matrix.eye(Nat.N3())
.plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
.plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
}
// Get translation component
final var translation =
J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
final var transform =
new Transform3d(
new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
new Rotation3d(twist.rx, twist.ry, twist.rz));
return this.plus(transform);
}
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output of a.Log(b), then
* a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
@SuppressWarnings("LocalVariableName")
public Twist3d log(Pose3d end) {
final var transform = end.relativeTo(this);
final var rotVec = transform.getRotation().getQuaternion().toRotationVector();
final var Omega = rotationVectorToMatrix(rotVec);
final var OmegaSq = Omega.times(Omega);
double thetaSq =
rotVec.get(0, 0) * rotVec.get(0, 0)
+ rotVec.get(1, 0) * rotVec.get(1, 0)
+ rotVec.get(2, 0) * rotVec.get(2, 0);
// Get left Jacobian inverse of SO3. See fourth line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> Jinv;
if (thetaSq < 1E-9 * 1E-9) {
// J⁻¹ = I − 0.5ω + 1/12 ω²
Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
} else {
double theta = Math.sqrt(thetaSq);
double halfTheta = 0.5 * theta;
// J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
Jinv =
Matrix.eye(Nat.N3())
.minus(Omega.times(0.5))
.plus(
OmegaSq.times(
(1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
}
// Get dtranslation component
final var dtranslation =
Jinv.times(
new MatBuilder<>(Nat.N3(), Nat.N1())
.fill(transform.getX(), transform.getY(), transform.getZ()));
return new Twist3d(
dtranslation.get(0, 0),
dtranslation.get(1, 0),
dtranslation.get(2, 0),
rotVec.get(0, 0),
rotVec.get(1, 0),
rotVec.get(2, 0));
}
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*
* @return A Pose2d representing this Pose3d projected into the X-Y plane.
*/
public Pose2d toPose2d() {
return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
}
@Override
public String toString() {
return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Pose3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Pose3d) {
return ((Pose3d) obj).m_translation.equals(m_translation)
&& ((Pose3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
@Override
@SuppressWarnings("ParameterName")
public Pose3d interpolate(Pose3d endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
var twist = this.log(endValue);
var scaledTwist =
new Twist3d(
twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
return this.exp(scaledTwist);
}
}
/**
* Applies the hat operator to a rotation vector.
*
* <p>It takes a rotation vector and returns the corresponding matrix representation of the Lie
* algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
private Matrix<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return new MatBuilder<>(Nat.N3(), Nat.N3())
.fill(
0.0,
-rotation.get(2, 0),
rotation.get(1, 0),
rotation.get(2, 0),
0.0,
-rotation.get(0, 0),
-rotation.get(1, 0),
rotation.get(0, 0),
0.0);
}
}

187
wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java Normal file
View File

@@ -0,0 +1,187 @@
// 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 edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
public class Quaternion {
private final double m_r;
private final Vector<N3> m_v;
/** Constructs a quaternion with a default angle of 0 degrees. */
public Quaternion() {
m_r = 1.0;
m_v = VecBuilder.fill(0.0, 0.0, 0.0);
}
/**
* Constructs a quaternion with the given components.
*
* @param w W component of the quaternion.
* @param x X component of the quaternion.
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
public Quaternion(double w, double x, double y, double z) {
m_r = w;
m_v = VecBuilder.fill(x, y, z);
}
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
* @return The quaternion product.
*/
public Quaternion times(Quaternion other) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
final var r1 = m_r;
final var v1 = m_v;
final var r2 = other.m_r;
final var v2 = other.m_v;
final var v1x = v1.get(0, 0);
final var v1y = v1.get(1, 0);
final var v1z = v1.get(2, 0);
final var v2x = v2.get(0, 0);
final var v2y = v2.get(1, 0);
final var v2z = v2.get(2, 0);
var cross =
VecBuilder.fill(v1y * v2z - v2y * v1z, v2x * v1z - v1x * v2z, v1x * v2y - v2x * v1y);
double dot = v1x * v2x + v1y * v2y + v1z * v2z;
final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
return new Quaternion(r1 * r2 - dot, v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
@Override
public String toString() {
return String.format(
"Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
}
/**
* Checks equality between this Quaternion and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Quaternion) {
var other = (Quaternion) obj;
final var r1 = m_r;
final var v1 = m_v;
final var r2 = other.m_r;
final var v2 = other.m_v;
final var v1x = v1.get(0, 0);
final var v1y = v1.get(1, 0);
final var v1z = v1.get(2, 0);
final var v2x = v2.get(0, 0);
final var v2y = v2.get(1, 0);
final var v2z = v2.get(2, 0);
return Math.abs(r1 * r2 + v1x * v2x + v1y * v2y + v1z * v2z) > 1.0 - 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_r, m_v);
}
/**
* Returns the inverse of the quaternion.
*
* @return The inverse quaternion.
*/
public Quaternion inverse() {
return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
}
/**
* Normalizes the quaternion.
*
* @return The normalized quaternion.
*/
public Quaternion normalize() {
double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
if (norm == 0.0) {
return new Quaternion();
} else {
return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
}
}
/**
* Returns W component of the quaternion.
*
* @return W component of the quaternion.
*/
public double getW() {
return m_r;
}
/**
* Returns X component of the quaternion.
*
* @return X component of the quaternion.
*/
public double getX() {
return m_v.get(0, 0);
}
/**
* Returns Y component of the quaternion.
*
* @return Y component of the quaternion.
*/
public double getY() {
return m_v.get(1, 0);
}
/**
* Returns Z component of the quaternion.
*
* @return Z component of the quaternion.
*/
public double getZ() {
return m_v.get(2, 0);
}
/**
* Returns the rotation vector representation of this quaternion.
*
* <p>This is also the log operator of SO(3).
*
* @return The rotation vector representation of this quaternion.
*/
public Vector<N3> toRotationVector() {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
double norm = m_v.normF();
if (norm < 1e-9) {
return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
} else {
if (getW() < 0.0) {
return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
} else {
return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
}
}
}
}

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -13,7 +13,7 @@ import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.util.Units;
import java.util.Objects;
/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
/** A rotation in a 2D coordinate frame represented a point on the unit circle (cosine and sine). */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation2d implements Interpolatable<Rotation2d> {
@@ -29,7 +29,7 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
}
/**
* Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
* Constructs a Rotation2d with the given radian value.
*
* @param value The value of the angle in radians.
*/

259
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java Normal file
View File

@@ -0,0 +1,259 @@
// 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 edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** A rotation in a 3D coordinate. */
public class Rotation3d implements Interpolatable<Rotation3d> {
private Quaternion m_q = new Quaternion();
/** Constructs a Rotation3d with a default angle of 0 degrees. */
public Rotation3d() {}
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
public Rotation3d(Quaternion q) {
m_q = q.normalize();
}
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
* than the body frame.
*
* @param roll The counterclockwise rotation angle around the X axis (roll) in radians.
* @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians.
* @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians.
*/
public Rotation3d(double roll, double pitch, double yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = Math.cos(roll * 0.5);
double sr = Math.sin(roll * 0.5);
double cp = Math.cos(pitch * 0.5);
double sp = Math.sin(pitch * 0.5);
double cy = Math.cos(yaw * 0.5);
double sy = Math.sin(yaw * 0.5);
m_q =
new Quaternion(
cr * cp * cy + sr * sp * sy,
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy);
}
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
* normalized.
*
* @param axis The rotation axis.
* @param angleRadians The rotation around the axis in radians.
*/
public Rotation3d(Vector<N3> axis, double angleRadians) {
double norm = axis.normF();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0));
m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
/**
* Adds two rotations together.
*
* @param other The rotation to add.
* @return The sum of the two rotations.
*/
public Rotation3d plus(Rotation3d other) {
return rotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new rotation.
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation3d minus(Rotation3d other) {
return rotateBy(other.unaryMinus());
}
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
public Rotation3d unaryMinus() {
return new Rotation3d(m_q.inverse());
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation3d.
*/
public Rotation3d times(double scalar) {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.getW() >= 0.0) {
return new Rotation3d(
VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()),
2.0 * scalar * Math.acos(m_q.getW()));
} else {
return new Rotation3d(
VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()),
2.0 * scalar * Math.acos(-m_q.getW()));
}
}
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation3d.
*/
public Rotation3d rotateBy(Rotation3d other) {
return new Rotation3d(other.m_q.times(m_q));
}
/**
* Returns the quaternion representation of the Rotation3d.
*
* @return The quaternion representation of the Rotation3d.
*/
public Quaternion getQuaternion() {
return m_q;
}
/**
* Returns the counterclockwise rotation angle around the X axis (roll) in radians.
*
* @return The counterclockwise rotation angle around the X axis (roll) in radians.
*/
public double getX() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
}
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch) in radians.
*
* @return The counterclockwise rotation angle around the Y axis (pitch) in radians.
*/
public double getY() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
double ratio = 2.0 * (w * y - z * x);
if (Math.abs(ratio) >= 1.0) {
return Math.copySign(Math.PI / 2.0, ratio);
} else {
return Math.asin(ratio);
}
}
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw) in radians.
*
* @return The counterclockwise rotation angle around the Z axis (yaw) in radians.
*/
public double getZ() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
}
/**
* Returns the axis in the axis-angle representation of this rotation.
*
* @return The axis in the axis-angle representation.
*/
public Vector<N3> getAxis() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
if (norm == 0.0) {
return VecBuilder.fill(0.0, 0.0, 0.0);
} else {
return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
}
}
/**
* Returns the angle in radians in the axis-angle representation of this rotation.
*
* @return The angle in radians in the axis-angle representation of this rotation.
*/
public double getAngle() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
return 2.0 * Math.atan2(norm, m_q.getW());
}
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
*
* @return A Rotation2d representing this Rotation3d projected into the X-Y plane.
*/
public Rotation2d toRotation2d() {
return new Rotation2d(getZ());
}
@Override
public String toString() {
return String.format("Rotation3d(%s)", m_q);
}
/**
* Checks equality between this Rotation3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Rotation3d) {
var other = (Rotation3d) obj;
return m_q.equals(other.m_q);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_q);
}
@Override
@SuppressWarnings("ParameterName")
public Rotation3d interpolate(Rotation3d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
}

152
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java Normal file
View File

@@ -0,0 +1,152 @@
// 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 java.util.Objects;
/** Represents a transformation for a Pose3d. */
public class Transform3d {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param last The final pose for the transformation.
*/
public Transform3d(Pose3d initial, Pose3d last) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation =
last.getTranslation()
.minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
m_rotation = last.getRotation().minus(initial.getRotation());
}
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
public Transform3d(Translation3d translation, Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
public Transform3d times(double scalar) {
return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
public Transform3d plus(Transform3d other) {
return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the transform.
*/
public Translation3d getTranslation() {
return m_translation;
}
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
public double getX() {
return m_translation.getX();
}
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
public double getY() {
return m_translation.getY();
}
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
public Rotation3d getRotation() {
return m_rotation;
}
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
public Transform3d inverse() {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return new Transform3d(
getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
getRotation().unaryMinus());
}
@Override
public String toString() {
return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Transform3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Transform3d) {
return ((Transform3d) obj).m_translation.equals(m_translation)
&& ((Transform3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

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

@@ -13,11 +13,10 @@ import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/**
* Represents a translation in 2d space. This object can be used to represent a point or a vector.
* Represents a translation in 2D space. This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes. When the robot is placed on
* the origin, facing toward the X direction, moving forward increases the X, whereas moving to the
* left increases the Y.
* <p>This assumes that you are using conventional mathematical axes. When the robot is at the
* origin facing in the positive X direction, forward is positive X and left is positive Y.
*/
@SuppressWarnings({"ParameterName", "MemberName"})
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -58,10 +57,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Calculates the distance between two translations in 2d space.
* Calculates the distance between two translations in 2D space.
*
* <p>This function uses the pythagorean theorem to calculate the distance. distance = sqrt((x2 -
* x1)^2 + (y2 - y1)^2)
* <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
@@ -73,7 +71,7 @@ public class Translation2d implements Interpolatable<Translation2d> {
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
* @return The X component of the translation.
*/
@JsonProperty
public double getX() {
@@ -83,7 +81,7 @@ public class Translation2d implements Interpolatable<Translation2d> {
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
* @return The Y component of the translation.
*/
@JsonProperty
public double getY() {
@@ -100,13 +98,18 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Applies a rotation to the translation in 2d space.
* Applies a rotation to the translation in 2D space.
*
* <p>This multiplies the translation vector by a counterclockwise rotation matrix of the given
* angle. [x_new] [other.cos, -other.sin][x] [y_new] = [other.sin, other.cos][y]
* angle.
*
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of
* {0, 2}.
* <pre>
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
* </pre>
*
* <p>For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will return a
* Translation2d of &lt;0, 2&gt;.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
@@ -117,9 +120,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Adds two translations in 2d space and returns the sum. This is similar to vector addition.
* Returns the sum of two translations in 2D space.
*
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0}
* <p>For example, Translation3d(1.0, 2.5) + Translation3d(2.0, 5.5) = Translation3d{3.0, 8.0).
*
* @param other The translation to add.
* @return The sum of the translations.
@@ -129,9 +132,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Subtracts the other translation from the other translation and returns the difference.
* Returns the difference between two translations.
*
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = Translation2d{4.0, 2.0}
* <p>For example, Translation2d(5.0, 4.0) - Translation2d(1.0, 2.0) = Translation2d(4.0, 2.0).
*
* @param other The translation to subtract.
* @return The difference between the two translations.
@@ -142,7 +145,7 @@ public class Translation2d implements Interpolatable<Translation2d> {
/**
* Returns the inverse of the current translation. This is equivalent to rotating by 180 degrees,
* flipping the point over both axes, or simply negating both components of the translation.
* flipping the point over both axes, or negating all components of the translation.
*
* @return The inverse of the current translation.
*/
@@ -151,9 +154,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Multiplies the translation by a scalar and returns the new translation.
* Returns the translation multiplied by a scalar.
*
* <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
* <p>For example, Translation2d(2.0, 2.5) * 2 = Translation2d(4.0, 5.0).
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
@@ -163,9 +166,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Divides the translation by a scalar and returns the new translation.
* Returns the translation divided by a scalar.
*
* <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
* <p>For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25).
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.

222
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java Normal file
View File

@@ -0,0 +1,222 @@
// 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 edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/**
* Represents a translation in 3D space. This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes. When the robot is at the
* origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
* positive Z.
*/
@SuppressWarnings({"ParameterName", "MemberName"})
public class Translation3d implements Interpolatable<Translation3d> {
private final double m_x;
private final double m_y;
private final double m_z;
/** Constructs a Translation3d with X, Y, and Z components equal to zero. */
public Translation3d() {
this(0.0, 0.0, 0.0);
}
/**
* Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
public Translation3d(double x, double y, double z) {
m_x = x;
m_y = y;
m_z = z;
}
/**
* Constructs a Translation3d with the provided distance and angle. This is essentially converting
* from polar coordinates to Cartesian coordinates.
*
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
public Translation3d(double distance, Rotation3d angle) {
final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
m_x = rectangular.getX();
m_y = rectangular.getY();
m_z = rectangular.getZ();
}
/**
* Calculates the distance between two translations in 3D space.
*
* <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
*/
public double getDistance(Translation3d other) {
return Math.sqrt(
Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
}
/**
* Returns the X component of the translation.
*
* @return The X component of the translation.
*/
public double getX() {
return m_x;
}
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
public double getY() {
return m_y;
}
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
public double getZ() {
return m_z;
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
public double getNorm() {
return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Applies a rotation to the translation in 3D space.
*
* <p>For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees around the Z axis
* will return a Translation3d of &lt;0, 2, 0&gt;.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation3d rotateBy(Rotation3d other) {
final var p = new Quaternion(0.0, m_x, m_y, m_z);
final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
/**
* Returns a Translation2d representing this Translation3d projected into the X-Y plane.
*
* @return A Translation2d representing this Translation3d projected into the X-Y plane.
*/
public Translation2d toTranslation2d() {
return new Translation2d(m_x, m_y);
}
/**
* Returns the sum of two translations in 3D space.
*
* <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) =
* Translation3d{3.0, 8.0, 11.0).
*
* @param other The translation to add.
* @return The sum of the translations.
*/
public Translation3d plus(Translation3d other) {
return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
}
/**
* Returns the difference between two translations.
*
* <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) =
* Translation3d(4.0, 2.0, 0.0).
*
* @param other The translation to subtract.
* @return The difference between the two translations.
*/
public Translation3d minus(Translation3d other) {
return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
}
/**
* Returns the inverse of the current translation. This is equivalent to negating all components
* of the translation.
*
* @return The inverse of the current translation.
*/
public Translation3d unaryMinus() {
return new Translation3d(-m_x, -m_y, -m_z);
}
/**
* Returns the translation multiplied by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0).
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
*/
public Translation3d times(double scalar) {
return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
}
/**
* Returns the translation divided by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
*/
public Translation3d div(double scalar) {
return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
}
@Override
public String toString() {
return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
}
/**
* Checks equality between this Translation3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Translation3d) {
return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9
&& Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y, m_z);
}
@Override
public Translation3d interpolate(Translation3d endValue, double t) {
return new Translation3d(
MathUtil.interpolate(this.getX(), endValue.getX(), t),
MathUtil.interpolate(this.getY(), endValue.getY(), t),
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
}
}

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
View File

@@ -7,8 +7,8 @@ package edu.wpi.first.math.geometry;
import java.util.Objects;
/**
* A change in distance along arc since the last pose update. We can use ideas from differential
* calculus to create new Pose2ds from a Twist2d and vise versa.
* A change in distance along a 2D arc since the last pose update. We can use ideas from
* differential calculus to create new Pose2ds from a Twist2d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/

86
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java Normal file
View File

@@ -0,0 +1,86 @@
// 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 java.util.Objects;
/**
* A change in distance along a 3D arc since the last pose update. We can use ideas from
* differential calculus to create new Pose3ds from a Twist3d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@SuppressWarnings("MemberName")
public class Twist3d {
/** Linear "dx" component. */
public double dx;
/** Linear "dy" component. */
public double dy;
/** Linear "dz" component. */
public double dz;
/** Rotation vector x component (radians). */
public double rx;
/** Rotation vector y component (radians). */
public double ry;
/** Rotation vector z component (radians). */
public double rz;
public Twist3d() {}
/**
* Constructs a Twist3d with the given values.
*
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dz Change in z direction relative to robot.
* @param rx Rotation vector x component.
* @param ry Rotation vector y component.
* @param rz Rotation vector z component.
*/
public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) {
this.dx = dx;
this.dy = dy;
this.dz = dz;
this.rx = rx;
this.ry = ry;
this.rz = rz;
}
@Override
public String toString() {
return String.format(
"Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)",
dx, dy, dz, rx, ry, rz);
}
/**
* Checks equality between this Twist3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Twist3d) {
return Math.abs(((Twist3d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist3d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist3d) obj).dz - dz) < 1E-9
&& Math.abs(((Twist3d) obj).rx - rx) < 1E-9
&& Math.abs(((Twist3d) obj).ry - ry) < 1E-9
&& Math.abs(((Twist3d) obj).rz - rz) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dz, rx, ry, rz);
}
}

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

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

139
wpimath/src/main/native/cpp/geometry/Pose3d.cpp Normal file
View File

@@ -0,0 +1,139 @@
// 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.
#include "frc/geometry/Pose3d.h"
#include <cmath>
using namespace frc;
namespace {
/**
* Applies the hat operator to a rotation vector.
*
* It takes a rotation vector and returns the corresponding matrix
* representation of the Lie algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
} // namespace
Pose3d::Pose3d(Translation3d translation, Rotation3d 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)) {}
Pose3d Pose3d::operator+(const Transform3d& other) const {
return TransformBy(other);
}
Transform3d Pose3d::operator-(const Pose3d& other) const {
const auto pose = this->RelativeTo(other);
return Transform3d(pose.Translation(), pose.Rotation());
}
bool Pose3d::operator==(const Pose3d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Pose3d::operator!=(const Pose3d& other) const {
return !operator==(other);
}
Pose3d Pose3d::TransformBy(const Transform3d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
m_rotation + other.Rotation()};
}
Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
const Transform3d transform{other, *this};
return {transform.Translation(), transform.Rotation()};
}
Pose3d Pose3d::Exp(const Twist3d& twist) const {
Matrixd<3, 3> Omega = RotationVectorToMatrix(
Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
Matrixd<3, 3> OmegaSq = Omega * Omega;
double thetaSq =
(twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
// Get left Jacobian of SO3. See first line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrixd<3, 3> J;
if (thetaSq < 1E-9 * 1E-9) {
// V = I + 0.5ω
J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
} else {
double theta = std::sqrt(thetaSq);
// J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
(theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
}
// Get translation component
Vectord<3> translation =
J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
const Transform3d transform{Translation3d{units::meter_t{translation(0)},
units::meter_t{translation(1)},
units::meter_t{translation(2)}},
Rotation3d{twist.rx, twist.ry, twist.rz}};
return *this + transform;
}
Twist3d Pose3d::Log(const Pose3d& end) const {
const auto transform = end.RelativeTo(*this);
Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
Matrixd<3, 3> OmegaSq = Omega * Omega;
double thetaSq = rotVec.squaredNorm();
// Get left Jacobian inverse of SO3. See fourth line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrixd<3, 3> Jinv;
if (thetaSq < 1E-9 * 1E-9) {
// J⁻¹ = I − 0.5ω + 1/12 ω²
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
} else {
double theta = std::sqrt(thetaSq);
double halfTheta = 0.5 * theta;
// J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
(1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
thetaSq * OmegaSq;
}
// Get dtranslation component
Vectord<3> dtranslation =
Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
transform.Z().value()};
return Twist3d{
units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}};
}
Pose2d Pose3d::ToPose2d() const {
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
}

80
wpimath/src/main/native/cpp/geometry/Quaternion.cpp Normal file
View File

@@ -0,0 +1,80 @@
// 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.
#include "frc/geometry/Quaternion.h"
using namespace frc;
Quaternion::Quaternion(double w, double x, double y, double z)
: m_r{w}, m_v{x, y, z} {}
Quaternion Quaternion::operator*(const Quaternion& other) const {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
const auto& r1 = m_r;
const auto& v1 = m_v;
const auto& r2 = other.m_r;
const auto& v2 = other.m_v;
Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
v2(0) * v1(2) - v1(0) * v2(2),
v1(0) * v2(1) - v2(0) * v1(1)};
Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
}
bool Quaternion::operator==(const Quaternion& other) const {
return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
}
bool Quaternion::operator!=(const Quaternion& other) const {
return !operator==(other);
}
Quaternion Quaternion::Inverse() const {
return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
}
Quaternion Quaternion::Normalize() const {
double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
if (norm == 0.0) {
return Quaternion{};
} else {
return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
}
}
double Quaternion::W() const {
return m_r;
}
double Quaternion::X() const {
return m_v(0);
}
double Quaternion::Y() const {
return m_v(1);
}
double Quaternion::Z() const {
return m_v(2);
}
Eigen::Vector3d Quaternion::ToRotationVector() const {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
double norm = m_v.norm();
if (norm < 1e-9) {
return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
} else {
if (W() < 0.0) {
return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
} else {
return 2.0 * std::atan2(norm, W()) / norm * m_v;
}
}
}

138
wpimath/src/main/native/cpp/geometry/Rotation3d.cpp Normal file
View File

@@ -0,0 +1,138 @@
// 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.
#include "frc/geometry/Rotation3d.h"
#include <cmath>
#include <wpi/numbers>
#include "units/math.h"
using namespace frc;
Rotation3d::Rotation3d(const Quaternion& q) {
m_q = q.Normalize();
}
Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
units::radian_t yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = units::math::cos(roll * 0.5);
double sr = units::math::sin(roll * 0.5);
double cp = units::math::cos(pitch * 0.5);
double sp = units::math::sin(pitch * 0.5);
double cy = units::math::cos(yaw * 0.5);
double sy = units::math::sin(yaw * 0.5);
m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
}
Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
double norm = axis.norm();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
}
Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
return RotateBy(other);
}
Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
return *this + -other;
}
Rotation3d Rotation3d::operator-() const {
return Rotation3d{m_q.Inverse()};
}
Rotation3d Rotation3d::operator*(double scalar) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.W() >= 0.0) {
return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
} else {
return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
}
}
bool Rotation3d::operator==(const Rotation3d& other) const {
return m_q == other.m_q;
}
bool Rotation3d::operator!=(const Rotation3d& other) const {
return !operator==(other);
}
Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
return Rotation3d{other.m_q * m_q};
}
const Quaternion& Rotation3d::GetQuaternion() const {
return m_q;
}
units::radian_t Rotation3d::X() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return units::radian_t{
std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
}
units::radian_t Rotation3d::Y() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
double ratio = 2.0 * (w * y - z * x);
if (std::abs(ratio) >= 1.0) {
return units::radian_t{std::copysign(wpi::numbers::pi / 2.0, ratio)};
} else {
return units::radian_t{std::asin(ratio)};
}
}
units::radian_t Rotation3d::Z() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return units::radian_t{
std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
}
Vectord<3> Rotation3d::Axis() const {
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
if (norm == 0.0) {
return {0.0, 0.0, 0.0};
} else {
return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
}
}
units::radian_t Rotation3d::Angle() const {
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
}
Rotation2d Rotation3d::ToRotation2d() const {
return Rotation2d{Z()};
}

2
wpimath/src/main/native/cpp/geometry/Transform2d.cpp
View File

@@ -19,7 +19,7 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
}
Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation(translation), m_rotation(rotation) {}
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations

41
wpimath/src/main/native/cpp/geometry/Transform3d.cpp Normal file
View File

@@ -0,0 +1,41 @@
// 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.
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/Pose3d.h"
using namespace frc;
Transform3d::Transform3d(Pose3d initial, Pose3d final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
}
Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Transform3d Transform3d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
Transform3d Transform3d::operator+(const Transform3d& other) const {
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
}
bool Transform3d::operator==(const Transform3d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Transform3d::operator!=(const Transform3d& other) const {
return !operator==(other);
}

71
wpimath/src/main/native/cpp/geometry/Translation3d.cpp Normal file
View File

@@ -0,0 +1,71 @@
// 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.
#include "frc/geometry/Translation3d.h"
#include "units/math.h"
using namespace frc;
Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x(x), m_y(y), m_z(z) {}
Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
m_y = rectangular.Y();
m_z = rectangular.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) +
units::math::pow<2>(other.m_z - m_z));
}
units::meter_t Translation3d::Norm() const {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
units::meter_t{qprime.Z()}};
}
Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
Translation3d Translation3d::operator+(const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
Translation3d Translation3d::operator-(const Translation3d& other) const {
return *this + -other;
}
Translation3d Translation3d::operator-() const {
return {-m_x, -m_y, -m_z};
}
Translation3d Translation3d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
Translation3d Translation3d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation3d::operator==(const Translation3d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m &&
units::math::abs(m_z - other.m_z) < 1E-9_m;
}
bool Translation3d::operator!=(const Translation3d& other) const {
return !operator==(other);
}

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

@@ -17,13 +17,12 @@ class json;
namespace frc {
/**
* Represents a 2d pose containing translational and rotational elements.
* Represents a 2D pose containing translational and rotational elements.
*/
class WPILIB_DLLEXPORT Pose2d {
public:
/**
* Constructs a pose at the origin facing toward the positive X axis.
* (Translation2d{0, 0} and Rotation{0})
*/
constexpr Pose2d() = default;
@@ -36,8 +35,8 @@ class WPILIB_DLLEXPORT Pose2d {
Pose2d(Translation2d translation, Rotation2d rotation);
/**
* Convenience constructors that takes in x and y values directly instead of
* having to construct a Translation2d.
* Constructs a pose with x and y translations instead of a separate
* Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
@@ -49,9 +48,11 @@ class WPILIB_DLLEXPORT Pose2d {
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
* <pre>
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
* [t_new] [0, 0, 1][transform.t]
* [t_new] [ 0, 0, 1][transform.t]
* </pre>
*
* @param other The transform to transform the pose by.
*
@@ -152,7 +153,7 @@ class WPILIB_DLLEXPORT Pose2d {
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
* update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
* update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}.
*
* @return The new pose of the robot.
*/

180
wpimath/src/main/native/include/frc/geometry/Pose3d.h Normal file
View File

@@ -0,0 +1,180 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "Pose2d.h"
#include "Transform3d.h"
#include "Translation3d.h"
#include "Twist3d.h"
namespace frc {
/**
* Represents a 3D pose containing translational and rotational elements.
*/
class WPILIB_DLLEXPORT Pose3d {
public:
/**
* Constructs a pose at the origin facing toward the positive X axis.
*/
constexpr Pose3d() = default;
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(Translation3d translation, Rotation3d rotation);
/**
* Constructs a pose with x, y, and z translations instead of a separate
* Translation3d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation);
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose3d operator+(const Transform3d& other) const;
/**
* Returns the Transform3d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
Transform3d operator-(const Pose3d& other) const;
/**
* Checks equality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Pose3d& other) const;
/**
* Checks inequality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Pose3d& other) const;
/**
* Returns the underlying translation.
*
* @return Reference to the translational component of the pose.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
const Rotation3d& Rotation() const { return m_rotation; }
/**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose3d TransformBy(const Transform3d& other) const;
/**
* Returns the other pose relative to the current pose.
*
* This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
* current pose.
*
* @param other The pose that is the origin of the new coordinate frame that
* the current pose will be converted into.
*
* @return The current pose relative to the new origin pose.
*/
Pose3d RelativeTo(const Pose3d& other) const;
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
* update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
* 0.5_deg}}.
*
* @return The new pose of the robot.
*/
Pose3d Exp(const Twist3d& twist) const;
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
*
* @return The twist that maps this to end.
*/
Twist3d Log(const Pose3d& end) const;
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*/
Pose2d ToPose2d() const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

95
wpimath/src/main/native/include/frc/geometry/Quaternion.h Normal file
View File

@@ -0,0 +1,95 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "frc/EigenCore.h"
namespace frc {
class WPILIB_DLLEXPORT Quaternion {
public:
/**
* Constructs a quaternion with a default angle of 0 degrees.
*/
Quaternion() = default;
/**
* Constructs a quaternion with the given components.
*
* @param w W component of the quaternion.
* @param x X component of the quaternion.
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
Quaternion(double w, double x, double y, double z);
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
*/
Quaternion operator*(const Quaternion& other) const;
/**
* Checks equality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Quaternion& other) const;
/**
* Checks inequality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Quaternion& other) const;
/**
* Returns the inverse of the quaternion.
*/
Quaternion Inverse() const;
/**
* Normalizes the quaternion.
*/
Quaternion Normalize() const;
/**
* Returns W component of the quaternion.
*/
double W() const;
/**
* Returns X component of the quaternion.
*/
double X() const;
/**
* Returns Y component of the quaternion.
*/
double Y() const;
/**
* Returns Z component of the quaternion.
*/
double Z() const;
/**
* Returns the rotation vector representation of this quaternion.
*
* This is also the log operator of SO(3).
*/
Eigen::Vector3d ToRotationVector() const;
private:
double m_r = 1.0;
Eigen::Vector3d m_v{0.0, 0.0, 0.0};
};
} // namespace frc

2
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -15,7 +15,7 @@ class json;
namespace frc {
/**
* A rotation in a 2d coordinate frame represented by a point on the unit circle
* A rotation in a 2D coordinate frame represented by a point on the unit circle
* (cosine and sine).
*/
class WPILIB_DLLEXPORT Rotation2d {

153
wpimath/src/main/native/include/frc/geometry/Rotation3d.h Normal file
View File

@@ -0,0 +1,153 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "Quaternion.h"
#include "Rotation2d.h"
#include "frc/EigenCore.h"
#include "units/angle.h"
namespace frc {
/**
* A rotation in a 3D coordinate frame.
*/
class WPILIB_DLLEXPORT Rotation3d {
public:
/**
* Constructs a Rotation3d with a default angle of 0 degrees.
*/
Rotation3d() = default;
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
explicit Rotation3d(const Quaternion& q);
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* Extrinsic rotations occur in that order around the axes in the fixed global
* frame rather than the body frame.
*
* @param roll The counterclockwise rotation angle around the X axis (roll).
* @param pitch The counterclockwise rotation angle around the Y axis (pitch).
* @param yaw The counterclockwise rotation angle around the Z axis (yaw).
*/
Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis
* doesn't have to be normalized.
*
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
Rotation3d(const Vectord<3>& axis, units::radian_t angle);
/**
* Adds two rotations together.
*
* @param other The rotation to add.
*
* @return The sum of the two rotations.
*/
Rotation3d operator+(const Rotation3d& other) const;
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
*
* @param other The rotation to subtract.
*
* @return The difference between the two rotations.
*/
Rotation3d operator-(const Rotation3d& other) const;
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
Rotation3d operator-() const;
/**
* Multiplies the current rotation by a scalar.
* @param scalar The scalar.
*
* @return The new scaled Rotation3d.
*/
Rotation3d operator*(double scalar) const;
/**
* Checks equality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Rotation3d& other) const;
/**
* Checks inequality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Rotation3d& other) const;
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
*
* @return The new rotated Rotation3d.
*/
Rotation3d RotateBy(const Rotation3d& other) const;
/**
* Returns the quaternion representation of the Rotation3d.
*/
const Quaternion& GetQuaternion() const;
/**
* Returns the counterclockwise rotation angle around the X axis (roll).
*/
units::radian_t X() const;
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch).
*/
units::radian_t Y() const;
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw).
*/
units::radian_t Z() const;
/**
* Returns the axis in the axis-angle representation of this rotation.
*/
Vectord<3> Axis() const;
/**
* Returns the angle in the axis-angle representation of this rotation.
*/
units::radian_t Angle() const;
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.
*/
Rotation2d ToRotation2d() const;
private:
Quaternion m_q;
};
} // namespace frc

121
wpimath/src/main/native/include/frc/geometry/Transform3d.h Normal file
View File

@@ -0,0 +1,121 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "Translation3d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose3d;
/**
* Represents a transformation for a Pose3d.
*/
class WPILIB_DLLEXPORT Transform3d {
public:
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform3d(Pose3d initial, Pose3d final);
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform3d(Translation3d translation, Rotation3d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform3d() = default;
/**
* Returns the translation component of the transformation.
*
* @return Reference to the translational component of the transform.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation3d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform3d Inverse() const;
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator*(double scalar) const {
return Transform3d(m_translation * scalar, m_rotation * scalar);
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
Transform3d operator+(const Transform3d& other) const;
/**
* Checks equality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Transform3d& other) const;
/**
* Checks inequality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Transform3d& other) const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

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

@@ -16,12 +16,12 @@ class json;
namespace frc {
/**
* Represents a translation in 2d space.
* Represents a translation in 2D space.
* This object can be used to represent a point or a vector.
*
* This assumes that you are using conventional mathematical axes.
* When the robot is placed on the origin, facing toward the X direction,
* moving forward increases the X, whereas moving to the left increases the Y.
* When the robot is at the origin facing in the positive X direction, forward
* is positive X and left is positive Y.
*/
class WPILIB_DLLEXPORT Translation2d {
public:
@@ -49,10 +49,9 @@ class WPILIB_DLLEXPORT Translation2d {
Translation2d(units::meter_t distance, const Rotation2d& angle);
/**
* Calculates the distance between two translations in 2d space.
* Calculates the distance between two translations in 2D space.
*
* This function uses the pythagorean theorem to calculate the distance.
* distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
* The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
*
* @param other The translation to compute the distance to.
*
@@ -63,14 +62,14 @@ class WPILIB_DLLEXPORT Translation2d {
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
* @return The X component of the translation.
*/
units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
* @return The Y component of the translation.
*/
units::meter_t Y() const { return m_y; }
@@ -82,16 +81,18 @@ class WPILIB_DLLEXPORT Translation2d {
units::meter_t Norm() const;
/**
* Applies a rotation to the translation in 2d space.
* Applies a rotation to the translation in 2D space.
*
* This multiplies the translation vector by a counterclockwise rotation
* matrix of the given angle.
*
* <pre>
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
* </pre>
*
* For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
* Translation2d of {0, 2}.
* For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will
* return a Translation2d of &lt;0, 2&gt;.
*
* @param other The rotation to rotate the translation by.
*
@@ -100,11 +101,10 @@ class WPILIB_DLLEXPORT Translation2d {
Translation2d RotateBy(const Rotation2d& other) const;
/**
* Adds two translations in 2d space and returns the sum. This is similar to
* vector addition.
* Returns the sum of two translations in 2D space.
*
* For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
* Translation2d{3.0, 8.0}
* For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} =
* Translation3d{3.0, 8.0}.
*
* @param other The translation to add.
*
@@ -113,11 +113,10 @@ class WPILIB_DLLEXPORT Translation2d {
Translation2d operator+(const Translation2d& other) const;
/**
* Subtracts the other translation from the other translation and returns the
* difference.
* Returns the difference between two translations.
*
* For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
* Translation2d{4.0, 2.0}
* Translation2d{4.0, 2.0}.
*
* @param other The translation to subtract.
*
@@ -127,17 +126,17 @@ class WPILIB_DLLEXPORT Translation2d {
/**
* Returns the inverse of the current translation. This is equivalent to
* rotating by 180 degrees, flipping the point over both axes, or simply
* negating both components of the translation.
* rotating by 180 degrees, flipping the point over both axes, or negating all
* components of the translation.
*
* @return The inverse of the current translation.
*/
Translation2d operator-() const;
/**
* Multiplies the translation by a scalar and returns the new translation.
* Returns the translation multiplied by a scalar.
*
* For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
* For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}.
*
* @param scalar The scalar to multiply by.
*
@@ -146,9 +145,9 @@ class WPILIB_DLLEXPORT Translation2d {
Translation2d operator*(double scalar) const;
/**
* Divides the translation by a scalar and returns the new translation.
* Returns the translation divided by a scalar.
*
* For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
* For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}.
*
* @param scalar The scalar to divide by.
*

185
wpimath/src/main/native/include/frc/geometry/Translation3d.h Normal file
View File

@@ -0,0 +1,185 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "Rotation3d.h"
#include "Translation2d.h"
#include "units/length.h"
namespace frc {
/**
* Represents a translation in 3D space.
* This object can be used to represent a point or a vector.
*
* This assumes that you are using conventional mathematical axes. When the
* robot is at the origin facing in the positive X direction, forward is
* positive X, left is positive Y, and up is positive Z.
*/
class WPILIB_DLLEXPORT Translation3d {
public:
/**
* Constructs a Translation3d with X, Y, and Z components equal to zero.
*/
constexpr Translation3d() = default;
/**
* Constructs a Translation3d with the X, Y, and Z components equal to the
* provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
/**
* Constructs a Translation3d with the provided distance and angle. This is
* essentially converting from polar coordinates to Cartesian coordinates.
*
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
/**
* Calculates the distance between two translations in 3D space.
*
* The distance between translations is defined as
* √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
*
* @param other The translation to compute the distance to.
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation3d& other) const;
/**
* Returns the X component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
units::meter_t Y() const { return m_y; }
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t Z() const { return m_z; }
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
/**
* Applies a rotation to the translation in 3D space.
*
* For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
* around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
*
* @param other The rotation to rotate the translation by.
*
* @return The new rotated translation.
*/
Translation3d RotateBy(const Rotation3d& other) const;
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.
*/
Translation2d ToTranslation2d() const;
/**
* Returns the sum of two translations in 3D space.
*
* For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
* Translation3d{3.0, 8.0, 11.0}.
*
* @param other The translation to add.
*
* @return The sum of the translations.
*/
Translation3d operator+(const Translation3d& other) const;
/**
* Returns the difference between two translations.
*
* For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
* Translation3d{4.0, 2.0, 0.0}.
*
* @param other The translation to subtract.
*
* @return The difference between the two translations.
*/
Translation3d operator-(const Translation3d& other) const;
/**
* Returns the inverse of the current translation. This is equivalent to
* negating all components of the translation.
*
* @return The inverse of the current translation.
*/
Translation3d operator-() const;
/**
* Returns the translation multiplied by a scalar.
*
* For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
* 9.0}.
*
* @param scalar The scalar to multiply by.
*
* @return The scaled translation.
*/
Translation3d operator*(double scalar) const;
/**
* Returns the translation divided by a scalar.
*
* For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
* 2.25}.
*
* @param scalar The scalar to divide by.
*
* @return The scaled translation.
*/
Translation3d operator/(double scalar) const;
/**
* Checks equality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Translation3d& other) const;
/**
* Checks inequality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Translation3d& other) const;
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
units::meter_t m_z = 0_m;
};
} // namespace frc

6
wpimath/src/main/native/include/frc/geometry/Twist2d.h
View File

@@ -12,9 +12,9 @@
namespace frc {
/**
* A change in distance along arc since the last pose update. We can use ideas
* from differential calculus to create new Pose2ds from a Twist2d and vise
* versa.
* A change in distance along a 2D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose2ds from a Twist2d and
* vise versa.
*
* A Twist can be used to represent a difference between two poses.
*/

87
wpimath/src/main/native/include/frc/geometry/Twist3d.h Normal file
View File

@@ -0,0 +1,87 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation3d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
/**
* A change in distance along a 3D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose3ds from a Twist3d and
* vise versa.
*
* A Twist can be used to represent a difference between two poses.
*/
struct WPILIB_DLLEXPORT Twist3d {
/**
* Linear "dx" component
*/
units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
units::meter_t dy = 0_m;
/**
* Linear "dz" component
*/
units::meter_t dz = 0_m;
/**
* Rotation vector x component.
*/
units::radian_t rx = 0_rad;
/**
* Rotation vector y component.
*/
units::radian_t ry = 0_rad;
/**
* Rotation vector z component.
*/
units::radian_t rz = 0_rad;
/**
* Checks equality between this Twist3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Twist3d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dz - other.dz) < 1E-9_m &&
units::math::abs(rx - other.rx) < 1E-9_rad &&
units::math::abs(ry - other.ry) < 1E-9_rad &&
units::math::abs(rz - other.rz) < 1E-9_rad;
}
/**
* Checks inequality between this Twist3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Twist3d& other) const { return !operator==(other); }
/**
* Scale this by a given factor.
*
* @param factor The factor by which to scale.
* @return The scaled Twist3d.
*/
Twist3d operator*(double factor) const {
return Twist3d{dx * factor, dy * factor, dz * factor,
rx * factor, ry * factor, rz * factor};
}
};
} // namespace frc

18
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

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

106
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java Normal file
View File

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

90
wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java Normal file
View File

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

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

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

293
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java Normal file
View File

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

69
wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java Normal file
View File

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

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

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

155
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java Normal file
View File

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

35
wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
View File

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

125
wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java Normal file
View File

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

42
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -9,51 +9,47 @@
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Pose2dTest, TransformBy) {
const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
const Pose2d initial{1_m, 2_m, Rotation2d{45_deg}};
const Transform2d transform{Translation2d{5_m, 0_m}, Rotation2d{5_deg}};
const auto transformed = initial + transform;
EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
EXPECT_DOUBLE_EQ(50.0, transformed.Rotation().Degrees().value());
}
TEST(Pose2dTest, RelativeTo) {
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
const Pose2d final{5_m, 5_m, Rotation2d{45.0_deg}};
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
kEpsilon);
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
}
TEST(Pose2dTest, Equality) {
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
const Pose2d b{0_m, 5_m, Rotation2d{43_deg}};
EXPECT_TRUE(a == b);
}
TEST(Pose2dTest, Inequality) {
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
const Pose2d b{0_m, 5_ft, Rotation2d{43_deg}};
EXPECT_TRUE(a != b);
}
TEST(Pose2dTest, Minus) {
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
const Pose2d final{5_m, 5_m, Rotation2d{45_deg}};
const auto transform = final - initial;
EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
}

74
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp Normal file
View File

@@ -0,0 +1,74 @@
// 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.
#include <cmath>
#include "frc/geometry/Pose3d.h"
#include "gtest/gtest.h"
using namespace frc;
TEST(Pose3dTest, TransformBy) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45.0_deg}};
const Transform3d transform{Translation3d{5_m, 0_m, 0_m},
Rotation3d{zAxis, 5_deg}};
const auto transformed = initial + transform;
EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
EXPECT_DOUBLE_EQ(transformed.Rotation().Z().value(),
units::radian_t{50_deg}.value());
}
TEST(Pose3dTest, RelativeTo) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Y().value());
EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Z().value());
}
TEST(Pose3dTest, Equality) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
const Pose3d b{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
EXPECT_TRUE(a == b);
}
TEST(Pose3dTest, Inequality) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
const Pose3d b{0_m, 5_ft, 0_m, Rotation3d{zAxis, 43_deg}};
EXPECT_TRUE(a != b);
}
TEST(Pose3dTest, Minus) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
const auto transform = final - initial;
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
EXPECT_DOUBLE_EQ(0.0, transform.Y().value());
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Z().value());
}
TEST(Pose3dTest, ToPose2d) {
Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}};
Pose2d expected{1_m, 2_m, 40_deg};
EXPECT_EQ(expected, pose.ToPose2d());
}

82
wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp Normal file
View File

@@ -0,0 +1,82 @@
// 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.
#include <wpi/numbers>
#include "frc/geometry/Quaternion.h"
#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/math.h"
using namespace frc;
TEST(QuaternionTest, Init) {
// Identity
Quaternion q1;
EXPECT_DOUBLE_EQ(1.0, q1.W());
EXPECT_DOUBLE_EQ(0.0, q1.X());
EXPECT_DOUBLE_EQ(0.0, q1.Y());
EXPECT_DOUBLE_EQ(0.0, q1.Z());
// Normalized
Quaternion q2{0.5, 0.5, 0.5, 0.5};
EXPECT_DOUBLE_EQ(0.5, q2.W());
EXPECT_DOUBLE_EQ(0.5, q2.X());
EXPECT_DOUBLE_EQ(0.5, q2.Y());
EXPECT_DOUBLE_EQ(0.5, q2.Z());
// Unnormalized
Quaternion q3{0.75, 0.3, 0.4, 0.5};
EXPECT_DOUBLE_EQ(0.75, q3.W());
EXPECT_DOUBLE_EQ(0.3, q3.X());
EXPECT_DOUBLE_EQ(0.4, q3.Y());
EXPECT_DOUBLE_EQ(0.5, q3.Z());
q3 = q3.Normalize();
double norm = std::sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
EXPECT_DOUBLE_EQ(0.75 / norm, q3.W());
EXPECT_DOUBLE_EQ(0.3 / norm, q3.X());
EXPECT_DOUBLE_EQ(0.4 / norm, q3.Y());
EXPECT_DOUBLE_EQ(0.5 / norm, q3.Z());
EXPECT_DOUBLE_EQ(1.0, q3.W() * q3.W() + q3.X() * q3.X() + q3.Y() * q3.Y() +
q3.Z() * q3.Z());
}
TEST(QuaternionTest, Multiply) {
// 90° CCW rotations around each axis
double c = units::math::cos(90_deg / 2.0);
double s = units::math::sin(90_deg / 2.0);
Quaternion xRot{c, s, 0.0, 0.0};
Quaternion yRot{c, 0.0, s, 0.0};
Quaternion zRot{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
auto expected = yRot;
auto actual = zRot * yRot * xRot;
EXPECT_NEAR(expected.W(), actual.W(), 1e-9);
EXPECT_NEAR(expected.X(), actual.X(), 1e-9);
EXPECT_NEAR(expected.Y(), actual.Y(), 1e-9);
EXPECT_NEAR(expected.Z(), actual.Z(), 1e-9);
// Identity
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
0.48507125007266594};
actual = q * q.Inverse();
EXPECT_DOUBLE_EQ(1.0, actual.W());
EXPECT_DOUBLE_EQ(0.0, actual.X());
EXPECT_DOUBLE_EQ(0.0, actual.Y());
EXPECT_DOUBLE_EQ(0.0, actual.Z());
}
TEST(QuaternionTest, Inverse) {
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
0.48507125007266594};
auto inv = q.Inverse();
EXPECT_DOUBLE_EQ(q.W(), inv.W());
EXPECT_DOUBLE_EQ(-q.X(), inv.X());
EXPECT_DOUBLE_EQ(-q.Y(), inv.Y());
EXPECT_DOUBLE_EQ(-q.Z(), inv.Z());
}

50
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -11,58 +11,56 @@
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Rotation2dTest, RadiansToDegrees) {
const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)};
const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)};
const Rotation2d rot1{units::radian_t{wpi::numbers::pi / 3.0}};
const Rotation2d rot2{units::radian_t{wpi::numbers::pi / 4.0}};
EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon);
EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon);
EXPECT_DOUBLE_EQ(60.0, rot1.Degrees().value());
EXPECT_DOUBLE_EQ(45.0, rot2.Degrees().value());
}
TEST(Rotation2dTest, DegreesToRadians) {
const auto rot1 = Rotation2d(45.0_deg);
const auto rot2 = Rotation2d(30.0_deg);
const auto rot1 = Rotation2d{45_deg};
const auto rot2 = Rotation2d{30_deg};
EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon);
EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon);
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot1.Radians().value());
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 6.0, rot2.Radians().value());
}
TEST(Rotation2dTest, RotateByFromZero) {
const Rotation2d zero;
auto sum = zero + Rotation2d(90.0_deg);
auto rotated = zero + Rotation2d(90_deg);
EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon);
EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon);
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 2.0, rotated.Radians().value());
EXPECT_DOUBLE_EQ(90.0, rotated.Degrees().value());
}
TEST(Rotation2dTest, RotateByNonZero) {
auto rot = Rotation2d(90.0_deg);
rot = rot + Rotation2d(30.0_deg);
auto rot = Rotation2d{90_deg};
rot = rot + Rotation2d{30_deg};
EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon);
EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
}
TEST(Rotation2dTest, Minus) {
const auto rot1 = Rotation2d(70.0_deg);
const auto rot2 = Rotation2d(30.0_deg);
const auto rot1 = Rotation2d{70_deg};
const auto rot2 = Rotation2d{30_deg};
EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon);
EXPECT_DOUBLE_EQ(40.0, (rot1 - rot2).Degrees().value());
}
TEST(Rotation2dTest, Equality) {
const auto rot1 = Rotation2d(43_deg);
const auto rot2 = Rotation2d(43_deg);
auto rot1 = Rotation2d{43_deg};
auto rot2 = Rotation2d{43_deg};
EXPECT_EQ(rot1, rot2);
const auto rot3 = Rotation2d(-180_deg);
const auto rot4 = Rotation2d(180_deg);
EXPECT_EQ(rot3, rot4);
rot1 = Rotation2d{-180_deg};
rot2 = Rotation2d{180_deg};
EXPECT_EQ(rot1, rot2);
}
TEST(Rotation2dTest, Inequality) {
const auto rot1 = Rotation2d(43_deg);
const auto rot2 = Rotation2d(43.5_deg);
const auto rot1 = Rotation2d{43_deg};
const auto rot2 = Rotation2d{43.5_deg};
EXPECT_NE(rot1, rot2);
}

246
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp Normal file
View File

@@ -0,0 +1,246 @@
// 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.
#include <cmath>
#include <wpi/MathExtras.h>
#include <wpi/numbers>
#include "frc/geometry/Rotation3d.h"
#include "gtest/gtest.h"
using namespace frc;
TEST(Rotation3dTest, Init) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Rotation3d rot1{xAxis, units::radian_t{wpi::numbers::pi / 3}};
const Rotation3d rot2{units::radian_t{wpi::numbers::pi / 3}, 0_rad, 0_rad};
EXPECT_EQ(rot1, rot2);
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
const Rotation3d rot3{yAxis, units::radian_t{wpi::numbers::pi / 3}};
const Rotation3d rot4{0_rad, units::radian_t{wpi::numbers::pi / 3}, 0_rad};
EXPECT_EQ(rot3, rot4);
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Rotation3d rot5{zAxis, units::radian_t{wpi::numbers::pi / 3}};
const Rotation3d rot6{0_rad, 0_rad, units::radian_t{wpi::numbers::pi / 3}};
EXPECT_EQ(rot5, rot6);
}
TEST(Rotation3dTest, RadiansToDegrees) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Rotation3d rot1{zAxis, units::radian_t{wpi::numbers::pi / 3}};
EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
EXPECT_DOUBLE_EQ(units::radian_t{60_deg}.value(), rot1.Z().value());
const Rotation3d rot2{zAxis, units::radian_t{wpi::numbers::pi / 4}};
EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
EXPECT_DOUBLE_EQ(units::radian_t{45_deg}.value(), rot2.Z().value());
}
TEST(Rotation3dTest, DegreesToRadians) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const auto rot1 = Rotation3d{zAxis, 45_deg};
EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot1.Z().value());
const auto rot2 = Rotation3d{zAxis, 30_deg};
EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 6.0, rot2.Z().value());
}
TEST(Rotation3dTest, RotationLoop) {
Rotation3d rot;
rot = rot + Rotation3d{90_deg, 0_deg, 0_deg};
Rotation3d expected{90_deg, 0_deg, 0_deg};
EXPECT_EQ(expected, rot);
rot = rot + Rotation3d{0_deg, 90_deg, 0_deg};
expected = Rotation3d{
{1.0 / std::sqrt(3), 1.0 / std::sqrt(3), -1.0 / std::sqrt(3)}, 120_deg};
EXPECT_EQ(expected, rot);
rot = rot + Rotation3d{0_deg, 0_deg, 90_deg};
expected = Rotation3d{0_deg, 90_deg, 0_deg};
EXPECT_EQ(expected, rot);
rot = rot + Rotation3d{0_deg, -90_deg, 0_deg};
EXPECT_EQ(Rotation3d{}, rot);
}
TEST(Rotation3dTest, RotateByFromZeroX) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Rotation3d zero;
auto rotated = zero + Rotation3d{xAxis, 90_deg};
Rotation3d expected{xAxis, 90_deg};
EXPECT_EQ(expected, rotated);
}
TEST(Rotation3dTest, RotateByFromZeroY) {
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
const Rotation3d zero;
auto rotated = zero + Rotation3d{yAxis, 90_deg};
Rotation3d expected{yAxis, 90_deg};
EXPECT_EQ(expected, rotated);
}
TEST(Rotation3dTest, RotateByFromZeroZ) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Rotation3d zero;
auto rotated = zero + Rotation3d{zAxis, 90_deg};
Rotation3d expected{zAxis, 90_deg};
EXPECT_EQ(expected, rotated);
}
TEST(Rotation3dTest, RotateByNonZeroX) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
auto rot = Rotation3d{xAxis, 90_deg};
rot = rot + Rotation3d{xAxis, 30_deg};
Rotation3d expected{xAxis, 120_deg};
EXPECT_EQ(expected, rot);
}
TEST(Rotation3dTest, RotateByNonZeroY) {
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
auto rot = Rotation3d{yAxis, 90_deg};
rot = rot + Rotation3d{yAxis, 30_deg};
Rotation3d expected{yAxis, 120_deg};
EXPECT_EQ(expected, rot);
}
TEST(Rotation3dTest, RotateByNonZeroZ) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
auto rot = Rotation3d{zAxis, 90_deg};
rot = rot + Rotation3d{zAxis, 30_deg};
Rotation3d expected{zAxis, 120_deg};
EXPECT_EQ(expected, rot);
}
TEST(Rotation3dTest, Minus) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const auto rot1 = Rotation3d{zAxis, 70_deg};
const auto rot2 = Rotation3d{zAxis, 30_deg};
EXPECT_DOUBLE_EQ(40.0, units::degree_t{(rot1 - rot2).Z()}.value());
}
TEST(Rotation3dTest, AxisAngle) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
Rotation3d rot1{xAxis, 90_deg};
EXPECT_EQ(xAxis, rot1.Axis());
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 2.0, rot1.Angle().value());
Rotation3d rot2{yAxis, 45_deg};
EXPECT_EQ(yAxis, rot2.Axis());
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot2.Angle().value());
Rotation3d rot3{zAxis, 60_deg};
EXPECT_EQ(zAxis, rot3.Axis());
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 3.0, rot3.Angle().value());
}
TEST(Rotation3dTest, ToRotation2d) {
Rotation3d rotation{20_deg, 30_deg, 40_deg};
Rotation2d expected{40_deg};
EXPECT_EQ(expected, rotation.ToRotation2d());
}
TEST(Rotation3dTest, Equality) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const auto rot1 = Rotation3d{zAxis, 43_deg};
const auto rot2 = Rotation3d{zAxis, 43_deg};
EXPECT_EQ(rot1, rot2);
const auto rot3 = Rotation3d{zAxis, -180_deg};
const auto rot4 = Rotation3d{zAxis, 180_deg};
EXPECT_EQ(rot3, rot4);
}
TEST(Rotation3dTest, Inequality) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const auto rot1 = Rotation3d{zAxis, 43_deg};
const auto rot2 = Rotation3d{zAxis, 43.5_deg};
EXPECT_NE(rot1, rot2);
}
TEST(Rotation3dTest, Interpolate) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
// 50 + (70 - 50) * 0.5 = 60
auto rot1 = Rotation3d{xAxis, 50_deg};
auto rot2 = Rotation3d{xAxis, 70_deg};
auto interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
// -160 minus half distance between 170 and -160 (15) = -175
rot1 = Rotation3d{xAxis, 170_deg};
rot2 = Rotation3d{xAxis, -160_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
// 50 + (70 - 50) * 0.5 = 60
rot1 = Rotation3d{yAxis, 50_deg};
rot2 = Rotation3d{yAxis, 70_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
// -160 plus half distance between 170 and -160 (165) = 5
rot1 = Rotation3d{yAxis, 170_deg};
rot2 = Rotation3d{yAxis, -160_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(-5.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.Z()}.value());
// 50 + (70 - 50) * 0.5 = 60
rot1 = Rotation3d{zAxis, 50_deg};
rot2 = Rotation3d{zAxis, 70_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Z()}.value());
// -160 minus half distance between 170 and -160 (15) = -175
rot1 = Rotation3d{zAxis, 170_deg};
rot2 = Rotation3d{zAxis, -160_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value());
}

22
wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
View File

@@ -12,8 +12,6 @@
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Transform2dTest, Inverse) {
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform{{5_m, 0_m}, 5_deg};
@@ -21,10 +19,10 @@ TEST(Transform2dTest, Inverse) {
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon);
EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon);
EXPECT_NEAR(initial.Rotation().Degrees().value(),
untransformed.Rotation().Degrees().value(), kEpsilon);
EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9);
EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value());
EXPECT_DOUBLE_EQ(initial.Rotation().Degrees().value(),
untransformed.Rotation().Degrees().value());
}
TEST(Transform2dTest, Composition) {
@@ -35,10 +33,10 @@ TEST(Transform2dTest, Composition) {
auto transformedSeparate = initial + transform1 + transform2;
auto transformedCombined = initial + (transform1 + transform2);
EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(),
kEpsilon);
EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(),
kEpsilon);
EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(),
transformedCombined.Rotation().Degrees().value(), kEpsilon);
EXPECT_DOUBLE_EQ(transformedSeparate.X().value(),
transformedCombined.X().value());
EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(),
transformedCombined.Y().value());
EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Degrees().value(),
transformedCombined.Rotation().Degrees().value());
}

49
wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp Normal file
View File

@@ -0,0 +1,49 @@
// 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.
#include <cmath>
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/Translation3d.h"
#include "gtest/gtest.h"
using namespace frc;
TEST(Transform3dTest, Inverse) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}};
const Transform3d transform{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}};
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9);
EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value());
EXPECT_DOUBLE_EQ(initial.Z().value(), untransformed.Z().value());
EXPECT_DOUBLE_EQ(initial.Rotation().Z().value(),
untransformed.Rotation().Z().value());
}
TEST(Transform3dTest, Composition) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}};
const Transform3d transform1{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}};
const Transform3d transform2{{0_m, 2_m, 0_m}, Rotation3d{zAxis, 5_deg}};
auto transformedSeparate = initial + transform1 + transform2;
auto transformedCombined = initial + (transform1 + transform2);
EXPECT_DOUBLE_EQ(transformedSeparate.X().value(),
transformedCombined.X().value());
EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(),
transformedCombined.Y().value());
EXPECT_DOUBLE_EQ(transformedSeparate.Z().value(),
transformedCombined.Z().value());
EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Z().value(),
transformedCombined.Rotation().Z().value());
}

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

@@ -9,69 +9,67 @@
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Translation2dTest, Sum) {
const Translation2d one{1.0_m, 3.0_m};
const Translation2d two{2.0_m, 5.0_m};
const Translation2d one{1_m, 3_m};
const Translation2d two{2_m, 5_m};
const auto sum = one + two;
EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon);
EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon);
EXPECT_DOUBLE_EQ(3.0, sum.X().value());
EXPECT_DOUBLE_EQ(8.0, sum.Y().value());
}
TEST(Translation2dTest, Difference) {
const Translation2d one{1.0_m, 3.0_m};
const Translation2d two{2.0_m, 5.0_m};
const Translation2d one{1_m, 3_m};
const Translation2d two{2_m, 5_m};
const auto difference = one - two;
EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
EXPECT_DOUBLE_EQ(-1.0, difference.X().value());
EXPECT_DOUBLE_EQ(-2.0, difference.Y().value());
}
TEST(Translation2dTest, RotateBy) {
const Translation2d another{3.0_m, 0.0_m};
const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
const Translation2d another{3_m, 0_m};
const auto rotated = another.RotateBy(Rotation2d(90_deg));
EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon);
EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
}
TEST(Translation2dTest, Multiplication) {
const Translation2d original{3.0_m, 5.0_m};
const Translation2d original{3_m, 5_m};
const auto mult = original * 3;
EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
EXPECT_DOUBLE_EQ(9.0, mult.X().value());
EXPECT_DOUBLE_EQ(15.0, mult.Y().value());
}
TEST(Translation2dTest, Division) {
const Translation2d original{3.0_m, 5.0_m};
const Translation2d original{3_m, 5_m};
const auto div = original / 2;
EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
EXPECT_DOUBLE_EQ(1.5, div.X().value());
EXPECT_DOUBLE_EQ(2.5, div.Y().value());
}
TEST(Translation2dTest, Norm) {
const Translation2d one{3.0_m, 5.0_m};
EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon);
const Translation2d one{3_m, 5_m};
EXPECT_DOUBLE_EQ(std::hypot(3.0, 5.0), one.Norm().value());
}
TEST(Translation2dTest, Distance) {
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon);
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), one.Distance(two).value());
}
TEST(Translation2dTest, UnaryMinus) {
const Translation2d original{-4.5_m, 7_m};
const auto inverted = -original;
EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
EXPECT_DOUBLE_EQ(4.5, inverted.X().value());
EXPECT_DOUBLE_EQ(-7.0, inverted.Y().value());
}
TEST(Translation2dTest, Equality) {
@@ -87,11 +85,11 @@ TEST(Translation2dTest, Inequality) {
}
TEST(Translation2dTest, PolarConstructor) {
Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
Translation2d one{std::sqrt(2) * 1_m, Rotation2d{45_deg}};
EXPECT_DOUBLE_EQ(1.0, one.X().value());
EXPECT_DOUBLE_EQ(1.0, one.Y().value());
Translation2d two{2_m, Rotation2d(60_deg)};
EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
Translation2d two{2_m, Rotation2d{60_deg}};
EXPECT_DOUBLE_EQ(1.0, two.X().value());
EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
}

128
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp Normal file
View File

@@ -0,0 +1,128 @@
// 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.
#include <cmath>
#include "frc/geometry/Translation3d.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Translation3dTest, Sum) {
const Translation3d one{1_m, 3_m, 5_m};
const Translation3d two{2_m, 5_m, 8_m};
const auto sum = one + two;
EXPECT_NEAR(3.0, sum.X().value(), kEpsilon);
EXPECT_NEAR(8.0, sum.Y().value(), kEpsilon);
EXPECT_NEAR(13.0, sum.Z().value(), kEpsilon);
}
TEST(Translation3dTest, Difference) {
const Translation3d one{1_m, 3_m, 5_m};
const Translation3d two{2_m, 5_m, 8_m};
const auto difference = one - two;
EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
EXPECT_NEAR(difference.Z().value(), -3.0, kEpsilon);
}
TEST(Translation3dTest, RotateBy) {
Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Translation3d translation{1_m, 2_m, 3_m};
const auto rotated1 = translation.RotateBy(Rotation3d{xAxis, 90_deg});
EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(rotated1.Y().value(), -3.0, kEpsilon);
EXPECT_NEAR(rotated1.Z().value(), 2.0, kEpsilon);
const auto rotated2 = translation.RotateBy(Rotation3d{yAxis, 90_deg});
EXPECT_NEAR(rotated2.X().value(), 3.0, kEpsilon);
EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
EXPECT_NEAR(rotated2.Z().value(), -1.0, kEpsilon);
const auto rotated3 = translation.RotateBy(Rotation3d{zAxis, 90_deg});
EXPECT_NEAR(rotated3.X().value(), -2.0, kEpsilon);
EXPECT_NEAR(rotated3.Y().value(), 1.0, kEpsilon);
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
}
TEST(Translation3dTest, ToTranslation2d) {
Translation3d translation{1_m, 2_m, 3_m};
Translation2d expected{1_m, 2_m};
EXPECT_EQ(expected, translation.ToTranslation2d());
}
TEST(Translation3dTest, Multiplication) {
const Translation3d original{3_m, 5_m, 7_m};
const auto mult = original * 3;
EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
EXPECT_NEAR(mult.Z().value(), 21.0, kEpsilon);
}
TEST(Translation3dTest, Division) {
const Translation3d original{3_m, 5_m, 7_m};
const auto div = original / 2;
EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
EXPECT_NEAR(div.Z().value(), 3.5, kEpsilon);
}
TEST(Translation3dTest, Norm) {
const Translation3d one{3_m, 5_m, 7_m};
EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5, 7), kEpsilon);
}
TEST(Translation3dTest, Distance) {
const Translation3d one{1_m, 1_m, 1_m};
const Translation3d two{6_m, 6_m, 6_m};
EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(3), kEpsilon);
}
TEST(Translation3dTest, UnaryMinus) {
const Translation3d original{-4.5_m, 7_m, 9_m};
const auto inverted = -original;
EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
EXPECT_NEAR(inverted.Z().value(), -9, kEpsilon);
}
TEST(Translation3dTest, Equality) {
const Translation3d one{9_m, 5.5_m, 3.5_m};
const Translation3d two{9_m, 5.5_m, 3.5_m};
EXPECT_TRUE(one == two);
}
TEST(Translation3dTest, Inequality) {
const Translation3d one{9_m, 5.5_m, 3.5_m};
const Translation3d two{9_m, 5.7_m, 3.5_m};
EXPECT_TRUE(one != two);
}
TEST(Translation3dTest, PolarConstructor) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
Translation3d one{std::sqrt(2) * 1_m, Rotation3d(zAxis, 45_deg)};
EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
EXPECT_NEAR(one.Z().value(), 0.0, kEpsilon);
Translation3d two{2_m, Rotation3d(zAxis, 60_deg)};
EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
}

54
wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -11,55 +11,57 @@
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Twist2dTest, Straight) {
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
const Twist2d straight{5_m, 0_m, 0_rad};
const auto straightPose = Pose2d().Exp(straight);
EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon);
EXPECT_DOUBLE_EQ(5.0, straightPose.X().value());
EXPECT_DOUBLE_EQ(0.0, straightPose.Y().value());
EXPECT_DOUBLE_EQ(0.0, straightPose.Rotation().Radians().value());
}
TEST(Twist2dTest, QuarterCircle) {
const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m,
units::radian_t(wpi::numbers::pi / 2.0)};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
const Twist2d quarterCircle{5_m / 2.0 * wpi::numbers::pi, 0_m,
units::radian_t{wpi::numbers::pi / 2.0}};
const auto quarterCirclePose = Pose2d{}.Exp(quarterCircle);
EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon);
EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.X().value());
EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.Y().value());
EXPECT_DOUBLE_EQ(90.0, quarterCirclePose.Rotation().Degrees().value());
}
TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
const auto diagonalPose = Pose2d().Exp(diagonal);
const Twist2d diagonal{2_m, 2_m, 0_deg};
const auto diagonalPose = Pose2d{}.Exp(diagonal);
EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
EXPECT_DOUBLE_EQ(2.0, diagonalPose.X().value());
EXPECT_DOUBLE_EQ(2.0, diagonalPose.Y().value());
EXPECT_DOUBLE_EQ(0.0, diagonalPose.Rotation().Degrees().value());
}
TEST(Twist2dTest, Equality) {
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
const Twist2d one{5_m, 1_m, 3_rad};
const Twist2d two{5_m, 1_m, 3_rad};
EXPECT_TRUE(one == two);
}
TEST(Twist2dTest, Inequality) {
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
const Twist2d one{5_m, 1_m, 3_rad};
const Twist2d two{5_m, 1.2_m, 3_rad};
EXPECT_TRUE(one != two);
}
TEST(Twist2dTest, Pose2dLog) {
const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
const Pose2d start{};
const Pose2d end{5_m, 5_m, Rotation2d{90_deg}};
const Pose2d start;
const auto twist = start.Log(end);
EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
Twist2d expected{units::meter_t{5.0 / 2.0 * wpi::numbers::pi}, 0_m,
units::radian_t{wpi::numbers::pi / 2.0}};
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
EXPECT_EQ(end, reapplied);
}

118
wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp Normal file
View File

@@ -0,0 +1,118 @@
// 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.
#include <cmath>
#include <wpi/numbers>
#include "frc/geometry/Pose3d.h"
#include "gtest/gtest.h"
using namespace frc;
TEST(Twist3dTest, StraightX) {
const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d().Exp(straight);
Pose3d expected{5_m, 0_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
}
TEST(Twist3dTest, StraightY) {
const Twist3d straight{0_m, 5_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d().Exp(straight);
Pose3d expected{0_m, 5_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
}
TEST(Twist3dTest, StraightZ) {
const Twist3d straight{0_m, 0_m, 5_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d().Exp(straight);
Pose3d expected{0_m, 0_m, 5_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
}
TEST(Twist3dTest, QuarterCircle) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Twist3d quarterCircle{
5_m / 2.0 * wpi::numbers::pi, 0_m, 0_m, 0_rad, 0_rad,
units::radian_t(wpi::numbers::pi / 2.0)};
const auto quarterCirclePose = Pose3d().Exp(quarterCircle);
Pose3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
EXPECT_EQ(expected, quarterCirclePose);
}
TEST(Twist3dTest, DiagonalNoDtheta) {
const Twist3d diagonal{2_m, 2_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto diagonalPose = Pose3d().Exp(diagonal);
Pose3d expected{2_m, 2_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, diagonalPose);
}
TEST(Twist3dTest, Equality) {
const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
const Twist3d two{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
EXPECT_TRUE(one == two);
}
TEST(Twist3dTest, Inequality) {
const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
const Twist3d two{5_m, 1.2_m, 0_m, 0_rad, 0_rad, 3_rad};
EXPECT_TRUE(one != two);
}
TEST(Twist3dTest, Pose3dLogX) {
const Pose3d end{0_m, 5_m, 5_m, Rotation3d{90_deg, 0_deg, 0_deg}};
const Pose3d start;
const auto twist = start.Log(end);
Twist3d expected{0_m, units::meter_t{5.0 / 2.0 * wpi::numbers::pi},
0_m, 90_deg,
0_deg, 0_deg};
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
EXPECT_EQ(end, reapplied);
}
TEST(Twist3dTest, Pose3dLogY) {
const Pose3d end{5_m, 0_m, 5_m, Rotation3d{0_deg, 90_deg, 0_deg}};
const Pose3d start;
const auto twist = start.Log(end);
Twist3d expected{0_m, 0_m, units::meter_t{5.0 / 2.0 * wpi::numbers::pi},
0_deg, 90_deg, 0_deg};
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
EXPECT_EQ(end, reapplied);
}
TEST(Twist3dTest, Pose3dLogZ) {
const Pose3d end{5_m, 5_m, 0_m, Rotation3d{0_deg, 0_deg, 90_deg}};
const Pose3d start;
const auto twist = start.Log(end);
Twist3d expected{units::meter_t{5.0 / 2.0 * wpi::numbers::pi},
0_m,
0_m,
0_deg,
0_deg,
90_deg};
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
EXPECT_EQ(end, reapplied);
}