// File: /workspace/github_cpp_makefile_star2_O3/sus304[P]ForRocket/src/dynamics/dynamics_6dof_aero.cpp
// Function: forrocket::Dynamics6dofAero::operator()(std::array<double, 14ul> const&, std::array<double, 14ul>&, double)
void forrocket::Dynamics6dofAero::operator()(const state& x, state& dx, const double t) {
    Coordinate coordinate;

    // Countup Time
    p_clock->SyncSolverTime(t);
    p_rocket->burn_clock.SyncSolverTime(t);

    // Update Flight Infomation
    coordinate.setECI2ECEF(t);

    p_rocket->position.ECI = Eigen::Map<Eigen::Vector3d>(std::vector<double>(x.begin()+0, x.begin()+3).data());
    p_rocket->position.ECEF = coordinate.dcm.ECI2ECEF * p_rocket->position.ECI;
    p_rocket->position.LLH = coordinate.ECEF2LLH(p_rocket->position.ECEF);

    coordinate.setECEF2NED(p_rocket->position.LLH);

    p_rocket->velocity.ECI = Eigen::Map<Eigen::Vector3d>(std::vector<double>(x.begin()+3, x.begin()+6).data());
    p_rocket->velocity.ECEF = coordinate.dcm.ECI2ECEF * (p_rocket->velocity.ECI - coordinate.dcm.EarthRotate * p_rocket->position.ECI);
    p_rocket->velocity.NED = coordinate.dcm.ECEF2NED * p_rocket->velocity.ECEF;

    p_rocket->attitude.quaternion = Eigen::Map<Eigen::Vector4d>(std::vector<double>(x.begin()+6, x.begin()+10).data()).normalized();

    coordinate.setNED2Body(p_rocket->attitude.quaternion);
    
    p_rocket->attitude.euler_angle = coordinate.EulerAngle();

    p_rocket->angular_velocity = Eigen::Map<Eigen::Vector3d>(std::vector<double>(x.begin()+10, x.begin()+13).data());

    p_rocket->mass.propellant = x[13];


    // Update Environment
    double altitude = p_rocket->position.LLH[2];
    EnvironmentAir air(altitude);
    EnvironmentAir air_sea_level(0.0);
    Eigen::Vector3d gravity_NED(0.0, 0.0, gravity(altitude));

    // Update Airspeed
    p_rocket->velocity.air_body = coordinate.dcm.NED2body * (p_rocket->velocity.NED - p_wind->getNED(altitude));
    p_rocket->dynamic_pressure = 0.5 * air.density * std::pow(p_rocket->velocity.air_body.norm(), 2);
    p_rocket->velocity.mach_number = p_rocket->velocity.air_body.norm() / air.speed_of_sound;

    // Update time and mach parameter
    p_rocket->inertia_tensor = p_rocket->getInertiaTensor();

    p_rocket->length_CG = p_rocket->getLengthCG();    
    p_rocket->length_CP = p_rocket->getLengthCP(p_rocket->velocity.mach_number);
    p_rocket->CA = p_rocket->getCA(p_rocket->velocity.mach_number);
    p_rocket->CNa = p_rocket->getCNa(p_rocket->velocity.mach_number);
    p_rocket->Cld = p_rocket->getCld(p_rocket->velocity.mach_number);
    p_rocket->Clp = p_rocket->getClp(p_rocket->velocity.mach_number);
    p_rocket->Cmq = p_rocket->getCmq(p_rocket->velocity.mach_number);
    p_rocket->Cnr = p_rocket->getCnr(p_rocket->velocity.mach_number);

    // Calculate AoA
    // p_rocket->angle_of_attack = std::atan2(p_rocket->velocity.air_body[2], p_rocket->velocity.air_body[0]);
    if (p_rocket->velocity.air_body.norm() <= 0.0) {
        p_rocket->angle_of_attack = 0.0;
        p_rocket->sideslip_angle = 0.0;
    } else {
        p_rocket->angle_of_attack = std::asin(p_rocket->velocity.air_body[2] / p_rocket->velocity.air_body.norm());
        p_rocket->sideslip_angle = std::asin(-p_rocket->velocity.air_body[1] / p_rocket->velocity.air_body.norm());
    }

    // Calculate Force
    p_rocket->force.thrust = p_rocket->getThrust(air.pressure);
    p_rocket->force.aero = AeroForce(p_rocket);
    p_rocket->force.gravity = (coordinate.dcm.NED2body * gravity_NED) * p_rocket->mass.Sum();

    // Calculate Acceleration
    p_rocket->acceleration.body = p_rocket->force.Sum() / p_rocket->mass.Sum();
    p_rocket->acceleration.ECI = coordinate.dcm.ECEF2ECI * (coordinate.dcm.NED2ECEF * (coordinate.dcm.body2NED * p_rocket->acceleration.body));

    // Calculate Moment
    p_rocket->moment.gyro = GyroEffectMoment(p_rocket);
    p_rocket->moment.thrust = ThrustMoment(p_rocket);
    p_rocket->moment.aero_force = AeroForceMoment(p_rocket);
    p_rocket->moment.aero_dumping = AeroDampingMoment(p_rocket);
    p_rocket->moment.jet_dumping = JetDampingMoment(p_rocket);
    
    // Calculate Angle Velocity
    p_rocket->angular_acceleration = p_rocket->inertia_tensor.inverse() * p_rocket->moment.Sum();

    // Calculate Quaternion
    p_rocket->quaternion_dot = 0.5 * (QuaternionDiff(p_rocket) * p_rocket->attitude.quaternion);


    dx[0] = p_rocket->velocity.ECI[0];  // vel_ECI => pos_ECI
    dx[1] = p_rocket->velocity.ECI[1];  // 
    dx[2] = p_rocket->velocity.ECI[2];  // 
    dx[3] = p_rocket->acceleration.ECI[0];  // acc_ECI => vel_ECI
    dx[4] = p_rocket->acceleration.ECI[1];  // 
    dx[5] = p_rocket->acceleration.ECI[2];  // 
    dx[6] = p_rocket->quaternion_dot[0];  // quatdot => quat
    dx[7] = p_rocket->quaternion_dot[1];  // 
    dx[8] = p_rocket->quaternion_dot[2];  // 
    dx[9] = p_rocket->quaternion_dot[3];  // 
    dx[10] = p_rocket->angular_acceleration[0];  // angular_acceleration => angular_velocity
    dx[11] = p_rocket->angular_acceleration[1];  // 
    dx[12] = p_rocket->angular_acceleration[2];  // 
    dx[13] = -p_rocket->engine.mdot_prop;  // massdot => mass_prop
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x558, %rsp            # imm = 0x558
movsd %xmm0, (%rsp)
movq %rdx, 0x38(%rsp)
movq %rsi, %r15
movq %rdi, %r14
leaq 0x290(%rsp), %rbx
movq %rbx, %rdi
callq 0x150d4
movq 0x10(%r14), %rdi
movsd (%rsp), %xmm0
callq 0x1779e
movq 0x8(%r14), %rdi
movsd (%rsp), %xmm0
callq 0x1779e
movq %rbx, %rdi
movsd (%rsp), %xmm0
callq 0x15e12
movq %r15, %rbx
addq $0x18, %r15
leaq 0x40(%rsp), %rbp
leaq 0xf0(%rsp), %rcx
movq %rbp, %rdi
movq %rbx, %rsi
movq %rbx, 0x10(%rsp)
movq %r15, %rdx
callq 0x228c8
movq (%rbp), %rdi
movq 0x8(%r14), %rax
movups (%rdi), %xmm0
movups %xmm0, 0x2a0(%rax)
movsd 0x10(%rdi), %xmm0
movsd %xmm0, 0x2b0(%rax)
movq 0x10(%rbp), %rsi
subq %rdi, %rsi
callq 0x5350
leaq 0x2d0(%rsp), %rcx
movq 0x8(%r14), %r12
movl $0x2a0, %r13d           # imm = 0x2A0
leaq (%r12,%r13), %rax
movupd 0x2a0(%r12), %xmm0
movupd 0x2a8(%r12), %xmm4
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movupd (%rcx), %xmm1
mulpd %xmm0, %xmm1
movupd 0x18(%rcx), %xmm0
movupd 0x30(%rcx), %xmm2
movapd %xmm4, %xmm3
unpcklpd %xmm4, %xmm3            # xmm3 = xmm3[0],xmm4[0]
mulpd %xmm0, %xmm3
addpd %xmm1, %xmm3
unpckhpd %xmm4, %xmm4            # xmm4 = xmm4[1,1]
mulpd %xmm2, %xmm4
addpd %xmm3, %xmm4
movapd %xmm4, (%rsp)
leaq 0x2e0(%rsp), %rdx
movq %rdx, (%rbp)
movq %rcx, 0x10(%rbp)
movl $0x2, %ecx
movq %rcx, 0x18(%rbp)
xorl %edx, %edx
movq %rdx, 0x20(%rbp)
movl $0x1, %ecx
movq %rcx, 0x28(%rbp)
movq %rax, 0x30(%rbp)
movq %rax, 0x40(%rbp)
movq %rdx, 0x48(%rbp)
movl $0x3, %eax
movq %rax, 0x58(%rbp)
leaq 0xf0(%rsp), %rbp
leaq 0x40(%rsp), %rsi
movq %rbp, %rdi
callq 0x1a0a4
movq 0x10(%rbp), %rax
movq 0x20(%rbp), %rcx
movupd (%rcx), %xmm0
movsd (%rax), %xmm1
movhpd 0x18(%rax), %xmm1       # xmm1 = xmm1[0],mem[0]
mulpd %xmm0, %xmm1
movsd 0x30(%rax), %xmm0
mulsd 0x10(%rcx), %xmm0
movapd %xmm1, %xmm2
unpckhpd %xmm1, %xmm2            # xmm2 = xmm2[1],xmm1[1]
addsd %xmm0, %xmm2
addsd %xmm1, %xmm2
movaps (%rsp), %xmm0
movups %xmm0, 0x2b8(%r12)
movsd %xmm2, 0x2c8(%r12)
movq 0x8(%r14), %rax
movq 0x2c8(%rax), %rcx
leaq 0x230(%rsp), %rdx
movq %rcx, 0x10(%rdx)
movups 0x2b8(%rax), %xmm0
movaps %xmm0, (%rdx)
leaq 0x40(%rsp), %rbp
movq %rbp, %rdi
leaq 0x290(%rsp), %r12
movq %r12, %rsi
callq 0x162e8
movq 0x8(%r14), %rax
movaps (%rbp), %xmm0
movups %xmm0, 0x2d0(%rax)
movsd 0x10(%rbp), %xmm0
movsd %xmm0, 0x2e0(%rax)
movq 0x8(%r14), %rax
movq 0x2e0(%rax), %rcx
leaq 0x210(%rsp), %rsi
movq %rcx, 0x10(%rsi)
movups 0x2d0(%rax), %xmm0
movaps %xmm0, (%rsi)
movq %r12, %rdi
callq 0x1600e
leaq 0x30(%rbx), %rdx
movq %rdx, (%rsp)
leaq 0x40(%rsp), %rbp
leaq 0xf0(%rsp), %rbx
movq %rbp, %rdi
movq %r15, %rsi
movq %rbx, %rcx
callq 0x228c8
movq (%rbp), %rdi
movq 0x8(%r14), %rax
movups (%rdi), %xmm0
movups %xmm0, 0x2e8(%rax)
movsd 0x10(%rdi), %xmm0
movsd %xmm0, 0x2f8(%rax)
movq 0x10(%rbp), %rsi
subq %rdi, %rsi
callq 0x5350
movq 0x8(%r14), %r12
leaq 0x510(%rsp), %rax
addq %r12, %r13
leaq 0x2d0(%rsp), %rcx
movq %rcx, (%rbx)
leaq 0xf8(%rsp), %r15
movupd 0x2a0(%r12), %xmm0
movupd 0x2a8(%r12), %xmm1
movupd 0x2e8(%r12), %xmm5
movsd 0x2f8(%r12), %xmm2
movsd %xmm2, 0x20(%rsp)
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movupd (%rax), %xmm2
mulpd %xmm0, %xmm2
movupd 0x18(%rax), %xmm0
movupd 0x30(%rax), %xmm3
movapd %xmm1, %xmm4
unpcklpd %xmm1, %xmm4            # xmm4 = xmm4[0],xmm1[0]
mulpd %xmm0, %xmm4
addpd %xmm2, %xmm4
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
mulpd %xmm3, %xmm1
addpd %xmm4, %xmm1
subpd %xmm1, %xmm5
movapd %xmm5, 0xc0(%rsp)
movupd %xmm5, (%r15)
leaq 0x520(%rsp), %rcx
movq %rcx, (%rbp)
movq %rax, 0x10(%rbp)
movl $0x2, %eax
movq %rax, 0x18(%rbp)
xorl %ecx, %ecx
movq %rcx, 0x20(%rbp)
movl $0x1, %eax
movq %rax, 0x28(%rbp)
movq %r13, 0x30(%rbp)
movq %r13, 0x40(%rbp)
movq %rcx, 0x48(%rbp)
movl $0x3, %eax
movq %rax, 0x58(%rbp)
leaq 0x178(%rsp), %rbx
leaq 0x40(%rsp), %r13
movq %rbx, %rdi
movq %r13, %rsi
callq 0x1a0a4
movq 0x10(%rbx), %rax
movq 0x20(%rbx), %rcx
movsd (%rax), %xmm0
movsd 0x18(%rax), %xmm1
mulsd (%rcx), %xmm0
mulsd 0x8(%rcx), %xmm1
movsd 0x30(%rax), %xmm2
mulsd 0x10(%rcx), %xmm2
addsd %xmm1, %xmm2
addsd %xmm0, %xmm2
movsd 0x20(%rsp), %xmm0
subsd %xmm2, %xmm0
movsd %xmm0, 0x10(%r15)
leaq 0x2d0(%rsp), %rdx
movq %rdx, 0x18(%r15)
movq %r15, 0x28(%r15)
movl $0x3, %ecx
movq %rcx, 0x38(%r15)
movapd 0xc0(%rsp), %xmm1
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
leaq 0x2e0(%rsp), %rax
movupd -0x10(%rax), %xmm0
mulpd %xmm1, %xmm0
movupd 0x8(%rax), %xmm1
movupd 0x20(%rax), %xmm2
movupd 0x8(%r15), %xmm3
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
mulpd %xmm1, %xmm3
addpd %xmm0, %xmm3
movupd 0x10(%r15), %xmm0
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
mulpd %xmm2, %xmm0
addpd %xmm3, %xmm0
movapd %xmm0, 0x20(%rsp)
movq %rax, (%r13)
movq %rdx, 0x10(%r13)
movl $0x2, %eax
movq %rax, 0x18(%r13)
xorl %eax, %eax
movq %rax, 0x20(%r13)
movl $0x1, %edx
movq %rdx, 0x28(%r13)
movq %r15, 0x30(%r13)
movq %r15, 0x40(%r13)
movq %rax, 0x48(%r13)
xorl %ebp, %ebp
movq %rcx, 0x58(%r13)
leaq 0x178(%rsp), %rbx
leaq 0x40(%rsp), %r13
movq %rbx, %rdi
movq %r13, %rsi
callq 0x1a0a4
movq 0x10(%rbx), %rax
movq 0x20(%rbx), %rcx
movupd (%rcx), %xmm0
movsd (%rax), %xmm1
movhpd 0x18(%rax), %xmm1       # xmm1 = xmm1[0],mem[0]
mulpd %xmm0, %xmm1
movsd 0x30(%rax), %xmm0
mulsd 0x10(%rcx), %xmm0
movapd %xmm1, %xmm2
unpckhpd %xmm1, %xmm2            # xmm2 = xmm2[1],xmm1[1]
addsd %xmm0, %xmm2
addsd %xmm1, %xmm2
movaps 0x20(%rsp), %xmm0
movups %xmm0, 0x300(%r12)
movsd %xmm2, 0x310(%r12)
leaq 0x318(%rsp), %rax
movq 0x8(%r14), %r15
leaq 0x300(%r15), %rcx
movupd 0x300(%r15), %xmm0
movupd 0x308(%r15), %xmm4
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movupd (%rax), %xmm1
mulpd %xmm0, %xmm1
movupd 0x18(%rax), %xmm0
movupd 0x30(%rax), %xmm2
movapd %xmm4, %xmm3
unpcklpd %xmm4, %xmm3            # xmm3 = xmm3[0],xmm4[0]
mulpd %xmm0, %xmm3
addpd %xmm1, %xmm3
unpckhpd %xmm4, %xmm4            # xmm4 = xmm4[1,1]
mulpd %xmm2, %xmm4
addpd %xmm3, %xmm4
movapd %xmm4, 0x20(%rsp)
leaq 0x328(%rsp), %rdx
movq %rdx, (%r13)
movq %rax, 0x10(%r13)
movl $0x2, %eax
movq %rax, 0x18(%r13)
movq %rbp, 0x20(%r13)
movl $0x1, %eax
movq %rax, 0x28(%r13)
movq %rcx, 0x30(%r13)
movq %rcx, 0x40(%r13)
movq %rbp, 0x48(%r13)
movl $0x3, %eax
movq %rax, 0x58(%r13)
leaq 0xf0(%rsp), %rbx
leaq 0x40(%rsp), %rsi
movq %rbx, %rdi
callq 0x1a0a4
movq 0x10(%rbx), %rax
movq 0x20(%rbx), %rcx
movsd (%rax), %xmm0
movhpd 0x18(%rax), %xmm0       # xmm0 = xmm0[0],mem[0]
movupd (%rcx), %xmm1
movsd 0x30(%rax), %xmm2
mulsd 0x10(%rcx), %xmm2
mulpd %xmm1, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
addsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movaps 0x20(%rsp), %xmm0
movups %xmm0, 0x318(%r15)
movsd %xmm1, 0x328(%r15)
movq 0x10(%rsp), %r15
leaq 0x50(%r15), %rbp
leaq 0x40(%rsp), %rbx
leaq 0xf0(%rsp), %rcx
movq %rbx, %rdi
movq (%rsp), %rsi
movq %rbp, %rdx
callq 0x228c8
movq (%rbx), %rdi
movupd (%rdi), %xmm0
movupd 0x10(%rdi), %xmm1
movapd %xmm0, %xmm2
mulpd %xmm0, %xmm2
movapd %xmm1, %xmm3
mulpd %xmm1, %xmm3
addpd %xmm2, %xmm3
movapd %xmm3, %xmm2
unpckhpd %xmm3, %xmm2            # xmm2 = xmm2[1],xmm3[1]
addpd %xmm3, %xmm2
xorpd %xmm3, %xmm3
ucomisd %xmm3, %xmm2
jbe 0x2439b
movq %xmm2, %xmm2            # xmm2 = xmm2[0],zero
sqrtpd %xmm2, %xmm2
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
divpd %xmm2, %xmm0
divpd %xmm2, %xmm1
movq 0x8(%r14), %rax
movapd %xmm0, 0x3f0(%rax)
movapd %xmm1, 0x400(%rax)
leaq 0x40(%rsp), %rbx
movq 0x10(%rbx), %rsi
subq %rdi, %rsi
callq 0x5350
movq 0x8(%r14), %rax
movaps 0x3f0(%rax), %xmm0
movaps 0x400(%rax), %xmm1
leaq 0x250(%rsp), %rsi
movaps %xmm1, 0x10(%rsi)
movaps %xmm0, (%rsi)
leaq 0x290(%rsp), %r12
movq %r12, %rdi
callq 0x15a9e
movq %rbx, %rdi
movq %r12, %rsi
callq 0x16a60
movq 0x8(%r14), %rax
movaps (%rbx), %xmm0
movups %xmm0, 0x3d0(%rax)
movsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x3e0(%rax)
leaq 0x68(%r15), %rdx
leaq 0x40(%rsp), %r12
leaq 0xf0(%rsp), %r13
movq %r12, %rdi
movq %rbp, %rsi
movq %r13, %rcx
callq 0x228c8
movq (%r12), %rdi
movq 0x8(%r14), %rax
movups (%rdi), %xmm0
movups %xmm0, 0x430(%rax)
movsd 0x10(%rdi), %xmm0
movsd %xmm0, 0x440(%rax)
movq 0x10(%r12), %rsi
subq %rdi, %rsi
callq 0x5350
movsd 0x68(%r15), %xmm0
movq 0x8(%r14), %rax
movsd %xmm0, 0x218(%rax)
movsd 0x2e0(%rax), %xmm0
movsd %xmm0, (%rsp)
leaq 0x158(%rsp), %rdi
callq 0x18044
leaq 0x270(%rsp), %rdi
xorps %xmm0, %xmm0
callq 0x18044
movsd 0x17eca(%rip), %xmm4    # 0x3c370
movsd (%rsp), %xmm3
movapd %xmm3, %xmm1
addsd %xmm4, %xmm1
movapd %xmm3, %xmm2
movapd %xmm3, %xmm0
xorpd %xmm3, %xmm3
cmpltsd %xmm3, %xmm2
movapd %xmm2, %xmm3
andnpd %xmm1, %xmm3
andpd %xmm4, %xmm2
orpd %xmm3, %xmm2
mulsd %xmm2, %xmm2
movsd 0x17e98(%rip), %xmm3    # 0x3c378
divsd %xmm2, %xmm3
xorpd %xmm1, %xmm1
movapd %xmm1, 0xd0(%rsp)
movsd %xmm3, 0xe0(%rsp)
leaq 0x360(%rsp), %rbp
movq 0x8(%r14), %r15
movq 0x18(%r14), %rsi
leaq 0x1d0(%rsp), %rbx
movq %rbx, %rdi
callq 0x18628
movq 0x8(%r14), %rax
movq %rax, (%rsp)
movq %rbp, (%r13)
movupd 0x318(%r15), %xmm0
subpd (%rbx), %xmm0
movupd %xmm0, 0x8(%r13)
movsd 0x328(%r15), %xmm1
subsd 0x10(%rbx), %xmm1
movsd %xmm1, 0x18(%r13)
movq %rbp, 0x20(%r13)
leaq 0xf8(%rsp), %rcx
movq %rcx, 0x30(%r13)
movl $0x3, %edx
movq %rdx, 0x40(%r13)
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movupd (%rbp), %xmm1
mulpd %xmm0, %xmm1
movupd 0x18(%rbp), %xmm0
movupd 0x30(%rbp), %xmm2
movupd 0x10(%r13), %xmm3
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
mulpd %xmm0, %xmm3
addpd %xmm1, %xmm3
movupd 0x18(%r13), %xmm0
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
mulpd %xmm2, %xmm0
addpd %xmm3, %xmm0
movapd %xmm0, 0x10(%rsp)
leaq 0x370(%rsp), %rax
movq %rax, (%r12)
movq %rbp, 0x10(%r12)
movq $0x2, 0x18(%r12)
xorl %eax, %eax
movq %rax, 0x20(%r12)
movq $0x1, 0x28(%r12)
movq %rcx, 0x30(%r12)
movq %rcx, 0x40(%r12)
movq %rax, 0x48(%r12)
movq %rdx, 0x58(%r12)
leaq 0x178(%rsp), %rbx
leaq 0x40(%rsp), %rsi
movq %rbx, %rdi
callq 0x1a0a4
movq 0x10(%rbx), %rax
movq 0x20(%rbx), %rcx
movupd (%rcx), %xmm0
movsd (%rax), %xmm1
movhpd 0x18(%rax), %xmm1       # xmm1 = xmm1[0],mem[0]
mulpd %xmm0, %xmm1
movsd 0x30(%rax), %xmm0
mulsd 0x10(%rcx), %xmm0
movapd %xmm1, %xmm2
unpckhpd %xmm1, %xmm2            # xmm2 = xmm2[1],xmm1[1]
addsd %xmm0, %xmm2
addsd %xmm1, %xmm2
movq (%rsp), %rax
movaps 0x10(%rsp), %xmm0
movups %xmm0, 0x330(%rax)
movsd %xmm2, 0x340(%rax)
leaq 0x158(%rsp), %rax
movsd 0x10(%rax), %xmm0
mulsd 0x160ce(%rip), %xmm0    # 0x3a728
movq 0x8(%r14), %rsi
movupd 0x330(%rsi), %xmm1
mulpd %xmm1, %xmm1
movapd %xmm1, %xmm2
unpckhpd %xmm1, %xmm2            # xmm2 = xmm2[1],xmm1[1]
addsd %xmm1, %xmm2
movsd 0x340(%rsi), %xmm1
mulsd %xmm1, %xmm1
addsd %xmm2, %xmm1
sqrtsd %xmm1, %xmm1
movapd %xmm1, %xmm2
mulsd %xmm1, %xmm2
mulsd %xmm0, %xmm2
movsd %xmm2, 0x350(%rsi)
divsd 0x18(%rax), %xmm1
movsd %xmm1, 0x348(%rsi)
leaq 0x40(%rsp), %rbx
movq %rbx, %rdi
callq 0x20a96
movq 0x8(%r14), %rax
movups 0x220(%rax), %xmm0
movaps (%rbx), %xmm1
movups %xmm1, 0x220(%rax)
movaps %xmm0, (%rbx)
movups 0x230(%rax), %xmm0
movaps 0x10(%rbx), %xmm1
movups %xmm1, 0x230(%rax)
movaps %xmm0, 0x10(%rbx)
movups 0x240(%rax), %xmm0
movaps 0x20(%rbx), %xmm1
movups %xmm1, 0x240(%rax)
movaps %xmm0, 0x20(%rbx)
movaps 0x30(%rbx), %xmm0
movups %xmm0, 0x250(%rax)
movsd 0x40(%rbx), %xmm0
movsd %xmm0, 0x260(%rax)
movq 0x8(%r14), %rdi
callq 0x207d8
movq 0x8(%r14), %rdi
movsd %xmm0, 0x200(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x20808
movq 0x8(%r14), %rdi
movsd %xmm0, 0x208(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x20822
movq 0x8(%r14), %rdi
movsd %xmm0, 0x268(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x2084c
movq 0x8(%r14), %rdi
movsd %xmm0, 0x270(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x20866
movq 0x8(%r14), %rdi
movsd %xmm0, 0x278(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x20880
movq 0x8(%r14), %rdi
movsd %xmm0, 0x280(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x208ae
movq 0x8(%r14), %rdi
movsd %xmm0, 0x288(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x208dc
movq 0x8(%r14), %rsi
movsd %xmm0, 0x290(%rsi)
movupd 0x330(%rsi), %xmm0
mulpd %xmm0, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
addsd %xmm0, %xmm1
movsd 0x340(%rsi), %xmm0
movapd %xmm0, %xmm2
mulsd %xmm0, %xmm2
addsd %xmm1, %xmm2
xorps %xmm1, %xmm1
sqrtsd %xmm2, %xmm1
xorpd %xmm2, %xmm2
ucomisd %xmm1, %xmm2
jae 0x2487d
divsd %xmm1, %xmm0
callq 0x5580
movq 0x8(%r14), %rax
movsd %xmm0, 0x460(%rax)
movsd 0x338(%rax), %xmm0
xorpd 0x15ed6(%rip), %xmm0    # 0x3a710
movupd 0x330(%rax), %xmm1
mulpd %xmm1, %xmm1
movapd %xmm1, %xmm2
unpckhpd %xmm1, %xmm2            # xmm2 = xmm2[1],xmm1[1]
addsd %xmm1, %xmm2
movsd 0x340(%rax), %xmm1
mulsd %xmm1, %xmm1
addsd %xmm2, %xmm1
sqrtsd %xmm1, %xmm1
divsd %xmm1, %xmm0
callq 0x5580
movq 0x8(%r14), %rsi
movsd %xmm0, 0x468(%rsi)
jmp 0x24889
xorpd %xmm0, %xmm0
movapd %xmm0, 0x460(%rsi)
movsd 0x160(%rsp), %xmm0
leaq 0x40(%rsp), %rbx
movq %rbx, %rdi
callq 0x2090a
movq 0x8(%r14), %rax
movaps (%rbx), %xmm0
movups %xmm0, 0x388(%rax)
movsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x398(%rax)
movq 0x8(%r14), %rdx
leaq 0x40(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x2100c
movq 0x8(%r14), %rax
movaps (%rbx), %xmm0
movups %xmm0, 0x3a0(%rax)
movsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x3b0(%rax)
movl $0x210, %r13d           # imm = 0x210
movq 0x8(%r14), %rdi
addq %r13, %rdi
callq 0x199e0
leaq 0xf0(%rsp), %r15
leaq 0x360(%rsp), %rax
movq %rax, (%r15)
leaq 0xd0(%rsp), %rax
movq %rax, 0x8(%r15)
movsd %xmm0, 0x18(%r15)
movq 0x8(%r14), %r12
leaq 0x48(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rsi
callq 0x23016
movsd 0x18(%r15), %xmm0
movsd %xmm0, 0x28(%rbx)
movq (%rbx), %rax
movupd (%rax), %xmm0
movupd 0x28(%rbx), %xmm1
movapd %xmm1, %xmm2
unpcklpd %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0]
mulpd %xmm0, %xmm2
movupd %xmm2, 0x3b8(%r12)
mulsd 0x10(%rax), %xmm1
movsd %xmm1, 0x3c8(%r12)
movl $0x388, %esi            # imm = 0x388
addq 0x8(%r14), %rsi
leaq 0x40(%rsp), %r12
movq %r12, %rdi
callq 0x19316
addq 0x8(%r14), %r13
movq %r13, %rdi
callq 0x199e0
movq 0x8(%r14), %rax
movapd (%r12), %xmm1
movapd %xmm0, %xmm2
unpcklpd %xmm0, %xmm2            # xmm2 = xmm2[0],xmm0[0]
divpd %xmm2, %xmm1
movupd %xmm1, 0x370(%rax)
movsd 0x10(%r12), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x380(%rax)
leaq 0x480(%rsp), %rcx
leaq 0x438(%rsp), %rdi
leaq 0x3f0(%rsp), %rdx
movq 0x8(%r14), %rsi
movq %rsi, (%rsp)
leaq 0x370(%rsi), %rax
leaq 0x180(%rsp), %r13
movq %rcx, -0x8(%r13)
movq %rdi, (%r15)
movupd 0x370(%rsi), %xmm0
movupd 0x378(%rsi), %xmm4
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movupd (%rdx), %xmm1
mulpd %xmm0, %xmm1
movupd 0x18(%rdx), %xmm0
movupd 0x30(%rdx), %xmm2
movapd %xmm4, %xmm3
unpcklpd %xmm4, %xmm3            # xmm3 = xmm3[0],xmm4[0]
mulpd %xmm0, %xmm3
addpd %xmm1, %xmm3
unpckhpd %xmm4, %xmm4            # xmm4 = xmm4[1,1]
mulpd %xmm2, %xmm4
addpd %xmm3, %xmm4
movapd %xmm4, 0x10(%rsp)
movupd %xmm4, 0x8(%r15)
leaq 0x400(%rsp), %rbp
movq %rbp, (%r12)
movq %rdx, 0x10(%r12)
movl $0x2, %ecx
movq %rcx, 0x18(%r12)
xorl %ecx, %ecx
movq %rcx, 0x20(%r12)
movl $0x1, %edx
movq %rdx, 0x28(%r12)
movq %rax, 0x30(%r12)
movq %rax, 0x40(%r12)
movq %rcx, 0x48(%r12)
movl $0x3, %eax
movq %rax, 0x58(%r12)
leaq 0x1d0(%rsp), %rbx
leaq 0x40(%rsp), %r12
movq %rbx, %rdi
movq %r12, %rsi
callq 0x1a0a4
movq 0x10(%rbx), %rax
movq 0x20(%rbx), %rcx
movsd (%rax), %xmm0
mulsd (%rcx), %xmm0
movsd 0x18(%rax), %xmm1
mulsd 0x8(%rcx), %xmm1
movsd 0x30(%rax), %xmm2
mulsd 0x10(%rcx), %xmm2
addsd %xmm1, %xmm2
addsd %xmm0, %xmm2
movsd %xmm2, 0x18(%r15)
leaq 0x438(%rsp), %rdx
movq %rdx, 0x20(%r15)
leaq 0xf8(%rsp), %rcx
movq %rcx, 0x30(%r15)
movl $0x3, %esi
movq %rsi, 0x40(%r15)
movapd 0x10(%rsp), %xmm1
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
movupd 0x38(%rbp), %xmm0
mulpd %xmm1, %xmm0
movupd 0x50(%rbp), %xmm1
movupd 0x68(%rbp), %xmm2
movupd 0x10(%r15), %xmm3
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
mulpd %xmm1, %xmm3
addpd %xmm0, %xmm3
movupd 0x18(%r15), %xmm0
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
mulpd %xmm2, %xmm0
addpd %xmm3, %xmm0
movapd %xmm0, 0x10(%rsp)
movupd %xmm0, (%r13)
leaq 0x448(%rsp), %rax
movq %rax, (%r12)
movq %rdx, 0x10(%r12)
movl $0x2, %ebx
movq %rbx, 0x18(%r12)
xorl %eax, %eax
movq %rax, 0x20(%r12)
movl $0x1, %ebp
movq %rbp, 0x28(%r12)
movq %rcx, 0x30(%r12)
movq %rcx, 0x40(%r12)
movq %rax, 0x48(%r12)
movq %rsi, 0x58(%r12)
leaq 0x1d0(%rsp), %r15
leaq 0x40(%rsp), %r12
movq %r15, %rdi
movq %r12, %rsi
callq 0x1a0a4
movq 0x10(%r15), %rax
movq 0x20(%r15), %rcx
movsd (%rax), %xmm0
mulsd (%rcx), %xmm0
movsd 0x18(%rax), %xmm1
mulsd 0x8(%rcx), %xmm1
movsd 0x30(%rax), %xmm2
mulsd 0x10(%rcx), %xmm2
addsd %xmm1, %xmm2
addsd %xmm0, %xmm2
movsd %xmm2, 0x10(%r13)
movq -0x8(%r13), %rax
movq %rax, 0x18(%r13)
movq %r13, 0x28(%r13)
movl $0x3, %edx
movq %rdx, 0x38(%r13)
movupd (%rax), %xmm0
movapd 0x10(%rsp), %xmm1
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
mulpd %xmm0, %xmm1
movapd %xmm1, %xmm2
movupd 0x8(%r13), %xmm0
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movupd 0x18(%rax), %xmm1
mulpd %xmm0, %xmm1
addpd %xmm2, %xmm1
movupd 0x30(%rax), %xmm0
movupd 0x10(%r13), %xmm2
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
mulpd %xmm0, %xmm2
addpd %xmm1, %xmm2
movapd %xmm2, 0x10(%rsp)
leaq 0x10(%rax), %rcx
movq %rcx, (%r12)
movq %rax, 0x10(%r12)
movq %rbx, 0x18(%r12)
xorl %eax, %eax
movq %rax, 0x20(%r12)
movq %rbp, 0x28(%r12)
movq %r13, 0x30(%r12)
movq %r13, 0x40(%r12)
movq %rax, 0x48(%r12)
movq %rdx, 0x58(%r12)
leaq 0xf0(%rsp), %rbx
leaq 0x40(%rsp), %rsi
movq %rbx, %rdi
callq 0x1a0a4
movq 0x10(%rbx), %rax
movq 0x20(%rbx), %rcx
movsd (%rax), %xmm0
movhpd 0x18(%rax), %xmm0       # xmm0 = xmm0[0],mem[0]
movupd (%rcx), %xmm1
movsd 0x30(%rax), %xmm2
mulsd 0x10(%rcx), %xmm2
mulpd %xmm1, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
addsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movq (%rsp), %rax
movaps 0x10(%rsp), %xmm0
movups %xmm0, 0x358(%rax)
movsd %xmm1, 0x368(%rax)
movq 0x8(%r14), %rdx
leaq 0x40(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x21122
movq 0x8(%r14), %rax
movaps (%rbx), %xmm0
movups %xmm0, 0x470(%rax)
movsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x480(%rax)
movq 0x8(%r14), %rdx
leaq 0x40(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x21280
movq 0x8(%r14), %rax
movaps (%rbx), %xmm0
movups %xmm0, 0x488(%rax)
movsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x498(%rax)
movq 0x8(%r14), %rdx
leaq 0x40(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x212de
movq 0x8(%r14), %rax
movaps (%rbx), %xmm0
movups %xmm0, 0x4a0(%rax)
movsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x4b0(%rax)
movq 0x8(%r14), %rdx
leaq 0x40(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x21370
movq 0x8(%r14), %rax
movaps (%rbx), %xmm0
movups %xmm0, 0x4b8(%rax)
movsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x4c8(%rax)
movq 0x8(%r14), %rdx
leaq 0x40(%rsp), %r12
movq %r12, %rdi
movq %r14, %rsi
callq 0x2141a
movq 0x8(%r14), %rax
movaps (%r12), %xmm0
movups %xmm0, 0x4d0(%rax)
movsd 0x10(%r12), %xmm0
movsd %xmm0, 0x4e0(%rax)
movq 0x8(%r14), %rsi
leaq 0x220(%rsi), %r13
addq $0x470, %rsi            # imm = 0x470
movq %r15, %rdi
callq 0x19c24
movq 0x8(%r14), %rbp
leaq 0x1c0(%rsp), %rbx
movq %r13, (%rbx)
movq %r15, 0x8(%rbx)
leaq 0xf0(%rsp), %r15
movq %r12, %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x250a2
movq 0x8(%rbx), %rax
movq %rax, 0x48(%r12)
movq %r12, 0x50(%r12)
movq %rax, 0x60(%r12)
movl $0x3, %edx
movq %rdx, 0x70(%r12)
movsd (%rax), %xmm0
movsd 0x8(%rax), %xmm1
movsd 0x10(%rax), %xmm3
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
mulpd (%r12), %xmm0
movupd 0x18(%r12), %xmm2
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
mulpd %xmm2, %xmm1
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
mulpd 0x30(%r12), %xmm3
addpd %xmm0, %xmm1
addpd %xmm1, %xmm3
movapd %xmm3, (%rsp)
leaq 0x50(%rsp), %r13
movq %r13, (%r15)
movq %r12, 0x10(%r15)
movl $0x2, %ecx
movq %rcx, 0x18(%r15)
xorl %esi, %esi
movq %rsi, 0x20(%r15)
movl $0x1, %ecx
movq %rcx, 0x28(%r15)
movq %rax, 0x30(%r15)
movq %rax, 0x40(%r15)
movq %rsi, 0x48(%r15)
movq %rdx, 0x58(%r15)
leaq 0x178(%rsp), %rbx
leaq 0xf0(%rsp), %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x1a0a4
movq 0x10(%rbx), %rax
movq 0x20(%rbx), %rcx
movsd (%rax), %xmm0
movhpd 0x18(%rax), %xmm0       # xmm0 = xmm0[0],mem[0]
movupd (%rcx), %xmm1
movsd 0x30(%rax), %xmm2
mulsd 0x10(%rcx), %xmm2
mulpd %xmm1, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
addsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movaps (%rsp), %xmm0
movups %xmm0, 0x448(%rbp)
movsd %xmm1, 0x458(%rbp)
movq 0x8(%r14), %rdx
movq %r12, %rdi
movq %r14, %rsi
callq 0x214c2
movq 0x8(%r14), %rax
movabsq $0x3fe0000000000000, %rcx # imm = 0x3FE0000000000000
movq %rcx, (%r15)
movq %r12, 0x8(%r15)
movapd (%r15), %xmm0
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movapd -0x10(%r13), %xmm1
mulpd %xmm0, %xmm1
movapd 0x3f0(%rax), %xmm2
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
mulpd %xmm2, %xmm1
movapd 0x10(%r13), %xmm4
mulpd %xmm0, %xmm4
movupd 0x3f8(%rax), %xmm3
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
mulpd %xmm3, %xmm4
addpd %xmm1, %xmm4
movapd 0x30(%r13), %xmm5
mulpd %xmm0, %xmm5
movupd 0x400(%rax), %xmm1
movapd %xmm1, %xmm6
unpcklpd %xmm1, %xmm6            # xmm6 = xmm6[0],xmm1[0]
mulpd %xmm6, %xmm5
addpd %xmm4, %xmm5
movapd 0x50(%r13), %xmm4
mulpd %xmm0, %xmm4
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
mulpd %xmm1, %xmm4
addpd %xmm5, %xmm4
movapd (%r13), %xmm5
mulpd %xmm0, %xmm5
mulpd %xmm2, %xmm5
movapd 0x20(%r13), %xmm2
mulpd %xmm0, %xmm2
mulpd %xmm3, %xmm2
addpd %xmm5, %xmm2
movapd 0x40(%r13), %xmm3
mulpd %xmm0, %xmm3
mulpd %xmm6, %xmm3
mulpd 0x60(%r13), %xmm0
addpd %xmm2, %xmm3
mulpd %xmm1, %xmm0
addpd %xmm3, %xmm0
movapd %xmm4, 0x410(%rax)
movapd %xmm0, 0x420(%rax)
movq 0x8(%r14), %rax
movsd 0x2e8(%rax), %xmm0
movq 0x38(%rsp), %rcx
movsd %xmm0, (%rcx)
movsd 0x2f0(%rax), %xmm0
movsd %xmm0, 0x8(%rcx)
movsd 0x2f8(%rax), %xmm0
movsd %xmm0, 0x10(%rcx)
movsd 0x358(%rax), %xmm0
movsd %xmm0, 0x18(%rcx)
movsd 0x360(%rax), %xmm0
movsd %xmm0, 0x20(%rcx)
movsd 0x368(%rax), %xmm0
movsd %xmm0, 0x28(%rcx)
movsd 0x410(%rax), %xmm0
movsd %xmm0, 0x30(%rcx)
movsd 0x418(%rax), %xmm0
movsd %xmm0, 0x38(%rcx)
movsd 0x420(%rax), %xmm0
movsd %xmm0, 0x40(%rcx)
movsd 0x428(%rax), %xmm0
movsd %xmm0, 0x48(%rcx)
movsd 0x448(%rax), %xmm0
movsd %xmm0, 0x50(%rcx)
movsd 0x450(%rax), %xmm0
movsd %xmm0, 0x58(%rcx)
movsd 0x458(%rax), %xmm0
movsd %xmm0, 0x60(%rcx)
movsd 0x60(%rax), %xmm0
xorps 0x15684(%rip), %xmm0    # 0x3a710
movlps %xmm0, 0x68(%rcx)
addq $0x558, %rsp            # imm = 0x558
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq