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