Vector.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.math.linalg;
 
import java.util.Objects;
import org.ejml.simple.SimpleMatrix;
import org.wpilib.math.linalg.proto.VectorProto;
import org.wpilib.math.linalg.struct.VectorStruct;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.util.Nat;
import org.wpilib.math.util.Num;
import org.wpilib.util.protobuf.Protobuf;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.Struct;
import org.wpilib.util.struct.StructSerializable;

public class Vector<R extends Num> extends Matrix<R, N1>
    implements ProtobufSerializable, StructSerializable {
  public Vector(Nat<R> rows) {
    super(rows, Nat.N1());
  }

  public Vector(SimpleMatrix storage) {
    super(storage);
  }

  public Vector(Matrix<R, N1> other) {
    super(other);
  }

  public double get(int row) {
    return get(row, 0);
  }
 
  @Override
  public Vector<R> times(double value) {
    return new Vector<>(this.m_storage.scale(value));
  }
 
  @Override
  public Vector<R> div(int value) {
    return new Vector<>(this.m_storage.divide(value));
  }
 
  @Override
  public Vector<R> div(double value) {
    return new Vector<>(this.m_storage.divide(value));
  }

  public final Vector<R> plus(Vector<R> value) {
    return new Vector<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
  }

  public final Vector<R> minus(Vector<R> value) {
    return new Vector<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
  }

  public double dot(Vector<R> other) {
    double dot = 0.0;
 
    for (int i = 0; i < getNumRows(); ++i) {
      dot += get(i, 0) * other.get(i, 0);
    }
 
    return dot;
  }

  public double norm() {
    return Math.sqrt(dot(this));
  }

  public Vector<R> unit() {
    return div(norm());
  }

  public Vector<R> projection(Vector<R> other) {
    return other.times(dot(other)).div(other.dot(other));
  }

  public static Vector<N3> cross(Vector<N3> a, Vector<N3> b) {
    return VecBuilder.fill(
        a.get(1) * b.get(2) - a.get(2) * b.get(1),
        a.get(2) * b.get(0) - a.get(0) * b.get(2),
        a.get(0) * b.get(1) - a.get(1) * b.get(0));
  }

  public static final <R extends Num> VectorProto<R> getProto(Nat<R> rows) {
    return new VectorProto<>(rows);
  }

  public static final <R extends Num> VectorStruct<R> getStruct(Nat<R> rows) {
    return new VectorStruct<>(rows);
  }
}