66 lines
2.0 KiB
Go
66 lines
2.0 KiB
Go
package calculator
|
||
|
||
import (
|
||
"math"
|
||
|
||
"gonum.org/v1/gonum/mat"
|
||
"gonum.org/v1/gonum/spatial/r3"
|
||
)
|
||
|
||
// OrbitToECMatrix 轨道坐标系到ECI、ECEF坐标系的变换矩阵
|
||
func OrbitToECMatrix(pos, vec []float64) *mat.Dense {
|
||
r := r3.Vec{X: pos[0], Y: pos[1], Z: pos[2]}
|
||
rmag := r3.Norm(r) // Magnitude
|
||
v := r3.Vec{X: vec[0], Y: vec[1], Z: vec[2]}
|
||
vmag := r3.Norm(v) // Magnitude of velocity vector
|
||
w := r3.Cross(v, r)
|
||
wmag := r3.Norm(w)
|
||
|
||
z0 := r3.Scale(-1/rmag, r) // z方向指向地心
|
||
y0 := r3.Scale(1/wmag, w)
|
||
x0 := r3.Scale(1/vmag, v)
|
||
|
||
m := mat.NewDense(3, 3, []float64{
|
||
x0.X, y0.X, z0.X,
|
||
x0.Y, y0.Y, z0.Y,
|
||
x0.Z, y0.Z, z0.Z,
|
||
})
|
||
|
||
return m
|
||
}
|
||
|
||
// IntersectionECEF 计算卫星与相机的交点，返回经纬度和高度
|
||
// FIXME: 该计算方法有误，待修正
|
||
func IntersectionECEF(Qsat2orbit Quaternion, satPos84, vec84 []float64, ucam 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} // 卫星（相机）坐标系下CCD成像方向向量
|
||
|
||
// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
|
||
Rcam := CameraRotMatrix(AngleCamSatX*math.Pi/180.0, AngleCamSatY*math.Pi/180.0, 0)
|
||
var dCam mat.VecDense
|
||
dCam.MulVec(Rcam, mat.NewVecDense(3, direction))
|
||
|
||
// -------- 转到轨道坐标系 --------
|
||
Rsat2orbit := Qsat2orbit.ToRotationMatrix()
|
||
var r0 mat.VecDense
|
||
r0.MulVec(Rsat2orbit, &dCam)
|
||
dOrbit := r0.RawVector().Data
|
||
|
||
// -------- 转到ECEF坐标系 --------
|
||
Rorbit2ecef := OrbitToECMatrix(satPos84, vec84)
|
||
var r1 mat.VecDense
|
||
r1.MulVec(Rorbit2ecef, mat.NewVecDense(3, dOrbit))
|
||
dECEF := r1.RawVector().Data
|
||
|
||
// -------- 计算交点 --------}
|
||
intersection, err := intersectWithEllipsoid(satPos84, dECEF)
|
||
if err != nil {
|
||
return IntersectionPoint{}, err
|
||
}
|
||
|
||
lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
|
||
|
||
return IntersectionPoint{Lat: lat, Lon: lon, H: h}, err
|
||
}
|