点到直线距离C++源码实现

原理

思路1(如果不记得叉乘公式的话)：
利用点乘：

思路2：
利用叉乘：

代码实现:

定义Point结构体，Line结构体包含两个点成员，Vec结构体代表向量

#include <string>
#include <math.h>
using namespace std;

struct Vec{
    float x_;
    float y_;
    float z_;
    Vec(){}
    float dot(const Vec& other) const{
        return (x_*other.x_+y_*other.y_+z_*other.z_);
    }
    Vec cross_norm(const Vec& other) const{
        Vec res;
        res.x_ = y_*other.z_-z_*other.y_;
        res.y_ = -(x_*other.z_-z_*other.x_);
        res.z_ = y_*other.x_-x_*other.y_;
        return res;
    }
    float norm()const{
        return sqrt(x_*x_+y_*y_+z_*z_);
    }
};

struct Point{
    float x_;
    float y_;
    float z_;
    Point(float x,float y,float z=0):x_(x),y_(y),z_(z){}
    Vec operator-(const Point& b) const{
        Vec vec;
        vec.x_ = x_-b.x_;
        vec.y_ = y_-b.y_;
        vec.z_ = z_-b.z_;
        return vec;
    }
    float norm()const{
        return sqrt(x_*x_+y_*y_+z_*z_);
    }
};

struct Line{
    Point a_;
    Point b_;
    Line(Point a,Point b): a_(a),b_(b){}
};

float p2l(const Point& c, const Line& l){
    Point a = l.a_;
    Point b = l.b_;
    Vec vec0 = l.b_ - l.a_;
    Vec vec1 = c - l.a_;
    float res = vec0.dot(vec1);
    float cs = res / vec0.norm() / vec1.norm();
    float si = sqrt(1-cs*cs);
    return vec1.norm()*si;
}

float p2l2(const Point& c, const Line& l){
    Point a = l.a_;
    Point b = l.b_;
    Vec vec0 = l.b_ - l.a_;
    Vec vec1 = c - l.a_;
    Vec res = vec0.cross_norm(vec1);
    return res.norm()/vec0.norm();
}

int main(){
    Point A(1.0,1.3,0.5);
    Point B(2.0,1.0);
    Point C(28,1.33,99);
    Line L(A,B);
    float res = p2l(C,L);
    float res2 = p2l2(C,L);

    printf("dis of point to line is %f vs %f",res,res2);

    return 0;
}