计算直线与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-z0

p0线段起始点
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 = 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);
        osg::Vec3d lineNormal(m, n, p);
        // 其实有两个解，根据你的需要选择t0还是t1。
        osg::Vec3d hitp = lineNormal*t0 + p0;
        return hitp;
    }
    return osg::Vec3d(0, 0, 0);
}