route_node* CRouter::InitCurDest(const D3DXMATRIX *mat)
{
	route_node *pDest = NULL;
	route_node *temp = m_pRouteList->GetRouteList();
	if (temp == NULL)
		return NULL;

	D3DXVECTOR3 vCur, vPrev;
	D3DXMATRIX mInverse;
	D3DXMatrixInverse(&mInverse, NULL, mat);
	float	fDistance = 1e10;
	do 
	{
		D3DXVec3TransformCoord(&vCur, temp->pPos, &mInverse);
		D3DXVec3TransformCoord(&vPrev, temp->pPrev->pPos, &mInverse);
		if ((vCur.z > 0) && (vPrev.z < 0))
		{
			vCur.y = vPrev.y = 0.0f;//消除俯仰偏角
			//避免存在平行街道，如果存在，取最近的一条
			if (fDistance > GetDistance(&vCur, &vPrev))
			{
				fDistance = GetDistance(&vCur, &vPrev);
				pDest = temp;
			}
		}
		temp = temp->pNext;

	}	while (temp != m_pRouteList->GetRouteList());

	return pDest;
}

void InterpolatorDistance2::GetInterpolationWeight(NNCandidate candidates[3], 
	int* indices, float* weights, Vertex v) 
{
	int K=3;
	float dist_v_c1 = GetDistance(v.pos, candidates[0].vertex.pos);
	float dist_v_c2 = GetDistance(v.pos, candidates[1].vertex.pos);
	float dist_v_c3 = GetDistance(v.pos, candidates[2].vertex.pos);
	
	float normalize = dist_v_c1 * dist_v_c2 + 
										dist_v_c1 * dist_v_c3 + 
										dist_v_c2 * dist_v_c3;

	if(normalize == 0.0f){
		for(int i=0; i<K; ++i){
			weights[i] = 1.0f/3.0f;
		}
	}
	else{
		weights[0] = dist_v_c2 * dist_v_c3 / normalize;
		weights[1] = dist_v_c1 * dist_v_c3 / normalize;
		weights[2] = dist_v_c1 * dist_v_c2 / normalize;
	}

	for(int i=0; i<K; ++i){
		indices[i] = candidates[i].index;
	}
}

/**
*  @brief
*    Sets the node position
*/
void GraphNode::SetPos(float fX, float fY, float fZ)
{
	// Set new position
	m_vPos.SetXYZ(fX, fY, fZ);

	{ // Calculate all node distances
		Iterator<Neighbour*> cIterator = m_lstNeighbours.GetIterator();
		while (cIterator.HasNext()) {
			Neighbour *pNeighbour = cIterator.Next();
			pNeighbour->fDistance = GetDistance(*pNeighbour->pNode);
		}
	}

	{ // Update all neighbour node distances
		Iterator<GraphNode*> cIterator = m_lstIsNeighbourFrom.GetIterator();
		while (cIterator.HasNext()) {
			// Find this node within the neighbour
			Iterator<Neighbour*> cNeighbourIterator = cIterator.Next()->m_lstNeighbours.GetIterator();
			while (cNeighbourIterator.HasNext()) {
				Neighbour *pNeighbour = cNeighbourIterator.Next();
				if (pNeighbour->pNode == this) {
					pNeighbour->fDistance = GetDistance(*pNeighbour->pNode);
					break;
				}
			}
		}
	}
}

void CheckNewPoint(void) {
    // 检查是不是有没有处理的新点
    if (chkedPntsN < spcPntsN && spcPnts[spcPntsN].dist + 4 < GetDistance() ) {
        if (lastAllWhiteDist == 0 || lastAllWhiteDist + 70 < GetDistance()) {
            if (spcPnts[spcPntsN].allWhite > 0) {
                // 进入坡道
                //PORTB = ~(1);
                lastAllWhiteDist = GetDistance();

                ProcRamp();

            }

            if (spcPnts[spcPntsN].allBlack > 0 || spcPnts[spcPntsN].likeStart > 0) {
                // 此处 乘3 因为allBlack 往往比 likeStart 大很多
                if (spcPnts[spcPntsN].allBlack > spcPnts[spcPntsN].likeStart * 3) {
                    // 交叉点
                    //PORTB = ~PORTB;
                    ProcCrossLine();

                } else {
                    // 出发线
                    //PORTB = 0x9;
                    ProcStartLine();


                }
            }
        }
        chkedPntsN++;
    }
}

