65 lines
1.6 KiB
Go
65 lines
1.6 KiB
Go
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],
|
|
}
|
|
}
|