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 java.util.Objects;
009import org.ejml.simple.SimpleMatrix;
010
011/**
012 * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
013 *
014 * <p>This class is intended to be used alongside the state space library.
015 *
016 * @param <R> The number of rows in this matrix.
017 */
018public class Vector<R extends Num> extends Matrix<R, N1> {
019  /**
020   * Constructs an empty zero vector of the given dimensions.
021   *
022   * @param rows The number of rows of the vector.
023   */
024  public Vector(Nat<R> rows) {
025    super(rows, Nat.N1());
026  }
027
028  /**
029   * Constructs a new {@link Vector} with the given storage. Caller should make sure that the
030   * provided generic bounds match the shape of the provided {@link Vector}.
031   *
032   * <p>NOTE:It is not recommended to use this constructor unless the {@link SimpleMatrix} API is
033   * absolutely necessary due to the desired function not being accessible through the {@link
034   * Vector} wrapper.
035   *
036   * @param storage The {@link SimpleMatrix} to back this vector.
037   */
038  public Vector(SimpleMatrix storage) {
039    super(storage);
040  }
041
042  /**
043   * Constructs a new vector with the storage of the supplied matrix.
044   *
045   * @param other The {@link Vector} to copy the storage of.
046   */
047  public Vector(Matrix<R, N1> other) {
048    super(other);
049  }
050
051  @Override
052  public Vector<R> times(double value) {
053    return new Vector<>(this.m_storage.scale(value));
054  }
055
056  @Override
057  public Vector<R> div(int value) {
058    return new Vector<>(this.m_storage.divide(value));
059  }
060
061  @Override
062  public Vector<R> div(double value) {
063    return new Vector<>(this.m_storage.divide(value));
064  }
065
066  /**
067   * Adds the given vector to this vector.
068   *
069   * @param value The vector to add.
070   * @return The resultant vector.
071   */
072  public final Vector<R> plus(Vector<R> value) {
073    return new Vector<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
074  }
075
076  /**
077   * Subtracts the given vector to this vector.
078   *
079   * @param value The vector to add.
080   * @return The resultant vector.
081   */
082  public final Vector<R> minus(Vector<R> value) {
083    return new Vector<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
084  }
085
086  /**
087   * Returns the dot product of this vector with another.
088   *
089   * @param other The other vector.
090   * @return The dot product.
091   */
092  public double dot(Vector<R> other) {
093    double dot = 0.0;
094
095    for (int i = 0; i < getNumRows(); ++i) {
096      dot += get(i, 0) * other.get(i, 0);
097    }
098
099    return dot;
100  }
101
102  /**
103   * Returns the norm of this vector.
104   *
105   * @return The norm.
106   */
107  public double norm() {
108    double squaredNorm = 0.0;
109
110    for (int i = 0; i < getNumRows(); ++i) {
111      squaredNorm += get(i, 0) * get(i, 0);
112    }
113
114    return Math.sqrt(squaredNorm);
115  }
116}