void HeroNode::updateCurrent(sf::Time dt)
{
correctDiagonalVelocity();
accelerate(trmb::Entity::getVelocity() * getMaxSpeed());
trmb::Entity::updateCurrent(dt);
correctPosition();
}
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--;
}
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;
}
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();
}
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);
}
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;
}
}
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();
}
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;
}
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
}
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);
}
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);
}
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();
}
// ===========================================================================
// 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());
}
}
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);
}
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;
}
}
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);
*/
}
}
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);
}
/**
* 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);
}
}
}
void Projectile::init()
{
Actor::init();
vel = mDir.normalize() * getMaxSpeed();
size = 5;
}
// ===========================================================================
// method definitions
// ===========================================================================
MSVehicleType::MSVehicleType(const SUMOVTypeParameter& parameter)
: myParameter(parameter), myIndex(myNextIndex++), myOriginalType(0) {
assert(getLength() > 0);
assert(getMaxSpeed() > 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;
}
