// File: /workspace/github_cpp_makefile_star2_O1/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
pushq %r14
pushq %rbx
subq $0x40, %rsp
movq %rdx, %rbx
movq %rsi, %r14
movq %rsi, %rdi
xorl %esi, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd %xmm0, %xmm0
addsd -0x18(%rbp), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
movsd %xmm0, -0x28(%rbp)
jb 0x85a3
sqrtsd %xmm0, %xmm0
jmp 0x85a8
callq 0x3120
movsd %xmm0, -0x38(%rbp)
movq %r14, %rdi
xorl %esi, %esi
callq 0x13aba
movsd (%rax), %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 $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl $0x3, %esi
callq 0x13aba
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
addsd -0x18(%rbp), %xmm0
divsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
xorl %esi, %esi
callq 0x13aba
movsd (%rax), %xmm1
movsd -0x18(%rbp), %xmm0
callq 0x3060
movsd %xmm0, -0x48(%rbp)
movq %r14, %rdi
xorl %esi, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x20(%rbp)
movq %r14, %rdi
movl $0x3, %esi
callq 0x13aba
movsd -0x20(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x20(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm1
mulsd (%rax), %xmm1
movsd -0x20(%rbp), %xmm0
subsd %xmm1, %xmm0
divsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x48(%rbp), %xmm0
callq 0x3310
movsd %xmm0, -0x18(%rbp)
movsd -0x48(%rbp), %xmm0
callq 0x3080
movsd %xmm0, -0x28(%rbp)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd -0x18(%rbp), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x5, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd -0x28(%rbp), %xmm0
addsd -0x30(%rbp), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13aba
movsd -0x28(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x28(%rbp)
movq %r14, %rdi
movl $0x5, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
mulsd (%rax), %xmm0
subsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %rbx, %rdi
xorl %esi, %esi
callq 0x13aba
movsd -0x48(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %rbx, %rdi
movl $0x1, %esi
callq 0x13aba
movsd -0x20(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0xd8bd(%rip), %xmm0     # 0x16038
divsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x20(%rbp)
movq %rbx, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x20(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %rbx, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %rbx, %rdi
movl $0x3, %esi
callq 0x13aba
movsd -0x40(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x30(%rbp), %xmm0
divsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %rbx, %rdi
movl $0x4, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x18(%rbp), %xmm0
divsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x18(%rbp)
movq %rbx, %rdi
movl $0x5, %esi
callq 0x13aba
movsd -0x18(%rbp), %xmm0
movsd %xmm0, (%rax)
addq $0x40, %rsp
popq %rbx
popq %r14
popq %rbp
retq