空间两条直线求交点 lua代码

假设直线Line1经过p1和p2点,Line2经过p3和p4点,计算两条直线的交点坐标:可以使用以下代码:

为了避免两条直线异面不相交,此处假设Line1和Line2的高度差不多,返回计算交点的x和y,z为p1的高度

返回的姿态是p2和p3点的slerp插补姿态

-- Author: chenliao@elibot.cn
-- Aug.4,2023
function GetCrossPoint(point1, point2, point4, point3)
    -- calculate line_point1_point4 with line_point2_point3 cross point
    local pD_x = point1[1]
    local pD_y = point1[2]
    local pA_x = point2[1]
    local pA_y = point2[2]

    local pC_x = point3[1]
    local pC_y = point3[2]
    local pB_x = point4[1]
    local pB_y = point4[2]
    local outpos = {}
    outpos[1] = point1[1]
    outpos[2] = point1[2]
    outpos[3] = point1[3]
    outpos[4] = point1[4]
    outpos[5] = point1[5]
    outpos[6] = point1[6]

    outpos[2] =
        (pB_x * pC_y * pD_y - pD_x * pB_y * pC_y - pA_y * pB_x * pD_y + pD_x * pB_y * pA_y + pC_x * pA_y * pD_y -
        pA_x * pC_y * pD_y -
        pC_x * pB_y * pA_y +
        pA_x * pB_y * pC_y) /
        (pD_y * pC_x - pA_x * pD_y - pB_y * pC_x + pA_x * pB_y + pB_x * pC_y - pD_x * pC_y - pA_y * pB_x + pA_y * pD_x)

    outpos[1] =
        (pD_y * (pC_x - pA_x) * (pB_x - pD_x) - pA_y * (pC_x - pA_x) * (pB_x - pD_x) +
        pA_x * (pC_y - pA_y) * (pB_x - pD_x) +
        pD_x * (pD_y - pB_y) * (pC_x - pA_x)) /
        ((pC_y - pA_y) * (pB_x - pD_x) + (pD_y - pB_y) * (pC_x - pA_x))

    return outpos
end

function CalCornerTarget(p1,p2,p3,p4)
    local p = GetCrossPoint(p1,p3,p2,p4) -- 计算p1p2和p3p4两条直线的交点(仅关注xy,返回z值同p1点的z值,避免两条直线异面不相交,返回姿态为p2和p3的slerp插补)
    p_ori = (p2[6]+p3[6])/2
    local p_ori = get_interp_pose(p2,p3,0.5)
    if p==nil then
        return {0,0,0,p_ori[4],p_ori[5],p_ori[6]}
    else
        -- return {p[1],p[2],p[3],p2[4],p2[5],p_ori}
        return {p[1],p[2],p[3],p_ori[4],p_ori[5],p_ori[6]}
    end    
end

v011 = {get_global_variable('V011')}
v012 = {get_global_variable('V012')}
v013 = {get_global_variable('V013')}
v014 = {get_global_variable('V014')}
v015 = CalCornerTarget(v011,v012,v013,v014)
点击显示全文
赞同0
发表评论
分享

手机扫码分享
0
97
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
举报
请选择举报理由
举报
举报说明