001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.geometry;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.Nat;
009
010/** A helper class that converts Pose3d objects between different standard coordinate frames. */
011public class CoordinateSystem {
012  private static final CoordinateSystem m_nwu =
013      new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
014  private static final CoordinateSystem m_edn =
015      new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
016  private static final CoordinateSystem m_ned =
017      new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
018
019  // Rotation from this coordinate system to NWU coordinate system
020  private final Rotation3d m_rotation;
021
022  /**
023   * Constructs a coordinate system with the given cardinal directions for each axis.
024   *
025   * @param positiveX The cardinal direction of the positive x-axis.
026   * @param positiveY The cardinal direction of the positive y-axis.
027   * @param positiveZ The cardinal direction of the positive z-axis.
028   * @throws IllegalArgumentException if the coordinate system isn't special orthogonal
029   */
030  public CoordinateSystem(
031      CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) {
032    // Construct a change of basis matrix from the source coordinate system to the
033    // NWU coordinate system. Each column vector in the change of basis matrix is
034    // one of the old basis vectors mapped to its representation in the new basis.
035    var R = new Matrix<>(Nat.N3(), Nat.N3());
036    R.assignBlock(0, 0, positiveX.m_axis);
037    R.assignBlock(0, 1, positiveY.m_axis);
038    R.assignBlock(0, 2, positiveZ.m_axis);
039
040    // The change of basis matrix should be a pure rotation. The Rotation3d
041    // constructor will verify this by checking for special orthogonality.
042    m_rotation = new Rotation3d(R);
043  }
044
045  /**
046   * Returns an instance of the North-West-Up (NWU) coordinate system.
047   *
048   * <p>The +X axis is north, the +Y axis is west, and the +Z axis is up.
049   *
050   * @return An instance of the North-West-Up (NWU) coordinate system.
051   */
052  public static CoordinateSystem NWU() {
053    return m_nwu;
054  }
055
056  /**
057   * Returns an instance of the East-Down-North (EDN) coordinate system.
058   *
059   * <p>The +X axis is east, the +Y axis is down, and the +Z axis is north.
060   *
061   * @return An instance of the East-Down-North (EDN) coordinate system.
062   */
063  public static CoordinateSystem EDN() {
064    return m_edn;
065  }
066
067  /**
068   * Returns an instance of the North-East-Down (NED) coordinate system.
069   *
070   * <p>The +X axis is north, the +Y axis is east, and the +Z axis is down.
071   *
072   * @return An instance of the North-East-Down (NED) coordinate system.
073   */
074  public static CoordinateSystem NED() {
075    return m_ned;
076  }
077
078  /**
079   * Converts the given translation from one coordinate system to another.
080   *
081   * @param translation The translation to convert.
082   * @param from The coordinate system the pose starts in.
083   * @param to The coordinate system to which to convert.
084   * @return The given translation in the desired coordinate system.
085   */
086  public static Translation3d convert(
087      Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
088    return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
089  }
090
091  /**
092   * Converts the given rotation from one coordinate system to another.
093   *
094   * @param rotation The rotation to convert.
095   * @param from The coordinate system the rotation starts in.
096   * @param to The coordinate system to which to convert.
097   * @return The given rotation in the desired coordinate system.
098   */
099  public static Rotation3d convert(
100      Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
101    return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
102  }
103
104  /**
105   * Converts the given pose from one coordinate system to another.
106   *
107   * @param pose The pose to convert.
108   * @param from The coordinate system the pose starts in.
109   * @param to The coordinate system to which to convert.
110   * @return The given pose in the desired coordinate system.
111   */
112  public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
113    return new Pose3d(
114        convert(pose.getTranslation(), from, to), convert(pose.getRotation(), from, to));
115  }
116
117  /**
118   * Converts the given transform from one coordinate system to another.
119   *
120   * @param transform The transform to convert.
121   * @param from The coordinate system the transform starts in.
122   * @param to The coordinate system to which to convert.
123   * @return The given transform in the desired coordinate system.
124   */
125  public static Transform3d convert(
126      Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
127    return new Transform3d(
128        convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
129  }
130}