Beispiel #1
0
void HeroNode::updateCurrent(sf::Time dt)
{
	correctDiagonalVelocity();
	accelerate(trmb::Entity::getVelocity() * getMaxSpeed());
	trmb::Entity::updateCurrent(dt);
	correctPosition();
}
Beispiel #2
0
	void move() 
	{
		auto map_size = Config::MAP_SIZE;

		if (x + radius + speed.x < map_size && x - radius + speed.x > 0)
			x += speed.x;
		else 
		{
			// долетаем до стенки
			x = max(radius, min(map_size - radius, x + speed.x));
			// занул¤ем проекцию скорости по dx
			speed.x = 0;
		}

		if (y + radius + speed.y < map_size && y - radius + speed.y > 0)
			y += speed.y;
		else 
		{
			// долетаем до стенки
			y = max(radius, min(map_size - radius, y + speed.y));
			// занул¤ем проекцию скорости по dy
			speed.y = 0;
		}

		if (isFast)
			applyViscosity(getMaxSpeed());
		
		if (ttf > 0) 
			ttf--;
	}
Beispiel #3
0
GUIParameterTableWindow*
GUIMEVehicle::getTypeParameterWindow(GUIMainWindow& app,
                                     GUISUMOAbstractView&) {
    GUIParameterTableWindow* ret =
        new GUIParameterTableWindow(app, *this, 10);
    // add items
    ret->mkItem("Type Information:", false, "");
    ret->mkItem("type [id]", false, myType->getID());
    ret->mkItem("length", false, myType->getLength());
    ret->mkItem("minGap", false, myType->getMinGap());
    ret->mkItem("vehicle class", false, SumoVehicleClassStrings.getString(myType->getVehicleClass()));
    ret->mkItem("emission class", false, PollutantsInterface::getName(myType->getEmissionClass()));
    ret->mkItem("maximum speed [m/s]", false, getMaxSpeed());
    //ret->mkItem("maximum acceleration [m/s^2]", false, getCarFollowModel().getMaxAccel());
    //ret->mkItem("maximum deceleration [m/s^2]", false, getCarFollowModel().getMaxDecel());
    //ret->mkItem("imperfection (sigma)", false, getCarFollowModel().getImperfection());
    //ret->mkItem("reaction time (tau)", false, getCarFollowModel().getHeadwayTime());
    ret->mkItem("person capacity", false, myType->getPersonCapacity());
    ret->mkItem("container capacity", false, myType->getContainerCapacity());

    ret->mkItem("type parameters [key:val]", false, toString(myType->getParameter().getMap()));
    // close building
    ret->closeBuilding();
    return ret;
}
Beispiel #4
0
void Character::moveForward()
{
    Ogre::Vector3 lookDirection = getLookingDirection();
    lookDirection.normalise();

    setVelocity(getMaxSpeed() * lookDirection);
}
cocos2d::Vec2 SteeringBehaviors::seek( const Vec2 &target )
{
    auto tmpMovingEntity    =   m_pOwner->getMovingEntity();
    Vec2 tmpDesiredVelocity = 
        (target - tmpMovingEntity.getPosition()).getNormalized()
        * tmpMovingEntity.getMaxSpeed();
    return tmpDesiredVelocity - tmpMovingEntity.getVelocity();
}
Beispiel #6
0
void Actor::update(float dt )
{
	accel.truncate(getMaxAccel());

	vel += accel * dt;
	vel.truncate(getMaxSpeed());
	pos += vel * dt;
}
cocos2d::Vec2 SteeringBehaviors::arrive( const Vec2& target )
{
    auto tmpMovingEntity    =   m_pOwner->getMovingEntity();
    Vec2 tmpToTarget        =   target - tmpMovingEntity.getPosition();

    float dist  =   tmpToTarget.length();
    dist        =   dist < m_arrivePrecision ? 0 : dist;
    if (dist > 0)
    {
        // 期望的速度,这里期望的肯定是以最快的方式接近过去
        float tmpSpeed  =   dist * 10;
        tmpSpeed = tmpSpeed > tmpMovingEntity.getMaxSpeed() ? tmpMovingEntity.getMaxSpeed() : tmpSpeed;
        Vec2 tmpDesiredVelocity =   tmpToTarget * tmpSpeed / dist;
        return tmpDesiredVelocity - tmpMovingEntity.getVelocity();
    }

    return Vec2(0,0);
}
Beispiel #8
0
bool
VehicleType::handleVariable(const std::string& objID, const int variable, VariableWrapper* wrapper) {
    switch (variable) {
        case TRACI_ID_LIST:
            return wrapper->wrapStringList(objID, variable, getIDList());
        case ID_COUNT:
            return wrapper->wrapInt(objID, variable, getIDCount());
        case VAR_LENGTH:
            return wrapper->wrapDouble(objID, variable, getLength(objID));
        case VAR_HEIGHT:
            return wrapper->wrapDouble(objID, variable, getHeight(objID));
        case VAR_MINGAP:
            return wrapper->wrapDouble(objID, variable, getMinGap(objID));
        case VAR_MAXSPEED:
            return wrapper->wrapDouble(objID, variable, getMaxSpeed(objID));
        case VAR_ACCEL:
            return wrapper->wrapDouble(objID, variable, getAccel(objID));
        case VAR_DECEL:
            return wrapper->wrapDouble(objID, variable, getDecel(objID));
        case VAR_EMERGENCY_DECEL:
            return wrapper->wrapDouble(objID, variable, getEmergencyDecel(objID));
        case VAR_APPARENT_DECEL:
            return wrapper->wrapDouble(objID, variable, getApparentDecel(objID));
        case VAR_ACTIONSTEPLENGTH:
            return wrapper->wrapDouble(objID, variable, getActionStepLength(objID));
        case VAR_IMPERFECTION:
            return wrapper->wrapDouble(objID, variable, getImperfection(objID));
        case VAR_TAU:
            return wrapper->wrapDouble(objID, variable, getTau(objID));
        case VAR_SPEED_FACTOR:
            return wrapper->wrapDouble(objID, variable, getSpeedFactor(objID));
        case VAR_SPEED_DEVIATION:
            return wrapper->wrapDouble(objID, variable, getSpeedDeviation(objID));
        case VAR_VEHICLECLASS:
            return wrapper->wrapString(objID, variable, getVehicleClass(objID));
        case VAR_EMISSIONCLASS:
            return wrapper->wrapString(objID, variable, getEmissionClass(objID));
        case VAR_SHAPECLASS:
            return wrapper->wrapString(objID, variable, getShapeClass(objID));
        case VAR_WIDTH:
            return wrapper->wrapDouble(objID, variable, getWidth(objID));
        case VAR_COLOR:
            return wrapper->wrapColor(objID, variable, getColor(objID));
        case VAR_MINGAP_LAT:
            return wrapper->wrapDouble(objID, variable, getMinGapLat(objID));
        case VAR_MAXSPEED_LAT:
            return wrapper->wrapDouble(objID, variable, getMaxSpeedLat(objID));
        case VAR_LATALIGNMENT:
            return wrapper->wrapString(objID, variable, getLateralAlignment(objID));
        case VAR_PERSON_CAPACITY:
            return wrapper->wrapInt(objID, variable, getPersonCapacity(objID));
        default:
            return false;
    }
}
Beispiel #9
0
bool BcmPort::supportsSpeed(cfg::PortSpeed speed) {
  // It would be nice if we could use the port_ability api here, but
  // that struct changes based on how many lanes are active. So does
  // opennsl_port_speed_max.
  //
  // Instead, we store the speed set in the bcm config file. This will
  // not work correctly if we performed a warm boot and the config
  // file changed port speeds. However, this is not supported by
  // broadcom for warm boot so this approach should be alright.
  return speed <= getMaxSpeed();
}
Beispiel #10
0
void Vehicle::print(bool full) {
    if (full) {
        std::cout << "--- vehicle ---" << std::endl;
        std::cout << "carType:" << getTyp() << std::endl;
        std::cout << "size: " << getWidth() << " x " << getLength() << std::endl;
        std::cout << "maxAcc: " << getMaxAcc() << std::endl;
        std::cout << "maxSpeed: " << getMaxSpeed() << std::endl;
        std::cout << "LaneChangeTime: " << getLaneChangeTime() << std::endl;
        std::cout << "Adress to lane: " << mpLane << std::endl;
    }
    std::cout << "pos: " << getPos() << " vel: " << getVel() << std::endl;
}
Beispiel #11
0
void Planet::updateMovementPattern(sf::Time dt)
{
	
	const std::vector<Direction>& directions = Table[mType].directions;
	if (!directions.empty())
	{
		float distanceToTravel = directions[mDirectionIndex].distance;

		if (mTravelledDistance > distanceToTravel)
		{
			mDirectionIndex = (mDirectionIndex + 1) % directions.size();
			mTravelledDistance = 0.f;
		}

		float radians = degreeToRadians(directions[mDirectionIndex].angle + 90.f);
		float vx = getMaxSpeed() * std::cos(radians);
		float vy = getMaxSpeed() * std::sin(radians);
		setVelocity(vx, vy);
		mTravelledDistance += getMaxSpeed() * dt.asSeconds();
	}
}
void TargetSplashSkill::updateMovement(double time_elapsed)
{

	_velocity = getMaxSpeed() * (_target->getPos() - _pos).getNormalized();

	//make sure vehicle does not exceed maximum velocity
	if (_velocity.getLength() > _max_speed)
	{
		_velocity.normalize();
		_velocity *= _max_speed;
	}

	movePos(_velocity);
}
/*PRIVATE FUNCTIONS*/
void Projectile::updateCurrent(sf::Time dt, CommandQueue& commands) {
	if (isGuided()) {
		const float approachRate = 200.f;

		sf::Vector2f newVel = unitVector(approachRate * dt.asSeconds() * targetDirection + getVelocity());
		newVel *= getMaxSpeed();
		float angle = std::atan2(newVel.y, newVel.x);

		setRotation(toDegree(angle) + 90.f);
		setVelocity(newVel); //rotation and velocity set
	}

	Entity::updateCurrent(dt, commands); //rotation and velocity drawn on screen
}
Beispiel #14
0
void Aircraft::updateMovementPattern(sf::Time dt)
{
	// Enemy airplane: Movement pattern
	const std::vector<Direction>& directions = Table[mType].directions;
	if (!directions.empty())
	{
		// Moved long enough in current direction: Change direction
		if (mTravelledDistance > directions[mDirectionIndex].distance)
		{
			mDirectionIndex = (mDirectionIndex + 1) % directions.size();
			mTravelledDistance = 0.f;
		}

		// Compute velocity from direction
		float radians = toRadian(directions[mDirectionIndex].angle + 90.f);
		float vx = getMaxSpeed() * std::cos(radians);
		float vy = getMaxSpeed() * std::sin(radians);

		setVelocity(vx, vy);

		mTravelledDistance += getMaxSpeed() * dt.asSeconds();
	}
}
void 
OpenSteer::SimpleVehicle::annotationVelocityAcceleration (float maxLengthA, 
                                                          float maxLengthV)
{
    const float desat = 0.4f;
    const float aScale = maxLengthA / getMaxForce();
    const float vScale = maxLengthV / getMaxSpeed();
    const Vector3& p = getPosition();
    const Color aColor (desat, desat, 1); // bluish
    const Color vColor (    1, desat, 1); // pinkish

    annotationLine (p, p + (velocity()           * vScale), vColor);
    annotationLine (p, p + (_smoothedAcceleration * aScale), aColor);
}
Beispiel #16
0
GUIParameterTableWindow*
GUIVehicle::getTypeParameterWindow(GUIMainWindow& app,
                                   GUISUMOAbstractView&) {
    GUIParameterTableWindow* ret =
        new GUIParameterTableWindow(app, *this, 26
                                    + (int)myType->getParameter().getParametersMap().size()
                                    + (int)myType->getParameter().lcParameter.size()
                                    + (int)myType->getParameter().jmParameter.size());
    // add items
    ret->mkItem("Type Information:", false, "");
    ret->mkItem("type [id]", false, myType->getID());
    ret->mkItem("length", false, myType->getLength());
    ret->mkItem("width", false, myType->getWidth());
    ret->mkItem("height", false, myType->getHeight());
    ret->mkItem("minGap", false, myType->getMinGap());
    ret->mkItem("vehicle class", false, SumoVehicleClassStrings.getString(myType->getVehicleClass()));
    ret->mkItem("emission class", false, PollutantsInterface::getName(myType->getEmissionClass()));
    ret->mkItem("carFollowModel", false, SUMOXMLDefinitions::CarFollowModels.getString((SumoXMLTag)getCarFollowModel().getModelID()));
    ret->mkItem("LaneChangeModel", false, SUMOXMLDefinitions::LaneChangeModels.getString(getLaneChangeModel().getModelID()));
    ret->mkItem("guiShape", false, getVehicleShapeName(myType->getGuiShape()));
    ret->mkItem("maximum speed [m/s]", false, getMaxSpeed());
    ret->mkItem("maximum acceleration [m/s^2]", false, getCarFollowModel().getMaxAccel());
    ret->mkItem("maximum deceleration [m/s^2]", false, getCarFollowModel().getMaxDecel());
    ret->mkItem("emergency deceleration [m/s^2]", false, getCarFollowModel().getEmergencyDecel());
    ret->mkItem("apparent deceleration [m/s^2]", false, getCarFollowModel().getApparentDecel());
    ret->mkItem("imperfection (sigma)", false, getCarFollowModel().getImperfection());
    ret->mkItem("desired headway (tau)", false, getCarFollowModel().getHeadwayTime());
    ret->mkItem("person capacity", false, myType->getPersonCapacity());
    ret->mkItem("boarding time", false, STEPS2TIME(myType->getBoardingDuration()));
    ret->mkItem("container capacity", false, myType->getContainerCapacity());
    ret->mkItem("loading time", false, STEPS2TIME(myType->getLoadingDuration()));
    if (MSGlobals::gLateralResolution > 0) {
        ret->mkItem("minGapLat", false, myType->getMinGapLat());
        ret->mkItem("maxSpeedLat", false, myType->getMaxSpeedLat());
        ret->mkItem("latAlignment", false, toString(myType->getPreferredLateralAlignment()));
    } else if (MSGlobals::gLaneChangeDuration > 0) {
        ret->mkItem("maxSpeedLat", false, myType->getMaxSpeedLat());
    }
    for (auto item : myType->getParameter().lcParameter) {
        ret->mkItem(toString(item.first).c_str(), false, toString(item.second));
    }
    for (auto item : myType->getParameter().jmParameter) {
        ret->mkItem(toString(item.first).c_str(), false, toString(item.second));
    }

    // close building
    ret->closeBuilding(&(myType->getParameter()));
    return ret;
}
void 
OpenSteer::SimpleVehicle::applySteeringForce (const Vector3& force,
                                              const float elapsedTime)
{

    const Vector3 adjustedForce = adjustRawSteeringForce (force, elapsedTime);

    // enforce limit on magnitude of steering force
    const Vector3 clippedForce = Vec3Utils::truncateLength(adjustedForce, getMaxForce());

    // compute acceleration and velocity
    Vector3 newAcceleration = (clippedForce / mass());
    Vector3 newVelocity = velocity();

    // damp out abrupt changes and oscillations in steering acceleration
    // (rate is proportional to time step, then clipped into useful range)
    if (elapsedTime > 0)
    {
        const float smoothRate = clip (9 * elapsedTime, 0.15f, 0.4f);
        blendIntoAccumulator (smoothRate,
                              newAcceleration,
                              _smoothedAcceleration);
    }

    // Euler integrate (per frame) acceleration into velocity
    newVelocity += _smoothedAcceleration * elapsedTime;

    // enforce speed limit
    newVelocity = Vec3Utils::truncateLength(newVelocity, getMaxSpeed());

    // update Speed
    setSpeed (newVelocity.length());

    // Euler integrate (per frame) velocity into position
    setPosition (getPosition() + (newVelocity * elapsedTime));

    // regenerate local space (by default: align vehicle's forward axis with
    // new velocity, but this behavior may be overridden by derived classes.)
    regenerateLocalSpace (newVelocity, elapsedTime);

    // maintain path curvature information
    measurePathCurvature (elapsedTime);

    // running average of recent positions
    blendIntoAccumulator (elapsedTime * 0.06f, // QQQ
                          getPosition(),
                          _smoothedPosition);
}
Beispiel #18
0
	void applyDirect(const Point &direct) 
	{
		if (isFast)
			return;

		double max_speed = getMaxSpeed();

		double dy = direct.y - y, dx = direct.x - x;
		double dist = sqrt(dx * dx + dy * dy);
		double ny = (dist > 0) ? (dy / dist) : 0;
		double nx = (dist > 0) ? (dx / dist) : 0;

		speed.x += (nx * max_speed - speed.x) * Config::INERTION_FACTOR / mass;
		speed.y += (ny * max_speed - speed.y) * Config::INERTION_FACTOR / mass;

		dropSpeed();
	}
