public Pose2dWithCurvature(Translation2d translation, Rotation2d rotation, double curvature,
double dcurvature_ds)
{
pose_ = new Pose2d(translation, rotation);
curvature_ = curvature;
dcurvature_ds_ = dcurvature_ds;
}
public Rotation2d interpolate(Rotation2d other, double x)
{
if (x <= 0)
{
return(new Rotation2d(this));
}
else if (x >= 1)
{
return(new Rotation2d(other));
}
double angle_diff = inverse().rotateBy(other).getRadians();
return(this.rotateBy(Rotation2d.fromRadians(angle_diff * x)));
}
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)));
}
public Rotation2d(Rotation2d other)
{
cos_angle_ = other.cos_angle_;
sin_angle_ = other.sin_angle_;
theta_degrees = Math.Atan2(sin_angle_, cos_angle_) * (180 / Math.PI);
}
public double distance(Rotation2d other)
{
return(inverse().rotateBy(other).getRadians());
}
public bool isParallel(Rotation2d other)
{
return(epsilonEquals(Translation2d.cross(toTranslation(), other.toTranslation()), 0.0));
}
/**
* We can rotate this Rotation2d by adding together the effects of it and
* another rotation.
*
* @param other The other rotation. See:
* https://en.wikipedia.org/wiki/Rotation_matrix
* @return This rotation rotated by other.
*/
public Rotation2d rotateBy(Rotation2d other)
{
return(new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_,
cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true));
}
public static Pose2d fromRotation(Rotation2d rotation)
{
return(new Pose2d(new Translation2d(), rotation));
}
public Pose2d(Pose2d other)
{
translation_ = new Translation2d(other.translation_);
rotation_ = new Rotation2d(other.rotation_);
}
public Pose2d(Translation2d translation, Rotation2d rotation)
{
translation_ = translation;
rotation_ = rotation;
}
public Pose2d(double x, double y, Rotation2d rotation)
{
translation_ = new Translation2d(x, y);
rotation_ = rotation;
}
public Pose2d()
{
translation_ = new Translation2d();
rotation_ = new Rotation2d();
}
/**
* The inverse of this transform "undoes" the effect of translating by this
* transform.
*
* @return The opposite of this transform.
*/
public Pose2d inverse()
{
Rotation2d rotation_inverted = rotation_.inverse();
return(new Pose2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted));
}
public static Translation2d fromPolar(Rotation2d direction, double magnitude)
{
return(new Translation2d(direction.cos() * magnitude, direction.sin() * magnitude));
}
/**
* We can also rotate Translation2d's. See:
* https://en.wikipedia.org/wiki/Rotation_matrix
*
* @param rotation The rotation to apply.
* @return This translation rotated by rotation.
*/
public Translation2d rotateBy(Rotation2d rotation)
{
return(new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos()));
}
