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;
006
007import edu.wpi.first.math.numbers.N1;
008import edu.wpi.first.math.numbers.N3;
009import java.util.Objects;
010import org.ejml.simple.SimpleMatrix;
011
012/**
013 * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
014 *
015 * <p>This class is intended to be used alongside the state space library.
016 *
017 * @param <R> The number of rows in this matrix.
018 */
019public class Vector<R extends Num> extends Matrix<R, N1> {
020  /**
021   * Constructs an empty zero vector of the given dimensions.
022   *
023   * @param rows The number of rows of the vector.
024   */
025  public Vector(Nat<R> rows) {
026    super(rows, Nat.N1());
027  }
028
029  /**
030   * Constructs a new {@link Vector} with the given storage. Caller should make sure that the
031   * provided generic bounds match the shape of the provided {@link Vector}.
032   *
033   * <p>NOTE:It is not recommended to use this constructor unless the {@link SimpleMatrix} API is
034   * absolutely necessary due to the desired function not being accessible through the {@link
035   * Vector} wrapper.
036   *
037   * @param storage The {@link SimpleMatrix} to back this vector.
038   */
039  public Vector(SimpleMatrix storage) {
040    super(storage);
041  }
042
043  /**
044   * Constructs a new vector with the storage of the supplied matrix.
045   *
046   * @param other The {@link Vector} to copy the storage of.
047   */
048  public Vector(Matrix<R, N1> other) {
049    super(other);
050  }
051
052  /**
053   * Returns an element of the vector at a specified row.
054   *
055   * @param row The row that the element is located at.
056   * @return An element of the vector.
057   */
058  public double get(int row) {
059    return get(row, 0);
060  }
061
062  @Override
063  public Vector<R> times(double value) {
064    return new Vector<>(this.m_storage.scale(value));
065  }
066
067  @Override
068  public Vector<R> div(int value) {
069    return new Vector<>(this.m_storage.divide(value));
070  }
071
072  @Override
073  public Vector<R> div(double value) {
074    return new Vector<>(this.m_storage.divide(value));
075  }
076
077  /**
078   * Adds the given vector to this vector.
079   *
080   * @param value The vector to add.
081   * @return The resultant vector.
082   */
083  public final Vector<R> plus(Vector<R> value) {
084    return new Vector<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
085  }
086
087  /**
088   * Subtracts the given vector to this vector.
089   *
090   * @param value The vector to add.
091   * @return The resultant vector.
092   */
093  public final Vector<R> minus(Vector<R> value) {
094    return new Vector<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
095  }
096
097  /**
098   * Returns the dot product of this vector with another.
099   *
100   * @param other The other vector.
101   * @return The dot product.
102   */
103  public double dot(Vector<R> other) {
104    double dot = 0.0;
105
106    for (int i = 0; i < getNumRows(); ++i) {
107      dot += get(i, 0) * other.get(i, 0);
108    }
109
110    return dot;
111  }
112
113  /**
114   * Returns the norm of this vector.
115   *
116   * @return The norm.
117   */
118  public double norm() {
119    return Math.sqrt(dot(this));
120  }
121
122  /**
123   * Returns the unit vector parallel with this vector.
124   *
125   * @return The unit vector.
126   */
127  public Vector<R> unit() {
128    return div(norm());
129  }
130
131  /**
132   * Returns the projection of this vector along another.
133   *
134   * @param other The vector to project along.
135   * @return The projection.
136   */
137  public Vector<R> projection(Vector<R> other) {
138    return other.times(dot(other)).div(other.dot(other));
139  }
140
141  /**
142   * Returns the cross product of 3 dimensional vectors a and b.
143   *
144   * @param a The vector to cross with b.
145   * @param b The vector to cross with a.
146   * @return The cross product of a and b.
147   */
148  public static Vector<N3> cross(Vector<N3> a, Vector<N3> b) {
149    return VecBuilder.fill(
150        a.get(1) * b.get(2) - a.get(2) * b.get(1),
151        a.get(2) * b.get(0) - a.get(0) * b.get(2),
152        a.get(0) * b.get(1) - a.get(1) * b.get(0));
153  }
154}