Beispiel #19
0
// ===========================================================================
// method definitions
// ===========================================================================
MSVehicleType::MSVehicleType(const std::string &id, SUMOReal length,
                             SUMOReal maxSpeed, SUMOReal prob,
                             SUMOReal speedFactor, SUMOReal speedDev,
                             SUMOVehicleClass vclass,
                             SUMOEmissionClass emissionClass,
                             SUMOVehicleShape shape,
                             SUMOReal guiWidth, SUMOReal guiOffset,
                             const std::string &lcModel,
                             const RGBColor &c) throw()
        : myID(id), myLength(length), myMaxSpeed(maxSpeed),
        myDefaultProbability(prob), mySpeedFactor(speedFactor),
        mySpeedDev(speedDev), myVehicleClass(vclass),
        myLaneChangeModel(lcModel),
        myEmissionClass(emissionClass), myColor(c),
        myWidth(guiWidth), myOffset(guiOffset), myShape(shape),
        myOriginalType(0) {
    assert(myLength > 0);
    assert(getMaxSpeed() > 0);
}
/*
Vector3 SteeringVehicle::predictFuturePosition(const float predictionTime) const
{
	//return position() +(velocity() * predictionTime);
	return getVelocity() * predictionTime;
}
*/
Vector3 SteeringVehicle::adjustRawSteeringForce(const Vector3& force)
{
    const float maxAdjustedSpeed = 0.2f * getMaxSpeed();

    if ((getSpeed() > maxAdjustedSpeed) ||(force == Vector3::ZERO))
    {
        return force;
    }
    else
    {
        const float range = getSpeed() / maxAdjustedSpeed;
        // const float cosine = interpolate(pow(range, 6), 1.0f, -1.0f);
        // const float cosine = interpolate(pow(range, 10), 1.0f, -1.0f);
        // const float cosine = interpolate(pow(range, 20), 1.0f, -1.0f);
        // const float cosine = interpolate(pow(range, 100), 1.0f, -1.0f);
        // const float cosine = interpolate(pow(range, 50), 1.0f, -1.0f);
        const float cosine = interpolate(pow(range, 20), 1.0f, -1.0f);
        return limitMaxDeviationAngle(force, cosine, getForward());
    }
}
Beispiel #21
0
void
MSVehicleType::saveState(std::ostream &os) {
    FileHelpers::writeString(os, myID);
    FileHelpers::writeFloat(os, myLength);
    FileHelpers::writeFloat(os, getMaxSpeed());
    FileHelpers::writeInt(os, (int) myVehicleClass);
    FileHelpers::writeInt(os, (int) myEmissionClass);
    FileHelpers::writeInt(os, (int) myShape);
    FileHelpers::writeFloat(os, myWidth);
    FileHelpers::writeFloat(os, myOffset);
    FileHelpers::writeFloat(os, myDefaultProbability);
    FileHelpers::writeFloat(os, mySpeedFactor);
    FileHelpers::writeFloat(os, mySpeedDev);
    FileHelpers::writeFloat(os, myColor.red());
    FileHelpers::writeFloat(os, myColor.green());
    FileHelpers::writeFloat(os, myColor.blue());
    FileHelpers::writeInt(os, myCarFollowModel->getModelID());
    FileHelpers::writeString(os, myLaneChangeModel);
    //myCarFollowModel->saveState(os);
}
Beispiel #22
0
void UnitBase::setSpeeds() {
	float speed = getMaxSpeed();

	if(!isAFlyingUnit()) {
		speed += speed*(1.0f - getTerrainDifficulty((TERRAINTYPE) currentGameMap->getTile(location)->getType()));
		if(isBadlyDamaged()) {
            speed *= HEAVILYDAMAGEDSPEEDMULTIPLIER;
		}
	}

	switch(drawnAngle){
        case LEFT:      xSpeed = -speed;                    ySpeed = 0;         break;
        case LEFTUP:    xSpeed = -speed*DIAGONALSPEEDCONST; ySpeed = xSpeed;    break;
        case UP:        xSpeed = 0;                         ySpeed = -speed;    break;
        case RIGHTUP:   xSpeed = speed*DIAGONALSPEEDCONST;  ySpeed = -xSpeed;   break;
        case RIGHT:     xSpeed = speed;                     ySpeed = 0;         break;
        case RIGHTDOWN: xSpeed = speed*DIAGONALSPEEDCONST;  ySpeed = xSpeed;    break;
        case DOWN:      xSpeed = 0;                         ySpeed = speed;     break;
        case LEFTDOWN:  xSpeed = -speed*DIAGONALSPEEDCONST; ySpeed = -xSpeed;   break;
	}
}
Beispiel #23
0
void SchoolFish::updateVelocity(Vector &accumulator)
{
	// Ok, now limit speeds
	accumulator.capLength2D(maxChange);
	// Save old velocity
	lastVel = vel;

	// Calculate new velocity and constrain it
	vel += accumulator;

	vel.capLength2D(getMaxSpeed() * maxSpeedLerp.x);
	vel.z = 0;
	if (fabs(vel.y) > fabs(vel.x))
	{
		/*
		float sign = vel.y / fabs(vel.y);
		vel.y = fabs(vel.x) * sign;
		*/
		//std::swap(vel.x, vel.y);
		// going up 
		/*
		float len = vel.getLength2D();
		if (vel.y < 0)
		{
			if (vel.x < 0)
				vel.y = vel.x;
			else
				vel.y = -vel.x;
		}
		else
		{
			if (vel.x < 0)
				vel.y = -vel.x;
			else
				vel.y = vel.x;
		}
		vel.setLength2D(len);
		*/
	}
}
Beispiel #24
0
void L6470::Dump(void) {
#ifndef NDEBUG
	uint8_t reg;

	printf("Registers:\n");
	printf("01:ABS_POS    - Current position: %ld\n", getPos());
	printf("02:EL_POS     - Electrical position: %ld\n", getParam(L6470_PARAM_EL_POS));
	printf("03:MARK       - Mark position: %ld\n", getMark());
	printf("04:SPEED      - Current speed: %ld (raw)\n", getParam(L6470_PARAM_SPEED));
	printf("05:ACC        - Acceleration: %.2f steps/s2\n", getAcc());
	printf("06:DEC        - Deceleration: %.2f steps/s2\n", getDec());
	printf("07:MAX_SPEED  - Maximum speed: %.2f steps/s\n", getMaxSpeed());
	printf("08:MIN_SPEED  - Minimum speed: %.2f steps/s\n", getMinSpeed());
	printf("09:KVAL_HOLD  - Holding KVAL: %d\n", (int) getHoldKVAL());
	printf("0A:KVAL_RUN   - Constant speed KVAL: %d\n", (int) getRunKVAL());
	printf("0B:KVAL_ACC   - Acceleration starting KVAL: %d\n", (int) getAccKVAL());
	printf("0C:KVAL_Dec   - Deceleration starting KVAL: %d\n", (int) getDecKVAL());
	printf("0D:INT_SPEED  - Intersect speed: 0x%.4X (raw)\n", (unsigned int) getParam(L6470_PARAM_INT_SPD));
	reg = getParam(L6470_PARAM_ST_SLP);
	printf("0E:ST_SLP     - Start slope: %.3f\%% s/step (0x%.2X)\n", (float) 100 * SlpCalcValueReg(reg), reg);
	reg = getParam(L6470_PARAM_FN_SLP_ACC);
	printf("0F:FN_SLP_ACC - Acceleration final slope: %.3f\%% s/step (0x%.2X)\n", (float) 100 * SlpCalcValueReg(reg), reg);
	reg = getParam(L6470_PARAM_FN_SLP_DEC);
	printf("10:FN_SLP_DEC - Deceleration final slope: %.3f\%% s/step (0x%.2X)\n", (float) 100 * SlpCalcValueReg(reg), reg);
	reg = getParam(L6470_PARAM_K_THERM);
	printf("11:K_THERM    - Thermal compensation factor: %.3f (0x%.2X)\n", KThermCalcValueReg(reg), reg);
	reg = getParam(L6470_PARAM_ADC_OUT);
	printf("12:ADC_OUT    - ADC output: 0x%.2X\n", reg & 0x1F);
	reg = getOCThreshold();
	printf("13:OCD_TH     - OCD threshold: %d mA (0x%.2X)\n", OcdThCalcValueReg(reg), reg);
	reg = getParam(L6470_PARAM_STALL_TH);
	printf("14:STALL_TH   - STALL threshold: %d mA (0x%.2X)\n", StallThCalcValueReg(reg & 0x7F), reg);
	printf("15:FS_SPD     - Full-step speed: %.2f steps/s\n", getFullSpeed());
	printf("16:STEP_MODE  - Step mode: %d microsteps\n", 1 << getStepMode());
	printf("17:ALARM_EN   - Alarm enable: 0x%.2X\n", (unsigned int) getParam(L6470_PARAM_ALARM_EN));
	printf("18:CONFIG     - IC configuration: 0x%.4X\n", (unsigned int) getParam(L6470_PARAM_CONFIG));
#endif
}
bool
MSLane::isEmissionSuccess(MSVehicle* aVehicle,
                          SUMOReal speed, SUMOReal pos,
                          bool patchSpeed, size_t startStripId) throw(ProcessError) {
    //  and the speed is not too high (vehicle should decelerate)
    // try to get a leader on consecutive lanes
    //  we have to do this even if we have found a leader on our lane because it may
    //  be driving into another direction
    //std::cerr<<"EMISSION speed:"<<speed<<std::endl;
	std::cerr<<"EMISSION vehicle:"<<aVehicle->getID()<<std::endl;
    size_t endStripId = startStripId + aVehicle->getWidth() - 1;
    assert(startStripId >=0 && endStripId < myStrips.size());
    aVehicle->getBestLanes(true, this);
    const MSCFModel &cfModel = aVehicle->getCarFollowModel();
    const std::vector<MSLane*> &bestLaneConts = aVehicle->getBestLanesContinuation(this);
    std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
    SUMOReal seen = getLength() - pos;
    SUMOReal dist = cfModel.brakeGap(speed);
    const MSRoute &r = aVehicle->getRoute();
    MSRouteIterator ce = r.begin();
    MSLane *currentLane = this;
    MSLane *nextLane = this;
    while (seen<dist&&ri!=bestLaneConts.end()&&nextLane!=0/*&&ce!=r.end()*/) {
        // get the next link used...
        MSLinkCont::const_iterator link = currentLane->succLinkSec(*aVehicle, 1, *currentLane, bestLaneConts);
        // ...and the next used lane (including internal)
        if (!currentLane->isLinkEnd(link) && (*link)->havePriority() && (*link)->getState()!=MSLink::LINKSTATE_TL_RED) { // red may have priority?
#ifdef HAVE_INTERNAL_LANES
            bool nextInternal = false;
            nextLane = (*link)->getViaLane();
            if (nextLane==0) {
                nextLane = (*link)->getLane();
            } else {
                nextInternal = true;
            }
#else
            nextLane = (*link)->getLane();
#endif
        } else {
            nextLane = 0;
        }
        // check how next lane effects the journey
        if (nextLane!=0) {
            SUMOReal gap = 0;
            //TODO: fix get & set partial occupator to strip level
            MSVehicle * leader = 0;//currentLane->getPartialOccupator();
            if (leader!=0) {
                gap = getPartialOccupatorEnd();
            } else {
                // check leader on next lane
                leader = nextLane->getLastVehicle(aVehicle->getStrips());
                if (leader!=0) {
                    gap = seen+leader->getPositionOnLane()-leader->getVehicleType().getLength();
                }
            }
            if (leader!=0) {
                SUMOReal nspeed = gap>=0 ? cfModel.ffeV(aVehicle, speed, gap, leader->getSpeed()) : 0;
                if (nspeed<speed) {
                    if (patchSpeed) {
                        speed = MIN2(nspeed, speed);
                        dist = cfModel.brakeGap(speed);
                    } else {
                        // we may not drive with the given velocity - we crash into the leader
                        return false;
                    }
                }
            }
            // check next lane's maximum velocity
            SUMOReal nspeed = nextLane->getMaxSpeed();
            if (nspeed<speed) {
                // patch speed if needed
                if (patchSpeed) {
                    speed = MIN2(cfModel.ffeV(aVehicle, speed, seen, nspeed), speed);
                    dist = cfModel.brakeGap(speed);
                } else {
                    // we may not drive with the given velocity - we would be too fast on the next lane
                    return false;
                }
            }
            // check traffic on next junctions
            const SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / speed);
#ifdef HAVE_INTERNAL_LANES
            const SUMOTime leaveTime = (*link)->getViaLane()==0 ? arrivalTime + TIME2STEPS((*link)->getLength() * speed) : arrivalTime + TIME2STEPS((*link)->getViaLane()->getLength() * speed);
#else
            const SUMOTime leaveTime = arrivalTime + TIME2STEPS((*link)->getLength() * speed);
#endif
            if ((*link)->hasApproachingFoe(arrivalTime, leaveTime)) {
                SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
                if (nspeed<speed) {
                    if (patchSpeed) {
                        speed = MIN2(nspeed, speed);
                        dist = cfModel.brakeGap(speed);
                    } else {
                        // we may not drive with the given velocity - we crash into the leader
                        return false;
                    }
                }
            } else {
                // we can only drive to the end of the current lane...
                SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
                if (nspeed<speed) {
                    if (patchSpeed) {
                        speed = MIN2(nspeed, speed);
                        dist = cfModel.brakeGap(speed);
                    } else {
                        // we may not drive with the given velocity - we crash into the leader
                        return false;
                    }
                }
            }
            seen += nextLane->getLength();
            ++ce;
            ++ri;
            currentLane = nextLane;
        }
    }
    if (seen<dist) {
        SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
        if (nspeed<speed) {
            if (patchSpeed) {
                speed = MIN2(nspeed, speed);
                dist = cfModel.brakeGap(speed);
            } else {
                // we may not drive with the given velocity - we crash into the leader
                MsgHandler::getErrorInstance()->inform("Vehicle '" + aVehicle->getID() + "' will not be able to emit using given velocity!");
                // !!! we probably should do something else...
                return false;
            }
        }
    }

    // get the pointer to the vehicle next in front of the given position
    MSVehicle *pred;
    std::vector<MSVehicle *> predCont;
    std::vector<MSVehicle *>::iterator predIt, it;
    for (unsigned int i=startStripId; i<=endStripId; ++i) {
        predCont.push_back(myStrips.at(i)->getPredAtPos(pos));
    }
    predIt = predCont.begin();
    SUMOReal currMin = -1;
    if (*predIt != 0) {
        currMin = (*predIt)->getPositionOnLane();
    } else {
        // signals no leader in front
        predIt = predCont.end();
    }
    for (it = predCont.begin(); it != predCont.end(); ++it) {
        if (*it == 0) continue;
        if ((*it)->getPositionOnLane() < currMin) {
            predIt = it;
            currMin = (*it)->getPositionOnLane();
        }
    }
    
    if (predIt != predCont.end()) {
        // ok, there is one (a leader)
        MSVehicle* leader = *predIt;
        SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
        SUMOReal gap = MSVehicle::gap(leader->getPositionOnLane(), leader->getVehicleType().getLength(), pos);
        if (gap<frontGapNeeded) {
            // too close to the leader on this lane
            return false;
        }
    }

    // FIXME: implement look back
    // check back vehicle
    if (false/*predIt!=myVehicles.begin()*/) {
        // there is direct follower on this lane
        MSVehicle *follower = *(predIt-1);
        SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
        SUMOReal gap = MSVehicle::gap(pos, aVehicle->getVehicleType().getLength(), follower->getPositionOnLane());
        if (gap<backGapNeeded) {
            // too close to the follower on this lane
            return false;
        }
    } else if (false) {
        // check approaching vehicle (consecutive follower)
        SUMOReal lspeed = getMaxSpeed();
        // in order to look back, we'd need the minimum braking ability of vehicles in the net...
        //  we'll assume it to be 4m/s^2
        //   !!!revisit
        SUMOReal dist = lspeed * lspeed * SUMOReal(1./2.*4.) + SPEED2DIST(lspeed);
        std::pair<const MSVehicle * const, SUMOReal> approaching = getFollowerOnConsecutive(dist, 0, speed, pos - aVehicle->getVehicleType().getLength());
        if (approaching.first!=0) {
            const MSVehicle *const follower = approaching.first;
            SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
            SUMOReal gap = approaching.second - pos - aVehicle->getVehicleType().getLength();
            if (gap<backGapNeeded) {
                // too close to the consecutive follower
                return false;
            }
        }
        // check for in-lapping vehicle
        MSVehicle* leader = getPartialOccupator();
        if (leader!=0) {
            SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
            SUMOReal gap = getPartialOccupatorEnd() - pos;
            if (gap<=frontGapNeeded) {
                // too close to the leader on this lane
                return false;
            }
        }
    }

    // may got negative while adaptation
    if (speed<0) {
        return false;
    }
    // enter
    //XXX: later change to enterStripAtEmit()?
    //if (speed < 0.0001) speed += 10.0;
    StripCont strips;
    strips.resize(aVehicle->getWidth());
    StripCont::iterator start = myStrips.begin() + startStripId;
    std::copy(start, start + aVehicle->getWidth(), strips.begin());
    aVehicle->enterLaneAtEmit(this, pos, speed, strips);
    bool wasInactive = getVehicleNumber()==0;
    if (true/*predIt==myVehicles.end()*/) {
        // vehicle will be the first on the lane
        //std::cerr<<"startStripId:"<<startStripId<<", NumStrips:"<<strips.size()<<", VehWidth:"<<aVehicle->getWidth()<<std::endl;
        for (size_t i=startStripId; i<startStripId+strips.size(); ++i) {
                this->getStrip(i)->pushIntoStrip(aVehicle);
                this->getStrip(i)->setVehLenSum(this->getStrip(i)->getVehLenSum() + 
                                                        aVehicle->getVehicleType().getLength());
        }
        aVehicle->printDebugMsg("Emitting");
        printDebugMsg();
    } else {
        //this->getStrip(0).insert(0, aVehicle);
    }
    //myVehicleLengthSum += aVehicle->getVehicleType().getLength();
    if (wasInactive) {
        MSNet::getInstance()->getEdgeControl().gotActive(this);
    }
    return true;
}
bool
MSLane::emit(MSVehicle& veh) throw(ProcessError) {
    SUMOReal pos = 0;
    SUMOReal speed = 0;
    bool patchSpeed = true; // whether the speed shall be adapted to infrastructure/traffic in front

    // determine the speed
    const SUMOVehicleParameter &pars = veh.getParameter();
    size_t stripId = getEmptyStartStripID(veh.getWidth());
    switch (pars.departSpeedProcedure) {
    case DEPART_SPEED_GIVEN:
        speed = pars.departSpeed;
        patchSpeed = false;
        break;
    case DEPART_SPEED_RANDOM:
        speed = RandHelper::rand(MIN2(veh.getMaxSpeed(), getMaxSpeed()));
        patchSpeed = true; // !!!(?)
        break;
    case DEPART_SPEED_MAX:
        speed = MIN2(veh.getMaxSpeed(), getMaxSpeed());
        patchSpeed = true; // !!!(?)
        break;
    case DEPART_SPEED_DEFAULT:
    default:
        // speed = 0 was set before
        patchSpeed = false; // !!!(?)
        break;
    }

    // determine the position
    switch (pars.departPosProcedure) {
    case DEPART_POS_GIVEN:
        if (pars.departPos >= 0.) {
            pos = pars.departPos;
        } else {
            pos = pars.departPos + getLength();
        }
        break;
    case DEPART_POS_RANDOM:
        pos = RandHelper::rand(getLength());
        break;
    case DEPART_POS_RANDOM_FREE: {
        for (unsigned int i=0; i < 10; i++) {
            // we will try some random positions ...
            pos = RandHelper::rand(getLength());
            if (isEmissionSuccess(&veh, speed, pos, patchSpeed, stripId)) {
                return true;
            }
        }
        // ... and if that doesn't work, we put the vehicle to the free position
        return freeEmit(veh, speed);
    }
    break;
    case DEPART_POS_FREE:
        return freeEmit(veh, speed);
    case DEPART_POS_DEFAULT:
    default:
        // pos = 0 was set before
        break;
    }

    // try to emit
    return isEmissionSuccess(&veh, speed, pos, patchSpeed, stripId);
}
Beispiel #27
0
/**
 * Order of actions:
 * a) transition of human into infected
 * b) giving birth to children - changes input
 * c) making love - changes input
 * d) movement
 */
