private static Translation2d intersectionInternal(Pose2d a, Pose2d b) { Rotation2d a_r = a.getRotation(); Rotation2d b_r = b.getRotation(); Translation2d a_t = a.getTranslation(); Translation2d b_t = b.getTranslation(); double tan_b = b_r.tan(); double t = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y()) / (a_r.sin() - a_r.cos() * tan_b); if (Double.IsNaN(t)) { return(new Translation2d(Double.PositiveInfinity, Double.PositiveInfinity)); } return(a_t.translateBy(a_r.toTranslation().scale(t))); }
/** * Transforming this RigidTransform2d means first translating by * other.translation and then rotating by other.rotation * * @param other The other transform. * @return This transform * other */ public Pose2d transformBy(Pose2d other) { return(new Pose2d(translation_.translateBy(other.translation_.rotateBy(rotation_)), rotation_.rotateBy(other.rotation_))); }