Examples with Translation2d com.team254.lib.geometry.Translation2d used on opensource projects

Example 6 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class AdaptivePurePursuitController method getCenter.

/**
 * Gives the center of the circle joining the lookahead point and robot pose
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return center of the circle joining the lookahead point and robot pose
 */
public static Translation2d getCenter(Pose2d pose, Translation2d point) {
    final Translation2d poseToPointHalfway = pose.getTranslation().interpolate(point, 0.5);
    final Rotation2d normal = pose.getTranslation().inverse().translateBy(poseToPointHalfway).direction().normal();
    final Pose2d perpendicularBisector = new Pose2d(poseToPointHalfway, normal);
    final Pose2d normalFromPose = new Pose2d(pose.getTranslation(), pose.getRotation().normal());
    if (normalFromPose.isColinear(perpendicularBisector.normal())) {
        // Special case: center is poseToPointHalfway.
        return poseToPointHalfway;
    }
    return normalFromPose.intersection(perpendicularBisector);
}

Example 7 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class AdaptivePurePursuitController method getDirection.

/**
 * Gives the direction the robot should turn to stay on the path
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return the direction the robot should turn: -1 is left, +1 is right
 */
public static int getDirection(Pose2d pose, Translation2d point) {
    Translation2d poseToPoint = new Translation2d(pose.getTranslation(), point);
    Translation2d robot = pose.getRotation().toTranslation();
    double cross = robot.x() * poseToPoint.y() - robot.y() * poseToPoint.x();
    // if robot < pose turn left
    return (cross < 0) ? -1 : 1;
}

Example 8 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class PathSegment method getRemainingDistance.

/**
 * Gets the remaining distance left on the segment from point <code>point</code>
 *
 * @param position result of <code>getClosestPoint()</code>
 * @return distance remaining
 */
public double getRemainingDistance(Translation2d position) {
    if (isLine) {
        return new Translation2d(end, position).norm();
    } else {
        Translation2d deltaPosition = new Translation2d(center, position);
        double angle = Translation2d.getAngle(deltaEnd, deltaPosition).getRadians();
        double totalAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians();
        return angle / totalAngle * getLength();
    }
}

Example 9 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class PathSegment method getPointByDistance.

/**
 * Calculates the point on the segment <code>dist</code> distance from the starting point along the segment.
 *
 * @param dist distance from the starting point
 * @return point on the segment <code>dist</code> distance from the starting point
 */
public Translation2d getPointByDistance(double dist) {
    double length = getLength();
    if (!extrapolateLookahead && dist > length) {
        dist = length;
    }
    if (isLine) {
        return start.translateBy(deltaStart.scale(dist / length));
    } else {
        double deltaAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians() * ((Translation2d.cross(deltaStart, deltaEnd) >= 0) ? 1 : -1);
        deltaAngle *= dist / length;
        Translation2d t = deltaStart.rotateBy(Rotation2d.fromRadians(deltaAngle));
        return center.translateBy(t);
    }
}