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