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

[wpimath] Make geometry classes constexpr (#7222)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-18 16:08:41 -07:00
committed by GitHub
parent 2054d0f57e
commit 95b9bd880b
30 changed files with 1324 additions and 1114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -295,7 +295,9 @@ public class Quaternion implements ProtobufSerializable, StructSerializable {
double v_scalar;
if (v_norm < 1e-9) {
// Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
// Taylor series expansion of atan2(y/x)/y at y = 0:
//
// 1/x - 1/3 y²/x³ + O(y⁴)
v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
} else {
v_scalar = Math.atan2(v_norm, getW()) / v_norm;

54
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -24,7 +24,6 @@ import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
/** A rotation in a 3D coordinate frame represented by a quaternion. */
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -230,27 +229,42 @@ public class Rotation3d
// there's no rotation. The default initialization of m_q will work.
m_q = new Quaternion();
} else if (dotNorm < -1.0 + 1E-9) {
// If the dot product is -1, the two vectors point in opposite directions
// so a 180 degree rotation is required. Any orthogonal vector can be used
// for it. Q in the QR decomposition is an orthonormal basis, so it
// contains orthogonal unit vectors.
var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
final var qr = DecompositionFactory_DDRM.qr(3, 1);
qr.decompose(X.getStorage().getMatrix());
final var Q = qr.getQ(null, false);
// If the dot product is -1, the two vectors are antiparallel, so a 180°
// rotation is required. Any other vector can be used to generate an
// orthogonal one.
// w = cos(θ/2) = cos(90°) = 0
//
// For x, y, and z, we use the second column of Q because the first is
// parallel instead of orthogonal. The third column would also work.
m_q = new Quaternion(0.0, Q.get(0, 1), Q.get(1, 1), Q.get(2, 1));
double x = Math.abs(initial.get(0, 0));
double y = Math.abs(initial.get(1, 0));
double z = Math.abs(initial.get(2, 0));
// Find vector that is most orthogonal to initial vector
Vector<N3> other;
if (x < y) {
if (x < z) {
// Use x-axis
other = VecBuilder.fill(1, 0, 0);
} else {
// Use z-axis
other = VecBuilder.fill(0, 0, 1);
}
} else {
if (y < z) {
// Use y-axis
other = VecBuilder.fill(0, 1, 0);
} else {
// Use z-axis
other = VecBuilder.fill(0, 0, 1);
}
}
var axis = Vector.cross(initial, other);
double axisNorm = axis.norm();
m_q =
new Quaternion(
0.0, axis.get(0, 0) / axisNorm, axis.get(1, 0) / axisNorm, axis.get(2, 0) / axisNorm);
} else {
// initial x last
var axis =
VecBuilder.fill(
initial.get(1, 0) * last.get(2, 0) - last.get(1, 0) * initial.get(2, 0),
last.get(0, 0) * initial.get(2, 0) - initial.get(0, 0) * last.get(2, 0),
initial.get(0, 0) * last.get(1, 0) - last.get(0, 0) * initial.get(1, 0));
var axis = Vector.cross(initial, last);
// https://stackoverflow.com/a/11741520
m_q =