rpc

pkg/calculator/intersection.go

@@ -10,12 +10,13 @@ import (
 )
 
 type IntersectionPoint struct {
-	Lat float64
-	Lon float64
-	H   float64
+	X84, Y84, Z84 float64
+	Lat           float64
+	Lon           float64
+	H             float64
 }
 
-func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
+func Camera2GroundPoint(q Quaternion, satPos84 []float64, satTime time.Time, ucam int, groundH int) (IntersectionPoint, error) {
 	// alpha := FOV * math.Pi / 180.0
 	// alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
 	// direction := []float64{0, math.Tan(alpha), -1.3}
@@ -37,7 +38,7 @@ func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, u
 	dECEF := []float64{x, y, z}
 
 	// -------- 计算与地球表面的交点 --------
-	intersection, err := intersectWithEllipsoid(satPos84, dECEF)
+	intersection, err := SolveEllipticEquation(satPos84, dECEF, groundH)
 	if err != nil {
 		return IntersectionPoint{}, err
 	}
@@ -73,20 +74,23 @@ func IntersectionECI(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satT
 	dECEF := []float64{x, y, z}
 
 	// -------- 计算交点 --------}
-	intersection, err := intersectWithEllipsoid(satPos84, dECEF)
+	intersection, err := SolveEllipticEquation(satPos84, dECEF, 0)
 	if err != nil {
 		return IntersectionPoint{}, err
 	}
 
 	lat, lon, h := ECEFGeocentricToGeodetic(intersection[0], intersection[1], intersection[2])
 
-	return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
+	return IntersectionPoint{
+		X84: intersection[0], Y84: intersection[1], Z84: intersection[2],
+		Lat: lat, Lon: lon, H: h,
+	}, nil
 }
 
 // 计算与椭球表面的交点
-func intersectWithEllipsoid(p0, d []float64) ([]float64, error) {
-	a2 := a * a
-	b2 := b * b
+func SolveEllipticEquation(p0, d []float64, groundH int) ([]float64, error) {
+	a2 := (a + float64(groundH)) * (a + float64(groundH))
+	b2 := (b + float64(groundH)) * (b + float64(groundH))
 
 	A := d[0]*d[0]/a2 + d[1]*d[1]/a2 + d[2]*d[2]/b2
 	B := 2 * (p0[0]*d[0]/a2 + p0[1]*d[1]/a2 + p0[2]*d[2]/b2)