计算分辨率

pkg/calculator/intersection.go

@@ -14,7 +14,13 @@ const (
 	nPixels = 9344 // 像素数
 )
 
-func Intersection(q Quaternion, satPos []float64, satTime time.Time, ucam int) (float64, float64) {
+type IntersectionPoint struct {
+	Lat float64
+	Lon float64
+	H   float64
+}
+
+func Intersection(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
 	alpha := FOV * math.Pi / 180.0
 	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(nPixels))
 	direction := []float64{0, math.Tan(alpha), -1.3}
@@ -33,11 +39,42 @@ func Intersection(q Quaternion, satPos []float64, satTime time.Time, ucam int) (
 
 	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])
+	intersection := intersectWithEllipsoid(satPos84, ecefDirection)
+	lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
+	return IntersectionPoint{Lat: lat, Lon: lon, H: h}
 
-	return lat, lon
+}
 
+func Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
+	alpha := FOV * math.Pi / 180.0
+	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(nPixels))
+	direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量
+
+	// -------- 转到轨道坐标系 --------
+	Rsat2orbit := Qsat2orbit.ToRotationMatrix()
+	Rsat2orbitT := &mat.Dense{}
+	Rsat2orbitT.Inverse(Rsat2orbit)
+	var r0 mat.VecDense
+	r0.MulVec(Rsat2orbit, mat.NewVecDense(3, direction))
+	dOrbit := r0.RawVector().Data
+
+	// -------- 转到ECI坐标系 --------
+	Rorbit2eci := Qorbit2eci.ToRotationMatrix()
+	Rorbit2eciT := &mat.Dense{}
+	Rorbit2eciT.Inverse(Rorbit2eci)
+	var r1 mat.VecDense
+	r1.MulVec(Rorbit2eci, mat.NewVecDense(3, dOrbit))
+	dECI := r1.RawVector().Data
+
+	// -------- 转到ECEF坐标系 --------
+	x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime)
+	dECEF := []float64{x, y, z}
+
+	// -------- 计算交点 --------}
+	intersection := intersectWithEllipsoid(satPos84, dECEF)
+	lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
+
+	return IntersectionPoint{Lat: lat, Lon: lon, H: h}
 }
 
 // 计算与椭球表面的交点

pkg/calculator/quaternion.go

@@ -1,6 +1,8 @@
 package calculator
 
 import (
+	"math"
+
 	"gonum.org/v1/gonum/mat"
 )
 
@@ -18,3 +20,118 @@ func (q Quaternion) ToRotationMatrix() *mat.Dense {
 		2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y,
 	})
 }
+
+// EulerToQuaternion converts Euler angles (yaw, pitch, roll) to a quaternion.
+// Assuming the order of rotations is ZYX (yaw-pitch-roll).
+func EulerToQuaternion(yaw, pitch, roll float64) Quaternion {
+	halfYaw := yaw / 2
+	halfPitch := pitch / 2
+	halfRoll := roll / 2
+
+	cy := math.Cos(halfYaw)
+	sy := math.Sin(halfYaw)
+	cp := math.Cos(halfPitch)
+	sp := math.Sin(halfPitch)
+	cr := math.Cos(halfRoll)
+	sr := math.Sin(halfRoll)
+
+	q := Quaternion{
+		W: cy*cp*cr + sy*sp*sr,
+		X: cy*cp*sr - sy*sp*cr,
+		Y: sy*cp*sr + cy*sp*cr,
+		Z: sy*cp*cr - cy*sp*sr,
+	}
+
+	return q
+}
+
+// EulerToRotMatrix converts Euler angles (roll, pitch, yaw) to a rotation matrix.
+func EulerToRotMatrix(phi, theta, psi float64) *mat.Dense {
+	// Calculate individual rotation matrices
+	Rx := mat.NewDense(3, 3, []float64{
+		1, 0, 0,
+		0, math.Cos(phi), -math.Sin(phi),
+		0, math.Sin(phi), math.Cos(phi),
+	})
+
+	Ry := mat.NewDense(3, 3, []float64{
+		math.Cos(theta), 0, math.Sin(theta),
+		0, 1, 0,
+		-math.Sin(theta), 0, math.Cos(theta),
+	})
+
+	Rz := mat.NewDense(3, 3, []float64{
+		math.Cos(psi), -math.Sin(psi), 0,
+		math.Sin(psi), math.Cos(psi), 0,
+		0, 0, 1,
+	})
+
+	// R = Rz * Ry * Rx
+	RyRx := mat.NewDense(3, 3, nil)
+	RyRx.Mul(Ry, Rx)
+
+	R := mat.NewDense(3, 3, nil)
+	R.Mul(Rz, RyRx)
+
+	return R
+}
+
+// RotMatrixToQuaternion converts a rotation matrix to a quaternion.
+func RotMatrixToQuaternion(R *mat.Dense) Quaternion {
+	m := R.RawMatrix().Data
+
+	trace := m[0] + m[4] + m[8]
+	var q Quaternion
+
+	if trace > 0 {
+		S := 0.5 / math.Sqrt(trace+1.0)
+		q.W = 0.25 / S
+		q.X = (m[7] - m[5]) * S
+		q.Y = (m[2] - m[6]) * S
+		q.Z = (m[3] - m[1]) * S
+	} else {
+		if m[0] > m[4] && m[0] > m[8] {
+			S := 2.0 * math.Sqrt(1.0+m[0]-m[4]-m[8])
+			q.W = (m[7] - m[5]) / S
+			q.X = 0.25 * S
+			q.Y = (m[1] + m[3]) / S
+			q.Z = (m[2] + m[6]) / S
+		} else if m[4] > m[8] {
+			S := 2.0 * math.Sqrt(1.0+m[4]-m[0]-m[8])
+			q.W = (m[2] - m[6]) / S
+			q.X = (m[1] + m[3]) / S
+			q.Y = 0.25 * S
+			q.Z = (m[5] + m[7]) / S
+		} else {
+			S := 2.0 * math.Sqrt(1.0+m[8]-m[0]-m[4])
+			q.W = (m[3] - m[1]) / S
+			q.X = (m[2] + m[6]) / S
+			q.Y = (m[5] + m[7]) / S
+			q.Z = 0.25 * S
+		}
+	}
+
+	return q
+}
+
+func RotMatrixToEuler(R *mat.Dense) (yaw, pitch, roll float64) {
+	m := R.RawMatrix().Data
+
+	if m[6] < 1 {
+		if m[6] > -1 {
+			pitch = math.Asin(-m[6])
+			roll = math.Atan2(m[7], m[8])
+			yaw = math.Atan2(m[3], m[0])
+		} else {
+			pitch = math.Pi / 2
+			roll = -math.Atan2(-m[1], m[4])
+			yaw = 0
+		}
+	} else {
+		pitch = -math.Pi / 2
+		roll = math.Atan2(-m[1], m[4])
+		yaw = 0
+	}
+
+	return yaw, pitch, roll
+}