前方交会

srand(time(NULL));

double xA = rand();

double yA = rand();

double xB = rand();

double yB = rand();

double xP = rand();

double yP = rand();

double sign = 1.0;

double A = atan2(yP - yA,xP - xA) - atan2(yB - yA,xB - xA);

double B = atan2(yA - yB,xA - xB) - atan2(yP - yB,xP - xB);

if((yP - yA) * (xB - xA) - (xP - xA) * (yB - yA) < 0.0)

{//A,B,P与方位角增加方向相反

A = -A;

B = -B;

sign = -1.0;

}

double cA = 1.0 / tan(A);

double cB = 1.0 / tan(B);

double XP = (xA * cB + xB * cA + (yA - yB) * sign) / (cA + cB);

double YP = (yA * cB + yB * cA + (xB - xA) * sign) / (cA + cB);

double deltaX = XP - xP;

double deltaY = YP - yP;