intelligentdrivermotion.java

linux下用于移动节点的移动活动生成工具

package de.uni_stuttgart.informatik.canu.uomm;

/**
 * <p>Title: User-Oriented Mobility Model</p>
 * <p>Description: </p>
 * <p>Copyright: Copyright (c) 2002</p>
 * <p>Company: University of Stuttgart</p>
 * @author Illya Stepanov
 * @version 1.1
 */

import de.uni_stuttgart.informatik.canu.mobisim.core.*;
import de.uni_stuttgart.informatik.canu.mobisim.mobilitymodels.Movement;
import de.uni_stuttgart.informatik.canu.mobisim.notifications.*;
import de.uni_stuttgart.informatik.canu.mobisim.extensions.Graph;
import de.uni_stuttgart.informatik.canu.senv.core.*;
import de.uni_stuttgart.informatik.canu.spatialmodel.core.*;
import de.uni_stuttgart.informatik.canu.spatialmodel.geometry.*;
import de.uni_stuttgart.informatik.canu.tripmodel.core.*;

/**
 * This class implements Intelligent Driver Motion Behavior.
 * 
 * The implementation is based on M. Treiber and D. Helbing,
 * "Explanation of Observed Features of Self-Organization in Traffic Flow",
 * Preprint cond-mat/9901239 (1999). <br>
 * <br> 
 * @author Illya Stepanov
 */
public class IntelligentDriverMotion extends UserOrientedMovement
{
  /**
   * Flag to indicate that a random stay duration must be chosen at the beginning of the simulation
   */
  protected boolean stayRandom = false;
  
  /**
   * Current stay duration at destination (ms)
   */
  protected int stay = 0;                 // in ms

  /**
   * Desired speed (in meters/ms)
   */
  protected float desiredSpeed = 0.0f;    // in m/ms

  /**
   * Current speed (in meters/ms)
   */
  protected float speed = 0.0f;           // in m/ms
  
  /**
   * Movement recalculation step (in steps)
   */
  protected int recalculation_step = 0;   // in steps

  /**
   * Vehicle length (in meters)
   */
  protected float vehicleLength = 5.0f;
  
  /**
   * Maximum vehicle acceleration (in meters/ms^2) 
   */
  protected float a = 0.6e-6f;               // in m/ms^2
  
  /**
   * "Normal" vehicle acceleration (in meters/ms^2)
   */
  protected float b = 0.9e-6f;               // in m/ms^2
  
  /**
   * Minimum "jam" distance to a standing vehicle (in meters)
   */
  protected float s0 = 2.0f;              // in m
  
  /**
   * Safe time headway (in ms)
   */
  protected float t = 1.5e3f;               // in ms
  
  /**
   * Destination of previous movement
   */
  protected Position3D oldPosition;

  /**
   * Destination of current movement
   */
  protected Position3D destination;

  /**
   * Current movement vector
   */
  protected Vector3D movement;

  /**
   * Current trip
   */
  protected Trip trip = new Trip();

  /**
   * Constructor
   */
  public IntelligentDriverMotion()
  {
  }

  /**
   * Returns the module's description. <br>
   * <br>
   * @return extension module's description
   */
  public String getDescription()
  {
    return "Intelligent Driver Movement Behavior";
  }

  /**
   * Performs the module initialization. <br>
   * <br>
   * The method is called after finishing the scenario file processing.
   */
  public void initialize()
  {
    Node node = (Node)owner;

    // set initial position
    Point pos = initialPositionGenerator.getInitialPosition(node);
    node.setPosition(pos.getPosition());
    oldPosition = pos.getPosition();
    
    if (stayRandom)
    {
      stay = tripGenerator.chooseStayDuration(node);
    }

    super.initialize();
  }

  /**
   * Gets the current speed of movement. <br>
   * <br>
   * @return the current speed of movement (in meters/ms)
   */
  public float getSpeed()
  {
    return speed;
  }

  /**
   * Gets the destination of the previous movement. <br>
   * <br>
   * @return the destination of the previous movement
   */
  public Position3D getOldPosition()
  {
    return oldPosition;
  }
  /**
   * Gets the destination of the current movement. <br>
   * <br>
   * @return the destination of the current movement
   */
  public Position3D getDestination()
  {
    return destination;
  }

  /**
   * Chooses a new movement path
   */
  protected void chooseNewPath()
  {
    java.util.Random rand = u.getRandom();
    
    Node node = (Node)this.owner;

    trip = tripGenerator.genTrip(node);

    u.sendNotification(new DebugNotification(this, u, "New trip generated:"));
    for (int i=0; i<trip.getPath().size(); i++)
    {
      Point p = (Point)trip.getPath().get(i);
      u.sendNotification(new DebugNotification(this, u,
        ""+p.getX()+" "+p.getY()));
    }
    
    // delete the current node position from the path
    trip.getPath().remove(0);

    desiredSpeed = (minSpeed + (maxSpeed-minSpeed)*rand.nextFloat());
  }

