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