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;
}
// Limit max acceleration around a curve based on
// centripetal acceleration.
double getMaxVel( void )
{
// Calculate a velocity limit based on centripetal accel
if( velCentrip <= 0 )
{
double A = getMaxAcc();
double D = getMaxDec();
if( D < A ) A = D;
velCentrip = sqrt( A * radius );
}
double max = PathElement::getMaxVel();
if( velCentrip < max )
return velCentrip;
return max;
}
/**
* Add this segment to the end of the
* passed path.
*/
void Add( PathElement *pe )
{
// Add this segment after the passed one.
if( pe ) pe->next = this;
this->prev = pe;
// Find the peak velocity that could be
// reached at the end of this segment if
// I didn't have to worry about stopping in
// the future.
//
// This is the previous segment's peak velocity
// plus the increase I could provide based on
// this segment's acceleration & length.
double Vstart = 0;
if( pe ) Vstart = pe->velPeak;
velPeak = getMaxVelInc( Vstart, getMaxAcc() );
}
// Do the main part of my calculations. This function fills in the array
// of times in each of the 7 possible sub-segments.
void CalcTimes( void )
{
double Ve = velEnd;
double Vs = getVelStart();
double P = length;
double A = getMaxAcc();
double D = getMaxDec();
double V = getMaxVel();
double J = getMaxJrk();
// We start out assuming that we will hit the max velocity.
// If this calculation is successful, then we're done
if( CalcForVel( V ) )
return;
// Make a quick check here for a zero length segment.
if( P <= 0.0 )
{
for( int i=0; i<7; i++ ) SegT[i] = 0;
return;
}
// OK, we aren't going to hit the velocity limit in this move.
// I'll try running at the accel & decel limits only
double ta = ( sqrt( 8*A*D*J*J*P*(A+D) +
4*J*J*(Vs*Vs*D*D + (Vs*Vs+Ve*Ve)*A*D + Ve*Ve*A*A) -
4*A*D*J*(Ve*D*D + (Vs+Ve)*A*D + Vs*A*A) +
A*A*D*D*( D*D + 2*A*D + A*A )
)
-2*J*Vs*(A+D) -A*D*D -3*A*A*D - 2*A*A*A
)
/ ( 2*A*J*(A+D) );
double tj = A/J;
double tk = D/J;
double td = (Vs - Ve + J*tj*ta + J*tj*tj - J*tk*tk)/(J*tk);
// If both of these times came out positive, we're done
if( (ta >= 0.0) && (td >= 0.0) )
{
SegT[0] = tj;
SegT[1] = ta;
SegT[2] = tj;
SegT[3] = 0;
SegT[4] = tk;
SegT[5] = td;
SegT[6] = tk;
return;
}
// We can't reach both the accel & decel limits.
// There isn't a simple formulat for calculating out the optimal times
// in this case, so I'll itterate with various maximum velocities until
// I find one that will work.
double Vup, Vdn;
// I'll find the max velocity that I'd use without jerk limiting as an
// initial upper limit
CalcNoJrk();
Vup = SegV[1];
// For my lower limit, I'll pick the higher of the starting or ending velocity
Vdn = (Vs>Ve) ? Vs : Ve;
// First calculation uses the lower velocity limit.
// This should never fail.
CalcForVel( Vdn, true );
// Itterate as many as 10 times to try to get a faster move.
// I'll quit when my constant velocity segment is less then
// 1 millisecond long.
for( int i=0; (i<10) && (SegT[3] > MIN_PVT_TIME); i++ )
{
V = (Vup+Vdn)/2;
if( CalcForVel( V ) )
Vdn = V;
else
Vup = V;
}
return;
}
bool CalcForVel( double V, bool force=false )
{
double Ve = velEnd;
double Vs = getVelStart();
double P = length;
double A = getMaxAcc();
double D = getMaxDec();
double J = getMaxJrk();
// We start out assuming that we will hit the max velocity.
// Find the times required in jerk and accel segments
// Also, find the distance moved getting up to velocity.
double tj, ta, Pup;
if( J * (V-Vs) < A*A )
{
ta = 0;
tj = sqrt( (V-Vs)/J );
Pup = J*tj*tj*tj + 2*Vs*tj;
}
else
{
tj = A/J;
ta = (V-Vs)/A - tj;
Pup = Vs*(2*tj+ta) + J*tj*tj*tj + 3*J/2*ta*tj*tj + J/2*ta*ta*tj;
}
// If I'm already past my position limit, then quit now
if( Pup > P )
{
if( force ) Pup = P;
else return false;
}
// Same thing for the deceleration portion of the segment
double tk, td, Pdn;
if( J * (V-Ve) < D*D )
{
td = 0;
tk = sqrt( (V-Ve)/J );
Pdn = J*tk*tk*tk + 2*Ve*tk;
}
else
{
tk = D/J;
td = (V-Ve)/D - tk;
Pdn = Ve*(2*tk+td) + J*tk*tk*tk + 3*J/2*td*tk*tk + J/2*td*td*tk;
}
// If the sum of these two distances exceeds my total length,
// then I can't hit this velocity during my move.
if( Pup+Pdn > P )
{
if( force ) Pdn = P - Pup;
else return false;
}
// Record the move times
SegT[0] = tj;
SegT[1] = ta;
SegT[2] = tj;
SegT[3] = (P-Pup-Pdn)/V;
SegT[4] = tk;
SegT[5] = td;
SegT[6] = tk;
if( SegT[3] < 0 ) SegT[3] = 0;
return true;
}
// Calculate run times with no jerk limits. This is quicker & simpler
// then the full blown calculations that include jerk limits.
virtual void CalcNoJrk( void )
{
double ve = velEnd;
double vs = getVelStart();
double P = length;
double A = getMaxAcc();
double D = getMaxDec();
double V = getMaxVel();
// Assume for the moment that we will hit our accel & decel limits.
double ta = (V-vs) / A;
double td = (V-ve) / D;
double tv;
double remain = P - (vs*ta + ta*ta*A/2) - (ve*td + td*td*D/2);
if( remain >= 0 )
tv = remain / V;
else
{
ta = sqrt( (A+D)*(D*vs*vs + A*ve*ve + 2*A*D*P) ) / (A*(A+D)) - vs/A;
td = (A*ta+vs-ve)/D;
tv = 0.0;
}
// Set the times for each sub-segment
SegT[0] = 0.0;
SegT[1] = ta;
SegT[2] = 0.0;
SegT[3] = tv;
SegT[4] = 0.0;
SegT[5] = td;
SegT[6] = 0.0;
// Set the acceleration at the end of each sub-segment
SegA[0] = A;
SegA[1] = 0;
SegA[2] = 0;
SegA[3] = 0;
SegA[4] = -D;
SegA[5] = 0;
SegA[6] = 0;
// Fill in the position, and velocity for
// the end of each of the sub-segments.
double p = 0;
double v = getVelStart();
SegP[0] = 0;
SegV[0] = v;
for( int i=1; i<7; i+=2 )
{
double t = SegT[i];
double a = SegA[i-1];
p += v*t + a*t*t/2;
v += a*t;
SegP[i] = SegP[i+1] = p;
SegV[i] = SegV[i+1] = v;
}
return;
}
