109 lines
3.3 KiB
Go
109 lines
3.3 KiB
Go
package calculator
|
||
|
||
import (
|
||
"fmt"
|
||
"math"
|
||
"time"
|
||
|
||
"github.com/sirupsen/logrus"
|
||
"gonum.org/v1/gonum/mat"
|
||
)
|
||
|
||
type IntersectionPoint struct {
|
||
Lat float64
|
||
Lon float64
|
||
H float64
|
||
}
|
||
|
||
func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, 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}
|
||
direction := CameraDirectionVec(0, float64(ucam))
|
||
|
||
// -------- 相机坐标系下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))
|
||
|
||
// -------- 转到ECI坐标系 --------
|
||
Ratt := q.ToRotationMatrix()
|
||
var result mat.VecDense
|
||
result.MulVec(Ratt, &dCam)
|
||
dECI := result.RawVector().Data
|
||
|
||
// -------- 转到ECEF坐标系 --------
|
||
x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime)
|
||
dECEF := []float64{x, y, z}
|
||
|
||
// -------- 计算与地球表面的交点 --------
|
||
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}, nil
|
||
}
|
||
|
||
func IntersectionECI(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, 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
|
||
|
||
// -------- 转到ECI坐标系 --------
|
||
Rorbit2eci := Qorbit2eci.ToRotationMatrix()
|
||
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, 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}, nil
|
||
}
|
||
|
||
// 计算与椭球表面的交点
|
||
func intersectWithEllipsoid(p0, d []float64) ([]float64, error) {
|
||
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 {
|
||
logrus.Error("line of sight: no intersection with ellipsoid")
|
||
return nil, fmt.Errorf("line of sight: no intersection with ellipsoid") // 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],
|
||
}, nil
|
||
}
|