71 lines
2.0 KiB
Go
71 lines
2.0 KiB
Go
package calculator
|
||
|
||
import (
|
||
"fmt"
|
||
"math"
|
||
)
|
||
|
||
// Quaternion represents a quaternion with scalar (w) and vector (x, y, z) parts
|
||
type Quaternion struct {
|
||
w, x, y, z float64
|
||
}
|
||
|
||
// Quaternion multiplication
|
||
func (q1 Quaternion) Mul(q2 Quaternion) Quaternion {
|
||
return Quaternion{
|
||
w: q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
|
||
x: q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y,
|
||
y: q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x,
|
||
z: q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w,
|
||
}
|
||
}
|
||
|
||
// Quaternion conjugate
|
||
func (q Quaternion) Conjugate() Quaternion {
|
||
return Quaternion{w: q.w, x: -q.x, y: -q.y, z: -q.z}
|
||
}
|
||
|
||
// Rotate vector by quaternion
|
||
func (q Quaternion) Rotate(v [3]float64) [3]float64 {
|
||
qv := Quaternion{w: 0, x: v[0], y: v[1], z: v[2]}
|
||
qConj := q.Conjugate()
|
||
qvRotated := q.Mul(qv).Mul(qConj)
|
||
return [3]float64{qvRotated.x, qvRotated.y, qvRotated.z}
|
||
}
|
||
|
||
func main() {
|
||
// 示例数据
|
||
qBI := Quaternion{w: 1, x: 0, y: 0, z: 0} // 本体相对惯性系四元数
|
||
posJ2000 := [3]float64{7000, 0, 0} // J2000位置
|
||
// velJ2000 := [3]float64{0, 7.5, 0} // J2000速度
|
||
|
||
// 相机参数
|
||
const numPixels = 9520
|
||
const fov = 10.0 * math.Pi / 180 // 假设视场角为10度
|
||
|
||
// 逐像素计算地面交点
|
||
for i := 0; i < numPixels; i++ {
|
||
// 计算像素点相对光轴的偏角
|
||
pixelOffset := (float64(i) - float64(numPixels)/2) / float64(numPixels)
|
||
angle := pixelOffset * fov
|
||
|
||
// 假设光轴在本体坐标系中指向-z方向，计算视线方向
|
||
dBody := [3]float64{-math.Sin(angle), 0, -math.Cos(angle)}
|
||
|
||
// 转换到惯性系
|
||
dInertial := qBI.Rotate(dBody)
|
||
|
||
// 计算地面交点（假设dInertial已经标准化）
|
||
k := -posJ2000[2] / dInertial[2] // 简化的交点计算
|
||
groundPoint := [3]float64{
|
||
posJ2000[0] + k*dInertial[0],
|
||
posJ2000[1] + k*dInertial[1],
|
||
posJ2000[2] + k*dInertial[2],
|
||
}
|
||
|
||
// 转换到地理坐标
|
||
lat, lon, _ := ECEFToGeodetic(groundPoint[0], groundPoint[1], groundPoint[2])
|
||
fmt.Printf("Pixel %d: Latitude: %f, Longitude: %f\n", i, lat, lon)
|
||
}
|
||
}
|