// File: istellartech[P]OpenTsiolkovsky/src/coordinate_transform.cpp
// Function: posECEF(Eigen::Matrix<double, 3, 1, 0, 3, 1>)
Vector3d posECEF(Vector3d posLLH_){
    Vector3d pos;
    double lat = deg2rad(posLLH_[0]); //degree
    double lon = deg2rad(posLLH_[1]);
    double alt = posLLH_[2];
    double a = 6378137; // 長半径a [m]
    double f = 1.0 / 298.257223563; // 扁平率
    double e = sqrt(2*f - f*f); // 離心率 e
    double e2 = e * e;
    double W = sqrt(1.0 - e2 * sin(lat) * sin(lat));
    double N = a / W; // 卯酉線曲率半径
    pos[0] = (N + alt) * cos(lat) * cos(lon);
    pos[1] = (N + alt) * cos(lat) * sin(lon);
    pos[2] = (N * (1 - e2) + alt) * sin(lat);
    return pos;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xc0, %rsp
movq %rsi, -0xc0(%rbp)
movq %rdi, -0x78(%rbp)
movq %rdi, %rax
movq %rax, -0x60(%rbp)
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
callq 0x1d670
movq -0xc0(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movsd (%rax), %xmm0
callq 0x1e000
movq -0xc0(%rbp), %rdi
movsd %xmm0, -0x18(%rbp)
movl $0x1, %esi
callq 0x1de40
movsd (%rax), %xmm0
callq 0x1e000
movq -0xc0(%rbp), %rdi
movsd %xmm0, -0x20(%rbp)
movl $0x2, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x28(%rbp)
movsd 0xd387(%rip), %xmm0     # 0x59750
movsd %xmm0, -0x30(%rbp)
movsd 0xd702(%rip), %xmm0     # 0x59ad8
movsd %xmm0, -0x38(%rbp)
movsd -0x38(%rbp), %xmm0
movsd -0x38(%rbp), %xmm1
mulsd -0x38(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
addsd %xmm0, %xmm0
addsd %xmm1, %xmm0
callq 0x6390
movsd %xmm0, -0x40(%rbp)
movsd -0x40(%rbp), %xmm0
mulsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x48(%rbp)
movsd -0x48(%rbp), %xmm0
movsd %xmm0, -0xb8(%rbp)
movsd -0x18(%rbp), %xmm0
callq 0x6120
movaps %xmm0, %xmm1
movsd -0xb8(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0xb0(%rbp)
movsd -0x18(%rbp), %xmm0
callq 0x6120
movaps %xmm0, %xmm1
movsd -0xb0(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm1, %xmm0
movsd 0x5bb8(%rip), %xmm1     # 0x52040
addsd %xmm1, %xmm0
callq 0x6390
movsd %xmm0, -0x50(%rbp)
movsd -0x30(%rbp), %xmm0
divsd -0x50(%rbp), %xmm0
movsd %xmm0, -0x58(%rbp)
movsd -0x58(%rbp), %xmm0
addsd -0x28(%rbp), %xmm0
movsd %xmm0, -0xa8(%rbp)
movsd -0x18(%rbp), %xmm0
callq 0x6810
movaps %xmm0, %xmm1
movsd -0xa8(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0xa0(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x6810
movq -0x78(%rbp), %rdi
movaps %xmm0, %xmm1
movsd -0xa0(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x98(%rbp)
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movsd -0x98(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x58(%rbp), %xmm0
addsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x90(%rbp)
movsd -0x18(%rbp), %xmm0
callq 0x6810
movaps %xmm0, %xmm1
movsd -0x90(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x88(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x6120
movq -0x78(%rbp), %rdi
movaps %xmm0, %xmm1
movsd -0x88(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x80(%rbp)
movl $0x1, %esi
callq 0x1de40
movsd -0x80(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x58(%rbp), %xmm0
movsd 0x5ab9(%rip), %xmm2     # 0x52040
subsd -0x48(%rbp), %xmm2
movsd -0x28(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x70(%rbp)
movsd -0x18(%rbp), %xmm0
callq 0x6120
movq -0x78(%rbp), %rdi
movaps %xmm0, %xmm1
movsd -0x70(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x68(%rbp)
movl $0x2, %esi
callq 0x1de40
movsd -0x68(%rbp), %xmm0
movq %rax, %rcx
movq -0x60(%rbp), %rax
movsd %xmm0, (%rcx)
addq $0xc0, %rsp
popq %rbp
retq