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_); }