GameObject* S013010C_Aaron_Smith_Steering::GetClosestObject(Vector2D ahead1, Vector2D ahead2)
{
	GameObject* closestObj = nullptr;

	obstacles = ObstacleManager::Instance()->GetObstacles();
	for (GameObject* obj : obstacles)
	{
		//bool collision = CheckLineInstersect(ahead1, ahead2, obj);
		
		bool collision = Collisions::Instance()->PointInBox(ahead2, obj->GetAdjustedBoundingBox());

		if (collision)
		{
		
			if (closestObj == NULL)
				closestObj = obj;
			else
			{
				if (GetDistance(mTank->GetCentrePosition(), obj->GetCentralPosition()) < GetDistance(mTank->GetCentrePosition(), closestObj->GetCentralPosition()))
					closestObj = obj;
			}
		}
		
	}

	return closestObj;

}

f32 Toolkit :: GetMinDistance(position2df p, Node2DInfo* pNode2DInfo)
{
	core::line2df l[4];
	f32 minDis = 0.f;
	l[0].start = pNode2DInfo->corner[0];
	l[0].end = pNode2DInfo->corner[1];

	l[1].start = pNode2DInfo->corner[1];
	l[1].end = pNode2DInfo->corner[2];

	l[2].start = pNode2DInfo->corner[2];
	l[2].end = pNode2DInfo->corner[3];

	l[3].start = pNode2DInfo->corner[3];
	l[3].end = pNode2DInfo->corner[0];
	minDis = GetDistance(l[0].getClosestPoint(p), p);
	for (int i = 0; i < 4; i++)
	{
		f32 t = GetDistance(l[i].getClosestPoint(p), p);
		if ( minDis > t )
		{
			minDis = t;
		}
	}
	
	return minDis;
}

bool NPC::DoWaypointNav (void)
{
	if (m_currentWaypointIndex < 0 || m_currentWaypointIndex >= g_numWaypoints)
		FindWaypoint();

	float waypointDistance = GetDistance(pev->origin, m_waypointOrigin);
	float desiredDistance = g_waypoint->g_waypointPointRadius[m_currentWaypointIndex];

	if (desiredDistance < 16.0f && waypointDistance < 30.0f)
	{
		Vector nextFrameOrigin = pev->origin + (pev->velocity * m_frameInterval);

		if (GetDistance(nextFrameOrigin, m_waypointOrigin) >= waypointDistance)
			desiredDistance = waypointDistance + 1.0f;
	}
	
	if (waypointDistance < desiredDistance)
	{
		if (m_goalWaypoint == m_currentWaypointIndex)
			return true;
		else if (m_navNode == null)
			return false;

		HeadTowardWaypoint();
		return false;
	}

	if (GetDistance(m_waypointOrigin, pev->origin) <= 2.0f)
		HeadTowardWaypoint();

	return false;
}

void Hero::Dash()
{
	HGE *hge = hgeCreate(HGE_VERSION);
	GameObject *temp = GameObject::MakePosObject(TargetX2, TargetY2);
	if (locked == false)
	{

		SowrdCharge -= GetDistance(temp);
		if ((int)SowrdCharge > 0) 
		{
			locked = true;
			adddmg = GetDistance(temp) / 20;
			TargetX = TargetX2;
			TargetY = TargetY2;
		}
		else
		{
			SowrdCharge += GetDistance(temp);
			return;
		}
	}
	if (locked)
	{	
		SetSpeed(SPEED + (GetDistance(temp) * 8));
	}
}

