// File: 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
subq $0x80, %rsp
movq %rdi, -0x10(%rbp)
movq %rsi, -0x18(%rbp)
movq %rdx, -0x20(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x68(%rbp)
movl $0x38, -0x24(%rbp)
movsd 0xfdd2(%rip), %xmm0     # 0xc57a0
movsd %xmm0, -0x30(%rbp)
movl $0x0, -0x34(%rbp)
cmpl $0x37, -0x34(%rbp)
jge 0xb5a86
movq -0x68(%rbp), %rax
movq (%rax), %rdi
movl -0x34(%rbp), %esi
callq 0xbdb30
movq %rax, %rdi
callq 0xbd1d0
movq %rax, %rdi
callq 0x28aa0
movq %rax, %rcx
movq -0x68(%rbp), %rax
movq %rcx, -0x70(%rbp)
movq (%rax), %rdi
callq 0xbc870
movq -0x70(%rbp), %rdi
movq %rax, %rsi
callq 0x120a0
testb $0x1, %al
jne 0xb5a28
jmp 0xb5a76
movq -0x68(%rbp), %rax
movq (%rax), %rdi
movl -0x34(%rbp), %esi
callq 0xbdb30
movq %rax, %rdi
callq 0xbd3a0
movsd (%rax), %xmm1
movsd -0x30(%rbp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0xb5a74
movq -0x68(%rbp), %rax
movq (%rax), %rdi
movl -0x34(%rbp), %esi
callq 0xbdb30
movq %rax, %rdi
callq 0xbd3a0
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movl -0x34(%rbp), %eax
movl %eax, -0x24(%rbp)
jmp 0xb5a76
jmp 0xb5a78
movl -0x34(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x34(%rbp)
jmp 0xb59da
cmpl $0x38, -0x24(%rbp)
jne 0xb5a95
movb $0x0, -0x1(%rbp)
jmp 0xb5bad
callq 0x798a0
movq %rax, %rdi
movsd -0x30(%rbp), %xmm0
callq 0xbdb60
movsd %xmm0, -0x30(%rbp)
callq 0x798a0
movq %rax, %rdi
movsd -0x30(%rbp), %xmm0
callq 0xbdce0
movq -0x20(%rbp), %rax
movsd %xmm0, (%rax)
movsd -0x30(%rbp), %xmm0
movsd -0x30(%rbp), %xmm1
movq -0x20(%rbp), %rax
addsd (%rax), %xmm1
movsd 0xb530(%rip), %xmm2     # 0xc1010
callq 0xb7d40
movq -0x68(%rbp), %rdi
movq -0x20(%rbp), %rax
movsd %xmm0, (%rax)
callq 0xbcea0
movq %rax, %rdi
callq 0x5c650
movq -0x68(%rbp), %rcx
cmpl 0x28(%rcx), %eax
je 0xb5b17
movq -0x20(%rbp), %rax
movsd 0xd3cd(%rip), %xmm0     # 0xc2ee0
movsd %xmm0, (%rax)
movq -0x68(%rbp), %rax
movq (%rax), %rdi
movl -0x24(%rbp), %esi
callq 0xbdb30
movq %rax, %rdi
callq 0xbdb10
movq -0x68(%rbp), %rdi
movq %rax, -0x78(%rbp)
movl 0x28(%rdi), %esi
callq 0xbd3c0
movq -0x68(%rbp), %rax
movsd %xmm0, -0x80(%rbp)
movq (%rax), %rdi
movl -0x24(%rbp), %esi
callq 0xbdb30
movq %rax, %rdi
callq 0xbd470
movsd -0x80(%rbp), %xmm0
addsd (%rax), %xmm0
movsd %xmm0, -0x60(%rbp)
leaq -0x30(%rbp), %rdi
leaq -0x60(%rbp), %rsi
callq 0x112c0
movq -0x78(%rbp), %rdi
movsd %xmm0, -0x58(%rbp)
movsd %xmm1, -0x50(%rbp)
leaq -0x58(%rbp), %rsi
callq 0x110a0
movsd %xmm0, -0x48(%rbp)
movsd %xmm1, -0x40(%rbp)
movq -0x18(%rbp), %rax
movq -0x48(%rbp), %rcx
movq %rcx, (%rax)
movq -0x40(%rbp), %rcx
movq %rcx, 0x8(%rax)
movb $0x1, -0x1(%rbp)
movb -0x1(%rbp), %al
andb $0x1, %al
addq $0x80, %rsp
popq %rbp
retq
nopl (%rax,%rax)