C++ (Cpp) calcPotential Examples

Example #1

File: ManyRestraintsBase.cpp Project: edoardob90/plumed2

void ManyRestraintsBase::transformBridgedDerivatives( const unsigned& current, MultiValue& invals, MultiValue& outvals ) const {
    outvals.setValue( 0, invals.get(0) );

    // Get the potential
    double dval=0, val=calcPotential( invals.get(1), dval );

    outvals.setValue( 1, val );
    for(unsigned i=0; i<invals.getNumberActive(); ++i) {
        unsigned jder=invals.getActiveIndex(i);
        outvals.addDerivative( 1, jder, dval*invals.getDerivative( 1, jder ) );
    }

    // Now update the outvals derivatives lists
    outvals.emptyActiveMembers();
    for(unsigned j=0; j<invals.getNumberActive(); ++j) outvals.updateIndex( invals.getActiveIndex(j) );
    outvals.completeUpdate();
    return;
}

Example #2

File: normalPF.cpp Project: HanwoolWooHaHa/LaneChangeEstimator

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