plane(const cv::Vec3f& point, const cv::Vec3f& normal) { a = normal[0]; b = normal[1]; c = normal[2]; d = -(point.dot(normal)); }
plane(const cv::Vec3f& point1, const cv::Vec3f& point2, const cv::Vec3f& point3) { cv::Vec3f v1 = point2 - point1; cv::Vec3f v2 = point3 - point1; cv::Vec3f normal = (v1.cross(v2)); normal /= cv::norm(normal, cv::NORM_L2); a = normal[0]; b = normal[1]; c = normal[2]; d = -(point1.dot(normal)); }
float Utility::vecAngle(cv::Vec3f& v1, cv::Vec3f& v2) { float cosVal = v1.dot(v2); return std::acos(cosVal) * 57.29578049; }