User:Skilgannon/Optimal Velocity

I'll be gone to Mozambique for a week, I expect people to find cases that break this code while I'm gone =)

  /**
    * Returns the new velocity based on the current velocity and distance to move.
    *
    * @param velocity the current velocity
    * @param distance the distance to move
    * @return the new velocity based on the current velocity and distance to move
    */
       private static double getNewVelocity(double velocity, double distance) {
      
         if (distance < 0) 
            return -getNewVelocity(-velocity, -distance);
      	// If the distance is negative, then change it to be positive
      	// and change the sign of the input velocity and the result
         
         double maxVel = currentCommands.getMaxVelocity();
         if (velocity < 0)
            return Math.min(velocity + Rules.DECELERATION,  Math.min(distance,maxVel));
         //we want to go in the opposite direction, so decelerate   
         	
         else{ 
         //we are going in the direction we want, test what happens if we accelerate
         
            double accel = Math.min(Rules.ACCELERATION, maxVel - velocity);
            //speed up, but not more than max velocity. decel if maxVel is less than vel.
         	
            accel = Math.max(accel, -Rules.DECELERATION);
             //prevent excess deceleration due to bot lowering the max velocity
            // while velocity is high
            
            if (distance > decelDistance(velocity + accel))
               return velocity + accel;
            else if(accel != 0 && distance > decelDistance(velocity))
               return velocity;
            else{
               if(distance < Rules.DECELERATION 
               //we'll be able to cover remaining distance in 1 tick and then decel to stop
               
               && velocity  <= Rules.DECELERATION + distance
               //and our velocity is low enough for us to get to that required velocity
               )
                  return Math.min(distance, maxVel);
     
               double distDiff = distance - decelDistance(velocity - Rules.DECELERATION);
               double decelTime =  Math.ceil(velocity / Rules.DECELERATION);
              
               accel = distDiff/decelTime - Rules.DECELERATION;
            
               double newVel = Math.max(-maxVel, Math.min(velocity + accel, maxVel));
               
               return  Math.max(velocity - Rules.DECELERATION, Math.min(newVel, 
                  velocity + Rules.ACCELERATION));
            }
         
         }
      
      }
    /**
    * Returns the linear distance it would take to decelerate from a given positive velocity
    *
    * @param velocity the positive velocity from which to test
    * @return the linear distance required to decelerate to a standstill
    */

       private static final double decelDistance(double velocity){
         double t =velocity / Rules.DECELERATION;
         double floor_t = Math.floor(t);
         return 0.5*floor_t*(floor_t+1)*Rules.DECELERATION + (velocity%2)*Math.ceil(t);
      }

--Skilgannon 08:25, 16 July 2009 (UTC)