合肥工业大学机器人技术期末_机器人技术第三次作业（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)
所用公式：
總結
以上是生活随笔為你收集整理的合肥工业大学机器人技术期末_机器人技术第三次作业（HFUT）的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： B 社公开《星空》游戏新内容：可购买房产
- 下一篇： tensorflow 小于_坐姿不对，屏