// File: /workspace/github_cpp_makefile_star2_O1/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 %r14
pushq %rbx
subq $0x40, %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, -0x18(%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), %r14
movq %r14, %rdi
xorl %esi, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0xfe08(%rip), %xmm0     # 0x16038
divsd 0x248(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x250(%rbx), %xmm0
divsd 0x248(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x18(%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 0x6634
movq %rbx, %rdi
callq 0x6e64
leaq 0x270(%rbx), %r14
movq %r14, %rdi
xorl %esi, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, 0x138(%rbx)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, 0x140(%rbx)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd 0xfd43(%rip), %xmm0     # 0x16038
divsd (%rax), %xmm0
movsd %xmm0, 0x148(%rbx)
movq %r14, %rdi
movl $0x3, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd 0x148(%rbx), %xmm0
movsd %xmm0, 0x150(%rbx)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd 0x148(%rbx), %xmm0
movsd %xmm0, 0x128(%rbx)
movq %r14, %rdi
movl $0x5, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd 0x148(%rbx), %xmm0
movsd %xmm0, 0x130(%rbx)
movsd 0x138(%rbx), %xmm0
callq 0x3310
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, -0x18(%rbp)
subsd 0x148(%rax), %xmm2
movsd %xmm2, -0x20(%rbp)
movsd 0x180(%rcx), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd 0x190(%rcx), %xmm0
movsd %xmm0, -0x38(%rbp)
movapd %xmm2, %xmm0
callq 0x3060
movsd %xmm0, 0x180(%rbx)
callq 0x3310
movsd %xmm0, -0x48(%rbp)
movsd 0x180(%rbx), %xmm0
callq 0x3080
movapd %xmm0, %xmm5
movsd -0x18(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd -0x20(%rbp), %xmm8
mulsd %xmm8, %xmm8
addsd %xmm0, %xmm8
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm8
jb 0x642a
xorps %xmm0, %xmm0
sqrtsd %xmm8, %xmm0
jmp 0x644a
movapd %xmm8, %xmm0
movsd %xmm5, -0x30(%rbp)
movsd %xmm8, -0x28(%rbp)
callq 0x3120
movsd -0x28(%rbp), %xmm8
movsd -0x30(%rbp), %xmm5
movsd %xmm0, 0x190(%rbx)
movsd -0x40(%rbp), %xmm6
movapd %xmm6, %xmm1
movsd -0x20(%rbp), %xmm4
mulsd %xmm4, %xmm1
movsd -0x18(%rbp), %xmm3
movapd %xmm3, %xmm2
movsd -0x38(%rbp), %xmm7
mulsd %xmm7, %xmm2
subsd %xmm1, %xmm2
divsd %xmm8, %xmm2
movsd %xmm2, 0x188(%rbx)
mulsd %xmm7, %xmm4
mulsd %xmm6, %xmm3
addsd %xmm4, %xmm3
divsd %xmm0, %xmm3
movsd %xmm3, 0x198(%rbx)
movq 0x310(%rbx), %rax
movsd 0x130(%rax), %xmm0
movsd %xmm0, 0x160(%rbx)
movsd 0x140(%rax), %xmm1
movsd %xmm1, 0x168(%rbx)
movapd %xmm5, %xmm2
mulsd %xmm1, %xmm2
movapd %xmm0, %xmm3
movsd -0x48(%rbp), %xmm4
mulsd %xmm4, %xmm3
addsd %xmm2, %xmm3
movsd %xmm3, 0x170(%rbx)
mulsd %xmm1, %xmm4
mulsd %xmm5, %xmm0
subsd %xmm0, %xmm4
movsd %xmm4, 0x178(%rbx)
movsd 0x180(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
leaq 0x1a0(%rbx), %r14
movq %r14, %rdi
xorl %esi, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x188(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0xfaeb(%rip), %xmm0     # 0x16038
divsd 0x190(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x198(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x3, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x170(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x178(%rbx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x5, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
addq $0x40, %rsp
popq %rbx
popq %r14
popq %rbp
retq
nop