// File: /workspace/github_cpp_makefile_star2_O1/DavidLBruce[P]sim3dof/build/../src/FcTseO.cpp
// Function: FcTseO::IntegrateRK2(Matrix&, double)
Matrix FcTseO::IntegrateRK2( Matrix &XX, double deltaTime  )
{
  double   half = 0.5;
  Matrix   stateDerivK(3,1);
  Matrix   stateDerivKp1(3,1);
  Matrix   stateIncrement(3,1);
  Matrix   stateTemp(3,1);

  //
  // Get derivative at current time
  //
  stateDerivK = CalcStateDeriv( XX );

  //
  // Get derivative at next time step
  //
  stateIncrement = stateDerivK * deltaTime;

  stateTemp = XX + stateIncrement;

  stateDerivKp1 = CalcStateDeriv( stateTemp );

  //
  // The final state increment is calculated from the average of the current
  // and next state derivatives.
  //
  stateIncrement = (stateDerivK + stateDerivKp1) * deltaTime * half;

  return( XX + stateIncrement );

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r14
pushq %rbx
subq $0x1d0, %rsp            # imm = 0x1D0
movsd %xmm0, -0x18(%rbp)
movq %rdx, %r14
movq %rdi, %rbx
leaq -0x48(%rbp), %rdi
xorps %xmm0, %xmm0
movl $0x3, %esi
movl $0x1, %edx
callq 0x13504
leaq -0x78(%rbp), %rdi
xorps %xmm0, %xmm0
movl $0x3, %esi
movl $0x1, %edx
callq 0x13504
leaq -0x30(%rbp), %rdi
xorps %xmm0, %xmm0
movl $0x3, %esi
movl $0x1, %edx
callq 0x13504
leaq -0x60(%rbp), %rdi
xorps %xmm0, %xmm0
movl $0x3, %esi
movl $0x1, %edx
callq 0x13504
leaq -0x168(%rbp), %rdi
movq %r14, %rdx
callq 0x5904
leaq -0x1e0(%rbp), %rdi
leaq -0x48(%rbp), %rsi
leaq -0x168(%rbp), %rdx
callq 0x13b92
leaq -0x1e0(%rbp), %rdi
callq 0x13642
leaq -0x168(%rbp), %rdi
callq 0x13642
leaq -0x150(%rbp), %rdi
leaq -0x48(%rbp), %rsi
movsd -0x18(%rbp), %xmm0
callq 0x14000
leaq -0x1c8(%rbp), %rdi
leaq -0x30(%rbp), %rsi
leaq -0x150(%rbp), %rdx
callq 0x13b92
leaq -0x1c8(%rbp), %rdi
callq 0x13642
leaq -0x150(%rbp), %rdi
callq 0x13642
leaq -0x120(%rbp), %rdi
leaq -0x30(%rbp), %rsi
callq 0x13576
leaq -0x138(%rbp), %rdi
leaq -0x120(%rbp), %rdx
movq %r14, %rsi
callq 0x13c48
leaq -0x1b0(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x138(%rbp), %rdx
callq 0x13b92
leaq -0x1b0(%rbp), %rdi
callq 0x13642
leaq -0x138(%rbp), %rdi
callq 0x13642
leaq -0x120(%rbp), %rdi
callq 0x13642
leaq -0x108(%rbp), %rdi
leaq -0x60(%rbp), %rdx
callq 0x5904
leaq -0x198(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0x108(%rbp), %rdx
callq 0x13b92
leaq -0x198(%rbp), %rdi
callq 0x13642
leaq -0x108(%rbp), %rdi
callq 0x13642
leaq -0xa8(%rbp), %rdi
leaq -0x78(%rbp), %rsi
callq 0x13576
leaq -0xc0(%rbp), %rdi
leaq -0x48(%rbp), %rsi
leaq -0xa8(%rbp), %rdx
callq 0x13c48
leaq -0xd8(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
movsd -0x18(%rbp), %xmm0
callq 0x14000
leaq -0xf0(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
movsd 0x109f2(%rip), %xmm0    # 0x16168
callq 0x14000
leaq -0x180(%rbp), %rdi
leaq -0x30(%rbp), %rsi
leaq -0xf0(%rbp), %rdx
callq 0x13b92
leaq -0x180(%rbp), %rdi
callq 0x13642
leaq -0xf0(%rbp), %rdi
callq 0x13642
leaq -0xd8(%rbp), %rdi
callq 0x13642
leaq -0xc0(%rbp), %rdi
callq 0x13642
leaq -0xa8(%rbp), %rdi
callq 0x13642
leaq -0x90(%rbp), %rdi
leaq -0x30(%rbp), %rsi
callq 0x13576
leaq -0x90(%rbp), %rdx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x13c48
leaq -0x90(%rbp), %rdi
callq 0x13642
leaq -0x60(%rbp), %rdi
callq 0x13642
leaq -0x30(%rbp), %rdi
callq 0x13642
leaq -0x78(%rbp), %rdi
callq 0x13642
leaq -0x48(%rbp), %rdi
callq 0x13642
movq %rbx, %rax
addq $0x1d0, %rsp            # imm = 0x1D0
popq %rbx
popq %r14
popq %rbp
retq
movq %rax, %rbx
leaq -0x90(%rbp), %rdi
jmp 0x58be
movq %rax, %rbx
leaq -0xf0(%rbp), %rdi
callq 0x13642
jmp 0x5852
movq %rax, %rbx
leaq -0xd8(%rbp), %rdi
callq 0x13642
jmp 0x5863
movq %rax, %rbx
leaq -0xc0(%rbp), %rdi
callq 0x13642
jmp 0x5874
movq %rax, %rbx
leaq -0xa8(%rbp), %rdi
jmp 0x58be
jmp 0x58d4
movq %rax, %rbx
leaq -0x108(%rbp), %rdi
jmp 0x58be
movq %rax, %rbx
leaq -0x138(%rbp), %rdi
callq 0x13642
jmp 0x589f
movq %rax, %rbx
leaq -0x120(%rbp), %rdi
jmp 0x58be
movq %rax, %rbx
leaq -0x150(%rbp), %rdi
jmp 0x58be
movq %rax, %rbx
leaq -0x168(%rbp), %rdi
callq 0x13642
jmp 0x58d7
movq %rax, %rbx
jmp 0x58e0
movq %rax, %rbx
jmp 0x58e9
movq %rax, %rbx
jmp 0x58f2
movq %rax, %rbx
leaq -0x60(%rbp), %rdi
callq 0x13642
leaq -0x30(%rbp), %rdi
callq 0x13642
leaq -0x78(%rbp), %rdi
callq 0x13642
leaq -0x48(%rbp), %rdi
callq 0x13642
movq %rbx, %rdi
callq 0x32d0
nop