29 lines
655 B
Go
29 lines
655 B
Go
package calculator
|
|
|
|
import (
|
|
"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
|
|
}
|