// ========== 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));
}
Vector2d DifferentialDriveKinematics::PartialInverse(const Pose2D<double> &a, const Pose2D<double> &b) {
Vector2d diff;
diff << b.x() - a.x(), b.y() - a.y();
// This rotation doesn't affect the magnitude of ds,
// but it so the sign can work out properly
Vector2d dr = a.rotationMatrix().inverse()*diff;
double ds = sqrt(dr(0)*dr(0)+dr(1)*dr(1));
if ( dr(0) < 0.0 ) {
ds = -ds;
}
double dtheta = b.heading() - a.heading(); // difference in headings
ecl::wrap_angle(dtheta);
// diff << ds, dtheta;
return (diff << ds, dtheta).finished();
}
