// File: 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
subq $0x240, %rsp            # imm = 0x240
movq %rdi, -0x228(%rbp)
movq %rdi, %rax
movq %rax, -0x220(%rbp)
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movsd %xmm0, -0x20(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x218(%rbp)
movabsq $0x3fe0000000000000, %rax # imm = 0x3FE0000000000000
movq %rax, -0x28(%rbp)
leaq -0x40(%rbp), %rdi
movl $0x3, %esi
movl %esi, -0x210(%rbp)
movl $0x1, %edx
movl %edx, -0x20c(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x208(%rbp)
callq 0x1f2e0
movl -0x210(%rbp), %esi
movl -0x20c(%rbp), %edx
movsd -0x208(%rbp), %xmm0
leaq -0x58(%rbp), %rdi
callq 0x1f2e0
jmp 0x66cf
leaq -0x80(%rbp), %rdi
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f2e0
jmp 0x66e7
leaq -0x98(%rbp), %rdi
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f2e0
jmp 0x6702
movq -0x218(%rbp), %rsi
movq -0x18(%rbp), %rdx
leaq -0xc8(%rbp), %rdi
callq 0x6b10
jmp 0x671b
leaq -0xb0(%rbp), %rdi
leaq -0x40(%rbp), %rsi
leaq -0xc8(%rbp), %rdx
callq 0x1ff00
jmp 0x6734
leaq -0xb0(%rbp), %rdi
callq 0x1f580
leaq -0xc8(%rbp), %rdi
callq 0x1f580
movsd -0x20(%rbp), %xmm0
leaq -0xf8(%rbp), %rdi
leaq -0x40(%rbp), %rsi
callq 0x20700
jmp 0x6763
leaq -0xe0(%rbp), %rdi
leaq -0x80(%rbp), %rsi
leaq -0xf8(%rbp), %rdx
callq 0x1ff00
jmp 0x677c
leaq -0xe0(%rbp), %rdi
callq 0x1f580
leaq -0xf8(%rbp), %rdi
callq 0x1f580
movq -0x18(%rbp), %rax
movq %rax, -0x230(%rbp)
leaq -0x140(%rbp), %rdi
leaq -0x80(%rbp), %rsi
callq 0x1f3e0
jmp 0x67b1
movq -0x230(%rbp), %rsi
leaq -0x128(%rbp), %rdi
leaq -0x140(%rbp), %rdx
callq 0x20020
jmp 0x67cd
leaq -0x110(%rbp), %rdi
leaq -0x98(%rbp), %rsi
leaq -0x128(%rbp), %rdx
callq 0x1ff00
jmp 0x67e9
leaq -0x110(%rbp), %rdi
callq 0x1f580
leaq -0x128(%rbp), %rdi
callq 0x1f580
leaq -0x140(%rbp), %rdi
callq 0x1f580
movq -0x218(%rbp), %rsi
leaq -0x170(%rbp), %rdi
leaq -0x98(%rbp), %rdx
callq 0x6b10
jmp 0x6829
leaq -0x158(%rbp), %rdi
leaq -0x58(%rbp), %rsi
leaq -0x170(%rbp), %rdx
callq 0x1ff00
jmp 0x6842
leaq -0x158(%rbp), %rdi
callq 0x1f580
leaq -0x170(%rbp), %rdi
callq 0x1f580
leaq -0x1e8(%rbp), %rdi
leaq -0x58(%rbp), %rsi
callq 0x1f3e0
jmp 0x686c
leaq -0x1d0(%rbp), %rdi
leaq -0x40(%rbp), %rsi
leaq -0x1e8(%rbp), %rdx
callq 0x20020
jmp 0x6885
movsd -0x20(%rbp), %xmm0
leaq -0x1b8(%rbp), %rdi
leaq -0x1d0(%rbp), %rsi
callq 0x20700
jmp 0x689f
movsd -0x28(%rbp), %xmm0
leaq -0x1a0(%rbp), %rdi
leaq -0x1b8(%rbp), %rsi
callq 0x20700
jmp 0x68b9
leaq -0x188(%rbp), %rdi
leaq -0x80(%rbp), %rsi
leaq -0x1a0(%rbp), %rdx
callq 0x1ff00
jmp 0x68d2
leaq -0x188(%rbp), %rdi
callq 0x1f580
leaq -0x1a0(%rbp), %rdi
callq 0x1f580
leaq -0x1b8(%rbp), %rdi
callq 0x1f580
leaq -0x1d0(%rbp), %rdi
callq 0x1f580
leaq -0x1e8(%rbp), %rdi
callq 0x1f580
movq -0x18(%rbp), %rax
movq %rax, -0x238(%rbp)
leaq -0x200(%rbp), %rdi
leaq -0x80(%rbp), %rsi
callq 0x1f3e0
jmp 0x692b
movq -0x238(%rbp), %rsi
movq -0x228(%rbp), %rdi
leaq -0x200(%rbp), %rdx
callq 0x20020
jmp 0x6947
leaq -0x200(%rbp), %rdi
callq 0x1f580
leaq -0x98(%rbp), %rdi
callq 0x1f580
leaq -0x80(%rbp), %rdi
callq 0x1f580
leaq -0x58(%rbp), %rdi
callq 0x1f580
leaq -0x40(%rbp), %rdi
callq 0x1f580
movq -0x220(%rbp), %rax
addq $0x240, %rsp            # imm = 0x240
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6afa
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6af1
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6ae8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6adc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0xc8(%rbp), %rdi
callq 0x1f580
jmp 0x6adc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0xf8(%rbp), %rdi
callq 0x1f580
jmp 0x6adc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6a2e
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0x128(%rbp), %rdi
callq 0x1f580
leaq -0x140(%rbp), %rdi
callq 0x1f580
jmp 0x6adc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0x170(%rbp), %rdi
callq 0x1f580
jmp 0x6adc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6ab6
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6aaa
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x6a9e
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0x1a0(%rbp), %rdi
callq 0x1f580
leaq -0x1b8(%rbp), %rdi
callq 0x1f580
leaq -0x1d0(%rbp), %rdi
callq 0x1f580
leaq -0x1e8(%rbp), %rdi
callq 0x1f580
jmp 0x6adc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0x200(%rbp), %rdi
callq 0x1f580
leaq -0x98(%rbp), %rdi
callq 0x1f580
leaq -0x80(%rbp), %rdi
callq 0x1f580
leaq -0x58(%rbp), %rdi
callq 0x1f580
leaq -0x40(%rbp), %rdi
callq 0x1f580
movq -0x60(%rbp), %rdi
callq 0x33d0
nopl (%rax)