// File: /workspace/github_cpp_makefile_star2_O2/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 %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rdx, %rbx
movq %rsi, %r12
movq %rsi, %rdi
xorl %esi, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x1
popq %r14
movq %r12, %rdi
movl %r14d, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd %xmm0, %xmm0
addsd -0x30(%rbp), %xmm0
movsd %xmm0, -0x40(%rbp)
sqrtsd %xmm0, %xmm0
movsd %xmm0, -0x58(%rbp)
movq %r12, %rdi
xorl %esi, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x2
popq %r15
movq %r12, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r12, %rdi
movl %r14d, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x50(%rbp)
pushq $0x3
popq %r13
movq %r12, %rdi
movl %r13d, %esi
callq 0x131c0
movsd -0x50(%rbp), %xmm0
mulsd (%rax), %xmm0
addsd -0x30(%rbp), %xmm0
divsd -0x58(%rbp), %xmm0
movsd %xmm0, -0x50(%rbp)
movq %r12, %rdi
movl %r14d, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r12, %rdi
xorl %esi, %esi
callq 0x131c0
movsd (%rax), %xmm1
movsd -0x30(%rbp), %xmm0
callq 0x3060
movsd %xmm0, -0x60(%rbp)
movq %r12, %rdi
xorl %esi, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r12, %rdi
movl %r13d, %esi
callq 0x131c0
movsd -0x38(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r12, %rdi
movl %r14d, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r12, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm1
mulsd (%rax), %xmm1
movsd -0x38(%rbp), %xmm0
subsd %xmm1, %xmm0
divsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x38(%rbp)
movsd -0x60(%rbp), %xmm0
callq 0x3360
movsd %xmm0, -0x30(%rbp)
movsd -0x60(%rbp), %xmm0
callq 0x3080
movsd %xmm0, -0x40(%rbp)
pushq $0x4
popq %r13
movq %r12, %rdi
movl %r13d, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd -0x30(%rbp), %xmm0
movsd %xmm0, -0x48(%rbp)
pushq $0x5
popq %r15
movq %r12, %rdi
movl %r15d, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd -0x40(%rbp), %xmm0
addsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x48(%rbp)
movq %r12, %rdi
movl %r13d, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r12, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
subsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %rbx, %rdi
xorl %esi, %esi
callq 0x131c0
movsd -0x60(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %rbx, %rdi
movl %r14d, %esi
callq 0x131c0
movsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0xca70(%rip), %xmm0     # 0x15038
divsd -0x58(%rbp), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %rbx, %rdi
pushq $0x2
popq %r14
movl %r14d, %esi
callq 0x131c0
movsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %rbx, %rdi
movl %r14d, %esi
callq 0x131c0
movsd -0x50(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x50(%rbp)
movq %rbx, %rdi
pushq $0x3
popq %rsi
callq 0x131c0
movsd -0x50(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x48(%rbp), %xmm0
divsd -0x58(%rbp), %xmm0
movsd %xmm0, -0x48(%rbp)
movq %rbx, %rdi
movl %r13d, %esi
callq 0x131c0
movsd -0x48(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x30(%rbp), %xmm0
divsd -0x58(%rbp), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %rbx, %rdi
movl %r15d, %esi
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