void MQ2NavigationPlugin::AttemptClick()
{
	if (!m_isActive && !m_bSpamClick)
		return;

	// don't execute if we've got an item on the cursor.
	if (GetCharInfo2()->pInventoryArray && GetCharInfo2()->pInventoryArray->Inventory.Cursor)
		return;

	clock::time_point now = clock::now();

	// only execute every 500 milliseconds
	if (now < m_lastClick + std::chrono::milliseconds(500))
		return;
	m_lastClick = now;

	if (m_pEndingDoor && GetDistance(m_pEndingDoor->X, m_pEndingDoor->Y) < 25)
	{
		ClickDoor(m_pEndingDoor);
	}
	else if (m_pEndingItem && GetDistance(m_pEndingItem->X, m_pEndingItem->Y) < 25)
	{
		ClickGroundItem(m_pEndingItem);
	}
}

void ProcStartLine(void) {
    nowLoop++;
    switch (nowLoop) {
    case 1:
        PORTB = ~0x22;

        StartLineDist[0] = GetDistance();
        break;
    case 2:
        StartLineDist[1] = GetDistance();

        PORTB = 0xA5;

        // 跑完第二圈后停下
        DistLimit = ( GetDistance() + (StartLineDist[1] - StartLineDist[0]) ) / 25 + 4;
        /** 在这里触发路径播放 **/
        StartPathPlay();

        //maxSpeed = 110;
        //minSpeed = 75;


        break;
    case 3:
        // 到第三圈后停下
        StartLineDist[2] = GetDistance();

        PORTB = 0x81;
        //DistLimit = GetDistance() / 25 + 2;
        break;
    default:
        break;
    }
}

void PreRanker::Filter(bool viewportSearch)
{
  using TSet = set<impl::PreResult1, LessFeatureID>;
  TSet theSet;

  sort(m_results.begin(), m_results.end(), ComparePreResult1());
  m_results.erase(unique(m_results.begin(), m_results.end(), EqualFeatureID()), m_results.end());

  sort(m_results.begin(), m_results.end(), &impl::PreResult1::LessDistance);

  if (m_limit != 0 && m_results.size() > m_limit)
  {
    // Priority is some kind of distance from the viewport or
    // position, therefore if we have a bunch of results with the same
    // priority, we have no idea here which results are relevant.  To
    // prevent bias from previous search routines (like sorting by
    // feature id) this code randomly selects tail of the
    // sorted-by-priority list of pre-results.

    double const last = m_results[m_limit - 1].GetDistance();

    auto b = m_results.begin() + m_limit - 1;
    for (; b != m_results.begin() && b->GetDistance() == last; --b)
      ;
    if (b->GetDistance() != last)
      ++b;

    auto e = m_results.begin() + m_limit;
    for (; e != m_results.end() && e->GetDistance() == last; ++e)
      ;

    // The main reason of shuffling here is to select a random subset
    // from the low-priority results. We're using a linear
    // congruential method with default seed because it is fast,
    // simple and doesn't need an external entropy source.
    //
    // TODO (@y, @m, @vng): consider to take some kind of hash from
    // features and then select a subset with smallest values of this
    // hash.  In this case this subset of results will be persistent
    // to small changes in the original set.
    minstd_rand engine;
    shuffle(b, e, engine);
  }
  theSet.insert(m_results.begin(), m_results.begin() + min(m_results.size(), m_limit));

  if (!viewportSearch)
  {
    size_t n = min(m_results.size(), m_limit);
    nth_element(m_results.begin(), m_results.begin() + n, m_results.end(),
                &impl::PreResult1::LessRank);
    theSet.insert(m_results.begin(), m_results.begin() + n);
  }

  m_results.reserve(theSet.size());
  m_results.clear();
  copy(theSet.begin(), theSet.end(), back_inserter(m_results));
}

