// 坐标轴旋转
public static double[][] transferAxis(double[][] pref, double roataTheta) {
double[][] T = {
{Math.cos(roataTheta), -Math.sin(roataTheta), 0},
{Math.sin(roataTheta), Math.cos(roataTheta), 0},
{0, 0, 1}
};
double[][] p_rotate = new double[pref.length][pref[0].length];
double[][] pref_transfer = new double[pref[0].length][pref.length];
pref_transfer = MatrixUtil2.exchange(pref, pref_transfer);
for (int i = 0; i < pref.length; i++) {
p_rotate[i][0] =
T[0][0] * pref_transfer[0][i]
+ T[0][1] * pref_transfer[1][i]
+ T[0][2] * pref_transfer[2][i];
p_rotate[i][1] =
T[1][0] * pref_transfer[0][i]
+ T[1][1] * pref_transfer[1][i]
+ T[1][2] * pref_transfer[2][i];
p_rotate[i][2] =
T[2][0] * pref_transfer[0][i]
+ T[2][1] * pref_transfer[1][i]
+ T[2][2] * pref_transfer[2][i];
}
return p_rotate;
}
// 选取第三个坐标点
public static double[][] chooseFiremanPosition3(
double[][] pref,
double round1,
double theta1,
double theta2,
ArrayList<FiremanPosition> mLastFiremanPositionArrayList,
double roataTheta,
int[] indexResult) {
double[] temp_pref1 = {
pref[0][0] + round1 * Math.cos(theta1 + theta2),
pref[0][1] + round1 * Math.sin(theta1 + theta2),
0
};
pref[2][0] = pref[0][0] + round1 * Math.cos(theta1 + theta2);
pref[2][1] = pref[0][1] + round1 * Math.sin(theta1 + theta2);
pref[2][2] = 0.0;
if (mLastFiremanPositionArrayList == null || mLastFiremanPositionArrayList.size() < 3) {
pref[2][0] = pref[0][0] + round1 * Math.cos(theta2 - theta1);
pref[2][1] = pref[0][1] + round1 * Math.sin(theta2 - theta1);
pref[2][2] = 0;
Log.i("roataTheta", "第三个坐标点x：" + pref[2][0]);
Log.i("roataTheta", "第三个坐标点y：" + pref[2][1]);
Log.i("roataTheta", "第三个坐标点z：" + pref[2][2]);
return pref;
}
double[][] error_reftemp1 = MatrixUtil2.transferAxis(pref, roataTheta);
// Log.i("Vincent","error_reftemp1 x3: "+error_reftemp1[2][0]);
// Log.i("Vincent", "error_reftemp1 y3: " + error_reftemp1[2][1]);
Log.i("Vincent", "last x3: " + mLastFiremanPositionArrayList.get(2).getX());
Log.i("Vincent", "last y3: " + mLastFiremanPositionArrayList.get(2).getY());
double distance1 =
Math.sqrt(
Math.pow(
error_reftemp1[2][0] - mLastFiremanPositionArrayList.get(indexResult[2]).getX(),
2)
+ Math.pow(
error_reftemp1[2][1] - mLastFiremanPositionArrayList.get(indexResult[2]).getY(),
2));
pref[2][0] = pref[0][0] + round1 * Math.cos(theta2 - theta1);
pref[2][1] = pref[0][1] + round1 * Math.sin(theta2 - theta1);
pref[2][2] = 0;
double[] temp_pref2 = {
pref[0][0] + round1 * Math.cos(theta2 - theta1),
pref[0][1] + round1 * Math.sin(theta2 - theta1),
0
};
double[][] error_reftemp2 = MatrixUtil2.transferAxis(pref, roataTheta);
// Log.i("Vincent","error_reftemp2 x3: "+error_reftemp2[2][0]);
// Log.i("Vincent", "error_reftemp2 y3: " + error_reftemp2[2][1]);
Log.i("Vincent", "last x3: " + mLastFiremanPositionArrayList.get(2).getX());
Log.i("Vincent", "last y3: " + mLastFiremanPositionArrayList.get(2).getY());
double distance2 =
Math.sqrt(
Math.pow(
error_reftemp2[2][0] - mLastFiremanPositionArrayList.get(indexResult[2]).getX(),
2)
+ Math.pow(
error_reftemp2[2][1] - mLastFiremanPositionArrayList.get(indexResult[2]).getY(),
2));
Log.i("Vincent", "distance1: " + distance1);
Log.i("Vincent", "distance2: " + distance2);
if (distance1 <= distance2) {
pref[2][0] = temp_pref1[0];
pref[2][1] = temp_pref1[1];
pref[2][2] = temp_pref1[2];
} else {
pref[2][0] = temp_pref2[0];
pref[2][1] = temp_pref2[1];
pref[2][2] = temp_pref2[2];
}
Log.i("roataTheta", "第三个坐标点x：" + pref[2][0]);
Log.i("roataTheta", "第三个坐标点y：" + pref[2][1]);
Log.i("roataTheta", "第三个坐标点z：" + pref[2][2]);
return pref;
}