IntersectWGS84.cpp

// line intersection with WGS84 ellipsoid
FVector IntersectWGS84(FVector p0, FVector p1)
{
	FVector result(0, 0, 0);
	FVector center(0, 0, 0);
	double a = 6378137.0;
	double b = 6356752.314245;
	double x0 = p0.X, y0 = p0.Y, z0 = p0.Z;
	double x1 = p1.X, y1 = p1.Y, z1 = p1.Z;
	double cx = center.X, cy = center.Y, cz = center.Z;
	double m = x1 - x0, n = y1 - y0, p = z1 - z0;
	double A = (m * m + n * n) / (a * a) + p * p / (b * b);
	double B = 2 * ((m * (x0 - cx) + n * (y0 - cy)) / (a * a) + p * (z0 - cz) / (b * b));
	double C = ((x0 - cx) * (x0 - cx) + (y0 - cy) * (y0 - cy)) / (a * a) + (z0 - cz) * (z0 - cz) / (b * b) - 1;
	double test = B * B - 4.0 * A * C;
	if (test >= 0.0)
	{
		double t0 = (-B - sqrt(test)) / (2.0 * A);
		double t1 = (-B + sqrt(test)) / (2.0 * A);
		FVector lineNormal(m, n, p);
		FVector s1 = lineNormal * t0 + p0;
		FVector s2 = lineNormal * t1 + p0;
		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!NOTE!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
		// maybe you want both postion but I only need the one that follows the direction from p0 to p1
		result = s2;
	}
	return result;
}