// File: DavidLBruce[P]sim3dof/build/../src/FcTse6.cpp
// Function: FcTse6::IntegrateRK2(Matrix&, double)
void FcTse6::IntegrateRK2( Matrix &XX, double deltaTime  )
{
  double  half = 0.5;
  Matrix  stateDerivK(6,1);
  Matrix  stateDerivKp1(6,1);
  Matrix  stateIncrement(6,1);
  Matrix  stateTemp(6,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;

  XX += stateIncrement;

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x220, %rsp            # imm = 0x220
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movsd %xmm0, -0x18(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x210(%rbp)
movabsq $0x3fe0000000000000, %rax # imm = 0x3FE0000000000000
movq %rax, -0x20(%rbp)
leaq -0x38(%rbp), %rdi
movl $0x6, %esi
movl %esi, -0x208(%rbp)
movl $0x1, %edx
movl %edx, -0x204(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x200(%rbp)
callq 0x1f2e0
movl -0x208(%rbp), %esi
movl -0x204(%rbp), %edx
movsd -0x200(%rbp), %xmm0
leaq -0x50(%rbp), %rdi
callq 0x1f2e0
jmp 0xa6ca
leaq -0x78(%rbp), %rdi
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f2e0
jmp 0xa6e2
leaq -0x90(%rbp), %rdi
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f2e0
jmp 0xa6fd
movq -0x210(%rbp), %rsi
movq -0x10(%rbp), %rdx
leaq -0xc0(%rbp), %rdi
callq 0xaf30
jmp 0xa716
leaq -0xa8(%rbp), %rdi
leaq -0x38(%rbp), %rsi
leaq -0xc0(%rbp), %rdx
callq 0x1ff00
jmp 0xa72f
leaq -0xa8(%rbp), %rdi
callq 0x1f580
leaq -0xc0(%rbp), %rdi
callq 0x1f580
movsd -0x18(%rbp), %xmm0
leaq -0xf0(%rbp), %rdi
leaq -0x38(%rbp), %rsi
callq 0x20700
jmp 0xa75e
leaq -0xd8(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0xf0(%rbp), %rdx
callq 0x1ff00
jmp 0xa777
leaq -0xd8(%rbp), %rdi
callq 0x1f580
leaq -0xf0(%rbp), %rdi
callq 0x1f580
movq -0x10(%rbp), %rax
movq %rax, -0x218(%rbp)
leaq -0x138(%rbp), %rdi
leaq -0x78(%rbp), %rsi
callq 0x1f3e0
jmp 0xa7ac
movq -0x218(%rbp), %rsi
leaq -0x120(%rbp), %rdi
leaq -0x138(%rbp), %rdx
callq 0x20020
jmp 0xa7c8
leaq -0x108(%rbp), %rdi
leaq -0x90(%rbp), %rsi
leaq -0x120(%rbp), %rdx
callq 0x1ff00
jmp 0xa7e4
leaq -0x108(%rbp), %rdi
callq 0x1f580
leaq -0x120(%rbp), %rdi
callq 0x1f580
leaq -0x138(%rbp), %rdi
callq 0x1f580
movq -0x210(%rbp), %rsi
leaq -0x168(%rbp), %rdi
leaq -0x90(%rbp), %rdx
callq 0xaf30
jmp 0xa824
leaq -0x150(%rbp), %rdi
leaq -0x50(%rbp), %rsi
leaq -0x168(%rbp), %rdx
callq 0x1ff00
jmp 0xa83d
leaq -0x150(%rbp), %rdi
callq 0x1f580
leaq -0x168(%rbp), %rdi
callq 0x1f580
leaq -0x1e0(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x1f3e0
jmp 0xa867
leaq -0x1c8(%rbp), %rdi
leaq -0x38(%rbp), %rsi
leaq -0x1e0(%rbp), %rdx
callq 0x20020
jmp 0xa880
movsd -0x18(%rbp), %xmm0
leaq -0x1b0(%rbp), %rdi
leaq -0x1c8(%rbp), %rsi
callq 0x20700
jmp 0xa89a
movsd -0x20(%rbp), %xmm0
leaq -0x198(%rbp), %rdi
leaq -0x1b0(%rbp), %rsi
callq 0x20700
jmp 0xa8b4
leaq -0x180(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0x198(%rbp), %rdx
callq 0x1ff00
jmp 0xa8cd
leaq -0x180(%rbp), %rdi
callq 0x1f580
leaq -0x198(%rbp), %rdi
callq 0x1f580
leaq -0x1b0(%rbp), %rdi
callq 0x1f580
leaq -0x1c8(%rbp), %rdi
callq 0x1f580
leaq -0x1e0(%rbp), %rdi
callq 0x1f580
leaq -0x1f8(%rbp), %rdi
leaq -0x78(%rbp), %rsi
callq 0x1f3e0
jmp 0xa91b
movq -0x10(%rbp), %rdi
leaq -0x1f8(%rbp), %rsi
callq 0x208d0
jmp 0xa92d
leaq -0x1f8(%rbp), %rdi
callq 0x1f580
leaq -0x90(%rbp), %rdi
callq 0x1f580
leaq -0x78(%rbp), %rdi
callq 0x1f580
leaq -0x50(%rbp), %rdi
callq 0x1f580
leaq -0x38(%rbp), %rdi
callq 0x1f580
addq $0x220, %rsp            # imm = 0x220
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaad9
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaad0
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaac7
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaabb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
leaq -0xc0(%rbp), %rdi
callq 0x1f580
jmp 0xaabb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
leaq -0xf0(%rbp), %rdi
callq 0x1f580
jmp 0xaabb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaa0d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
leaq -0x120(%rbp), %rdi
callq 0x1f580
leaq -0x138(%rbp), %rdi
callq 0x1f580
jmp 0xaabb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
leaq -0x168(%rbp), %rdi
callq 0x1f580
jmp 0xaabb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaa95
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaa89
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
jmp 0xaa7d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
leaq -0x198(%rbp), %rdi
callq 0x1f580
leaq -0x1b0(%rbp), %rdi
callq 0x1f580
leaq -0x1c8(%rbp), %rdi
callq 0x1f580
leaq -0x1e0(%rbp), %rdi
callq 0x1f580
jmp 0xaabb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x58(%rbp)
movl %eax, -0x5c(%rbp)
leaq -0x1f8(%rbp), %rdi
callq 0x1f580
leaq -0x90(%rbp), %rdi
callq 0x1f580
leaq -0x78(%rbp), %rdi
callq 0x1f580
leaq -0x50(%rbp), %rdi
callq 0x1f580
leaq -0x38(%rbp), %rdi
callq 0x1f580
movq -0x58(%rbp), %rdi
callq 0x33d0
nopl (%rax,%rax)