vector<PositionNode*> AstarPathfind::FindPath(Vector2 startPos, Vector2 targetPos)
{
	PositionNode* startNode = NodeFromWorldPoint(startPos);
	PositionNode* targetNode = NodeFromWorldPoint(targetPos);

	vector<PositionNode*> openSet;
	vector<PositionNode*> closeSet;

	openSet.push_back(startNode);

	while(openSet.size() > 0)
	{
		PositionNode* currentNode = openSet[0];
		
		for(int i = 1; i < openSet.size(); ++i)
		{
			if(openSet[i]->GetfCost() < currentNode->GetfCost() || (openSet[i]->GetfCost() == currentNode->GetfCost() && openSet[i]->GethCost() < currentNode->GethCost()))
			{
				currentNode = openSet[i];
			}
		}

		openSet.erase(std::remove(openSet.begin(), openSet.end(), currentNode), openSet.end());
		closeSet.push_back(currentNode);

		if(currentNode == targetNode)
		{
			openSet.clear();
			closeSet.clear();
			return RetracePath(startNode, targetNode);
		}

		vector<PositionNode*> neighbours = GetNeighbours(currentNode);

		for(int i = 0; i < neighbours.size(); ++i)
		{
			if(!neighbours[i]->GetWalkable() || (std::find(closeSet.begin(), closeSet.end(), neighbours[i]) != closeSet.end()))
			{
				continue;
			}

			int newMovementCostToNeighbour = currentNode->GetgCost() + GetDistance(currentNode, neighbours[i]);

			if(newMovementCostToNeighbour < neighbours[i]->GetgCost() || !(std::find(openSet.begin(), openSet.end(), neighbours[i]) != openSet.end()))
			{
				neighbours[i]->SetgCost(newMovementCostToNeighbour);
				neighbours[i]->SethCost(GetDistance(neighbours[i], targetNode));
				neighbours[i]->SetParentNode(currentNode);

				if(!(std::find(openSet.begin(), openSet.end(), neighbours[i]) != openSet.end()))
				{
					openSet.push_back(neighbours[i]);
				}
			}
		}
	}
}

double F::DISTANCE::GetLineSegmentPointDistance(const CLineSegment &_ls, const CVector &_p )
{
	CLine ln(_ls);
	double dist = GetLinePointDistance(ln,_p);

	if(F::INTERSECTION::PointProjectionOnLineSegment(_p, _ls))
		return dist;

	return F::MISC::Min(GetDistance(_ls.a,_p), GetDistance(_ls.b,_p));
}

// 检测是不是同一个点, 不是则新开一个
void NewSpecialPoint(void) {
    if (GetDistance() - spcPnts[spcPntsN].dist > 8) {
        if (spcPntsN > 28) return;
        spcPntsN++;
        spcPnts[spcPntsN].allWhite = 0;
        spcPnts[spcPntsN].allBlack = 0;
        spcPnts[spcPntsN].likeStart = 0;
        spcPnts[spcPntsN].dist = GetDistance();
    }
}

	Side Plane::IsInside(const BoxBounds& aabb) const
	{
		if (GetDistance(aabb.GetVertexP(mNormal)) < 0) {
			return Side::Out;
		}
		if (GetDistance(aabb.GetVertexN(mNormal)) > 0)
		{
			return Side::In;
		}
		return Side::Cross;
	}

long ComputeDistance(struct person **company, unsigned char count)
{
	long retVal = 0;
	int i = 0;
	while (i < count-1) {
		retVal += GetDistance(company[i], company[i+1]);
		i++;
	}
	retVal += GetDistance(company[i], company[0]);
	return retVal;
}

void GpsTrackFilter::Process(vector<location::GpsInfo> const & inPoints,
                             vector<location::GpsTrackInfo> & outPoints)
{
  outPoints.reserve(inPoints.size());

  if (m_minAccuracy == 0)
  {
    // Debugging trick to turn off filtering
    outPoints.insert(outPoints.end(), inPoints.begin(), inPoints.end());
    return;
  }

  for (location::GpsInfo const & currInfo : inPoints)
  {
    // Do not accept points from the predictor
    if (currInfo.m_source == location::EPredictor)
      continue;

    // Skip any function without speed
    if (!currInfo.HasSpeed())
      continue;

    if (m_countAcceptedInfo < 2 || currInfo.m_timestamp < GetLastAcceptedInfo().m_timestamp)
    {
      AddLastInfo(currInfo);
      AddLastAcceptedInfo(currInfo);
      continue;
    }

    if (IsGoodPoint(currInfo))
    {
      double const predictionDistance = GetDistance(m_lastInfo[1], m_lastInfo[0]); // meters
      double const realDistance = GetDistance(m_lastInfo[0], currInfo); // meters

      m2::PointD const predictionDirection = GetDirection(m_lastInfo[1], m_lastInfo[0]);
      m2::PointD const realDirection = GetDirection(m_lastInfo[0], currInfo);

      // Cosine of angle between prediction direction and real direction is
      double const cosine = m2::DotProduct(predictionDirection, realDirection);

      // Acceptable angle must be from 0 to 45 or from 0 to -45.
      // Acceptable distance must be not great than 2x than predicted, otherwise it is jump.
      if (cosine >= kCosine45degrees &&
          realDistance <= std::max(kClosePointDistanceMeters, 2. * predictionDistance))
      {
        outPoints.emplace_back(currInfo);
        AddLastAcceptedInfo(currInfo);
      }
    }

    AddLastInfo(currInfo);
  }
}

