mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
44 lines
1.3 KiB
C++
44 lines
1.3 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#pragma once
|
|
|
|
#include <utility>
|
|
|
|
#include "frc/geometry/Pose2d.h"
|
|
#include "frc/geometry/Rotation2d.h"
|
|
#include "units/length.h"
|
|
|
|
namespace frc {
|
|
|
|
constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
|
|
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
|
|
|
|
constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
|
|
Rotation2d rotation)
|
|
: m_translation(x, y), m_rotation(std::move(rotation)) {}
|
|
|
|
constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
|
|
return TransformBy(other);
|
|
}
|
|
|
|
constexpr Pose2d Pose2d::operator*(double scalar) const {
|
|
return Pose2d{m_translation * scalar, m_rotation * scalar};
|
|
}
|
|
|
|
constexpr Pose2d Pose2d::operator/(double scalar) const {
|
|
return *this * (1.0 / scalar);
|
|
}
|
|
|
|
constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const {
|
|
return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
|
|
}
|
|
|
|
constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
|
|
return {m_translation + (other.Translation().RotateBy(m_rotation)),
|
|
other.Rotation() + m_rotation};
|
|
}
|
|
|
|
} // namespace frc
|