MatrixTest.java

import java.util.ArrayList;
import java.util.List;

public class MatrixTest {
    static class Point {
        double x, y;
        Point(double x, double y) { this.x = x; this.y = y; }
        double[] asArray() { return new double[] { x, y }; }
        public String toString() { return String.format("[%.2f, %.2f]", x, y); }
    }
    
    static class Line {
        Point pt1, pt2;
        private double angle, sinAngle, cosAngle;
        
        Line(Point pt1, Point pt2) {
            this.pt1 = pt1;
            this.pt2 = pt2;
            this.angle = Math.atan2(pt2.y - pt1.y, pt2.x - pt1.x);
            this.sinAngle = Math.sin(angle);
            this.cosAngle = Math.cos(angle);
        }
        double getAngle() { return angle; }
        double getAngleDeg() { return Math.toDegrees(angle); }
        
        Point rotate(Point pt) {
            double xTran = pt.x - pt1.x;
            double yTran = pt.y - pt1.y;
            double x, y;
//  TODO: figure out where this part of the algorithm is needed
//            if (angle > 0.0) {
                x = (xTran * cosAngle) + (yTran * sinAngle);
                y = -(xTran * sinAngle) + (yTran * cosAngle);
//            } else {
//                x = (xTran * cosAngle) - (yTran * sinAngle);
//                y = (xTran * sinAngle) + (yTran * cosAngle);
//            }
            return new Point(x, y);
        }

        public String toString() { return String.format("  line: %s -> %s; angle=%.2f", pt1.toString(), pt2.toString(), getAngleDeg()); }
    }
    
    /**
     * Command line format: lookAheadDist robotX robotY pt1X pt1Y pt2X pt2Y
     */
    public static void main(String[] args) {
        double lookAhead = 8.0;
        Point pt1 = new Point(0.0, 24.0);
        Point pt2 = new Point(36.0, 36.0);
        Point robotPt = new Point(0.001, 16.0);
        if (args.length > 0) { lookAhead = Double.parseDouble(args[0]); }
        if (args.length > 2) { robotPt = new Point(Double.parseDouble(args[1]), Double.parseDouble(args[2])); }
        if (args.length > 4) { pt1 = new Point(Double.parseDouble(args[3]), Double.parseDouble(args[4])); }
        if (args.length > 6) { pt2 = new Point(Double.parseDouble(args[5]), Double.parseDouble(args[6])); }
        
        Line ln = new Line(pt1, pt2);
        Point robotPtT = ln.rotate(robotPt);
        Point pt2t = ln.rotate(pt2);
        double desiredHeadingRad = Math.asin(-robotPtT.y / lookAhead);
        double desiredHdgDeg = Math.toDegrees(desiredHeadingRad);
        System.out.printf("Point 2=%s;  Point 2t=%s\n", pt2.toString(), pt2t.toString());
        System.out.printf("  Angle=%5.2f, robot: %s,  rotated robot: %s; desiredHdg=%.2f, desiredHdgRot=%.2f\n",
                ln.getAngleDeg(), robotPt.toString(), robotPtT.toString(), desiredHdgDeg, desiredHdgDeg + ln.getAngleDeg());
    }
}