  /**
   * Chooses a new destination and movement speed
   */
  protected void chooseNewMovement()
  {
    Node owner = (Node)this.owner;

    if (trip.getPath().size()==0)
      chooseNewPath();

    Point p = (Point)trip.getPath().get(0);
    trip.getPath().remove(0);

    destination = p.getPosition();

    stay = 0;

    movement = owner.getPosition().getNormalizedDirectionVector(destination);
    
    recalculateSpeed();
  }

  /**
   * Chooses a time of staying at current position or continues
   * the movement to destination.
   */
  protected void chooseNewStayDuration()
  {
    Node owner = (Node)this.owner;

    oldPosition = owner.getPosition();

    if (trip.getPath().size()==0)
    {
      // wait at destination
      stay = tripGenerator.chooseStayDuration(owner);
      
      u.sendNotification(new DestinationReachedNotification(this, u,
        owner.getPosition(), stay/1000.0f));
        
      speed = 0.0f;        
    }
    else
    {
      chooseNewMovement();
    }
  }

  /**
   * Recalculates the vehicle's speed
   */
  protected void recalculateSpeed()
  {
    SpatialModel spatialModel = (SpatialModel)u.getExtension("SpatialModel");
    Graph graph = spatialModel.getGraph();
    
    if (graph!=null)
    {
      Node owner = (Node)this.owner;

      Vertex vs = graph.getVertex(oldPosition.getX(), oldPosition.getY());
      Vertex vd = graph.getVertex(destination.getX(), destination.getY());
      
      Edge currentEdge = null;
      if ((vs!=null)&&(vd!=null))
        currentEdge = spatialModel.findEdge(vs, vd);
        
      if (currentEdge!=null)
      {
        IntelligentDriverMotion n1_mf = null;
        float d_speed1 = 0.0f;
        float d_1 = Float.MAX_VALUE;

        float d_speed2 = 0.0f;
        float d_2 = Float.MAX_VALUE;
        
        // find two closest cars in front
        java.util.Iterator iter = u.getNodes().iterator();
        while (iter.hasNext())
        {
          Node node = (Node)iter.next();
          if (owner==node)
            continue;

          Movement n_m = (Movement)node.getExtension("Movement");
          if (!(n_m instanceof IntelligentDriverMotion))
            continue;
          
          IntelligentDriverMotion n_mf = (IntelligentDriverMotion)n_m;
          // ignore paused cars
          if ((n_mf.destination==null)||(n_mf.stay!=0))
            continue;

          Vertex n_vs = graph.getVertex(n_mf.oldPosition.getX(), n_mf.oldPosition.getY());
          Vertex n_vd = graph.getVertex(n_mf.destination.getX(), n_mf.destination.getY());

          // check if on the same edge
          if ((vs!=n_vs)||(vd!=n_vd))
            continue;
          
          // check if in front
          Position3D p_vd = new Point(vd.getX(), vd.getY()).getPosition();
          if (owner.getPosition().getDistance(p_vd)>=node.getPosition().getDistance(p_vd))
          {
            float d = (float)owner.getPosition().getDistance(node.getPosition())-vehicleLength;
            // check if closer than at jam distance
            if (d<s0)
            {
              if (speed==0)
              {
                // wait till nodes with lower IDs depart
                if (owner.getID().compareTo(node.getID())>=0)
                {
                  speed = Float.NaN;
                  break;
                }
                else
                  // ignore the node
                  continue;
              }
            }
            
            if (d<d_1)
            {
              d_2 = d_1;
              d_speed2 = d_speed1;

              d_1 = d;
              d_speed1 = speed - n_mf.speed;
              n1_mf = n_mf;
            }
            else
            if (d<d_2)
            {
              d_2 = d;
              d_speed2 = speed - n_mf.speed;
            }
          }
        }
        
        float ss = (float)(s0 + Math.max(speed*t+speed*d_speed1/(2.0*Math.sqrt(a*b)), 0));
        float dv = (float)(a*(1-Math.pow(speed/desiredSpeed, 4)-Math.pow(ss/d_1, 2)));
        
        speed+=dv*u.getStepDuration()*recalculation_step;
        
        if (speed>desiredSpeed)
          speed = desiredSpeed;

        // do a traffic jam
        if (Float.isNaN(speed)||(speed<0.0f))
          speed = 0.0f;
      }
    }
    
    u.sendNotification(new DebugNotification(this, u, "at "+u.getTimeAsString()+" "+((Node)owner).getID()+" changes speed to "+speed*1000.0f+" m/s"));    
  }

