127 lines
2.8 KiB
Go
127 lines
2.8 KiB
Go
package auxilary
|
||
|
||
import (
|
||
"fmt"
|
||
"math"
|
||
"os"
|
||
)
|
||
|
||
type Attitudes struct {
|
||
Atts []*Attitude
|
||
}
|
||
|
||
func (atts Attitudes) Save(attFile string) error {
|
||
f, err := os.Create(attFile)
|
||
if err != nil {
|
||
return err
|
||
}
|
||
defer f.Close()
|
||
|
||
for _, att := range atts.Atts {
|
||
content := fmt.Sprintf("%.8f %.8f %.8f %.8f %.8f\n", att.UTCTimestampSec, att.Q0, att.Q1, att.Q2, att.Q3)
|
||
f.WriteString(content)
|
||
}
|
||
|
||
return nil
|
||
}
|
||
|
||
// 四元数球面线性插值
|
||
func (atts Attitudes) Slerp(t float64) *Attitude {
|
||
// 取 t 前后的四元数
|
||
var q0, q1 *Attitude
|
||
|
||
for i, att := range atts.Atts {
|
||
if att.UTCTimestampSec >= t {
|
||
if i == 0 {
|
||
q0 = att
|
||
q1 = atts.Atts[i+1]
|
||
} else {
|
||
q0 = atts.Atts[i-1]
|
||
q1 = att
|
||
}
|
||
break
|
||
}
|
||
}
|
||
|
||
if q0 == nil || q1 == nil {
|
||
return atts.Atts[len(atts.Atts)-1]
|
||
}
|
||
|
||
var w0, x0, y0, z0, w1, x1, y1, z1 float64
|
||
w0, x0, y0, z0 = q0.Q0, q0.Q1, q0.Q2, q0.Q3
|
||
w1, x1, y1, z1 = q1.Q0, q1.Q1, q1.Q2, q1.Q3
|
||
|
||
// 用点乘计算两个四元数夹角的cos值
|
||
cosOmega := w0*w1 + x0*x1 + y0*y1 + z0*z1
|
||
|
||
// 如果点乘为负，则反转一个四元数以取得短的4D弧
|
||
if cosOmega < 0.0 {
|
||
w1 = -w1
|
||
x1 = -x1
|
||
y1 = -y1
|
||
z1 = -z1
|
||
cosOmega = -cosOmega
|
||
}
|
||
|
||
var k0, k1 float64
|
||
t0, t1 := q0.UTCTimestampSec, q1.UTCTimestampSec
|
||
if cosOmega > 0.99999999 { // cos=1的时候就是夹角为0，重合
|
||
k0 = (t1 - t) / (t1 - t0)
|
||
k1 = (t - t0) / (t1 - t0)
|
||
} else {
|
||
// 用三角公式sin^2+cos^2=1计算sin值
|
||
sinOmega := math.Sqrt(1.0 - cosOmega*cosOmega)
|
||
// 通过sin和cos计算角度
|
||
omega := math.Atan2(sinOmega, cosOmega) // 计算点(cosOmega,sinOmega)与x轴正向的夹角
|
||
// 计算分母的倒数，这样就只需要一次除法
|
||
oneOverSinOmega := 1.0 / sinOmega
|
||
//计算插值变量
|
||
k0 = math.Sin((t1-t)/(t1-t0)*omega) * oneOverSinOmega
|
||
k1 = math.Sin((t-t0)/(t1-t0)*omega) * oneOverSinOmega
|
||
}
|
||
// 插值
|
||
w := w0*k0 + w1*k1
|
||
x := x0*k0 + x1*k1
|
||
y := y0*k0 + y1*k1
|
||
z := z0*k0 + z1*k1
|
||
|
||
return &Attitude{
|
||
UTCTimestampSec: t,
|
||
Q0: w,
|
||
Q1: x,
|
||
Q2: y,
|
||
Q3: z,
|
||
}
|
||
}
|
||
|
||
type Attitude struct {
|
||
UTCTimestampSec float64
|
||
Q0, Q1, Q2, Q3 float64
|
||
}
|
||
|
||
func StoreAtt(aps []*AuxPlatform, attFile string) (*Attitudes, error) {
|
||
atts := ExtractAttitude(aps)
|
||
return atts, atts.Save(attFile)
|
||
}
|
||
|
||
func ExtractAttitude(aps []*AuxPlatform) *Attitudes {
|
||
var atts Attitudes
|
||
var sec, microsec uint32
|
||
for _, ap := range aps {
|
||
if ap.UTCTimeSec != sec || ap.Microsecond != microsec {
|
||
sec, microsec = ap.UTCTimeSec, ap.Microsecond
|
||
att := Attitude{
|
||
UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) +
|
||
float64(transfromGPSandAttMicrosec(microsec))/1e6,
|
||
Q0: ap.QuatAttstarQ0,
|
||
Q1: ap.QuatAttstarQ1,
|
||
Q2: ap.QuatAttstarQ2,
|
||
Q3: ap.QuatAttstarQ3,
|
||
}
|
||
atts.Atts = append(atts.Atts, &att)
|
||
}
|
||
}
|
||
|
||
return &atts
|
||
}
|