DQ_Kinematics::point_to_line_segment / line_segment_to_point

[chewheel]

Murilo sensei, I think having these jacobians can make many problems simpler. I wish these methods could be added if you have any time and are pleased to do so. :>

static MatrixXd point_to_line_segment_squared_distance_jacobian(const MatrixXd& robot_point_jacobian,
    const DQ& robot_point,
    const DQ& workspace_line,
    const DQ& workspace_point_1,
    const DQ& workspace_point_2)
{
    DQ projection_point = workspace_point_1 + P(workspace_line) * dot(P(workspace_line), (robot_point - workspace_point_1));
    double d_projection_point_to_line_point1 = norm(vec3(projection_point - workspace_point_1));
    double d_projection_point_to_line_point2 = norm(vec3(projection_point - workspace_point_2));
    double line_segment_length = norm(vec3(workspace_point_1 - workspace_point_2));
    if (d_projection_point_to_line_point1 <= line_segment_length && d_projection_point_to_line_point2 <= line_segment_length)
        // line_state = body
        return DQ_Kinematics::point_to_line_distance_jacobian(robot_point_jacobian, robot_point, workspace_line);
    else if (d_projection_point_to_line_point1 < d_projection_point_to_line_point2)
        // point 1 
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_jacobian, robot_point, workspace_point_1);
    else
        // point 2
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_jacobian, robot_point, workspace_point_2);
}

static MatrixXd line_segement_to_point_squared_distance_jacobian(const MatrixXd& robot_line_jacobian,
    const MatrixXd& robot_point_1_translation_jacobian,
    const MatrixXd& robot_point_2_translation_jacobian,
    const DQ& robot_line,
    const DQ& robot_point_1,
    const DQ& robot_point_2,
    const DQ& workspace_point)
{
    DQ projection_point = robot_point_1 + P(robot_line) * dot(P(robot_line), (workspace_point - robot_point_1));
    double d_projection_point_to_line_point1 = norm(vec3(projection_point - robot_point_1));
    double d_projection_point_to_line_point2 = norm(vec3(projection_point - robot_point_2));
    double line_segment_length = norm(vec3(robot_point_1 - robot_point_2));
    if (d_projection_point_to_line_point1 <= line_segment_length && d_projection_point_to_line_point2 <= line_segment_length)
        // line_state = body
        return DQ_Kinematics::line_to_point_distance_jacobian(robot_line_jacobian, robot_line, workspace_point);
    else if (d_projection_point_to_line_point1 < d_projection_point_to_line_point2)
        // point 1 
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_1_translation_jacobian, robot_point_1, workspace_point);
    else
        // point 2
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_2_translation_jacobian, robot_point_2, workspace_point);
}