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.geometry; 006 007import static edu.wpi.first.units.Units.Meters; 008 009import edu.wpi.first.math.geometry.proto.Transform2dProto; 010import edu.wpi.first.math.geometry.struct.Transform2dStruct; 011import edu.wpi.first.units.Distance; 012import edu.wpi.first.units.Measure; 013import edu.wpi.first.util.protobuf.ProtobufSerializable; 014import edu.wpi.first.util.struct.StructSerializable; 015import java.util.Objects; 016 017/** Represents a transformation for a Pose2d in the pose's frame. */ 018public class Transform2d implements ProtobufSerializable, StructSerializable { 019 /** 020 * A preallocated Transform2d representing no transformation. 021 * 022 * <p>This exists to avoid allocations for common transformations. 023 */ 024 public static final Transform2d kZero = new Transform2d(); 025 026 private final Translation2d m_translation; 027 private final Rotation2d m_rotation; 028 029 /** 030 * Constructs the transform that maps the initial pose to the final pose. 031 * 032 * @param initial The initial pose for the transformation. 033 * @param last The final pose for the transformation. 034 */ 035 public Transform2d(Pose2d initial, Pose2d last) { 036 // We are rotating the difference between the translations 037 // using a clockwise rotation matrix. This transforms the global 038 // delta into a local delta (relative to the initial pose). 039 m_translation = 040 last.getTranslation() 041 .minus(initial.getTranslation()) 042 .rotateBy(initial.getRotation().unaryMinus()); 043 044 m_rotation = last.getRotation().minus(initial.getRotation()); 045 } 046 047 /** 048 * Constructs a transform with the given translation and rotation components. 049 * 050 * @param translation Translational component of the transform. 051 * @param rotation Rotational component of the transform. 052 */ 053 public Transform2d(Translation2d translation, Rotation2d rotation) { 054 m_translation = translation; 055 m_rotation = rotation; 056 } 057 058 /** 059 * Constructs a transform with x and y translations instead of a separate Translation2d. 060 * 061 * @param x The x component of the translational component of the transform. 062 * @param y The y component of the translational component of the transform. 063 * @param rotation The rotational component of the transform. 064 */ 065 public Transform2d(double x, double y, Rotation2d rotation) { 066 m_translation = new Translation2d(x, y); 067 m_rotation = rotation; 068 } 069 070 /** 071 * Constructs a transform with x and y translations instead of a separate Translation2d. The X and 072 * Y translations will be converted to and tracked as meters. 073 * 074 * @param x The x component of the translational component of the transform. 075 * @param y The y component of the translational component of the transform. 076 * @param rotation The rotational component of the transform. 077 */ 078 public Transform2d(Measure<Distance> x, Measure<Distance> y, Rotation2d rotation) { 079 this(x.in(Meters), y.in(Meters), rotation); 080 } 081 082 /** Constructs the identity transform -- maps an initial pose to itself. */ 083 public Transform2d() { 084 m_translation = Translation2d.kZero; 085 m_rotation = Rotation2d.kZero; 086 } 087 088 /** 089 * Multiplies the transform by the scalar. 090 * 091 * @param scalar The scalar. 092 * @return The scaled Transform2d. 093 */ 094 public Transform2d times(double scalar) { 095 return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar)); 096 } 097 098 /** 099 * Divides the transform by the scalar. 100 * 101 * @param scalar The scalar. 102 * @return The scaled Transform2d. 103 */ 104 public Transform2d div(double scalar) { 105 return times(1.0 / scalar); 106 } 107 108 /** 109 * Composes two transformations. The second transform is applied relative to the orientation of 110 * the first. 111 * 112 * @param other The transform to compose with this one. 113 * @return The composition of the two transformations. 114 */ 115 public Transform2d plus(Transform2d other) { 116 return new Transform2d(Pose2d.kZero, Pose2d.kZero.transformBy(this).transformBy(other)); 117 } 118 119 /** 120 * Returns the translation component of the transformation. 121 * 122 * @return The translational component of the transform. 123 */ 124 public Translation2d getTranslation() { 125 return m_translation; 126 } 127 128 /** 129 * Returns the X component of the transformation's translation. 130 * 131 * @return The x component of the transformation's translation. 132 */ 133 public double getX() { 134 return m_translation.getX(); 135 } 136 137 /** 138 * Returns the Y component of the transformation's translation. 139 * 140 * @return The y component of the transformation's translation. 141 */ 142 public double getY() { 143 return m_translation.getY(); 144 } 145 146 /** 147 * Returns the rotational component of the transformation. 148 * 149 * @return Reference to the rotational component of the transform. 150 */ 151 public Rotation2d getRotation() { 152 return m_rotation; 153 } 154 155 /** 156 * Invert the transformation. This is useful for undoing a transformation. 157 * 158 * @return The inverted transformation. 159 */ 160 public Transform2d inverse() { 161 // We are rotating the difference between the translations 162 // using a clockwise rotation matrix. This transforms the global 163 // delta into a local delta (relative to the initial pose). 164 return new Transform2d( 165 getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()), 166 getRotation().unaryMinus()); 167 } 168 169 @Override 170 public String toString() { 171 return String.format("Transform2d(%s, %s)", m_translation, m_rotation); 172 } 173 174 /** 175 * Checks equality between this Transform2d and another object. 176 * 177 * @param obj The other object. 178 * @return Whether the two objects are equal or not. 179 */ 180 @Override 181 public boolean equals(Object obj) { 182 if (obj instanceof Transform2d) { 183 return ((Transform2d) obj).m_translation.equals(m_translation) 184 && ((Transform2d) obj).m_rotation.equals(m_rotation); 185 } 186 return false; 187 } 188 189 @Override 190 public int hashCode() { 191 return Objects.hash(m_translation, m_rotation); 192 } 193 194 /** Transform2d protobuf for serialization. */ 195 public static final Transform2dProto proto = new Transform2dProto(); 196 197 /** Transform2d struct for serialization. */ 198 public static final Transform2dStruct struct = new Transform2dStruct(); 199}