使用定姿四元数计算图像位置

pkg/calculator/intersection.go

@@ -0,0 +1,64 @@
+package calculator
+
+import (
+	"math"
+	"time"
+
+	"gonum.org/v1/gonum/mat"
+)
+
+// 常量
+const (
+	focal   = 1.3  // 焦距, m
+	FOV     = 1.7  // 视场角,degree
+	nPixels = 9344 // 像素数
+)
+
+func Intersection(q Quaternion, satPos []float64, satTime time.Time, ucam int) (float64, float64) {
+	alpha := FOV * math.Pi / 180.0
+	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(nPixels))
+	direction := []float64{0, math.Tan(alpha), -1.3}
+
+	Ratt := q.ToRotationMatrix()
+	RattT := &mat.Dense{}
+	RattT.Inverse(Ratt)
+
+	v := mat.NewVecDense(3, direction)
+	var result mat.VecDense
+	result.MulVec(Ratt, v)
+	eciDirection := result.RawVector().Data
+
+	// intersection := intersectWithEllipsoid(satPos, eciDirection)
+	// lat, lon, _ := J2000ToWGS84(intersection[0], intersection[1], intersection[2], satTime)
+
+	x, y, z := ECItoECEF(eciDirection[0], eciDirection[1], eciDirection[2], satTime)
+	ecefDirection := []float64{x, y, z}
+	intersection := intersectWithEllipsoid(satPos, ecefDirection)
+	lat, lon, _ := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
+
+	return lat, lon
+
+}
+
+// 计算与椭球表面的交点
+func intersectWithEllipsoid(p0, d []float64) []float64 {
+	a2 := a * a
+	b2 := b * b
+
+	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)
+	C := p0[0]*p0[0]/a2 + p0[1]*p0[1]/a2 + p0[2]*p0[2]/b2 - 1
+
+	delta := B*B - 4*A*C
+	if delta < 0 {
+		return nil // No intersection
+	}
+	t1 := (-B + math.Sqrt(delta)) / (2 * A)
+	t2 := (-B - math.Sqrt(delta)) / (2 * A)
+	t := math.Max(t1, t2)
+	return []float64{
+		p0[0] + t*d[0],
+		p0[1] + t*d[1],
+		p0[2] + t*d[2],
+	}
+}