// File: /workspace/github_cpp_makefile_star2_O3/wrighteagle2d[P]wrighteaglebase/Debug/../src/WorldState.cpp
// Function: WorldStateUpdater::ComputeSelfPos(Vector&, double&)
bool WorldStateUpdater::ComputeSelfPos(Vector &vec, double &eps) {
  //寻找最近的标志
  int sample = FLAG_NONE; //最近标志的标示
  double min = 2000.0; //最近标志的距离 初始值应该不能比这个更小了吧！

  for (int i = 0; i < FLAG_MAX; i++) {
    if (mpObserver->Marker((MarkerType)i).GetDir().time() ==
        mpObserver->LatestSightTime()) {
      if (mpObserver->Marker((MarkerType)i).Dist() < min) {
        min = mpObserver->Marker((MarkerType)i).Dist();
        sample = i;
      }
    }
  }

  //没有看到的 则返回false
  if (sample == FLAG_NONE) {
    return false;
  }

  //修正及计算eps
  min = PlayerParam::instance().ConvertMarkDist(min);
  eps = PlayerParam::instance().GetEpsInMark(min);

  eps = calcEps(min, min + eps, 1);

  if (GetSelf().GetBodyDirDelay() != mSightDelay) {
    //不去计算身体角度误差，没有更新认为其极不准
    eps = 10000;
  }

  //计算公式见USTC_Material 第七章
  // playerPos = flagPos – polar2vector(flagDist, neckGlobalAngle + flagDir)
  vec = mpObserver->Marker((MarkerType)sample).GlobalPosition() -
        Polar2Vector(min, GetNeckGlobalDirFromSightDelay(mSightDelay) +
                              mpObserver->Marker((MarkerType)sample).Dir());

  return true;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rdx, %r15
movq %rsi, -0x48(%rbp)
movq %rdi, %r14
movq (%rdi), %rax
movl 0x14(%rax), %ecx
movl 0x18(%rax), %edx
addq $0x564, %rax            # imm = 0x564
movsd 0xb7a8(%rip), %xmm1     # 0x6d728
movl $0x38, %r12d
xorl %esi, %esi
cmpl %edx, (%rax)
jne 0x61fa3
cmpl %ecx, -0x4(%rax)
jne 0x61fa3
movsd -0x1c(%rax), %xmm0
ucomisd %xmm0, %xmm1
jbe 0x61fa3
movapd %xmm0, %xmm1
movl %esi, %r12d
incq %rsi
addq $0x58, %rax
cmpq $0x37, %rsi
jne 0x61f88
cmpl $0x38, %r12d
je 0x621ae
movsd %xmm1, -0x30(%rbp)
callq 0x43dce
movq %rax, %rdi
movsd -0x30(%rbp), %xmm0
callq 0x67aa6
movsd %xmm0, -0x30(%rbp)
callq 0x43dce
movq %rax, %rdi
movsd -0x30(%rbp), %xmm0
callq 0x67bd4
movapd %xmm0, %xmm1
movsd -0x30(%rbp), %xmm2
addsd %xmm2, %xmm1
movsd 0x7083(%rip), %xmm0     # 0x69080
mulsd %xmm2, %xmm0
mulsd %xmm1, %xmm0
mulsd %xmm1, %xmm1
mulsd %xmm2, %xmm2
addsd %xmm1, %xmm2
mulsd 0xb717(%rip), %xmm0     # 0x6d730
addsd %xmm2, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x6202d
sqrtsd %xmm0, %xmm0
jmp 0x62032
callq 0x93c0
movsd %xmm0, (%r15)
movslq 0x14(%r14), %rax
movq 0x8(%r14), %rcx
imulq $0x178, %rax, %rsi      # imm = 0x178
movl 0x28(%r14), %edx
cmpl %edx, -0x18(%rsi,%rcx)
je 0x6205d
movabsq $0x40c3880000000000, %rdi # imm = 0x40C3880000000000
movq %rdi, (%r15)
movq (%r14), %rdi
movl %r12d, %r8d
imulq $0x58, %r8, %rbx
leaq (%rdi,%rbx), %r13
addq $0x588, %r13            # imm = 0x588
testl %edx, %edx
je 0x6213b
decq %rax
cmpl $0xa, %edx
movl $0xa, %esi
cmovll %edx, %esi
movq 0x8(%rcx), %rcx
movslq %esi, %rdx
imulq $0x66666667, %rdx, %rsi # imm = 0x66666667
movq %rsi, %rdi
shrq $0x3f, %rdi
sarq $0x22, %rsi
addl %edi, %esi
addl %esi, %esi
leal (%rsi,%rsi,4), %esi
movl 0x14b90(%rcx), %edi
subl %edx, %esi
leal (%rdi,%rsi), %edx
addl $0xa, %edx
movslq %edx, %rdx
imulq $0x66666667, %rdx, %rsi # imm = 0x66666667
movq %rsi, %rdi
shrq $0x3f, %rdi
sarq $0x22, %rsi
addl %edi, %esi
addl %esi, %esi
leal (%rsi,%rsi,4), %esi
subl %esi, %edx
movslq %edx, %rdx
movq %r13, -0x40(%rbp)
imulq $0x2128, %rdx, %r13     # imm = 0x2128
addq %rcx, %r13
imulq $0x178, %rax, %rax      # imm = 0x178
movq %rax, -0x38(%rbp)
callq 0x49360
movq %rax, %r15
callq 0x49360
movq -0x38(%rbp), %rcx
movsd 0x140(%rcx,%r13), %xmm1
movq -0x40(%rbp), %r13
maxsd 0x3168(%r15), %xmm1
movsd 0x3160(%rax), %xmm0
minsd %xmm1, %xmm0
movslq 0x14(%r14), %rax
movq 0x8(%r14), %rcx
imulq $0x178, %rax, %rax      # imm = 0x178
addsd -0x20(%rcx,%rax), %xmm0
jmp 0x6214b
leaq (%rsi,%rcx), %rdi
addq $-0xd0, %rdi
callq 0x59a22
movq (%r14), %rax
addsd 0x558(%rax,%rbx), %xmm0
mulsd 0x6ef1(%rip), %xmm0     # 0x69050
divsd 0x6ee1(%rip), %xmm0     # 0x69048
leaq -0x58(%rbp), %r14
leaq -0x50(%rbp), %r15
movq %r14, %rdi
movq %r15, %rsi
callq 0x92d0
movsd (%r15), %xmm0
movsd -0x30(%rbp), %xmm2
mulsd %xmm2, %xmm0
mulsd (%r14), %xmm2
movsd (%r13), %xmm1
subsd %xmm0, %xmm1
movsd 0x8(%r13), %xmm0
subsd %xmm2, %xmm0
movq -0x48(%rbp), %rax
movsd %xmm1, (%rax)
movsd %xmm0, 0x8(%rax)
cmpl $0x38, %r12d
setne %al
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq