cv::Point3f GeometryUtils::intersection(const cv::Vec3f &ray,
const cv::Point3f& normal, const cv::Point3f& point) {
double num = -normal.dot(point);
double den = normal.dot(ray);
double t = num / den;
cv::Point3f intersection(t * ray[0], t * ray[1], t * ray[2]);
return intersection;
}
// The normal vector must be of length 1
float distPointToPlane (cv::Point3f point, cv::Point3f normal) {
float sn, sd, sb;
sn = -normal.dot(point);
//sd = normal.dot(normal);
//sb = sn/sb;
//std::cout << "sn, sd, sb: " << sn << ", " << sd << ", " << sb << endl;
/*
sn = -dot( PL.n, (P - PL.V0));
sd = dot(PL.n, PL.n);
sb = sn / sd;
*/
return sn;
}