// File: /workspace/github_cpp_makefile_star2_O2/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 $0x18, %rsp
movq %rdx, %r15
movq %rsi, %rbx
movq %rdi, %r14
movq (%rdi), %rax
movl 0x14(%rax), %ecx
movl 0x18(%rax), %edx
addq $0x564, %rax            # imm = 0x564
pushq $0x38
popq %r12
movsd 0x9ab3(%rip), %xmm1     # 0x5b708
xorl %esi, %esi
cmpq $0x37, %rsi
je 0x51c86
cmpl %edx, (%rax)
jne 0x51c7d
cmpl %ecx, -0x4(%rax)
jne 0x51c7d
movsd -0x1c(%rax), %xmm0
ucomisd %xmm0, %xmm1
jbe 0x51c7d
movsd %xmm0, -0x38(%rbp)
movapd %xmm0, %xmm1
movl %esi, %r12d
incq %rsi
addq $0x58, %rax
jmp 0x51c57
cmpl $0x38, %r12d
je 0x51d8d
movsd %xmm1, -0x30(%rbp)
callq 0x38d94
movq %rax, %rdi
movsd -0x30(%rbp), %xmm0
callq 0x55a9c
movsd %xmm0, -0x30(%rbp)
movsd %xmm0, -0x38(%rbp)
callq 0x38d94
movq %rax, %rdi
movsd -0x30(%rbp), %xmm0
callq 0x55b94
movsd -0x30(%rbp), %xmm2
addsd %xmm2, %xmm0
movapd %xmm2, %xmm1
mulsd 0x5380(%rip), %xmm2     # 0x57058
mulsd %xmm0, %xmm2
mulsd %xmm0, %xmm0
mulsd %xmm1, %xmm1
addsd %xmm0, %xmm1
mulsd 0x9a20(%rip), %xmm2     # 0x5b710
addsd %xmm1, %xmm2
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm2
movapd %xmm2, %xmm0
jb 0x51d08
sqrtsd %xmm0, %xmm0
jmp 0x51d0d
callq 0x93e0
movsd %xmm0, (%r15)
movslq 0x14(%r14), %rax
movq 0x8(%r14), %rcx
imulq $0x178, %rax, %rax      # imm = 0x178
movl 0x28(%r14), %esi
cmpl %esi, -0x18(%rcx,%rax)
je 0x51d38
movabsq $0x40c3880000000000, %rax # imm = 0x40C3880000000000
movq %rax, (%r15)
movq (%r14), %r15
movl %r12d, %eax
imulq $0x58, %rax, %r13
movq %r14, %rdi
callq 0x5593e
movq (%r14), %rax
addsd 0x558(%rax,%r13), %xmm0
leaq -0x40(%rbp), %rsi
movsd %xmm0, (%rsi)
leaq -0x38(%rbp), %rdi
callq 0xd15b
movsd 0x588(%r15,%r13), %xmm2
movsd 0x590(%r15,%r13), %xmm3
subsd %xmm0, %xmm2
subsd %xmm1, %xmm3
movsd %xmm2, (%rbx)
movsd %xmm3, 0x8(%rbx)
cmpl $0x38, %r12d
setne %al
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop