机器人技术第三次作业（HFUT）

第三次作业

 

本人代码水平十分有限，仅供参考，有错误请指出

java源码：

package robathomework3;

 

import java.lang.Math;

 

//点类

class point {

    double x = 0;

    double y = 0;

 

    point(double x, double y) {

        this.x = x;

        this.y = y;

    }

 

    void print() {

        System.out.println("(" + x + "," + y + ")");

    }

}

 

//圆类，给定圆心点和半径R即可确定唯一一个圆

class Circle {

    private double x;

    private double y;

    private double r;

 

//构造函数，用point对象和一个double变量初始化

    public Circle(point point, double R) {

        this.x = point.x;

        this.y = point.y;

        this.r = R;

    }

 

    public double getX() {

        return x;

    }

 

    public void setX(double x) {

        this.x = x;

    }

 

    public double getY() {

        return y;

    }

 

    public void setY(double y) {

        this.y = y;

    }

 

    public double getR() {

        return r;

    }

 

    public void setR(double r) {

        this.r = r;

    }

 

    public void print() {

        System.out.println("(" + this.x + "," + this.y + ")" + this.r);

    }

 

}

 

//计算类

class Function {

    // 初始化，防止Nullpointer异常

    private Circle c1 = new Circle(new point(0, 0), 0);

    private Circle c2 = new Circle(new point(0, 0), 0);

    // deg1/2是储存以自身为中心的极角

    private double deg1;

    private double deg2;

 

//构造函数，截取字符串，初始化各个变量

    public Function(String s) {

        String[] s1 = s.substring(1, s.indexOf(")")).split(" ");

        String[] s2 = s.substring(s.indexOf(")") + 3, s.length() - 1).split(" ");

        this.c1 = new Circle(Getpoint(s1[0]), Double.parseDouble(s1[1]));

        this.c2 = new Circle(Getpoint(s2[0]), Double.parseDouble(s2[1]));

        this.deg1 = Double.parseDouble(s1[2]);

        this.deg1 = Double.parseDouble(s2[2]);

 

    }

 

//通过字符标志返回点

    public point Getpoint(String p) {

        switch (p) {

        case "P1":

            return new point(-52.5, -32);

        case "P2":

            return new point(-52.5, 32);

        case "P3":

            return new point(52.5, 32);

        case "P4":

            return new point(52.5, -32);

        case "P5":

            return new point(0, -32);

        case "P6":

            return new point(0, 32);

        case "P7":

            return new point(-30, -7);

        case "P8":

            return new point(-30, 7);

        case "P9":

            return new point(30, 7);

        case "P10":

            return new point(30, -7);

        case "C":

            return new point(0, 0);

        case "G1":

            return new point(-52.5, 0);

        case "G2":

            return new point(52.5, 0);

        case "T1":

            return new point(2, 0);

        case "T2":

            return new point(0, 1);

        default:

            return new point(0, 0);

        }

    }

 

//判断两个圆有几个交点

    public int Judeg() {

        double d = Math.sqrt(

                (c1.getX() - c2.getX()) * (c1.getX() - c2.getX()) + (c1.getY() - c2.getY()) * (c1.getY() - c2.getY()));

        if (this.c1.getR() + this.c2.getR() < d) {

            return 0;

        } else if (this.c1.getR() + this.c2.getR() == d) {

            return 1;

        } else {

            return 2;

 

        }

    }

 

//关键方法，如果有两个交点，判断哪个交点是机器人所在位置 

//主要思路：假定机器人所在位置是坐标原点，建立一个直角坐标系，通过给定的两个点，可分别求出其在上述坐标轴中，与机器人所在点形成的连线与X轴的夹角

//此夹角配合给定的自身极角，就可以分别计算出头部在此坐标系中面向的角度。

//理论上，正确的位置，两个定点求出的头部方向应该相同，但是，计算难免有误差，因此我想到，用这两个 头部角度做差

//理论上的正确位置的差，就算计算有误差，其值应该也比非正确位置上的差小。这样，通过比较差值，便可选择到正确的点。

