// ========== STATIC HELPER METHODS =========== const Pose2D Pose2D::average(const Pose2D& p1, const Pose2D& p2, const double& weight1, const double& weight2) { double divider = weight1 + weight2; double x = (p1.x() * weight1 + p2.x() * weight2) / divider; double y = (p1.y() * weight1 + p2.y() * weight2) / divider; return Pose2D(x, y, Angle::average(p1.getAngle(), p2.getAngle(), weight1, weight2)); }
const Pose2D Pose2D::interpolate(const Pose2D& startPose, const Pose2D& endPose, const double& progress) { if (progress <= 0) { return startPose; } else if (progress >= 1) { return endPose; } Vector2d diff = endPose.getPosition() - startPose.getPosition(); Angle angle = Angle::interpolate(startPose.getAngle(), endPose.getAngle(), progress); return Pose2D(startPose.getPosition() + (diff * progress), angle); }
const Pose2D Pose2D::operator+(const Pose2D& other) const { return Pose2D(getPosition() + other.getPosition(), getAngle() + other.getAngle()); }