/// <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); }