  /**
   * Executes the extension. <br>
   * <br>
   * The method is called on every simulation timestep.
   * @return 0 - the module should be executed on next timesteps,
   *        -1 - the module should not be executed on further timesteps and should be removed from the extensions' list
   */
  public int act()
  {
    Node owner = (Node)this.owner;

    boolean speedChanged = false;
    
    //if node has arrived to destination and stayed enough, a new destination
    //choosen
    if ((destination==null)||(owner.getPosition().equals(destination)))
    {
      if(movement != null)
      {
        movement = null;
        chooseNewStayDuration();
        speedChanged = true;        
      }
      else
      if(stay <= 0)
      {
        chooseNewMovement();
        speedChanged = true;
      }
      else
        stay -= u.getStepDuration();
    }

    if (movement!=null)
    {
      if ((!speedChanged) && (u.getTimeInSteps()%recalculation_step==0))
      {
        recalculateSpeed();
        speedChanged = true;
      }

      // do not move in a jam, wait for the next recalculation
      if (speed==0.0f)
        return 0;
      
      //move towards destination
      Vector3D m = movement.mult(speed*u.getStepDuration());
      if(owner.getPosition().getDistance(destination) >= m.getLength())
      {
        if (speedChanged)
        {
          // check if the next speed change event is about to occur before arriving to destination
          double dist = (recalculation_step-u.getTimeInSteps()%recalculation_step)*u.getStepDuration()*(double)speed;
          if (owner.getPosition().getDistance(destination) < dist)
          {
            // move to destination
            u.sendNotification(new MovementChangedNotification(this, u, destination, speed*1000f));
          }
          else
          {
            // move until the next speed change event occur
            u.sendNotification(new MovementChangedNotification(this, u, owner.getPosition().add(movement.mult(dist)), speed*1000f));
          }
        }
        
        owner.setPosition(owner.getPosition().add(m));
      }
      else
      {
        if (speedChanged)
          u.sendNotification(new MovementChangedNotification(this, u, destination, speed*1000f));

        owner.setPosition(destination);
      }
    }
    
    return 0;
  }

  /**
   * Initializes the object from XML tag. <br>
   * <br>
   * @param element source tag
   * @throws Exception Exception if parameters are invalid
   */
  public void load(org.w3c.dom.Element element) throws Exception
  {
    u.sendNotification(new LoaderNotification(this, u,
      "Loading IntelligentDriverMotion extension"));

    super.load(element);

    org.w3c.dom.Node n;
    
    n = element.getElementsByTagName("minspeed").item(0);
    if(n==null)
      throw new Exception("<minspeed> is missing!");
    minSpeed = Float.parseFloat(n.getFirstChild().getNodeValue())/1000;

    n = element.getElementsByTagName("maxspeed").item(0);
    if(n==null)
      throw new Exception("<maxspeed> is missing!");
    maxSpeed = Float.parseFloat(n.getFirstChild().getNodeValue())/1000;

    n = element.getElementsByTagName("step").item(0);
    if(n==null)
      throw new Exception("<step> is missing!");
    int i = (int)(Float.parseFloat(n.getFirstChild().getNodeValue())*1000);
    if ((i<=u.getStepDuration())||(i%u.getStepDuration()!=0))
      throw new Exception("Invalid <step> value: "+(float)i/1000.0f);      
    recalculation_step = i/u.getStepDuration();

    n = element.getElementsByTagName("l").item(0);
    if(n!=null)
      vehicleLength = Float.parseFloat(n.getFirstChild().getNodeValue());
    
    n = element.getElementsByTagName("a").item(0);
    if(n!=null)
      a = Float.parseFloat(n.getFirstChild().getNodeValue())/1e6f;

    n = element.getElementsByTagName("b").item(0);
    if(n!=null)
      b = Float.parseFloat(n.getFirstChild().getNodeValue())/1e6f;

    n = element.getElementsByTagName("s0").item(0);
    if(n!=null)
      s0 = Float.parseFloat(n.getFirstChild().getNodeValue());

    n = element.getElementsByTagName("t").item(0);
    if(n!=null)
      t = Float.parseFloat(n.getFirstChild().getNodeValue())*1e3f;

    n = element.getElementsByTagName("stay").item(0);
    if (n!=null)
    {
      String randTag = ((org.w3c.dom.Element)n).getAttribute("random");
      if ((randTag.length()>0) && Boolean.valueOf(randTag).booleanValue())
        stayRandom = true;
      else
        stay=(int)(Float.parseFloat(n.getFirstChild().getNodeValue())*1000);
    }

    // check
    if (minSpeed<=0)
      throw new Exception("Invalid <minspeed> value: "+(float)minSpeed*1000);
    if (maxSpeed<minSpeed)
      throw new Exception("Invalid <maxspeed> value: "+(float)maxSpeed*1000);
    if (vehicleLength<0)
      throw new Exception("Invalid <l> value: "+vehicleLength);
    if (a<=0)
      throw new Exception("Invalid <a> value: "+a*1e6f);
    if (b<=0)
      throw new Exception("Invalid <b> value: "+b*1e6f);
    if (s0<=0)
      throw new Exception("Invalid <s0> value: "+s0);
    if (t<=0)
      throw new Exception("Invalid <t> value: "+t/1e3f);
    if (stay<0)
      throw new Exception("Invalid <stay> value: "+(float)stay/1000);

    u.sendNotification(new LoaderNotification(this, u,
      "Finished loading IntelligentDriverMotion extension"));
  }
}