// File: 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
subq $0xb0, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x50(%rbp)
callq 0x3c40
movq -0x50(%rbp), %rdi
movsd %xmm0, 0x2f8(%rdi)
movq 0x308(%rdi), %rax
movsd 0x128(%rax), %xmm0
movsd %xmm0, 0x240(%rdi)
movq 0x308(%rdi), %rax
movsd 0x110(%rax), %xmm0
movsd %xmm0, 0x1b8(%rdi)
movq 0x308(%rdi), %rax
movsd 0x138(%rax), %xmm0
movsd %xmm0, 0x248(%rdi)
movq 0x308(%rdi), %rax
movsd 0x120(%rax), %xmm0
movsd %xmm0, 0x2d8(%rdi)
movq 0x308(%rdi), %rax
movsd 0x130(%rax), %xmm0
movsd %xmm0, 0x250(%rdi)
movq 0x308(%rdi), %rax
movsd 0x118(%rax), %xmm0
movsd %xmm0, 0x2b8(%rdi)
movsd 0x240(%rdi), %xmm0
movsd %xmm0, -0xb0(%rbp)
addq $0x258, %rdi            # imm = 0x258
xorl %esi, %esi
callq 0x1fd40
movsd -0xb0(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x1c880(%rip), %xmm0    # 0x24008
divsd 0x248(%rdi), %xmm0
movsd %xmm0, -0xa8(%rbp)
addq $0x258, %rdi            # imm = 0x258
movl $0x1, %esi
callq 0x1fd40
movsd -0xa8(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x250(%rdi), %xmm0
divsd 0x248(%rdi), %xmm0
movsd %xmm0, -0xa0(%rbp)
addq $0x258, %rdi            # imm = 0x258
movl $0x2, %esi
callq 0x1fd40
movsd -0xa0(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movq 0x308(%rdi), %rax
movsd 0x150(%rax), %xmm0
movsd %xmm0, 0x158(%rdi)
movsd 0x8(%rdi), %xmm0
callq 0x7cb0
movq -0x50(%rbp), %rdi
callq 0x8900
movq -0x50(%rbp), %rdi
addq $0x270, %rdi            # imm = 0x270
xorl %esi, %esi
callq 0x1fd40
movq -0x50(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, 0x138(%rdi)
addq $0x270, %rdi            # imm = 0x270
movl $0x1, %esi
callq 0x1fd40
movq -0x50(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, 0x140(%rdi)
addq $0x270, %rdi            # imm = 0x270
movl $0x2, %esi
callq 0x1fd40
movq -0x50(%rbp), %rdi
movsd 0x1c78c(%rip), %xmm0    # 0x24008
divsd (%rax), %xmm0
movsd %xmm0, 0x148(%rdi)
addq $0x270, %rdi            # imm = 0x270
movl $0x3, %esi
callq 0x1fd40
movq -0x50(%rbp), %rdi
movsd (%rax), %xmm0
mulsd 0x148(%rdi), %xmm0
movsd %xmm0, 0x150(%rdi)
addq $0x270, %rdi            # imm = 0x270
movl $0x4, %esi
callq 0x1fd40
movq -0x50(%rbp), %rdi
movsd (%rax), %xmm0
mulsd 0x148(%rdi), %xmm0
movsd %xmm0, 0x128(%rdi)
addq $0x270, %rdi            # imm = 0x270
movl $0x5, %esi
callq 0x1fd40
movq %rax, %rcx
movq -0x50(%rbp), %rax
movsd (%rcx), %xmm0
mulsd 0x148(%rax), %xmm0
movsd %xmm0, 0x130(%rax)
movsd 0x138(%rax), %xmm0
callq 0x3430
movq -0x50(%rbp), %rax
movsd %xmm0, -0x10(%rbp)
movsd 0x138(%rax), %xmm0
callq 0x30a0
movq -0x50(%rbp), %rax
movsd %xmm0, -0x28(%rbp)
movq 0x310(%rax), %rcx
movsd 0x160(%rcx), %xmm0
movq 0x308(%rax), %rcx
subsd 0x140(%rcx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq 0x310(%rax), %rcx
movsd 0x168(%rcx), %xmm0
movq 0x308(%rax), %rcx
subsd 0x148(%rcx), %xmm0
movsd %xmm0, -0x20(%rbp)
movq 0x310(%rax), %rcx
movsd 0x180(%rcx), %xmm0
movsd %xmm0, -0x38(%rbp)
movq 0x310(%rax), %rax
movsd 0x190(%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd -0x20(%rbp), %xmm0
movsd -0x18(%rbp), %xmm1
callq 0x3070
movq -0x50(%rbp), %rax
movsd %xmm0, 0x180(%rax)
movsd 0x180(%rax), %xmm0
callq 0x3430
movq -0x50(%rbp), %rax
movsd %xmm0, -0x10(%rbp)
movsd 0x180(%rax), %xmm0
callq 0x30a0
movsd %xmm0, -0x28(%rbp)
movsd -0x18(%rbp), %xmm0
callq 0x4450
movsd %xmm0, -0x98(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x4450
movaps %xmm0, %xmm1
movsd -0x98(%rbp), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x30(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x3190
movq -0x50(%rbp), %rdi
movsd %xmm0, 0x190(%rdi)
movsd -0x18(%rbp), %xmm0
movsd -0x40(%rbp), %xmm2
movsd -0x20(%rbp), %xmm1
mulsd -0x38(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x188(%rdi)
movsd -0x18(%rbp), %xmm0
movsd -0x38(%rbp), %xmm2
movsd -0x20(%rbp), %xmm1
mulsd -0x40(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd 0x190(%rdi), %xmm0
movsd %xmm0, 0x198(%rdi)
movq 0x310(%rdi), %rax
movsd 0x130(%rax), %xmm0
movsd %xmm0, 0x160(%rdi)
movq 0x310(%rdi), %rax
movsd 0x140(%rax), %xmm0
movsd %xmm0, 0x168(%rdi)
movsd 0x160(%rdi), %xmm0
movsd -0x10(%rbp), %xmm2
movsd 0x168(%rdi), %xmm1
mulsd -0x28(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x170(%rdi)
movsd 0x160(%rdi), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd -0x28(%rbp), %xmm2
movsd 0x168(%rdi), %xmm1
mulsd -0x10(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x178(%rdi)
movsd 0x180(%rdi), %xmm0
movsd %xmm0, -0x90(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
xorl %esi, %esi
callq 0x1fd40
movsd -0x90(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x188(%rdi), %xmm0
movsd %xmm0, -0x88(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x1, %esi
callq 0x1fd40
movsd -0x88(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x1c473(%rip), %xmm0    # 0x24008
divsd 0x190(%rdi), %xmm0
movsd %xmm0, -0x80(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x2, %esi
callq 0x1fd40
movsd -0x80(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x198(%rdi), %xmm0
movsd %xmm0, -0x78(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x2, %esi
callq 0x1fd40
movsd -0x78(%rbp), %xmm0
movq -0x50(%rbp), %rdi
mulsd (%rax), %xmm0
movsd %xmm0, -0x70(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x3, %esi
callq 0x1fd40
movsd -0x70(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x170(%rdi), %xmm0
movsd %xmm0, -0x68(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x2, %esi
callq 0x1fd40
movsd -0x68(%rbp), %xmm0
movq -0x50(%rbp), %rdi
mulsd (%rax), %xmm0
movsd %xmm0, -0x60(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x4, %esi
callq 0x1fd40
movsd -0x60(%rbp), %xmm0
movq -0x50(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x178(%rdi), %xmm0
movsd %xmm0, -0x58(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x2, %esi
callq 0x1fd40
movsd -0x58(%rbp), %xmm0
movq -0x50(%rbp), %rdi
mulsd (%rax), %xmm0
movsd %xmm0, -0x48(%rbp)
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x5, %esi
callq 0x1fd40
movsd -0x48(%rbp), %xmm0
movsd %xmm0, (%rax)
addq $0xb0, %rsp
popq %rbp
retq
nop