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