opencv点到直线垂线计算

#include “opencv2/opencv.hpp”

void FindFoot(cv::Point2f pntSart, cv::Point2f pntEnd, cv::Point2f pA, cv::Point2f &pFoot)
{
float k = 0.0;
if(pntSart.x == pntEnd.x)
{
pFoot.x = pntSart.x;
pFoot.y = pA.y;
return;
}
k = (pntEnd.y - pntSart.y) * 1.0 / (pntEnd.x - pntSart.x);
float A = k;
float B = -1.0;
float C = pntSart.y - k * pntSart.x;

pFoot.x = (B * B * pA.x -A * B * pA.y-A * C) / (A * A + B * B);
pFoot.y = (A * A * pA.y - A * B * pA.x-B * C) / (A * A + B * B);

}

int _tmain(int argc, _TCHAR* argv[])
{
cv::Mat matBgrImg = cv::Mat(600, 600, CV_8UC3, cv::Scalar(0,0,0));

std::cout << matBgrImg.cols << std::endl;
cv::Point2f p1(100, 200);
cv::Point2f p2(123, 200);
cv::Point2f pA(10, 20);
cv::Point2f pFoot;

FindFoot(p1, p2, pA, pFoot);
std::cout << pFoot.x << " " << pFoot.y << std::endl;

cv::line(matBgrImg, p1, p2, cv::Scalar(0, 255, 255, 0), 1);
cv::line(matBgrImg, pA, pFoot, cv::Scalar(255, 0, 0, 0), 1);

cv::imshow("name", matBgrImg);
cv::waitKey(0);
std::cout << "Hello, World!" << std::endl;
return 0;

}