已知直线的两个端点 (x1,y1)(x2,y2)直线的长度和圆心(x3,y3)圆半径R,求直线与圆的交点坐标

2008-09-24 11:30:59 993 阅读 AI 写同款 · GEO 优化AI 选题数据监测一键分发 ›

机器人笛卡尔空间坐标系轨迹规划的研究 1引言随着6R机械人的使用性越来越广，对机械人的轨迹规划的要求也越来越高，机器人轨迹规划是机器人执行作业任务的基础。使用的轨迹规划算法，直接决定了机器人的运动轨迹、平滑程度，且使点位运动有足够高的精度且满足规划路径的约束条件。本文将在对机器人完成建模以及运动学解算后的基础上，对机器人进行笛卡尔空间轨迹规划的研究，主要分为直线轨迹规划与圆弧轨迹规划。 2任务位置规划 ... 阅读详情

代码如下:
Option Explicit

Private Type MyPoint
x As Single
y As Single
End Type

Private Sub Command1_Click()
Dim MyA As MyPoint
Dim MyB As MyPoint
Dim MyC As MyPoint
Dim myD As MyPoint
Dim K As Double
Dim R As Double
MyA.x = 1000
MyA.y = 1000
MyB.x = 3500
MyB.y = 5000
R = 1500
Me.Line (MyA.x, MyA.y)-(MyB.x, MyB.y), vbGreen
K = (MyA.y - MyB.y) / (MyA.x - MyB.x)
Me.Circle (MyB.x, MyB.y), R, vbRed
myD = stuSlope(MyB, MyA, MyB, K, R)
Me.DrawWidth = 3
Me.Line (myD.x, myD.y)-(myD.x, myD.y), vbBlue
End Sub

''倾斜且线段一点在圆内返回横纵坐标
Private Function stuSlope(MyA As MyPoint, MyB As MyPoint, MyR As MyPoint, ByVal K As Single, ByVal R As Single) As MyPoint
Dim slTemp As MyPoint
Dim slaNx1 As Single, slaNx2 As Single, flaNx As Single, B As Single
''直线截距用于简化后续计算
B = MyA.y - K * (MyA.x)
slaNx1 = (MyR.x - K * (B - MyR.y) + Sqr((1 + K * K) * R * R + 2 * K * (MyR.x) * (MyR.y - B) - ((MyR.y - B) ^ 2) - (K * (MyR.x) ^ 2))) / (1 + K * K)
slaNx2 = (MyR.x - K * (B - MyR.y) - Sqr((1 + K * K) * R * R + 2 * K * (MyR.x) * (MyR.y - B) - ((MyR.y - B) ^ 2) - (K * (MyR.x) ^ 2))) / (1 + K * K)
If (slaNx1 - MyA.x) * (slaNx1 - MyB.x) < 0 Then
flaNx = slaNx1
Else
flaNx = slaNx2
End If
slTemp.x = flaNx
slTemp.y = MyA.y + K * (flaNx - MyA.x)
stuSlope = slTemp
End Function

''计算两点间距离的函数
Private Function flaDis(Myf As MyPoint, Mys As MyPoint) As Single
flaDis = Sqr((Myf.x - Mys.x) ^ 2 + (Myf.y - Mys.y) ^ 2)
End Function

''斜率计算函数
Private Function flaK(MyA As MyPoint, MyB As MyPoint) As Single
flaK = (MyA.y - MyB.y) / (MyA.x - MyB.x)
End Function

''计算直线与圆心距离,SV用于标识斜率是否存在(1为存在0为不存在)
Private Function flaRDis(MyA As MyPoint, MyB As MyPoint, MyR As MyPoint, ByVal K As Single, ByVal SV As Byte) As Single
If SV = 0 Then
flaRDis = Abs(MyR.x - MyA.x)
Else
flaRDis = Abs(K * MyR.x - MyR.y - K * MyA.x + MyA.y) / Sqr((K ^ 2) + 1)
End If
End Function

''直线竖直放置时圆交点的纵坐标计算
Private Function flaVerty(MyA As MyPoint, MyB As MyPoint, MyR As MyPoint, ByVal R As Single) As Single
Dim flaNy1 As Single, flaNy2 As Single
flaNy1 = MyR.y + Sqr((R ^ 2) - ((MyA.x - MyR.x) ^ 2))
flaNy2 = MyR.y - Sqr((R ^ 2) - ((MyA.x - MyR.x) ^ 2))
If (flaNy1 - MyA.y) * (flaNy1 - MyB.y) < 0 Then
flaVerty = flaNy1
Else
flaVerty = flaNy2
End If
End Function

''计算交点的函数
''PN:取负值点和正值点的区分(1为正点0为负点)
''SV:斜率是否存在的区分标志(1为存在0为不存在)
Private Function stuCross(MyA As MyPoint, MyB As MyPoint, MyR As MyPoint, _
ByVal K As Double, ByVal R As Double, ByVal SV As Byte, _
PN As Long) As MyPoint
Dim scTemp As MyPoint
Dim flaCx1 As Double, flaCx2 As Double, flaCy1 As Double, flaCy2 As Double
Dim flaCx As Double, flaCy As Double
Dim B As Double
''先计算斜率存在的情况
If SV = 1 Then
''直线截距用于简化后续计算
B = MyA.y - K * MyA.x
flaCx1 = (MyR.x - K * (B - MyR.y) + Sqr((1 + K * K) * R * R + 2 * K * (MyR.x) * (MyR.y - B) - ((MyR.y - B) ^ 2) - (K * (MyR.x) ^ 2))) / (1 + K * K)
flaCx2 = (MyR.x - K * (B - MyR.y) - Sqr((1 + K * K) * R * R + 2 * K * (MyR.x) * (MyR.y - B) - ((MyR.y - B) ^ 2) - (K * (MyR.x) ^ 2))) / (1 + K * K)
If 1 = PN Then
flaCx = flaCx1
Else
flaCx = flaCx2
flaCy = MyA.y + K * (flaCx - MyA.x)
End If
Else
flaCy1 = MyR.y + Sqr((R ^ 2) - ((MyA.x - MyA.x) ^ 2))
flaCy2 = MyR.y - Sqr((R ^ 2) - ((MyA.x - MyR.x) ^ 2))
If 1 = PN Then
flaCy = flaCy1
Else
flaCy = flaCy2
flaCx = MyA.x
End If
End If
scTemp.x = flaCx
scTemp.y = flaCy
stuCross = scTemp
End Function

结果如下: