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.VecBuilder;
008import edu.wpi.first.math.Vector;
009import edu.wpi.first.math.numbers.N3;
010
011/** A class representing a coordinate system axis within the NWU coordinate system. */
012public class CoordinateAxis {
013  private static final CoordinateAxis m_n = new CoordinateAxis(1.0, 0.0, 0.0);
014  private static final CoordinateAxis m_s = new CoordinateAxis(-1.0, 0.0, 0.0);
015  private static final CoordinateAxis m_e = new CoordinateAxis(0.0, -1.0, 0.0);
016  private static final CoordinateAxis m_w = new CoordinateAxis(0.0, 1.0, 0.0);
017  private static final CoordinateAxis m_u = new CoordinateAxis(0.0, 0.0, 1.0);
018  private static final CoordinateAxis m_d = new CoordinateAxis(0.0, 0.0, -1.0);
019
020  final Vector<N3> m_axis;
021
022  /**
023   * Constructs a coordinate system axis within the NWU coordinate system and normalizes it.
024   *
025   * @param x The x component.
026   * @param y The y component.
027   * @param z The z component.
028   */
029  public CoordinateAxis(double x, double y, double z) {
030    double norm = Math.sqrt(x * x + y * y + z * z);
031    m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
032  }
033
034  /**
035   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
036   *
037   * @return A coordinate axis corresponding to +X in the NWU coordinate system.
038   */
039  public static CoordinateAxis N() {
040    return m_n;
041  }
042
043  /**
044   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
045   *
046   * @return A coordinate axis corresponding to -X in the NWU coordinate system.
047   */
048  public static CoordinateAxis S() {
049    return m_s;
050  }
051
052  /**
053   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
054   *
055   * @return A coordinate axis corresponding to -Y in the NWU coordinate system.
056   */
057  public static CoordinateAxis E() {
058    return m_e;
059  }
060
061  /**
062   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
063   *
064   * @return A coordinate axis corresponding to +Y in the NWU coordinate system.
065   */
066  public static CoordinateAxis W() {
067    return m_w;
068  }
069
070  /**
071   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
072   *
073   * @return A coordinate axis corresponding to +Z in the NWU coordinate system.
074   */
075  public static CoordinateAxis U() {
076    return m_u;
077  }
078
079  /**
080   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
081   *
082   * @return A coordinate axis corresponding to -Z in the NWU coordinate system.
083   */
084  public static CoordinateAxis D() {
085    return m_d;
086  }
087}