Function compute_point_ray_distance_c

pointersect/pr/cpp/pr.cpp:741–763

Compute the distance between a point and its projection on a ray. Args: p_p: (3, ) float pointer to xyz p_ro: (3, ) float pointer to xyz of ray origin p_rd: (3, ) float pointer to ray direction (normalized) Returns: dist: float t: float

//      t:     float
// 
inline std::tuple<float, float> compute_point_ray_distance_c(
    const float * p_p,  // (3,)
    const float * p_ro,  // (3,)
    const float * p_rd  // (3,)
) {
    float dv[3] = {
        p_p[0] - p_ro[0], 
        p_p[1] - p_ro[1], 
        p_p[2] - p_ro[2] 
    };

    float t = dv[0] * p_rd[0] + dv[1] * p_rd[1] + dv[2] * p_rd[2];

    float dd[3] = {
        p_p[0] - (p_ro[0] + t * p_rd[0]),
        p_p[1] - (p_ro[1] + t * p_rd[1]),
        p_p[2] - (p_ro[2] + t * p_rd[2])
    };

    float dist = sqrt(dd[0] * dd[0] + dd[1] * dd[1] + dd[2] * dd[2]);

    return std::make_tuple(dist, t);
}