static void simulateStep2(WorldPtr input, WorldPtr output) {
	simClock clock = output->clock;
	// notice that we iterate over xx and yy
	// and the real x and y are randomly switched between two directions
	double xxDir = randomDouble();
	double yyDir = randomDouble();

	// we want to force static scheduling because we suppose that the load
	// is distributed evenly over the map and we need to have predictable locking
#ifdef _OPENMP
	// at least three columns per thread
	int threads = omp_get_max_threads();
	int numThreads = MIN(MAX(input->localWidth / 3, 1), threads);
#pragma omp parallel for num_threads(numThreads) schedule(static)
#endif
	for (int xx = input->xStart; xx <= input->xEnd; xx++) {
		int x = (xxDir < 0.5) ? xx : (input->xEnd + input->xStart - xx);
		// stats are counted per column and summed at the end
		Stats stats = NO_STATS;
		lockColumn(output, x);
		for (int yy = input->yStart; yy <= input->yEnd; yy++) {
			int y = (yyDir < 0.5) ? yy : (input->yEnd + input->yStart - yy);
			Entity entity = GET_CELL(input, x, y);
			if (entity.type == NONE) {
				continue;
			}

			// Convert Human to Infected
			if (entity.type == HUMAN) {
				int zombieCount = countNeighbouringZombies(input, x, y);
				double infectionChance = zombieCount * PROBABILITY_INFECTION;

				if (randomDouble() <= infectionChance) {
					if (entity.gender == FEMALE) {
						stats.humanFemalesBecameInfected++;
					} else {
						stats.humanMalesBecameInfected++;
					}
					toInfected(&entity, clock);
					LOG_EVENT("A Human became infected\n");
				}
			}

			// Here are performed natural processed of humans and infected
			if (entity.type == HUMAN || entity.type == INFECTED) {
				// giving birth
				if (entity.gender == FEMALE && entity.children > 0) {
					if (entity.origin + entity.borns <= clock) {
						if (entity.type == HUMAN) {
							stats.humanFemalesGivingBirth++;
						} else {
							stats.infectedFemalesGivingBirth++;
						}

						Entity * freePtr;
						while (entity.children > 0 && (freePtr =
								getFreeAdjacent(input, output, x, y)) != NULL) {
							Entity child = giveBirth(&entity, clock);
							if (child.type == HUMAN) {
								if (child.gender == FEMALE) {
									stats.humanFemalesBorn++;
								} else {
									stats.humanMalesBorn++;
								}
							} else {
								if (child.gender == FEMALE) {
									stats.infectedFemalesBorn++;
								} else {
									stats.infectedMalesBorn++;
								}
							}
							*freePtr = child;
							LOG_EVENT("A %s child was born\n",
									child.type == HUMAN ? "Human" : "Infected");
						}
					} else {
						if (entity.type == HUMAN) {
							stats.humanFemalesPregnant++;
						} else {
							stats.infectedFemalesPregnant++;
						}
					}
				}

				// making love
				if (entity.gender == FEMALE && entity.children == 0
						&& clock >= entity.origin + entity.fertilityStart
						&& clock < entity.origin + entity.fertilityEnd) { // can have baby
					EntityPtr adjacentMale = findAdjacentFertileMale(input, x,
							y, clock);
					if (adjacentMale != NULL) {
						stats.couplesMakingLove++;
						makeLove(&entity, adjacentMale, clock, input->stats);

						stats.childrenConceived += entity.children;
						LOG_EVENT("A couple made love\n");
					}
				}
			}

			if (entity.type == HUMAN) {
				if (entity.gender == FEMALE) {
					stats.humanFemales++;
				} else {
					stats.humanMales++;
				}
			} else if (entity.type == INFECTED) {
				if (entity.gender == FEMALE) {
					stats.infectedFemales++;
				} else {
					stats.infectedMales++;
				}
			} else {
				stats.zombies++;
			}

			// MOVEMENT

			bearing bearing_ = getBearing(input, x, y); // optimal bearing
			bearing_ += getRandomBearing() * BEARING_FLUCTUATION;

			Direction dir = bearingToDirection(bearing_);
			if (dir != STAY) {
				double bearingRandomQuotient = (randomDouble() - 0.5)
						* BEARING_ABS_QUOTIENT_VARIANCE
						+ BEARING_ABS_QUOTIENT_MEAN;
				entity.bearing = bearing_ / cabsf(bearing_)
						* bearingRandomQuotient;
			} else {
				entity.bearing = bearing_;
			}

			// some randomness in direction
			// the entity will never go in the opposite direction
			if (dir != STAY) {
				if (randomDouble() < getMaxSpeed(&entity, clock)) {
					double dirRnd = randomDouble();
					if (dirRnd < DIRECTION_MISSED) {
						dir = DIRECTION_CCW(dir); // turn counter-clock-wise
					} else if (dirRnd < DIRECTION_MISSED * 2) {
						dir = DIRECTION_CW(dir); // turn clock-wise
					} else if (dirRnd
							> DIRECTION_FOLLOW + DIRECTION_MISSED * 2) {
						dir = STAY;
					}
				} else {
					dir = STAY;
				}
			} else {
				// if the entity would STAY, we'll try again to make it move
				// to make the entity bearing variable in terms of absolute value
				double bearingRandomQuotient = (randomDouble() - 0.5)
						* BEARING_ABS_QUOTIENT_VARIANCE
						+ BEARING_ABS_QUOTIENT_MEAN;

				bearing_ += getRandomBearing() * bearingRandomQuotient;
				dir = bearingToDirection(bearing_);
			}

			// we will try to find the cell in the chosen direction
			CellPtr destPtr = NULL;
			if (dir != STAY) {
				destPtr = IF_CAN_MOVE_TO(x, y, dir);
				if (randomDouble() < MOVEMENT_TRY_ALTERNATIVE) {
					if (destPtr == NULL) {
						destPtr = IF_CAN_MOVE_TO(x, y, DIRECTION_CCW(dir));
					}
					if (destPtr == NULL) {
						destPtr = IF_CAN_MOVE_TO(x, y, DIRECTION_CW(dir));
					}
				}
			}
			if (destPtr == NULL) {
				destPtr = GET_CELL_PTR(output, x, y);
			}

			// actual assignment of entity to its destination
			*destPtr = entity;
		}
		unlockColumn(output, x);
#ifdef _OPENMP
#pragma omp critical (StatsCriticalRegion2)
#endif
		{
			mergeStats(&output->stats, stats, true);
		}
	}
}
Beispiel #28
0
void Projectile::init()
{
	Actor::init();
	vel = mDir.normalize() * getMaxSpeed();
	size = 5;
}
Beispiel #29
0
// ===========================================================================
// method definitions
// ===========================================================================
MSVehicleType::MSVehicleType(const SUMOVTypeParameter& parameter)
    : myParameter(parameter), myIndex(myNextIndex++), myOriginalType(0) {
    assert(getLength() > 0);
    assert(getMaxSpeed() > 0);
}
Beispiel #30
0
void CollideEntity::updateMovement(float dt)
{	
	if (isEntityDead()) return;
	if (position.isFollowingPath()) return;
	vel.capLength2D(getMaxSpeed()*maxSpeedLerp.x);
	/*
	if (vel.getSquaredLength2D() > sqr(getMaxSpeed()))
	{
		vel.setLength2D(getMaxSpeed());
		vel.z = 0;
	}
	*/
	//Vector lastPos = pos;

	updateVel2(dt);

	if (doCusion)
	{
		Vector push;
		TileVector t(position+vel*dt);
		if (dsq->game->isObstructed(TileVector(t.x-1, t.y)))
		{
			push += Vector(1.25,0);
		}
		if (dsq->game->isObstructed(TileVector(t.x+1, t.y)))
		{
			push += Vector(-1.25,0);
		}
		if (dsq->game->isObstructed(TileVector(t.x, t.y-1)))
		{
			push += Vector(0,1.25);
		}
		if (dsq->game->isObstructed(TileVector(t.x, t.y+1)))
		{
			push += Vector(0,-1.25);
		}
		if (dsq->game->isObstructed(TileVector(t.x-1, t.y-1)))
		{
			push += Vector(0.5,0.5);
		}
		if (dsq->game->isObstructed(TileVector(t.x-1, t.y+1)))
		{
			push += Vector(0.5,-0.5);
		}
		if (dsq->game->isObstructed(TileVector(t.x+1, t.y-1)))
		{
			push += Vector(-0.5,0.5);
		}
		if (dsq->game->isObstructed(TileVector(t.x+1, t.y+1)))
		{
			push += Vector(-0.5,-0.5);
		}

		// cushion
		
		if (push.x != 0 || push.y != 0)
		{
			if (vel.getSquaredLength2D() > sqr(10))
			{
				push.setLength2D(100 * dt * 60);
				push.z = 0;
			}
			vel += push;
		}
	}
	
	Vector lastPosition = position;

	bool underWater = isUnderWater();
	if (!canLeaveWater)
	{
		if (!underWater && wasUnderWater)
		{
			// do clamp
			if (waterBubble)
			{
				waterBubble->clampPosition(&position, collideRadius);
			}
			else
			{
				position.y = dsq->game->getWaterLevel()+collideRadius;
			}
			
		}
	}
	/*
	if (!canLeaveWater)
	{
		if (waterBubble)
		{
		}
		else
		{
			if (position.y-collideRadius < dsq->game->getWaterLevel())
			{
				
			}
		}
	}
	*/

	bool collided = false;
	
	if (vel.x != 0 || vel.y != 0)
	{

		const int hw = collideRadius;
		bool freeRange = false;

		if (isv(EV_COLLIDELEVEL,1))
		{
			
			

			bool doesFreeRange = !isPullable();
			if (doesFreeRange)
			{
				if (dsq->game->collideCircleWithGrid(position, hw))
				{
					// starting in a collision state
					freeRange = true;
				}
			}
		}
		
		//Vector lastPosition = lastPos;
		Vector newPosition = position + (getMoveVel() * dt);
		position = newPosition;

		if (isv(EV_COLLIDELEVEL,1))
		{
			if (getState() == STATE_PUSH)
			{
				if (!freeRange && dsq->game->collideCircleWithGrid(position, hw))
				{
					position = lastPosition;
					collided = true;
					bounce(bounceAmount);
				}
			}
			else
			{			
				if (!freeRange && ((!canLeaveWater && !isUnderWater() && wasUnderWater) || dsq->game->collideCircleWithGrid(position, hw)))
				{
					position = lastPosition;
					onHitWall();
					bounce(bounceAmount);
					collided = true;
				}
			}
		}
	}

	if (collided && friction != 0 && (vel.x != 0 || vel.y != 0))
	{
		Vector fric = vel;
		fric.setLength2D(-friction);
		vel.z = 0;
		vel += fric*dt;
	}

	//doFriction(dt);
	
	if (!collided && weight != 0)
	{
		vel += Vector(0, weight*dt);
	}
	for (int i = 0; i < attachedEntities.size(); i++)
	{
		attachedEntities[i]->position = this->position + attachedEntitiesOffsets[i];
		attachedEntities[i]->rotation = this->rotation;
	}	

	wasUnderWater = underWater;
}