// ========== 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());
}
