static void AdjustRadius(int prev, int i, int next, double TargetRInverse, double Security = 0.0)
{
double OldLane = rlseg[i].lane;
// Start by aligning points for a reasonable initial lane
rlseg[i].lane =
(-(rlseg[next].t.y - rlseg[prev].t.y) * (rlseg[i].l.x - rlseg[prev].t.x) +
( rlseg[next].t.x - rlseg[prev].t.x) * (rlseg[i].l.y - rlseg[prev].t.y)) /
( (rlseg[next].t.y - rlseg[prev].t.y) * (rlseg[i].r.x - rlseg[i].l.x) -
( rlseg[next].t.x - rlseg[prev].t.x) * (rlseg[i].r.y - rlseg[i].l.y));
if (rlseg[i].lane < -0.2)
rlseg[i].lane = -0.2;
else if (rlseg[i].lane > 1.2)
rlseg[i].lane = 1.2;
UpdateTxTy(i);
// Newton-like resolution method
const double dLane = 0.0001;
vec2d d = dLane * (rlseg[i].r - rlseg[i].l);
double dRInverse = GetRInverse(prev, rlseg[i].t + d, next);
if (dRInverse > 0.000000001) {
rlseg[i].lane += (dLane / dRInverse) * TargetRInverse;
double ExtLane = (SideDistExt + Security) / trackWidth;
double IntLane = (SideDistInt + Security) / trackWidth;
if (ExtLane > 0.5)
ExtLane = 0.5;
if (IntLane > 0.5)
IntLane = 0.5;
if (TargetRInverse >= 0.0)
{
if (rlseg[i].lane < IntLane)
rlseg[i].lane = IntLane;
if (1 - rlseg[i].lane < ExtLane)
{
if (1 - OldLane < ExtLane)
rlseg[i].lane = MIN(OldLane, rlseg[i].lane);
else
rlseg[i].lane = 1 - ExtLane;
}
}
else
{
if (rlseg[i].lane < ExtLane)
{
if (OldLane < ExtLane)
rlseg[i].lane = MAX(OldLane, rlseg[i].lane);
else
rlseg[i].lane = ExtLane;
}
if (1 - rlseg[i].lane < IntLane)
rlseg[i].lane = 1 - IntLane;
}
}
UpdateTxTy(i);
}
bool K1999::LoadData(ROADSTRIP* road)
{
tx.clear();
ty.clear();
tRInverse.clear();
txLeft.clear();
tyLeft.clear();
txRight.clear();
tyRight.clear();
tLane.clear();
const std::list <ROADPATCH> & patchlist = road->GetPatchList();
Divs = patchlist.size();
int count = 0;
for (std::list <ROADPATCH>::const_iterator i = patchlist.begin(); i != patchlist.end(); ++i)
{
txLeft.push_back(i->GetPatch().GetPoint(3,0)[0]);
tyLeft.push_back(-i->GetPatch().GetPoint(3,0)[2]);//this originally looked at the z coordinate -JDV
txRight.push_back(i->GetPatch().GetPoint(3,3)[0]);
tyRight.push_back(-i->GetPatch().GetPoint(3,3)[2]);//this originally looked at the z coordinate -JDV
tLane.push_back(0.5);
tx.push_back(0.0);
ty.push_back(0.0);
tRInverse.push_back(0.0);
UpdateTxTy(count);
count++;
}
if (road->GetClosed()) //a closed circuit
return true;
else
return false;
}
bool K1999::LoadData(const RoadStrip & road)
{
tx.clear();
ty.clear();
tRInverse.clear();
txLeft.clear();
tyLeft.clear();
txRight.clear();
tyRight.clear();
tLane.clear();
const std::vector<RoadPatch> & patchlist = road.GetPatches();
Divs = patchlist.size();
int count = 0;
for (const auto & p : patchlist)
{
txLeft.push_back(p.GetPatch().GetPoint(3,0)[1]);
tyLeft.push_back(-p.GetPatch().GetPoint(3,0)[0]);
txRight.push_back(p.GetPatch().GetPoint(3,3)[1]);
tyRight.push_back(-p.GetPatch().GetPoint(3,3)[0]);
tLane.push_back(0.5);
tx.push_back(0.0);
ty.push_back(0.0);
tRInverse.push_back(0.0);
UpdateTxTy(count);
count++;
}
if (road.GetClosed()) //a closed circuit
return true;
else
return false;
}
/////////////////////////////////////////////////////////////////////////////
// Change lane value to reach a given radius
/////////////////////////////////////////////////////////////////////////////
void K1999::AdjustRadius(int prev, int i, int next, double TargetRInverse, double Security)
{
double OldLane = tLane[i];
double Width = Mag((txLeft[i]-txRight[i]),(tyLeft[i]-tyRight[i]));
//
// Start by aligning points for a reasonable initial lane
//
tLane[i] = (-(ty[next] - ty[prev]) * (txLeft[i] - tx[prev]) +
(tx[next] - tx[prev]) * (tyLeft[i] - ty[prev])) /
( (ty[next] - ty[prev]) * (txRight[i] - txLeft[i]) -
(tx[next] - tx[prev]) * (tyRight[i] - tyLeft[i]));
// the original algorithm allows going outside the track
/*
if (tLane[i] < -0.2)
tLane[i] = -0.2;
else if (tLane[i] > 1.2)
tLane[i] = 1.2;
*/
if (tLane[i] < 0.0)
tLane[i] = 0.0;
else if (tLane[i] > 1.0)
tLane[i] = 1.0;
UpdateTxTy(i);
//
// Newton-like resolution method
//
const double dLane = 0.0001;
double dx = dLane * (txRight[i] - txLeft[i]);
double dy = dLane * (tyRight[i] - tyLeft[i]);
double dRInverse = GetRInverse(prev, tx[i] + dx, ty[i] + dy, next);
if (dRInverse > 0.000000001)
{
tLane[i] += (dLane / dRInverse) * TargetRInverse;
double ExtLane = (SideDistExt + Security) / Width;
double IntLane = (SideDistInt + Security) / Width;
if (ExtLane > 0.5)
ExtLane = 0.5;
if (IntLane > 0.5)
IntLane = 0.5;
if (TargetRInverse >= 0.0)
{
if (tLane[i] < IntLane)
tLane[i] = IntLane;
if (1 - tLane[i] < ExtLane)
{
if (1 - OldLane < ExtLane)
tLane[i] = Min(OldLane, tLane[i]);
else
tLane[i] = 1 - ExtLane;
}
}
else
{
if (tLane[i] < ExtLane)
{
if (OldLane < ExtLane)
tLane[i] = Max(OldLane, tLane[i]);
else
tLane[i] = ExtLane;
}
if (1 - tLane[i] < IntLane)
tLane[i] = 1 - IntLane;
}
}
UpdateTxTy(i);
}
