題目：已知2個點的信息，定位自己的絕對坐標。

解題思路：分別以已知兩點為圓心，以兩點到自己的距離為半徑做兩個圓，兩個圓的交點，其中有一個為自己的坐標,最后用極角來判斷哪個點是自己的位置

import java.util.Scanner; import java.util.function.DoubleToLongFunction; class VecPosition//坐標{public double mx;public double my;public void setpoint(double x,double y){mx = x;my = y;}} class Circle圓 {VecPosition center;double R;void setcircle(VecPosition my_center, double my_R){center = my_center;R = my_R;} }class GetInformation extends VecPosition //從字符串中獲取所需要的信息 {VecPosition get_p;double distance;double theta; Position getposition(String[] mymessage,int i)//獲取點的坐標{ VecPosition get_p = new VecPosition();if(mymessage[i].equals('C')==true) get_p.setpoint(0, 0); else if(mymessage[i].equals("P1")==true) get_p.setpoint(-52.4,-32.0); else if(mymessage[i].equals("P2")==true) get_p.setpoint(-52.4,32.0); else if(mymessage[i].equals("P3")==true)get_p.setpoint(52.5,32.0); else if(mymessage[i].equals("P4")==true)get_p.setpoint(52.5,-32.0); else if(mymessage[i].equals("P5")==true) get_p.setpoint(0,-32 ); else if(mymessage[i].equals("P6")==true)get_p.setpoint(0,32); else if(mymessage[i].equals("P7")==true)get_p.setpoint(-30,-7); else if(mymessage[i].equals("P8")==true)get_p.setpoint(-30,7); else if(mymessage[i].equals("P9")==true)get_p.setpoint(30, 7); else if(mymessage[i].equals("P10")==true)get_p.setpoint(30,-7); else if(mymessage[i].equals("G1")==true)get_p.setpoint(-52.5,0); else if(mymessage[i].equals("G2")==true)get_p.setpoint(-52.5,0); else get_p=null;return get_p;}double getdistance(String[] distance,int i)//獲取點到點的距離{ this.distance = Double.parseDouble(distance[i]); return this.distance;}double gettheta(String[] theta,int i)//獲取極角{this.theta= Double.parseDouble(theta[i]);return this.theta ;}}public class Robot extends Circle{VecPosition getmyposition(Circle c1, Circle c2,double theta1,double theta2){//判斷兩個圓的交點,求出自身坐標VecPosition P1 = new VecPosition();VecPosition P2 = new VecPosition();double a1, b1, R1, a2, b2, R2, theta_value;a1 = c1.center.mx;b1 = c1.center.my;R1 = c1.R;a2 = c2.center.mx;b2 = c2.center.my;R2 = c2.R;theta_value = theta2 - theta1;double R1R1 = R1*R1;double a1a1 = a1*a1;double b1b1 = b1*b1;double a2a2 = a2*a2;double b2b2 = b2*b2;double R2R2 = R2*R2;double subs1 = a1a1 - 2 * a1*a2 + a2a2 + b1b1 - 2 * b1*b2 + b2b2;double subs2 = -R1R1 * a1 + R1R1 * a2 + R2R2 * a1 - R2R2 * a2 + a1a1*a1 - a1a1 * a2 - a1*a2a2 + a1*b1b1 - 2 * a1*b1*b2 + a1*b2b2 + a2a2*a2 + a2*b1b1 - 2 * a2*b1*b2 + a2*b2b2;double subs3 = -R1R1 * b1 + R1R1 * b2 + R2R2 * b1 - R2R2 * b2 + a1a1*b1 + a1a1 * b2 - 2 * a1*a2*b1 - 2 * a1*a2*b2 + a2a2 * b1 + a2a2 * b2 + b1b1*b1 - b1b1 * b2 - b1*b2b2 + b2b2*b2;double sigma = Math.sqrt((R1R1 + 2 * R1*R2 + R2R2 - a1a1 + 2 * a1*a2 - a2a2 - b1b1 + 2 * b1*b2 - b2b2)*(-R1R1 + 2 * R1*R2 - R2R2 + subs1));if(Math.abs(subs1)>0.0000001)//分母不為0{if(theta_value<0){P1.mx = (subs2 - sigma*b1 + sigma*b2) / (2 * subs1);P1.my = (subs3 + sigma*a1 - sigma*a2) / (2 * subs1);}else{P1.mx= (subs2 + sigma*b1 - sigma*b2) / (2 * subs1);P1.my = (subs3 - sigma*a1 + sigma*a2) / (2 * subs1);}}return P1;}public static void main(String[] args)throws NullPointerException {// TODO Auto-generated method stubVecPosition my_positon,pos1=null,pos2=null ;//分別表示我的絕對位置，獲取的兩個點 Circle c1 = new Circle();Circle c2 = new Circle();double R1,R2;//圓的半徑double theta1,theta2;//圓的極角 Scanner in = new Scanner(System.in); String str=in.nextLine(); String strafter2=str.replaceAll("\\(|\\)", "\\,"); String[] mymessage=strafter2.split("\\s+|\\,"); //將字符創轉化為字符數組；Robot Getposition1 = new Robot();GetInformation get_p1 = new GetInformation();GetInformation get_p2 = new GetInformation(); pos1 = get_p1.getposition(mymessage,1);//獲取點的坐標pos2 = get_p2.getposition(mymessage,5);R1 = get_p1.getdistance(mymessage,2);//獲取點到我的位置的距離R2 = get_p2.getdistance(mymessage,6);theta1 =get_p1.gettheta(mymessage,3);//獲取極角theta2 =get_p2.gettheta(mymessage,7);c1.setcircle(pos1, R1); //設置圓c2.setcircle(pos2, R2);my_positon= Getposition1.getmyposition(c1, c2,theta1,theta2);//求我的位置；System.out.println("Myposition: "+ "("+my_positon.mx+" , "+my_positon.my+")");//打印我的位置的坐標；}}

總結

以上是生活随笔為你收集整理的足球机器人第三次作业：求自己的位置的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。