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.kinematics;
006
007import java.util.Arrays;
008import java.util.Objects;
009
010/** Represents the wheel positions for a swerve drive drivetrain. */
011public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
012  /** The distances driven by the wheels. */
013  public SwerveModulePosition[] positions;
014
015  /**
016   * Creates a new SwerveDriveWheelPositions instance.
017   *
018   * @param positions The swerve module positions. This will be deeply copied.
019   */
020  public SwerveDriveWheelPositions(SwerveModulePosition[] positions) {
021    this.positions = new SwerveModulePosition[positions.length];
022    for (int i = 0; i < positions.length; i++) {
023      this.positions[i] = positions[i].copy();
024    }
025  }
026
027  @Override
028  public boolean equals(Object obj) {
029    if (obj instanceof SwerveDriveWheelPositions) {
030      SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj;
031      return Arrays.equals(this.positions, other.positions);
032    }
033    return false;
034  }
035
036  @Override
037  public int hashCode() {
038    // Cast to interpret positions as single argument, not array of the arguments
039    return Objects.hash((Object) positions);
040  }
041
042  @Override
043  public String toString() {
044    return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
045  }
046
047  @Override
048  public SwerveDriveWheelPositions copy() {
049    return new SwerveDriveWheelPositions(positions);
050  }
051
052  @Override
053  public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
054    if (endValue.positions.length != positions.length) {
055      throw new IllegalArgumentException("Inconsistent number of modules!");
056    }
057    var newPositions = new SwerveModulePosition[positions.length];
058    for (int i = 0; i < positions.length; i++) {
059      newPositions[i] = positions[i].interpolate(endValue.positions[i], t);
060    }
061    return new SwerveDriveWheelPositions(newPositions);
062  }
063}