/// <summary>
/// 获取和rpd1与rpd2在空间中垂直的经过rpd1的两个顶点
/// </summary>
/// <param name="rpd1"></param>
/// <param name="rpd2"></param>
/// <param name="v1"></param>
/// <param name="v2"></param>
//public static void GetVerticalVector(PointXYZR rpd1, PointXYZR rpd2, out Vector<double> v1, out Vector<double> v2)
//{
// double a1 = rpd1.X, b1 = rpd1.Y, c1 = rpd1.Z;
// double a2 = rpd2.X, b2 = rpd2.Y, c2 = rpd2.Z;
// //计算垂直于该线段的点坐标
// //double a3 = rpd1.X - StaticVar.Parameters.workpieceLength * Math.Sin(rpd1.Ry);
// //double b3 = rpd1.Y + StaticVar.Parameters.workpieceLength * Math.Sin(rpd1.Rx);
// //double c3 = rpd1.Z + StaticVar.Parameters.workpieceLength * (2 - Math.Cos(rpd1.Rx) - Math.Cos(rpd1.Ry));
// double a3, b3, c3;
// GetVerticalPoint(rpd1, out a3, out b3, out c3);
// double m1 = a1 - a2, n1 = b1 - b2, p1 = c1 - c2;
// double m2 = a1 - a3, n2 = b1 - b3, p2 = c1 - c3;
// double s = p1 * m2 - p2 * m1;
// double t = m2 * n1 - m1 * n2;
// double Z = 1;
// double Y = s * Z / t + c1 * s / t + b1;
// double X = (p1 * (c1 - Z) + n1 * (b1 - Y)) / m1 + a1;
// if (rpd1.Rx == 0 && rpd1.Ry == 0)
// {
// X = rpd1.X - 100 * Math.Sin(toRadian(rpd1.Rz));
// Y = rpd1.Y + 100 * Math.Cos(toRadian(rpd1.Rz));
// Z = rpd1.Z;
// }
// v1 = Vector<double>.Build.DenseOfArray(new double[] { rpd1.X, rpd1.Y, rpd1.Z });
// v2 = Vector<double>.Build.DenseOfArray(new double[] { X, Y, Z });
//}
/// <summary>
/// 获取在空间中垂直于当前点的坐标，即回撤点
/// </summary>
/// <param name="rpd"></param>
/// <param name="newX"></param>
/// <param name="newY"></param>
/// <param name="newZ"></param>
public static void GetVerticalPoint(RobotPositionData rpd, out double newX, out double newY, out double newZ)
{
newX = rpd.X - 100 * Math.Sin(toRadian(rpd.Ry));
newY = rpd.Y + 100 * Math.Sin(toRadian(rpd.Rx));
//newZ = rpd.Z - 100 * (Math.Cos(toRadian(rpd.Rx)) + Math.Cos(toRadian(rpd.Ry)));
newZ = rpd.Z - 100 * Math.Cos(toRadian(rpd.Rx)) * Math.Cos(toRadian(rpd.Ry));
}
/// <summary>
/// 把一个2D的坐标点转化为3D的坐标点
/// </summary>
/// <param name="bmpname"></param>
/// <param name="sourceRPD"></param>
/// <returns></returns>
public static RobotPositionData TransTo3DRPD(string bmpname, double x, double y, double z, double rx, double ry, double rz)
{
RobotPositionData result = new RobotPositionData(x, y, z, rx, ry, rz);
Matrix <double> T1 = Matrix <double> .Build.Dense(4, 4);
double Trx = 0, Try = 0, Trz = 0;
bool res = ResolveImageName(bmpname, ref Trx, ref Try, ref Trz, ref T1);
if (!res) //如果不是3D文件格式，则是2D文件格式，直接返回原始
{
return(result);
}
Vector <double> v = Vector <double> .Build.DenseOfArray(new double[] { x, y, z, 1.0 });
Vector <double> newv = T1 * v;
result.X = newv[0];
result.Y = newv[1];
result.Z = newv[2];
result.Rx = rx + Trx;
result.Ry = ry + Try;
result.Rz = rz + Trz;
return(result);
}
/// <summary>
/// 获取和rpd1与rpd2在空间中垂直的经过rpd1的两个顶点
/// </summary>
/// <param name="rpd1"></param>
/// <param name="rpd2"></param>
/// <param name="v1"></param>
/// <param name="v2"></param>
public static void GetVerticalVector(RobotPositionData rpd1, RobotPositionData rpd2, out Vector <double> v1, out Vector <double> v2)
{
double a1 = rpd1.X, b1 = rpd1.Y, c1 = rpd1.Z;
double a2 = rpd2.X, b2 = rpd2.Y, c2 = rpd2.Z;
//计算垂直于该线段的点坐标
//double a3 = rpd1.X - StaticVar.Parameters.workpieceLength * Math.Sin(rpd1.Ry);
//double b3 = rpd1.Y + StaticVar.Parameters.workpieceLength * Math.Sin(rpd1.Rx);
//double c3 = rpd1.Z + StaticVar.Parameters.workpieceLength * (2 - Math.Cos(rpd1.Rx) - Math.Cos(rpd1.Ry));
double a3, b3, c3;
GetVerticalPoint(rpd1, out a3, out b3, out c3);
double m1 = a1 - a2, n1 = b1 - b2, p1 = c1 - c2;
double m2 = a1 - a3, n2 = b1 - b3, p2 = c1 - c3;
double s = p1 * m2 - p2 * m1;
double t = m2 * n1 - m1 * n2;
double Z = 1;
double Y = s * Z / t + c1 * s / t + b1;
double X = (p1 * (c1 - Z) + n1 * (b1 - Y)) / m1 + a1;
if (rpd1.Rx == 0 && rpd1.Ry == 0)
{
X = rpd1.X - 100 * Math.Sin(toRadian(rpd1.Rz));
Y = rpd1.Y + 100 * Math.Cos(toRadian(rpd1.Rz));
Z = rpd1.Z;
}
v1 = Vector <double> .Build.DenseOfArray(new double[] { rpd1.X, rpd1.Y, rpd1.Z });
v2 = Vector <double> .Build.DenseOfArray(new double[] { X, Y, Z });
}
/// <summary>
/// 把2D图像中的点坐标转换为原始3D点云坐标
/// </summary>
/// <param name="basePoint"></param>
/// <param name="rx"></param>
/// <param name="ry"></param>
/// <param name="rz"></param>
/// <param name="tx"></param>
/// <param name="ty"></param>
/// <param name="tz"></param>
/// <returns></returns>
public static RobotPositionData Restore2OCS(RobotPositionData basePoint, double rx, double ry, double rz, double tx, double ty, double tz)
{
double A = Math.Cos(rz), B = Math.Sin(rz), C = Math.Cos(ry), D = Math.Sin(ry), E = Math.Cos(rx), F = Math.Sin(rx);
Matrix <double> T1 = DenseMatrix.OfArray(new double[, ]
{
{ A *C, A *D *F - B *E, B *F + A *D *E, tx },
{ B *C, A *E - B *D *F, B *D *E - A *F, ty },
{ -D, C * F, C * E, tz },
{ 0, 0, 0, 1 }
});
double Trx = Math.Atan2(T1[2, 1], T1[2, 2]) / Math.PI * 180;
double Try = Math.Asin(-T1[2, 0]) / Math.PI * 180;
double Trz = Math.Atan2(T1[1, 0], T1[0, 0]) / Math.PI * 180;
Vector <double> v = Vector <double> .Build.DenseOfArray(new double[] { basePoint.X, basePoint.Y, basePoint.Z, 1.0 });
Vector <double> newv = T1 * v;
double newrx = basePoint.Rx + Trx, newry = basePoint.Ry + Try, newrz = basePoint.Rz + Trz;
return(new RobotPositionData(newv[0], newv[1], newv[2], newrx, newry, newrz));
}
public static RobotPositionData[] GetRPD(RobotPositionData rpd1, RobotPositionData rpd2)
{
return(null);
}
