目录
- [一、gen_robot_tool_and_base_object_model_3d 函数调用](#一、gen_robot_tool_and_base_object_model_3d 函数调用)
- [二、gen_arrow_object_model_3d 函数调用](#二、gen_arrow_object_model_3d 函数调用)
首先说明一下这部分代码在find_box_3d这个例程中，非常好用的一个坐标系生成函数。
一、gen_robot_tool_and_base_object_model_3d 函数调用
csharp
RobotToolSize := 0.03
gen_robot_tool_and_base_object_model_3d (RobotToolSize * 0.05, RobotToolSize, OM3DToolOrigin, OM3DBase)
csharp
* This procedure creates 3D models that represent the tool and the base
* of the robot.
*
if (ArrowThickness <= 0)
throw ('ArrowThickness should be > 0')
endif
if (ArrowLength <= 0)
throw ('ArrowLength should be > 0')
endif
create_pose (0, 0, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', IdentityPose)
*
* 3D model for the tool.
create_pose (ArrowLength, 0, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransXPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransXPose, OM3DToolXOrigin)
create_pose (0, ArrowLength, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransYPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransYPose, OM3DToolYOrigin)
create_pose (0, 0, ArrowLength, 0, 0, 0, 'Rp+T', 'gba', 'point', TransZPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransZPose, OM3DToolZOrigin)
OM3DToolOrigin := [OM3DToolXOrigin,OM3DToolYOrigin,OM3DToolZOrigin]
*
* 3D model for the base.
FactorVisBase := ArrowThickness * 10
gen_box_object_model_3d (IdentityPose, FactorVisBase * 1.5, FactorVisBase * 1.5, FactorVisBase / 12.0, OM3DBasePlate)
create_pose (ArrowLength, 0, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransXPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransXPose, OM3DBaseX)
create_pose (0, ArrowLength, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransYPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransYPose, OM3DBaseY)
create_pose (0, 0, ArrowLength, 0, 0, 0, 'Rp+T', 'gba', 'point', TransZPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransZPose, OM3DBaseZ)
OM3DBase := [OM3DBaseX,OM3DBaseY,OM3DBaseZ,OM3DBasePlate]
return ()二、gen_arrow_object_model_3d 函数调用
这个函数绘制一个箭头，从ArrowStart点开始，到ArrowEnd点结束。
csharp
*
* This procedure draws an arrow that starts at the point ArrowStart and ends at ArrowEnd.
*
* Get parameters.
DirectionVector := ArrowEnd[0:2] - ArrowStart[0:2]
ArrowLength := sqrt(DirectionVector[0] * DirectionVector[0] + DirectionVector[1] * DirectionVector[1] + DirectionVector[2] * DirectionVector[2])
ConeRadius := 2.0 * ArrowThickness
ConeLength := min([2.0 * ConeRadius,ArrowLength * 0.9])
CylinderLength := ArrowLength - ConeLength
*
* Create cone.
pi := rad(180)
X := 0
Y := 0
Z := CylinderLength + ConeLength
for Index := 0 to 2 * pi by 0.1
X := [X,ConeRadius * cos(Index)]
Y := [Y,ConeRadius * sin(Index)]
Z := [Z,CylinderLength]
endfor
gen_object_model_3d_from_points (X, Y, Z, OM3DConeTmp)
convex_hull_object_model_3d (OM3DConeTmp, OM3DCone)
clear_object_model_3d (OM3DConeTmp)
*
* Create cylinder.
X := []
Y := []
for Index := 0 to 2 * pi by 0.1
X := [X,ArrowThickness * cos(Index)]
Y := [Y,ArrowThickness * sin(Index)]
endfor
tuple_gen_const (|Y|, 0, ZZero)
tuple_gen_const (|Y|, CylinderLength, ZTop)
gen_object_model_3d_from_points ([X,X], [Y,Y], [ZZero,ZTop], OM3DCylinderTmp)
convex_hull_object_model_3d (OM3DCylinderTmp, OM3DCylinder)
clear_object_model_3d (OM3DCylinderTmp)
*
* Union cone and cylinder Create arrow.
union_object_model_3d ([OM3DCone,OM3DCylinder], 'points_surface', OM3DArrowTmp)
clear_object_model_3d (OM3DCone)
clear_object_model_3d (OM3DCylinder)
Scale := CylinderLength / ArrowLength
OriginX := [0, 0, 0]
OriginY := [0, 0, 0]
OriginZ := [0,CylinderLength,ArrowLength]
TargetX := [ArrowStart[0],ArrowStart[0] + Scale * DirectionVector[0],ArrowEnd[0]]
TargetY := [ArrowStart[1],ArrowStart[1] + Scale * DirectionVector[1],ArrowEnd[1]]
TargetZ := [ArrowStart[2],ArrowStart[2] + Scale * DirectionVector[2],ArrowEnd[2]]
vector_to_hom_mat3d ('rigid', OriginX, OriginY, OriginZ, TargetX, TargetY, TargetZ, HomMat3D)
affine_trans_object_model_3d (OM3DArrowTmp, HomMat3D, OM3DArrow)
clear_object_model_3d (OM3DArrowTmp)
return ()获取参数：
计算方向向量DirectionVector，这是ArrowEnd和ArrowStart的前两个坐标的差。
计算箭头的长度ArrowLength，这是方向向量的长度（即三维空间中的直线距离）。
计算锥体的半径ConeRadius，这是箭头粗细ArrowThickness的两倍。
计算锥体的长度ConeLength，这是箭头长度的90%和两倍锥体半径中的较小值。
计算圆柱体的长度CylinderLength，这是箭头总长度减去锥体长度。
创建锥体：
使用循环生成锥体底面的点，这些点位于一个圆上，圆的半径为ConeRadius。
使用这些点生成一个临时三维对象模型OM3DConeTmp。
使用convex_hull_object_model_3d函数从临时模型生成一个凸包模型OM3DCone，这是最终的锥体模型。
清除临时模型OM3DConeTmp。
创建圆柱体：
使用循环生成圆柱体侧面的点，这些点位于一个圆上，圆的半径为ArrowThickness。
生成两个常数值数组ZZero和ZTop，分别代表圆柱体的底部和顶部在Z轴上的位置。
使用这些点生成一个临时三维对象模型OM3DCylinderTmp。
使用convex_hull_object_model_3d函数从临时模型生成一个凸包模型OM3DCylinder，这是最终的圆柱体模型。
清除临时模型OM3DCylinderTmp。
合并锥体和圆柱体以创建箭头：
使用union_object_model_3d函数将锥体和圆柱体合并成一个临时模型OM3DArrowTmp。
清除原始的锥体和圆柱体模型OM3DCone和OM3DCylinder。
调整箭头的位置和方向：
计算缩放比例Scale，这是圆柱体长度与箭头总长度的比例。
计算原点和目标点的坐标，用于将箭头从原点（临时模型的位置）变换到实际的位置和方向。
使用vector_to_hom_mat3d函数计算一个从原点到目标点的刚体变换矩阵HomMat3D。
使用affine_trans_object_model_3d函数应用这个变换矩阵，将箭头从临时模型OM3DArrowTmp变换到最终的位置和方向，生成最终的箭头模型OM3DArrow。
清除临时模型OM3DArrowTmp。