double CNormalPF::calcPotential(int nIndex, int nTick, double dOwnPosX, double dOwnPosY)
{
	CDatabase* pDatabase = CDatabase::GetInstance();

	int nNumAdjVehicles = pDatabase->GetNumAdjacentVehicles();

	double dMinDistance = DBL_MAX;
	//! For repulsive energy
	double dCoefficientG = 5.0;
	double dCoefficientO = 200.0;
	double dCorrelationG = 20.0;
	double dCorrelationO = 5.0;

	for (int n = 0; n < nNumAdjVehicles; n++)
	{
		if (n == nIndex)
			continue;

		double dPosX = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_POSX);
		double dPosY = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_POSY);
		double dVelX = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_VELX);
		double dVelY = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_VELY);
		int nLane = (dPosY - 0.5*LANE_WIDTH) / LANE_WIDTH;

		//! 周辺車両が自車より後ろにいる場合は考慮しない
		if ((dPosX - dOwnPosX) < 0.0)
			continue;

		if (dOwnPosY != dPosY)
			continue;

		double dDistance = qAbs(dPosX - dOwnPosX);

		if (dDistance < dMinDistance)
		{
			dMinDistance = dDistance;
		}
	}

	double dDistanceOfGoal = 0.0;
	double dDistanceOfObstacle = 0.0;

	if (dMinDistance == DBL_MAX)
	{
		return 0.0;
	}
	else
	{
		dDistanceOfObstacle = dMinDistance;
		dDistanceOfGoal = dDistanceOfObstacle + 10.0;
	}

	double dPotential = dCoefficientO * qExp(-dDistanceOfObstacle*dDistanceOfObstacle / (dCorrelationO*dCorrelationO))*(1 - qExp(-dDistanceOfGoal*dDistanceOfGoal / (dCorrelationG*dCorrelationG)));
	dPotential -= dCoefficientG * qExp(-dDistanceOfGoal*dDistanceOfGoal / (dCorrelationG*dCorrelationG));
	dPotential += dCoefficientG;

	return dPotential;
}
void CNormalPF::CalculateAccelerate(int nIndex, int nTick, double* pdAccX, double* pdAccY)
{
	CDatabase* pDatabase = CDatabase::GetInstance();

	int nNumAdjVehicles = pDatabase->GetNumAdjacentVehicles();

	//! For repulsive energy
	double dCoefficientG = 5.0;
	double dCoefficientO = 200.0;
	double dCorrelationG = 20.0;
	double dCorrelationO = 5.0;

	//! Own vehicleの位置を計算する.
	double dOwnPosX = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, nIndex, PACKET_SIMUL_POSX);
	double dOwnPosY = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, nIndex, PACKET_SIMUL_POSY);
	double dOwnVelX = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, nIndex, PACKET_SIMUL_VELX);
	double dOwnVelY = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, nIndex, PACKET_SIMUL_VELY);
	int nOwnLane = (dOwnPosY - 0.5*LANE_WIDTH) / LANE_WIDTH;

	
	//! 前後を走行する車両を検出する
	double dMinDistance = DBL_MAX;
	double dRelVelX = 0.0;

	for (int n = 0; n < nNumAdjVehicles; n++)
	{
		if (n == nIndex)
			continue;

		double dPosX = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_POSX);
		double dPosY = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_POSY);
		double dVelX = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_VELX);
		double dVelY = pDatabase->GetData(CDatabase::SIMULATION, nTick - 1, n, PACKET_SIMUL_VELY);
		int nLane = (dPosY - 0.5*LANE_WIDTH) / LANE_WIDTH;

		//! 周辺車両が自車より後ろにいる場合は考慮しない
		if ((dPosX - dOwnPosX) < 0.0)
			continue;

		if (nLane != nOwnLane)
			continue;

		double dDistance = qAbs(dPosX - dOwnPosX);

		if (dDistance < dMinDistance)
		{
			dMinDistance = dDistance;
			dRelVelX = dVelX - dOwnVelX;
		}
	}

	double dDistanceOfGoal = 0.0;
	double dDistanceOfObstacle = 0.0;

	if (dMinDistance == DBL_MAX)
	{
		*pdAccX = 0.0;
		*pdAccY = 0.0;

		return;
	}
	else
	{
		dDistanceOfObstacle = dMinDistance;
		dDistanceOfGoal = dDistanceOfObstacle + 10.0;
	}

	double dForceX = (2 * dCoefficientO * dDistanceOfObstacle) / (dCorrelationO*dCorrelationO) * qExp(-dDistanceOfObstacle*dDistanceOfObstacle / (dCorrelationO*dCorrelationO)) * (1 - qExp(-dDistanceOfGoal*dDistanceOfGoal / (dCorrelationG*dCorrelationG)));
	double dForceX2 = dCoefficientO * qExp(-dDistanceOfObstacle*dDistanceOfObstacle / (dCorrelationO*dCorrelationO))*(-2 * dDistanceOfGoal / (dCorrelationG*dCorrelationG)*qExp(-dDistanceOfGoal*dDistanceOfGoal / (dCorrelationG*dCorrelationG)));
	double dForceX3 = -(2 * dCoefficientG*dDistanceOfGoal / (dCorrelationG*dCorrelationG))*qExp(-dDistanceOfGoal*dDistanceOfGoal / (dCorrelationG*dCorrelationG));

	double dPotential1 = calcPotential(nIndex, nTick, dOwnPosX, dOwnPosY);
	double dPotential2 = calcPotential(nIndex, nTick, dOwnPosX+0.001, dOwnPosY);

	double dCheck = -(dPotential2 - dPotential1) / 0.001;
	double dCheck2 = -(dForceX + dForceX2 + dForceX3);

	//! 求まった加速度を記録する
	*pdAccX = -(dForceX + dForceX2 + dForceX3);
	*pdAccY = 0.0;
}