    public double IsMyPoint(point point) {

        double deg_1 = 0, deg_2 = 0;

        deg_1 = Math.acos(Math.abs((c1.getX() - point.x) / Math

                .sqrt((c1.getX() - point.x) * (c1.getX() - point.x) + (c1.getY() - point.y) * (c1.getY() - point.y))));

        deg_1 = Math.toDegrees(deg_1);

        deg_2 = Math.acos(Math.abs((c2.getX() - point.x) / Math

                .sqrt((c2.getX() - point.x) * (c2.getX() - point.x) + (c2.getY() - point.y) * (c2.getY() - point.y))));

        deg_2 = Math.toDegrees(deg_2);

//以机器人自身为原点建立坐标系，为了保证角度计算的一致性，我们可以讨论顶点落在的象限，就可以统一角度

//这里我规定了逆时针方向为增，同时所有象限的角都为正，即从x轴正方向开始，0-360.

//而机器人的视野角度是规定的逆时针为负，因此可以看到，后面是减去机器人的自身极角的

        if (point.x <= c1.getX() && point.y <= c1.getY()) {

            ;

        } else if (point.x > c1.getX() && point.y <= c1.getY()) {

            deg_1 = 180 - deg_1;

        } else if (point.x > c1.getX() && point.y > c1.getY()) {

            deg_1 = 180 + deg_1;

        } else {

            deg_1 = 360 - deg_1;

        }

        if (point.x <= c2.getX() && point.y <= c2.getY()) {

            ;

        } else if (point.x > c2.getX() && point.y <= c2.getY()) {

            deg_2 = 180 - deg_2;

        } else if (point.x > c2.getX() && point.y > c2.getY()) {

            deg_2 = 180 + deg_2;

        } else {

            deg_2 = 360 - deg_2;

        }

        deg_1 = deg_1 - this.deg1;

        deg_2 = deg_2 - this.deg2;

        // System.out.println(deg_1);

        // System.out.println(deg_2);

        if (deg_1 < 0) {

            deg_1 = 360 - deg_1;

        }

        if (deg_2 < 0) {

            deg_2 = 360 - deg_2;

        }

        return Math.abs(deg_1 - deg_2);

    }

 

//计算函数，计算交点，配合上述选择交点的函数，返回一个正确位置

    public point GetIntersection() {

 

        double a = 0, b = 0, c = 0;

        // double d =Math.sqrt((c1.getX()-c2.getX())+(c1.getY()-c2.getY()));

        double x1 = 0, y1 = 0, x2 = 0, y2 = 0;

        if (c1.getY() != c2.getY()) {

            double A = (c1.getX() * c1.getX() - c2.getX() * c2.getX() + c1.getY() * c1.getY() - c2.getY() * c2.getY()

                   + c2.getR() * c2.getR() - c1.getR() * c1.getR()) / (2 * (c1.getY() - c2.getY()));

            double B = (c1.getX() - c2.getX()) / (c1.getY() - c2.getY());

            a = 1 + B * B;

            b = -2 * (c1.getX() + (A - c1.getY()) * B);

            c = c1.getX() * c1.getX() + (A - c1.getY()) * (A - c1.getY()) - c1.getR() * c1.getR();

            if (Judeg() == 2) {

 

                x1 = (-b + Math.sqrt(b * b - 4 * a * c)) / (2 * a);

                x2 = (-b - Math.sqrt(b * b - 4 * a * c)) / (2 * a);

                y1 = A - B * x1;

                y2 = A - B * x2;

                if (IsMyPoint(new point(x1, y1)) < IsMyPoint(new point(x2, y2))) {

                   return new point(x1, y1);

                } else {

                   return new point(x2, y2);

                }

            } else if (Judeg() == 1) {

                return new point(-b / (2 * a), A - B * x1);

            } else {

                System.out.println("两个圆不相交");

                return null;

            }

        } else if (c1.getX() != c2.getX()) {

 

            // 当y1=y2时，x的两个解相等

            x1 = x2 = (c1.getX() * c1.getX() - c2.getX() * c2.getX() + c2.getR() * c2.getR() - c1.getR() * c1.getR())

                   / (2 * (c1.getX() - c2.getX()));

            a = 1;

            b = -2 * c1.getY();

            c = c1.getY() * c1.getY() - c1.getR() * c1.getR() + (x1 - c1.getX()) * (x1 - c1.getX());

            if (Judeg() == 2) {

                y1 = (-b + Math.sqrt(b * b - 4 * a * c)) / (2 * a);

                y2 = (-b - Math.sqrt(b * b - 4 * a * c)) / (2 * a);

                if (IsMyPoint(new point(x1, y1)) < IsMyPoint(new point(x2, y2))) {

                   return new point(x1, y1);

                } else {

                   return new point(x2, y2);

                }

            } else if (Judeg() == 1) {

                y1 = y2 = -b / (2 * a);

                return new point(x1, y1);

            } else {

                System.out.println("两个圆不相交");

                return null;

            }

        } else {

            System.out.println("两圆心重合");

            return null;

        }

 

    }

}

 

public class RobatHomework3 {

 

    public static void main(String args[]) {

        // 返回的是一个点对象

        Function function = new Function("(C 2 0) (T1 2 60)");

        function.GetIntersection().print();

        Function function2 = new Function("(C 2 0) (T1 2 -60)");

        function2.GetIntersection().print();

        Function function3 = new Function("(P7 7 0) (P8 7 -180)");

        function3.GetIntersection().print();

        Function function4 = new Function("(P8 22 0) (P7 15 30)");

        function4.GetIntersection().print();

        Function function5 = new Function("(P6 30 0) (P3 22.5 180)");

        function5.GetIntersection().print();

 

    }

}

 

运行效果图：

输入数据分别为：

1. (C 2 0) (T1 2 60) 其中T1（2,0）
2. (C 2 0) (T1 2 -60)
3. (P7 7 0) (P8 7 -180)
4. (P8 22 0) (P7 15 30)
5. (P6 30 0) (P3 22.5 180)

 

 

 

 

 

所用公式：