合肥工业大学机器人技术期末_机器人技术第三次作业（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();

}

}

运行效果图：

输入数据分别为：

(C 2 0) (T1 2 60) 其中T1(2,0)

(C 2 0) (T1 2 -60)

(P7 7 0) (P8 7 -180)

(P8 22 0) (P7 15 30)

(P6 30 0) (P3 22.5 180)

所用公式：