// File: /workspace/github_cpp_makefile_star2_O2/DavidLBruce[P]sim3dof/build/../src/FcTse6.cpp
// Function: FcTse6::FrameUpdate()
void FcTse6::FrameUpdate()
{
  double cth;
  double posXrel;
  double posYrel;
  double sth;
  double truRng2;
  double velXrel;
  double velYrel;

  simTime = GetSimTime();

  //
  // Get the measurements
  //
  measAng = pRdr->measAng;
  angSig = pRdr->rinAng; 

  measRng = pRdr->measRng;
  rngSig = pRdr->rinRng;

  measRdot = pRdr->measRdot;
  rDotSig = pRdr->rinRdot;

  measVec(0) = measAng;
  measVec(1) = 1.0 / measRng;
  measVec(2) = measRdot / measRng;

  timeStamp = pRdr->timeStamp;

  PropStates( frameTimeStep );

  UpdateStates();

  estLosAng = mscState(0);
  estLosr = mscState(1);
  estRng = 1.0 / mscState(2);
  estRdot = mscState(3) * estRng;
  estAccX = mscState(4) * estRng;
  estAccY = mscState(5) * estRng;

  cth = cos( estLosAng );
  sth = sin( estLosAng );

  //
  // Get truth msc State
  //
  posXrel = pTgt->pxf - pRdr->pxf;
  posYrel = pTgt->pyf - pRdr->pyf;

  velXrel = pTgt->vxf;
  velYrel = pTgt->vyf;

  truLosAng = atan2( posYrel, posXrel );
  cth = cos( truLosAng );
  sth = sin( truLosAng );

  truRng2 = SQ(posXrel) + SQ(posYrel);

  truRng = sqrt( truRng2 );

  truLosr = (posXrel * velYrel - posYrel * velXrel ) / truRng2;

  truRdot = (posXrel * velXrel + posYrel * velYrel ) / truRng;

  truAccX = pTgt->axf;
  truAccY = pTgt->ayf;

  truAccLosX =  truAccX * cth + truAccY * sth;
  truAccLosY = -truAccX * sth + truAccY * cth;

  truState(0) = truLosAng;

  truState(1) = truLosr;

  truState(2) = 1.0 / truRng;

  truState(3) = truRdot * truState(2);

  truState(4) = truAccLosX  * truState(2);

  truState(5) = truAccLosY  * truState(2);

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rdi, %rbx
movq 0x38(%rdi), %rax
movsd (%rax), %xmm0
movsd %xmm0, 0x2f8(%rdi)
movq 0x308(%rdi), %rax
movsd 0x128(%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movsd %xmm0, 0x240(%rdi)
movsd 0x110(%rax), %xmm0
movsd %xmm0, 0x1b8(%rdi)
movsd 0x138(%rax), %xmm0
movsd %xmm0, 0x248(%rdi)
movsd 0x120(%rax), %xmm0
movsd %xmm0, 0x2d8(%rdi)
movsd 0x130(%rax), %xmm0
movsd %xmm0, 0x250(%rdi)
movsd 0x118(%rax), %xmm0
movsd %xmm0, 0x2b8(%rdi)
leaq 0x258(%rdi), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0xef12(%rip), %xmm0     # 0x15038
movsd %xmm0, -0x30(%rbp)
divsd 0x248(%rbx), %xmm0
movsd %xmm0, -0x38(%rbp)
pushq $0x1
popq %r15
movq %r12, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x250(%rbx), %xmm0
divsd 0x248(%rbx), %xmm0
movsd %xmm0, -0x38(%rbp)
pushq $0x2
popq %r14
movq %r12, %rdi
movl %r14d, %esi
callq 0x131c0
movsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movq 0x308(%rbx), %rax
movsd 0x150(%rax), %xmm0
movsd %xmm0, 0x158(%rbx)
movsd 0x8(%rbx), %xmm0
movq %rbx, %rdi
callq 0x64ea
movq %rbx, %rdi
callq 0x6cec
leaq 0x270(%rbx), %r13
movq %r13, %rdi
xorl %esi, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, 0x138(%rbx)
movq %r13, %rdi
movl %r15d, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, 0x140(%rbx)
movq %r13, %rdi
movl %r14d, %esi
callq 0x131c0
movsd 0xee48(%rip), %xmm0     # 0x15038
divsd (%rax), %xmm0
movsd %xmm0, 0x148(%rbx)
pushq $0x3
popq %r12
movq %r13, %rdi
movl %r12d, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd 0x148(%rbx), %xmm0
movsd %xmm0, 0x150(%rbx)
pushq $0x4
popq %rsi
movq %r13, %rdi
callq 0x131c0
movsd (%rax), %xmm0
mulsd 0x148(%rbx), %xmm0
movsd %xmm0, 0x128(%rbx)
pushq $0x5
popq %rsi
movq %r13, %rdi
callq 0x131c0
movsd (%rax), %xmm0
mulsd 0x148(%rbx), %xmm0
movsd %xmm0, 0x130(%rbx)
movsd 0x138(%rbx), %xmm0
callq 0x3360
movsd 0x138(%rbx), %xmm0
callq 0x3080
movq 0x308(%rbx), %rax
movq 0x310(%rbx), %rcx
movsd 0x160(%rcx), %xmm1
movsd 0x168(%rcx), %xmm2
subsd 0x140(%rax), %xmm1
movsd %xmm1, -0x40(%rbp)
subsd 0x148(%rax), %xmm2
movsd %xmm2, -0x48(%rbp)
movsd 0x180(%rcx), %xmm0
movsd %xmm0, -0x50(%rbp)
movsd 0x190(%rcx), %xmm0
movsd %xmm0, -0x58(%rbp)
movapd %xmm2, %xmm0
callq 0x3060
movsd %xmm0, 0x180(%rbx)
callq 0x3360
movsd %xmm0, -0x38(%rbp)
movsd 0x180(%rbx), %xmm0
callq 0x3080
movsd -0x40(%rbp), %xmm7
movapd %xmm7, %xmm1
mulsd %xmm7, %xmm1
movsd -0x48(%rbp), %xmm5
movapd %xmm5, %xmm2
mulsd %xmm5, %xmm2
addsd %xmm1, %xmm2
sqrtsd %xmm2, %xmm1
movsd %xmm1, 0x190(%rbx)
movsd -0x50(%rbp), %xmm6
movapd %xmm6, %xmm3
mulsd %xmm5, %xmm3
movapd %xmm7, %xmm4
movapd %xmm7, %xmm8
movsd -0x58(%rbp), %xmm7
mulsd %xmm7, %xmm4
subsd %xmm3, %xmm4
divsd %xmm2, %xmm4
movsd %xmm4, 0x188(%rbx)
mulsd %xmm7, %xmm5
movapd %xmm8, %xmm2
mulsd %xmm6, %xmm2
addsd %xmm5, %xmm2
divsd %xmm1, %xmm2
movsd %xmm2, 0x198(%rbx)
movq 0x310(%rbx), %rax
movsd 0x130(%rax), %xmm1
movsd %xmm1, 0x160(%rbx)
movsd 0x140(%rax), %xmm2
movsd %xmm2, 0x168(%rbx)
movapd %xmm1, %xmm3
mulsd %xmm0, %xmm1
mulsd %xmm2, %xmm0
movsd -0x38(%rbp), %xmm4
mulsd %xmm4, %xmm3
addsd %xmm0, %xmm3
movsd %xmm3, 0x170(%rbx)
mulsd %xmm2, %xmm4
subsd %xmm1, %xmm4
movsd %xmm4, 0x178(%rbx)
movsd 0x180(%rbx), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq 0x1a0(%rbx), %r13
movq %r13, %rdi
xorl %esi, %esi
callq 0x131c0
movsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x188(%rbx), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r13, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x30(%rbp), %xmm0
divsd 0x190(%rbx), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r13, %rdi
movl %r14d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x198(%rbx), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r13, %rdi
movl %r14d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r13, %rdi
movl %r12d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x170(%rbx), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r13, %rdi
movl %r14d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r13, %rdi
pushq $0x4
popq %rsi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x178(%rbx), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r13, %rdi
movl %r14d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r13, %rdi
pushq $0x5
popq %rsi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq