public static bool VectAngle(DataType.StaubliRobotData.St_PointRx x_pPoint1, DataType.StaubliRobotData.St_PointRx x_pPoint2, DataType.StaubliRobotData.St_PointRx x_pPoint3, ref double x_nAngle) { BasicDataType.vector l_v1, l_v2, l_v3; l_v1 = PointTool.Point2Vector(x_pPoint1); l_v2 = PointTool.Point2Vector(x_pPoint2); l_v3 = PointTool.Point2Vector(x_pPoint3); bool l_bResult = VectAngle(l_v1, l_v2, l_v3, ref x_nAngle); return(l_bResult); }
public static bool VectAngle(DataType.BasicDataType.vector x_vVector1, DataType.BasicDataType.vector x_vVector2, DataType.BasicDataType.vector x_vVector3, ref double x_nAngle) { DataType.BasicDataType.vector l_vCenter = new BasicDataType.vector(); double l_nRadius = new double(); bool l_bResult = PointTool.CreatCircle(x_vVector1, x_vVector2, x_vVector3, out l_vCenter, out l_nRadius); if (l_bResult == true) { double l_nDistance1 = VectorDistance(x_vVector1, x_vVector2); double l_nDistance2 = VectorDistance(x_vVector2, x_vVector3); double l_ntheta1 = 2 * Math.Asin(l_nDistance1 / (2 * l_nRadius)); double l_ntheta2 = 2 * Math.Asin(l_nDistance2 / (2 * l_nRadius)); double l_ntheta = l_ntheta1 + l_ntheta2; x_nAngle = Radian2Angle(l_ntheta); return(true); } else { return(false); } }