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