Translation2d.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.geometry;
 
import static org.wpilib.units.Units.Meters;
 
import io.avaje.jsonb.Json;
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;
import java.util.Objects;
import java.util.Optional;
import org.wpilib.math.geometry.proto.Translation2dProto;
import org.wpilib.math.geometry.struct.Translation2dStruct;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.linalg.Vector;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.util.MathUtil;
import org.wpilib.units.measure.Distance;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;

@Json
public final class Translation2d
    implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable {
  public static final Translation2d ZERO = new Translation2d();
 
  @Json.Property("x")
  private final double m_x;
 
  @Json.Property("y")
  private final double m_y;

  public Translation2d() {
    this(0.0, 0.0);
  }

  @Json.Creator
  public Translation2d(double x, double y) {
    m_x = x;
    m_y = y;
  }

  public Translation2d(double distance, Rotation2d angle) {
    m_x = distance * angle.getCos();
    m_y = distance * angle.getSin();
  }

  public Translation2d(Distance x, Distance y) {
    this(x.in(Meters), y.in(Meters));
  }

  public Translation2d(Vector<N2> vector) {
    this(vector.get(0), vector.get(1));
  }

  public double getDistance(Translation2d other) {
    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
  }

  public double getSquaredDistance(Translation2d other) {
    double dx = other.m_x - m_x;
    double dy = other.m_y - m_y;
    return dx * dx + dy * dy;
  }

  public double getX() {
    return m_x;
  }

  public double getY() {
    return m_y;
  }

  public Distance getMeasureX() {
    return Meters.of(m_x);
  }

  public Distance getMeasureY() {
    return Meters.of(m_y);
  }

  public Vector<N2> toVector() {
    return VecBuilder.fill(m_x, m_y);
  }

  public double getNorm() {
    return Math.hypot(m_x, m_y);
  }

  public double getSquaredNorm() {
    return m_x * m_x + m_y * m_y;
  }

  public Optional<Rotation2d> getAngle() {
    if (Math.hypot(m_x, m_y) > 1e-6) {
      return Optional.of(new Rotation2d(m_x, m_y));
    } else {
      return Optional.empty();
    }
  }

  public Translation2d rotateBy(Rotation2d other) {
    return new Translation2d(
        m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos());
  }

  public Translation2d rotateAround(Translation2d other, Rotation2d rot) {
    return new Translation2d(
        (m_x - other.getX()) * rot.getCos() - (m_y - other.getY()) * rot.getSin() + other.getX(),
        (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY());
  }

  public double dot(Translation2d other) {
    return m_x * other.m_x + m_y * other.m_y;
  }

  public double cross(Translation2d other) {
    return m_x * other.m_y - m_y * other.m_x;
  }

  public Translation2d plus(Translation2d other) {
    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
  }

  public Translation2d minus(Translation2d other) {
    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
  }

  public Translation2d unaryMinus() {
    return new Translation2d(-m_x, -m_y);
  }

  public Translation2d times(double scalar) {
    return new Translation2d(m_x * scalar, m_y * scalar);
  }

  public Translation2d div(double scalar) {
    return new Translation2d(m_x / scalar, m_y / scalar);
  }

  public Translation2d nearest(Collection<Translation2d> translations) {
    return Collections.min(translations, Comparator.comparing(this::getDistance));
  }
 
  @Override
  public String toString() {
    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
  }

  @Override
  public boolean equals(Object obj) {
    return obj instanceof Translation2d other
        && Math.abs(other.m_x - m_x) < 1E-9
        && Math.abs(other.m_y - m_y) < 1E-9;
  }
 
  @Override
  public int hashCode() {
    return Objects.hash(m_x, m_y);
  }
 
  @Override
  public Translation2d interpolate(Translation2d endValue, double t) {
    return new Translation2d(
        MathUtil.lerp(this.getX(), endValue.getX(), t),
        MathUtil.lerp(this.getY(), endValue.getY(), t));
  }

  public static final Translation2dProto proto = new Translation2dProto();

  public static final Translation2dStruct struct = new Translation2dStruct();
}