// File: /workspace/github_cpp_makefile_star2_O2/istellartech[P]OpenTsiolkovsky/src/rocket.cpp
// Function: RocketStage::power_flight_3dof(std::array<double, 14ul> const&, double)
void RocketStage::power_flight_3dof(const RocketStage::state& x, double t){
    flight_mode = "power_3DoF";
    // dcmNED2BODY_ = dcmNED2BODY(azimuth, elevation, roll);
    Matrix3d dcmNAVI2BODY_ = this->quatNAVI2BODY(t).toRotationMatrix();
    Matrix3d dcmNED2NAVI_ = dcmNED2BODY(azimuth_target, elevation_target, roll_target);
    dcmNED2BODY_ = dcmNAVI2BODY_ * dcmNED2NAVI_;
    Vector3d azelro = azimuth_elevation_roll(dcmNED2BODY_);
    elevation = azelro[1];
    azimuth = ((deg2rad(90.0) - elevation_target < 1e-9) && (deg2rad(90.0) - elevation < 1e-9))? azimuth_target : azelro[0];
    roll = ((deg2rad(90.0) - elevation_target < 1e-9) && (deg2rad(90.0) - elevation < 1e-9))? roll_target : azelro[2];
    vel_AIR_BODYframe_ = vel_AIR_BODYframe(dcmNED2BODY_, vel_ECEF_NEDframe_, vel_wind_NEDframe_);
    angle_of_attack_ = angle_of_attack(vel_AIR_BODYframe_);
    dcmECI2BODY_ = dcmECI2BODY(dcmNED2BODY_, dcmECI2NED_);
    dcmBODY2ECI_ = dcmECI2BODY_.transpose();

    //    aerodynamics term
    vel_AIR_BODYframe_abs = vel_AIR_BODYframe_.norm();
    mach_number = vel_AIR_BODYframe_abs / air.airspeed;
    update_from_mach_number();
    dynamic_pressure = 0.5 * air.density * vel_AIR_BODYframe_abs * vel_AIR_BODYframe_abs;
    force_axial        = CA       * dynamic_pressure * body_area;
    force_normal_yaw   = CN_yaw   * dynamic_pressure * body_area;
    force_normal_pitch = CN_pitch * dynamic_pressure * body_area;
    force_air_vector_BODYframe << - force_axial, - force_normal_yaw, - force_normal_pitch;

    //    thrust term
    double dXt = pos_Controller - pos_CG;
    double dYt = Ycg_offset - Yt_offset;
    double dZt = Zcg_offset - Zt_offset;
    double dXp_yaw = pos_CG - pos_CP_yaw;
    double dXp_pitch = pos_CG - pos_CP_pitch;
    double dYp = Ycp_offset - Ycg_offset;
    double dZp = Zcp_offset - Zcg_offset;
    double term_yaw   = (- force_air_vector_BODYframe[1] * dXp_yaw + force_air_vector_BODYframe[0] * dYp)
                       / thrust / sqrt(dXt * dXt + dYt * dYt);
    double term_pitch = (- force_air_vector_BODYframe[2] * dXp_pitch + force_air_vector_BODYframe[0] * dZp)
                       / thrust / sqrt(dXt * dXt + dZt * dZt);
    double gimbal_angle_yaw_0;
    if ( is_consider_neutrality 
            && term_yaw < 1 && term_yaw > -1
            && term_pitch < 1 && term_pitch > -1 ) {
        gimbal_angle_yaw_0 = asin(term_yaw) - atan2(dYt, dXt);
        gimbal_angle_pitch = asin(term_pitch) - atan2(dZt, dXt);
        if ( gimbal_angle_yaw_0 < pi/2 && gimbal_angle_yaw_0 > -pi/2
                && gimbal_angle_pitch < pi/2 && gimbal_angle_pitch > -pi/2 ) {
            // Consider the compound angle
            gimbal_angle_yaw = atan(tan(gimbal_angle_yaw_0) * cos(gimbal_angle_pitch));
            force_thrust_vector << thrust * cos(gimbal_angle_yaw) * cos(gimbal_angle_pitch),
                                 - thrust * sin(gimbal_angle_yaw),
                                 - thrust * cos(gimbal_angle_yaw) * sin(gimbal_angle_pitch);  // body coordinate
        } else {
            gimbal_angle_yaw = gimbal_angle_pitch = 0.0;
            force_thrust_vector << thrust, 0.0, 0.0;  // body coordinate
        }
    } else {
        gimbal_angle_yaw = gimbal_angle_pitch = 0.0;
        force_thrust_vector << thrust, 0.0, 0.0;  // body coordinate
    }
    //    dv/dt
    accECI_ = 1/x[0] * (dcmBODY2ECI_ * (force_thrust_vector + force_air_vector_BODYframe))
            + gravityECI_;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x2f8, %rsp            # imm = 0x2F8
movsd %xmm0, (%rsp)
movq %rsi, 0x38(%rsp)
movq %rdi, %rbx
addq $0x230, %rdi            # imm = 0x230
leaq 0x136a1(%rip), %rsi     # 0x20a9f
callq 0x56a0
leaq 0x40(%rsp), %r15
movq %r15, %rdi
movq %rbx, %rsi
movsd (%rsp), %xmm0
callq 0xe7dc
leaq 0x2b0(%rsp), %r12
movq %r12, %rdi
movq %r15, %rsi
callq 0x10cb0
movsd 0x940(%rbx), %xmm0
movsd 0x948(%rbx), %xmm1
movsd 0x950(%rbx), %xmm2
leaq 0x268(%rsp), %r13
movq %r13, %rdi
callq 0x1d3c3
movq %r12, (%r15)
movq %r13, 0x8(%r15)
leaq 0xc90(%rbx), %r15
leaq 0x40(%rsp), %rsi
movq %r15, %rdi
callq 0x158da
leaq 0x220(%rsp), %r12
movq %r12, %rdi
movq %r15, %rsi
callq 0x152a2
leaq 0xc0(%rsp), %r13
movq %r13, %rdi
movq %r12, %rsi
callq 0x1d79e
movsd 0x8(%r13), %xmm0
movsd %xmm0, 0x930(%rbx)
movsd 0x1297e(%rip), %xmm3    # 0x1fe28
movapd %xmm3, %xmm2
subsd 0x948(%rbx), %xmm2
movsd 0x12972(%rip), %xmm1    # 0x1fe30
ucomisd %xmm2, %xmm1
jbe 0xd4eb
movapd %xmm3, %xmm2
subsd %xmm0, %xmm2
ucomisd %xmm2, %xmm1
jbe 0xd4eb
leaq 0x950(%rbx), %rax
movsd 0x940(%rbx), %xmm0
movsd %xmm0, 0x928(%rbx)
jmp 0xd504
movsd 0xc0(%rsp), %xmm0
movsd %xmm0, 0x928(%rbx)
leaq 0xd0(%rsp), %rax
movsd (%rax), %xmm0
movsd %xmm0, 0x938(%rbx)
leaq 0x1d8(%rsp), %r12
movq %r12, %rdi
movq %r15, %rsi
callq 0x152a2
leaq 0xbb8(%rbx), %rsi
leaq 0x130(%rsp), %r13
movq %r13, %rdi
callq 0x129c6
leaq 0xbd0(%rbx), %rsi
leaq 0x118(%rsp), %rbp
movq %rbp, %rdi
callq 0x129c6
leaq 0x40(%rsp), %r14
movq %r14, %rdi
movq %r12, %rsi
movq %r13, %rdx
movq %rbp, %rcx
callq 0x1d287
leaq 0xbe8(%rbx), %r12
movq %r14, %rdi
movq %r12, %rsi
callq 0x15314
leaq 0x100(%rsp), %r14
movq %r14, %rdi
movq %r12, %rsi
callq 0x129c6
leaq 0x40(%rsp), %r13
movq %r13, %rdi
movq %r14, %rsi
callq 0x1d2c8
leaq 0xc30(%rbx), %rsi
movq %r13, %rdi
callq 0x15314
leaq 0x190(%rsp), %r14
movq %r14, %rdi
movq %r15, %rsi
callq 0x152a2
leaq 0xb28(%rbx), %rsi
leaq 0x148(%rsp), %r15
movq %r15, %rdi
callq 0x152a2
leaq 0x40(%rsp), %r13
movq %r13, %rdi
movq %r14, %rsi
movq %r15, %rdx
callq 0x1d862
leaq 0xcd8(%rbx), %r14
movq %r13, %rdi
movq %r14, %rsi
callq 0x151a4
movq %r14, (%r13)
leaq 0xd20(%rbx), %r15
leaq 0x40(%rsp), %r13
movq %r15, %rdi
movq %r13, %rsi
callq 0x1536a
movq %r12, %rdi
callq 0x10c94
movsd %xmm0, 0x7c8(%rbx)
divsd 0x900(%rbx), %xmm0
movsd %xmm0, 0x958(%rbx)
movq %rbx, %rdi
callq 0xe4a6
movsd 0x910(%rbx), %xmm0
mulsd 0x127eb(%rip), %xmm0    # 0x1fe38
movsd 0x7c8(%rbx), %xmm1
mulsd %xmm1, %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, 0x7d8(%rbx)
movsd 0x510(%rbx), %xmm1
movsd 0x7b0(%rbx), %xmm2
mulsd %xmm0, %xmm2
mulsd %xmm1, %xmm2
movsd %xmm2, 0x7f0(%rbx)
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
mulpd 0x7a0(%rbx), %xmm0
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
mulpd %xmm0, %xmm1
movapd %xmm1, 0x7e0(%rbx)
leaq 0xdf8(%rbx), %rbp
movapd 0x11960(%rip), %xmm0    # 0x1f010
movq %rbp, (%r13)
andq $0x0, 0x8(%r13)
xorpd %xmm0, %xmm1
pushq $0x1
popq %r12
movq %r12, 0x10(%r13)
movq %r12, 0x18(%r13)
movlpd %xmm1, 0xdf8(%rbx)
xorpd %xmm0, %xmm2
leaq 0xd8(%rsp), %rsi
movlpd %xmm2, (%rsi)
leaq 0x40(%rsp), %rdi
callq 0xfd60
movsd 0x7e8(%rbx), %xmm0
xorpd 0x11915(%rip), %xmm0    # 0x1f010
leaq 0x98(%rsp), %rsi
movlpd %xmm0, (%rsi)
movq %rax, %rdi
callq 0xfd60
leaq 0x40(%rsp), %rdi
callq 0xfe08
movsd 0x990(%rbx), %xmm4
movsd 0x978(%rbx), %xmm1
subsd %xmm1, %xmm4
movupd 0xdf8(%rbx), %xmm0
movsd 0x760(%rbx), %xmm5
movapd 0x6d0(%rbx), %xmm2
movupd 0x6e8(%rbx), %xmm3
subpd %xmm2, %xmm3
movapd %xmm2, %xmm6
subpd 0x700(%rbx), %xmm6
movapd 0x980(%rbx), %xmm2
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
shufpd $0x1, %xmm2, %xmm2      # xmm2 = xmm2[1,0]
subpd %xmm2, %xmm1
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
mulpd %xmm3, %xmm0
mulpd 0xe00(%rbx), %xmm1
subpd %xmm1, %xmm0
movapd %xmm5, %xmm1
unpcklpd %xmm5, %xmm1            # xmm1 = xmm1[0],xmm5[0]
divpd %xmm1, %xmm0
movapd %xmm6, %xmm1
mulpd %xmm6, %xmm1
movapd %xmm4, %xmm2
mulsd %xmm4, %xmm2
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
addpd %xmm1, %xmm2
sqrtpd %xmm2, %xmm1
divpd %xmm1, %xmm0
movapd 0x11896(%rip), %xmm1    # 0x1f050
andpd %xmm0, %xmm1
cmpltpd 0x12869(%rip), %xmm1    # 0x20030
movmskpd %xmm1, %eax
movl %eax, %ecx
andb $0x2, %cl
shrb %cl
andb 0x680(%rbx), %al
andb %cl, %al
cmpb $0x1, %al
jne 0xd9a4
movsd %xmm4, 0x18(%rsp)
movapd %xmm0, 0x20(%rsp)
movapd %xmm6, 0xb0(%rsp)
callq 0x55d0
movapd %xmm0, (%rsp)
movapd 0xb0(%rsp), %xmm0
movsd 0x18(%rsp), %xmm1
callq 0x5090
movapd (%rsp), %xmm1
subsd %xmm0, %xmm1
movapd %xmm1, (%rsp)
movaps 0x20(%rsp), %xmm0
movhlps %xmm0, %xmm0            # xmm0 = xmm0[1,1]
callq 0x55d0
movaps %xmm0, 0x20(%rsp)
movapd 0xb0(%rsp), %xmm0
unpckhpd %xmm0, %xmm0            # xmm0 = xmm0[1,1]
movsd 0x18(%rsp), %xmm1
callq 0x5090
movapd 0x20(%rsp), %xmm4
movapd (%rsp), %xmm3
subsd %xmm0, %xmm4
movsd %xmm4, 0x968(%rbx)
movapd 0x117e4(%rip), %xmm0    # 0x1f050
movapd %xmm3, %xmm1
andpd %xmm0, %xmm1
movsd 0x125ac(%rip), %xmm2    # 0x1fe28
ucomisd %xmm1, %xmm2
jbe 0xd9fb
andpd %xmm0, %xmm4
ucomisd %xmm4, %xmm2
jbe 0xd9fb
movapd %xmm3, %xmm0
callq 0x5230
movsd %xmm0, (%rsp)
movsd 0x968(%rbx), %xmm0
callq 0x5680
mulsd (%rsp), %xmm0
callq 0x5300
movsd %xmm0, 0x970(%rbx)
leaq 0xe28(%rbx), %r13
movsd 0x760(%rbx), %xmm1
movsd %xmm1, (%rsp)
callq 0x5680
mulsd (%rsp), %xmm0
movsd %xmm0, (%rsp)
movsd 0x968(%rbx), %xmm0
callq 0x5680
leaq 0x40(%rsp), %r14
movq %r13, (%r14)
andq $0x0, 0x8(%r14)
mulsd (%rsp), %xmm0
movq %r12, 0x10(%r14)
movq %r12, 0x18(%r14)
movsd %xmm0, 0xe28(%rbx)
movsd 0x760(%rbx), %xmm0
xorps 0x116ee(%rip), %xmm0    # 0x1f010
movaps %xmm0, (%rsp)
movsd 0x970(%rbx), %xmm0
callq 0x50e0
mulsd (%rsp), %xmm0
leaq 0xd8(%rsp), %rsi
movsd %xmm0, (%rsi)
movq %r14, %rdi
callq 0xfd60
movq %rax, %r12
movsd 0x760(%rbx), %xmm0
xorps 0x116b2(%rip), %xmm0    # 0x1f010
movaps %xmm0, (%rsp)
movsd 0x970(%rbx), %xmm0
callq 0x5680
mulsd (%rsp), %xmm0
movsd %xmm0, (%rsp)
movsd 0x968(%rbx), %xmm0
callq 0x50e0
mulsd (%rsp), %xmm0
leaq 0x98(%rsp), %rsi
movsd %xmm0, (%rsi)
movq %r12, %rdi
callq 0xfd60
jmp 0xda5b
xorpd %xmm1, %xmm1
movupd %xmm1, 0x968(%rbx)
leaq 0xe28(%rbx), %rax
leaq 0x40(%rsp), %rdi
movq %rax, (%rdi)
andq $0x0, 0x8(%rdi)
movq %r12, 0x10(%rdi)
movq %r12, 0x18(%rdi)
movsd %xmm5, 0xe28(%rbx)
leaq 0xd8(%rsp), %rsi
andq $0x0, (%rsi)
callq 0xfd60
leaq 0x98(%rsp), %rsi
andq $0x0, (%rsi)
movq %rax, %rdi
callq 0xfd60
jmp 0xda5b
leaq 0x968(%rbx), %rax
xorpd %xmm0, %xmm0
movupd %xmm0, (%rax)
leaq 0xe28(%rbx), %rax
leaq 0x40(%rsp), %rdi
movq %rax, (%rdi)
andq $0x0, 0x8(%rdi)
movq %r12, 0x10(%rdi)
movq %r12, 0x18(%rdi)
movsd 0x760(%rbx), %xmm0
movsd %xmm0, 0xe28(%rbx)
leaq 0xd8(%rsp), %rsi
andq $0x0, (%rsi)
callq 0xfd60
leaq 0x98(%rsp), %rsi
andq $0x0, (%rsi)
movq %rax, %rdi
callq 0xfd60
leaq 0x40(%rsp), %rdi
callq 0xfe08
movsd 0x115f3(%rip), %xmm0    # 0x1f060
movq 0x38(%rsp), %rax
divsd (%rax), %xmm0
movsd %xmm0, (%rsp)
leaq 0xe28(%rbx), %rax
leaq 0x98(%rsp), %rsi
movq %rax, (%rsi)
movq %rbp, 0x8(%rsi)
leaq 0x48(%rsp), %rdi
movq %r15, -0x8(%rdi)
callq 0x15aec
leaq 0x40(%rsp), %rdi
callq 0x15b5c
leaq 0xe58(%rbx), %rcx
leaq 0xd8(%rsp), %rsi
movq %rax, 0x8(%rsi)
movsd (%rsp), %xmm0
movsd %xmm0, 0x10(%rsi)
movq %rcx, 0x18(%rsi)
addq $0x9f0, %rbx            # imm = 0x9F0
movq %rbx, %rdi
callq 0x15c52
addq $0x2f8, %rsp            # imm = 0x2F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0xdaf9
jmp 0xdaf9
jmp 0xdaf9
jmp 0xdaf9
jmp 0xdaf9
jmp 0xdaf9
jmp 0xdaf9
movq %rax, %rbx
leaq 0x40(%rsp), %rdi
callq 0xfe08
movq %rbx, %rdi
callq 0x5640