public Pose2dWithCurvature(Translation2d translation, Rotation2d rotation, double curvature,
double dcurvature_ds)
{
pose_ = new Pose2d(translation, rotation);
curvature_ = curvature;
dcurvature_ds_ = dcurvature_ds;
}
/**
* Return true if this pose is (nearly) colinear with the another.
*/
public bool isColinear(Pose2d other)
{
if (!getRotation().isParallel(other.getRotation()))
{
return(false);
}
Twist2d twist = log(inverse().transformBy(other));
return(epsilonEquals(twist.dy, 0.0) && epsilonEquals(twist.dtheta, 0.0));
}
/**
* Do twist interpolation of this pose assuming constant curvature.
*/
public Pose2d interpolate(Pose2d other, double x)
{
if (x <= 0)
{
return(new Pose2d(this));
}
else if (x >= 1)
{
return(new Pose2d(other));
}
Twist2d twist = Pose2d.log(inverse().transformBy(other));
return(transformBy(Pose2d.exp(twist.scaled(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)));
}
/**
* Logical inverse of the above.
*/
public static Twist2d log(Pose2d transform)
{
double dtheta = transform.getRotation().getRadians();
double half_dtheta = 0.5 * dtheta;
double cos_minus_one = transform.getRotation().cos() - 1.0;
double halftheta_by_tan_of_halfdtheta;
if (Math.Abs(cos_minus_one) < kEps)
{
halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
}
else
{
halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
}
Translation2d translation_part = transform.getTranslation()
.rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));
return(new Twist2d(translation_part.x(), translation_part.y(), dtheta));
}
public Pose2dWithCurvature transformBy(Pose2d transform)
{
return(new Pose2dWithCurvature(getPose().transformBy(transform), getCurvature(), getDCurvatureDs()));
}
public Pose2dWithCurvature(Pose2d pose, double curvature, double dcurvature_ds)
{
pose_ = pose;
curvature_ = curvature;
dcurvature_ds_ = dcurvature_ds;
}
public Pose2dWithCurvature(Pose2d pose, double curvature)
{
pose_ = pose;
curvature_ = curvature;
dcurvature_ds_ = 0.0;
}
public Pose2dWithCurvature()
{
pose_ = new Pose2d();
curvature_ = 0.0;
dcurvature_ds_ = 0.0;
}
public Pose2d(Pose2d other)
{
translation_ = new Translation2d(other.translation_);
rotation_ = new Rotation2d(other.rotation_);
}
public double distance(Pose2d other)
{
return(Pose2d.log(inverse().transformBy(other)).norm());
}
/**
* Transforming this RigidTransform2d means first translating by
* other.translation and then rotating by other.rotation
*
* @param other The other transform.
* @return This transform * other
*/
public Pose2d transformBy(Pose2d other)
{
return(new Pose2d(translation_.translateBy(other.translation_.rotateBy(rotation_)),
rotation_.rotateBy(other.rotation_)));
}