// Description: Calculates tour distance
int TSPSolver::CalcTourDistance()
{
	int dist = 0;
	int size = tour.size();

	for (int i = 0; i < size - 1; i++)
		dist += GetDistance(tour.at(i), tour.at(i + 1));	// accumulate all distances

	// And back to the beginning
	dist += GetDistance(tour.at(size - 1), tour.at(0));

	return dist;
}

int IsStable(REAL ax,REAL ay,REAL ra,REAL ex,REAL ey,REAL re)
{

	if((GetDistance(ax,ay,ex,ey)-GetPlateauEqual(ra,re))<0.01/*&&(GetDistance(ax,ay,ex,ey)-GetPlateauEqual(ra,re))>-0.01*/)
=======
	if((GetDistance(ax,ay,ex,ey)-GetPlateauEqual(ra,re))<0.001/*&&(GetDistance(ax,ay,ex,ey)-GetPlateauEqual(ra,re))>-0.01*/)
	{ cout<<GetDistance(ax,ay,ex,ey)-GetPlateauEqual(ra,re)<<endl;
		cout<<GetDistance(ax,ay,ex,ey)<<endl;
		cout<<GetPlateauEqual(ra,re)<<endl;
		return 1;
	}
	else return 0;
}

void Zombie_UseBrain(Scene* scene, GameObject* zombie, int index, bool netUpdate)
{
    int dx, dy;
    
    if(zombie->ai.atkTimer > 0)
    {
        zombie->ai.atkTimer--;
    }
    if(zombie->ai.tsCounter>0)
    {
        zombie->ai.tsCounter--;
    }
    else
    {
        zombie->ai.target = NULL;
        zombie->ai.tsCounter=zombie->ai.tsFreq;
    }
    if(zombie->ai.target == NULL)//Searching for a target
    {
        zombie->ai.target = FindPlayer(scene, zombie, zombie->ai.detectRange);
    }
    if(zombie->ai.target == NULL)
        return;
    
    dx = zombie->rect.x - (zombie->ai.target->x + (zombie->ai.target->w /2));
    dy = zombie->rect.y - (zombie->ai.target->y + (zombie->ai.target->h /2));
    zombie->rotation = 270 + (atan2(dy,dx)*180/M_PI);
    
    dx = 0;
    dy = 0;
    
    dy -= sin((zombie->rotation + 90) * M_PI / 180.0f) * zombie->ai.speed; //object y's speed
    dx -= cos((zombie->rotation + 90) * M_PI / 180.0f) * zombie->ai.speed; //object x's speed
    
    if(GetDistance(*zombie->ai.target,zombie->rect) > zombie->ai.attackRange)//Sent object pos to server
    {
        MoveObject(zombie, scene, dx,dy, index);
        
    }
    else if(GetDistance(*zombie->ai.target,zombie->rect) <= zombie->ai.attackRange)
    {
        if(zombie->ai.ai == AI_SPITTER)
            Zombie_Shoot(zombie, scene);
        else if(zombie->ai.ai == AI_ZOMBIE)
            Zombie_Attack(zombie, scene);
    }
    if(netUpdate)
        SendObjectPos(zombie->obj_id, zombie->rect.x, zombie->rect.y, (int)zombie->rotation);
}