/////////////////////////////////////////////////////////////////////////////
// Smooth path
/////////////////////////////////////////////////////////////////////////////
void K1999::Smooth(int Step)
{
int prev = ((Divs - Step) / Step) * Step;
int prevprev = prev - Step;
int next = Step;
int nextnext = next + Step;
assert(prev >= 0);
//std::cout << Divs << ", " << Step << ", " << prev << ", " << tx.size() << std::endl;
assert(prev < (int)tx.size());
assert(prev < (int)ty.size());
assert(next < (int)tx.size());
assert(next < (int)ty.size());
for (int i = 0; i <= Divs - Step; i += Step)
{
double ri0 = GetRInverse(prevprev, tx[prev], ty[prev], i);
double ri1 = GetRInverse(i, tx[next], ty[next], nextnext);
double lPrev = Mag(tx[i] - tx[prev], ty[i] - ty[prev]);
double lNext = Mag(tx[i] - tx[next], ty[i] - ty[next]);
double TargetRInverse = (lNext * ri0 + lPrev * ri1) / (lNext + lPrev);
double Security = lPrev * lNext / (8 * SecurityR);
AdjustRadius(prev, i, next, TargetRInverse, Security);
prevprev = prev;
prev = i;
next = nextnext;
nextnext = next + Step;
if (nextnext > Divs - Step)
nextnext = 0;
}
}
static void Smooth(int Step)
{
int prev = ((nSegments - Step) / Step) * Step;
int prevprev = prev - Step;
int next = Step;
int nextnext = next + Step;
for (int i = 0; i <= nSegments - Step; i += Step) {
double ri0 = GetRInverse(prevprev, rlseg[prev].t, i);
double ri1 = GetRInverse(i, rlseg[next].t, nextnext);
double lPrev = (rlseg[i].t-rlseg[prev].t).len();
double lNext = (rlseg[i].t-rlseg[next].t).len();
double TargetRInverse = (lNext * ri0 + lPrev * ri1) / (lNext + lPrev);
double Security = lPrev * lNext / (8 * SecurityR);
AdjustRadius(prev, i, next, TargetRInverse, Security);
prevprev = prev;
prev = i;
next = nextnext;
nextnext = next + Step;
if (nextnext > nSegments - Step)
nextnext = 0;
}
}
void K1999::CalcRaceLine()
{
const unsigned int stepsize = 128;
//abort if the track isn't long enough
if (tx.size() < stepsize)
return;
//
// Smoothing loop
//
for (int Step = stepsize; (Step /= 2) > 0;)
{
for (int i = Iterations * int(sqrt(float(Step))); --i >= 0;)
Smooth(Step);
Interpolate(Step);
}
//
// Compute curvature along the path
//
for (int i = Divs; --i >= 0;)
{
int next = (i + 1) % Divs;
int prev = (i - 1 + Divs) % Divs;
double rInverse = GetRInverse(prev, tx[i], ty[i], next);
tRInverse[i] = rInverse;
}
#ifdef DRAWPATH
std::ofstream ofs("k1999.path");
DrawPath(ofs);
#endif
}
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);
}
static void StepInterpolate(int iMin, int iMax, int Step)
{
int next = (iMax + Step) % nSegments;
if (next > nSegments - Step)
next = 0;
int prev = (((nSegments + iMin - Step) % nSegments) / Step) * Step;
if (prev > nSegments - Step)
prev -= Step;
double ir0 = GetRInverse(prev, rlseg[iMin].t, iMax % nSegments);
double ir1 = GetRInverse(iMin, rlseg[iMax % nSegments].t, next);
for (int k = iMax; --k > iMin;) {
double x = double(k - iMin) / double(iMax - iMin);
double TargetRInverse = x * ir1 + (1 - x) * ir0;
AdjustRadius(iMin, k, iMax % nSegments, TargetRInverse);
}
}
/////////////////////////////////////////////////////////////////////////////
// Interpolate between two control points
/////////////////////////////////////////////////////////////////////////////
void K1999::StepInterpolate(int iMin, int iMax, int Step)
{
int next = (iMax + Step) % Divs;
if (next > Divs - Step)
next = 0;
int prev = (((Divs + iMin - Step) % Divs) / Step) * Step;
if (prev > Divs - Step)
prev -= Step;
double ir0 = GetRInverse(prev, tx[iMin], ty[iMin], iMax % Divs);
double ir1 = GetRInverse(iMin, tx[iMax % Divs], ty[iMax % Divs], next);
for (int k = iMax; --k > iMin;)
{
double x = double(k - iMin) / double(iMax - iMin);
double TargetRInverse = x * ir1 + (1 - x) * ir0;
AdjustRadius(iMin, k, iMax % Divs, TargetRInverse);
}
}
/////////////////////////////////////////////////////////////////////////////
// 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);
}
