'L4、L5的C点移动。注意L4、L5质心不在L4、L5中点
Static Lcxy, Zold As Single
'前一位置旋转后c点坐标
point1(0) = Lcxy * Cos(Tem3): point1(1) = Lcxy * Sin(Tem3): point1(2) = Zold
'若为第一次运动
If NotFirstClick = False Then point1(0) = 0: point1(1) = 0: point1(2) = L1 + L2 + L3
End If
ObjL4.Move point1, CPoint
ObjL4.Update
ObjL5.Move point1, CPoint
ObjL5.Update
'铰C移动
JiaoC.Move JiaoC.Centroid, CPoint
JiaoC.Update
NotFirstClick = True
'L3在XY平面投影
Lcxy = L3 * Cos(Th1)
Zold = CPoint(2)
Static Lcxy, Zold As Single
'前一位置旋转后c点坐标
point1(0) = Lcxy * Cos(Tem3): point1(1) = Lcxy * Sin(Tem3): point1(2) = Zold
'若为第一次运动
If NotFirstClick = False Then point1(0) = 0: point1(1) = 0: point1(2) = L1 + L2 + L3
End If
ObjL4.Move point1, CPoint
ObjL4.Update
ObjL5.Move point1, CPoint
ObjL5.Update
'铰C移动
JiaoC.Move JiaoC.Centroid, CPoint
JiaoC.Update
NotFirstClick = True
'L3在XY平面投影
Lcxy = L3 * Cos(Th1)
Zold = CPoint(2)