public Pose2dWithCurvature(Translation2d translation, Rotation2d rotation, double curvature,
double dcurvature_ds)
{
pose_ = new Pose2d(translation, rotation);
curvature_ = curvature;
dcurvature_ds_ = dcurvature_ds;
}
public Translation2d interpolate(Translation2d other, double x)
{
if (x <= 0)
{
return(this);
}
else if (x >= 1)
{
return(other);
}
return(extrapolate(other, 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 Pose2d()
{
translation_ = new Translation2d();
rotation_ = new Rotation2d();
}
public Rotation2d(Translation2d direction, bool normalize) : this(direction.x(), direction.y(), normalize)
{
}
public bool isParallel(Rotation2d other)
{
return(epsilonEquals(Translation2d.cross(toTranslation(), other.toTranslation()), 0.0));
}
public static Pose2d fromTranslation(Translation2d translation)
{
return(new Pose2d(translation, new Rotation2d()));
}
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 Translation2d extrapolate(Translation2d other, double x)
{
return(new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_));
}
/**
* We can compose Translation2d's by adding together the x and y shifts.
*
* @param other The other translation to add.
* @return The combined effect of translating by this object and the other.
*/
public Translation2d translateBy(Translation2d other)
{
return(new Translation2d(x_ + other.x_, y_ + other.y_));
}
public Translation2d(Translation2d other)
{
x_ = other.x_;
y_ = other.y_;
}
public Translation2d(Translation2d start, Translation2d end)
{
x_ = end.x_ - start.x_;
y_ = end.y_ - start.y_;
}
public double distance(Translation2d other)
{
return(inverse().translateBy(other).norm());
}
public static double cross(Translation2d a, Translation2d b)
{
return(a.x_ * b.y_ - a.y_ * b.x_);
}