计算直线与WGS84椭球的交点

/************************************************************************/
/*线段与WGS84椭球求交
x^2/a^2+y^2/a^2+z^2/b^2=1
(x-x0)/m=(y-y0)/n=(z-z0)/p=t
m=x1-x0
n=y1-y0
p=z1-z0p0线段起始点
p1线段终点
center椭球球心
a = osg::WGS_84_RADIUS_EQUATOR;//长轴
b = osg::WGS_84_RADIUS_POLAR;//短轴
/************************************************************************/
osg::Vec3d lineSegment_WGS84Ellipsoid_intersection(osg::Vec3d p0, osg::Vec3d p1,
osg::Vec3d center=osg::Vec3d(), double a = osg::WGS_84_RADIUS_EQUATOR, double b = osg::WGS_84_RADIUS_POLAR)
{
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 = * ((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) - ;
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);
osg::Vec3d lineNormal(m, n, p);
// 其实有两个解，根据你的需要选择t0还是t1。
osg::Vec3d hitp = lineNormal*t0 + p0;
return hitp;
}
return osg::Vec3d(, , );
}