void ClothoidPath::OptimiseLine( const CarModel& cm, int idx, int step, double hLimit, PathPt* l3, const PathPt* l2, const PathPt* l4 )
{
LinearRegression l;
const int NSEG = m_pTrack->GetSize();
int i = (idx + NSEG - step) % NSEG;
while( m_pPath[i].h > hLimit )
{
l.Sample( m_pPath[i].pt.GetXY() );
i = (i + NSEG - step) % NSEG;
}
l.Sample( m_pPath[i].pt.GetXY() );
i = idx;
while( m_pPath[i].h > hLimit )
{
l.Sample( m_pPath[i].pt.GetXY() );
i = (i + step) % NSEG;
}
l.Sample( m_pPath[i].pt.GetXY() );
GfOut( "%4d ", idx );
Vec2d p, v;
l.CalcLine( p, v );
double t;
Utils::LineCrossesLine( l3->Pt().GetXY(), l3->Norm().GetXY(), p, v, t );
SetOffset( cm, 0, t, l3, l2, l4 );
}
