// File: DavidLBruce[P]sim3dof/build/../src/FcTse6.cpp
// Function: FcTse6::CartToMsc(Matrix&, Matrix&)
void FcTse6::CartToMsc( Matrix &cartRel,
                        Matrix &mscState )
{
  double range2 = SQ( cartRel(0) ) + SQ( cartRel(1) );

  double range = sqrt( range2 );

  double rDot = ( cartRel(0) * cartRel(2) + cartRel(1) * cartRel(3) ) / range;

  double losAng = atan2(cartRel(1), cartRel(0) );

  double losr = ( cartRel(0) * cartRel(3) - cartRel(1) * cartRel(2) ) / range2;

  double cth = cos( losAng );
  double sth = sin( losAng );

  double Ar =  cartRel(4) * cth + cartRel(5) * sth;
  double Ac = -cartRel(4) * sth + cartRel(5) * cth;

  mscState(0) = losAng;
  mscState(1) = losr;
  mscState(2) = 1.0 / range;
  mscState(3) = mscState(2) * rDot;
  mscState(4) = Ar / range;
  mscState(5) = Ac / range;

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xf0, %rsp
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq -0x10(%rbp), %rdi
xorl %esi, %esi
callq 0x1fd40
movsd (%rax), %xmm0
callq 0x4450
movsd %xmm0, -0xf0(%rbp)
movq -0x10(%rbp), %rdi
movl $0x1, %esi
callq 0x1fd40
movsd (%rax), %xmm0
callq 0x4450
movaps %xmm0, %xmm1
movsd -0xf0(%rbp), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x3190
movsd %xmm0, -0x28(%rbp)
movq -0x10(%rbp), %rdi
xorl %esi, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xe0(%rbp)
movq -0x10(%rbp), %rdi
movl $0x2, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xd8(%rbp)
movq -0x10(%rbp), %rdi
movl $0x1, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xe8(%rbp)
movq -0x10(%rbp), %rdi
movl $0x3, %esi
callq 0x1fd40
movsd -0xe8(%rbp), %xmm1
movsd -0xe0(%rbp), %xmm0
movsd -0xd8(%rbp), %xmm2
mulsd (%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x30(%rbp)
movq -0x10(%rbp), %rdi
movl $0x1, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xd0(%rbp)
movq -0x10(%rbp), %rdi
xorl %esi, %esi
callq 0x1fd40
movsd -0xd0(%rbp), %xmm0
movsd (%rax), %xmm1
callq 0x3070
movsd %xmm0, -0x38(%rbp)
movq -0x10(%rbp), %rdi
xorl %esi, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xc0(%rbp)
movq -0x10(%rbp), %rdi
movl $0x3, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xb8(%rbp)
movq -0x10(%rbp), %rdi
movl $0x1, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xc8(%rbp)
movq -0x10(%rbp), %rdi
movl $0x2, %esi
callq 0x1fd40
movsd -0xc8(%rbp), %xmm1
movsd -0xc0(%rbp), %xmm0
movsd -0xb8(%rbp), %xmm2
mulsd (%rax), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x20(%rbp), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd -0x38(%rbp), %xmm0
callq 0x3430
movsd %xmm0, -0x48(%rbp)
movsd -0x38(%rbp), %xmm0
callq 0x30a0
movsd %xmm0, -0x50(%rbp)
movq -0x10(%rbp), %rdi
movl $0x4, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movsd %xmm0, -0xb0(%rbp)
movsd -0x48(%rbp), %xmm0
movsd %xmm0, -0xa8(%rbp)
movq -0x10(%rbp), %rdi
movl $0x5, %esi
callq 0x1fd40
movsd -0xb0(%rbp), %xmm0
movsd -0xa8(%rbp), %xmm2
movsd (%rax), %xmm1
mulsd -0x50(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x58(%rbp)
movq -0x10(%rbp), %rdi
movl $0x4, %esi
callq 0x1fd40
movsd (%rax), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, -0xa0(%rbp)
movsd -0x50(%rbp), %xmm0
movsd %xmm0, -0x98(%rbp)
movq -0x10(%rbp), %rdi
movl $0x5, %esi
callq 0x1fd40
movsd -0xa0(%rbp), %xmm0
movsd -0x98(%rbp), %xmm2
movsd (%rax), %xmm1
mulsd -0x48(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x60(%rbp)
movsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x90(%rbp)
movq -0x18(%rbp), %rdi
xorl %esi, %esi
callq 0x1fd40
movsd -0x90(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x88(%rbp)
movq -0x18(%rbp), %rdi
movl $0x1, %esi
callq 0x1fd40
movsd -0x88(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x1920d(%rip), %xmm0    # 0x24008
divsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x80(%rbp)
movq -0x18(%rbp), %rdi
movl $0x2, %esi
callq 0x1fd40
movsd -0x80(%rbp), %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0x2, %esi
callq 0x1fd40
movsd (%rax), %xmm0
mulsd -0x30(%rbp), %xmm0
movsd %xmm0, -0x78(%rbp)
movq -0x18(%rbp), %rdi
movl $0x3, %esi
callq 0x1fd40
movsd -0x78(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x58(%rbp), %xmm0
divsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x70(%rbp)
movq -0x18(%rbp), %rdi
movl $0x4, %esi
callq 0x1fd40
movsd -0x70(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x60(%rbp), %xmm0
divsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x68(%rbp)
movq -0x18(%rbp), %rdi
movl $0x5, %esi
callq 0x1fd40
movsd -0x68(%rbp), %xmm0
movsd %xmm0, (%rax)
addq $0xf0, %rsp
popq %rbp
retq
nopw %cs:(%rax,%rax)