|
@@ -501,6 +501,7 @@ void CalcSide(std::vector<PlanPoint> & xPlan)
|
|
|
double yold = xPlan[i].y;
|
|
|
xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
|
|
|
xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
|
|
|
+ xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
|
|
|
}
|
|
|
|
|
|
for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
|
|
@@ -511,6 +512,7 @@ void CalcSide(std::vector<PlanPoint> & xPlan)
|
|
|
double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
|
|
|
xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
|
|
|
xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
|
|
|
+ xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
|
|
|
}
|
|
|
return;
|
|
|
}
|