2866-Decode/RefitSquare.java at main · Pattonville-Robotics/2866-Decode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
package org.firstinspires.ftc.teamcode;


public class RefitSquare {
    public RefitSquare(){}

    private double[] find_squareLimit(double[] pos)
    {
        double limit = Math.max(Math.abs(pos[0]), Math.abs(pos[1]));
        return (new double[] {pos[0] / limit, pos[1] / limit});
    }

    private double[] find_circleLimit(double[] pos)
    {
        double x = pos[0]/Math.sqrt(Math.pow(pos[0],2)+Math.pow(pos[1],2));
        double y = pos[1]/Math.sqrt(Math.pow(pos[0],2)+Math.pow(pos[1],2));
        return(new double[] {x,y});
    }

    private double find_distFromOrigin(double[] pos)
    {
        return(Math.sqrt(Math.pow(pos[0],2)+Math.pow(pos[1],2)));
    }

    private double find_squareCircleLimitRatio(double[] pos)
    {
        double[] squareLimit = find_squareLimit(pos);
        double[] circleLimit = find_circleLimit(pos);
        double squareLimitDistance = find_distFromOrigin(squareLimit);
        double circleLimitDistance = find_distFromOrigin(circleLimit);
        double ratio = squareLimitDistance/circleLimitDistance;
        return(ratio);
    }

    private double[] multiply_position(double[] pos, double coefficient)
    {
        return(new double[] {pos[0]*coefficient,pos[1]*coefficient});
    }

    public double[] convertPoint(double[] pos)
    {
        double[] new_position = multiply_position(pos,find_squareCircleLimitRatio(pos));
        if (Double.isNaN(new_position[0])){return(new double[] {0,0});}
        return(new_position);
    }
}

//Willow Harpole, 1/18/2026