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; }