public Tracks Movement(Tracks ip, Voltages V, double t)
{ // ip - initial position of tracks, V - Velocities, t - time
double k = 0.001;
double a = 0.0;
double b = 0.0;
double l = 0.0;
double lx = 0.0;
double ly = 0.0;
double rx = 0.0;
double ry = 0.0;
if (Math.Abs(ip.lx - ip.rx) < k)
{
a = Math.PI / 2.0;
b = Math.PI - a;
l = Math.Abs((ip.ly - ip.ry) / 2.0);
}
else
{
a = Math.Atan((ip.ly - ip.ry) / (ip.lx - ip.rx));
if (a <= 0.0)
{
a = Math.PI + a;
}
b = Math.PI - a;
l = Math.Abs((ip.lx - ip.rx) / (2.0 * Math.Cos(b)));
}
if (Math.Abs(V.lV - V.rV) < k)
{
double S = 0.0;
double c = 0.0;
S = ((V.lV + V.rV) / 2.0) * t;
if (ip.lx > ip.rx)
{
S = -S;
}
else if ((Math.Abs(ip.lx - ip.rx) < k) && (ip.ly < ip.ry))
{
S = -S;
}
if (a >= (Math.PI / 2.0))
{
c = a - Math.PI / 2.0;
}
else
{
c = Math.PI / 2.0 + a;
}
lx = ip.lx + S * Math.Cos(c);
ly = ip.ly + S * Math.Sin(c);
rx = ip.rx + S * Math.Cos(c);
ry = ip.ry + S * Math.Sin(c);
}
else
{
double R = 0.0;
double c = 0.0;
double Rx = 0.0;
double Ry = 0.0;
double lr = 0.0;
double rr = 0.0;
double ld = 0.0;
double rd = 0.0;
R = ((V.lV + V.rV) / (V.lV - V.rV)) * l;
if (Math.Abs(R - l) < k)
{
c = (V.lV / (R + l)) * t;
}
else
{
c = (V.rV / (R - l)) * t;
}
rx = ip.rx - ip.lx;
ry = ip.ry - ip.ly;
Rx = GetX(GetPolarR(rx, ry) + (R - l), GetPolarF(rx, ry)) + ip.lx;
Ry = GetY(GetPolarR(rx, ry) + (R - l), GetPolarF(rx, ry)) + ip.ly;
lx = ip.lx - Rx;
ly = ip.ly - Ry;
rx = ip.rx - Rx;
ry = ip.ry - Ry;
lr = GetPolarR(lx, ly);
rr = GetPolarR(rx, ry);
ld = GetPolarF(lx, ly) - c;
while (ld < 0)
{
ld = 2.0 * Math.PI + ld;
}
while (ld > 2.0 * Math.PI)
{
ld = ld - 2.0 * Math.PI;
}
rd = GetPolarF(rx, ry) - c;
while (rd < 0)
{
rd = 2.0 * Math.PI + rd;
}
while (rd > 2.0 * Math.PI)
{
rd = rd - 2.0 * Math.PI;
}
lx = GetX(lr, ld) + Rx;
ly = GetY(lr, ld) + Ry;
rx = GetX(rr, rd) + Rx;
ry = GetY(rr, rd) + Ry;
}
return(new Tracks(lx, ly, rx, ry));
}
public void Step()
{
float l = (float)(Math.Sqrt(Math.Pow((T.lx - T.rx), 2) + Math.Pow((T.ly - T.ry), 2)) / 2);
bool rayDirection;
if ((Math.Abs((float)T.lx - T.rx) < Constants.EPS) && (T.ly < T.ry))
{
rayDirection = true;
}
else
{
rayDirection = false;
}
if (T.lx > T.rx)
{
rayDirection = true;
}
else
{
rayDirection = false;
}
float angleA;
if (Math.Abs(T.lx - T.rx) < 0.00001)
{
angleA = (float)(Math.PI / 2.0);
}
else
{
angleA = (float)Math.Atan((T.ly - T.ry) / (T.lx - T.rx));
if (angleA <= 0.0)
{
angleA = (float)(Math.PI + angleA);
}
}
double c = 0.0; // new angle
if (rayDirection)
{
l = -l;
}
if (angleA >= (Math.PI / 2.0))
{
c = angleA - Math.PI / 2.0;
}
else
{
c = Math.PI / 2.0 + angleA;
}
float lxNew = (float)(T.lx + l * Math.Cos(c));
float lyNew = (float)(T.ly + l * Math.Sin(c));
float rxNew = (float)(T.rx + l * Math.Cos(c));
float ryNew = (float)(T.ry + l * Math.Sin(c));
sensor = new DistancesSensor();
sensor.LeftEquation = new LineEquation(new PointF((float)(-T.lx), (float)T.ly), new PointF(-lxNew, lyNew));
sensor.RightEquation = new LineEquation(new PointF((float)(-T.rx), (float)T.ry), new PointF(-rxNew, ryNew));
sensor.PlatrormLine = new Line2D(new PointF((float)-T.lx, (float)T.ly), new PointF((float)-T.rx, (float)T.ry));
sensor.Scan(mapModel.WallsEquations, mapModel.Walls);
PointF rD = sensor.RightPoint;
PointF lD = sensor.LeftPoint;
T = L.Movement(T, L.LogicMethod(sensor.LeftDistance, sensor.RightDistance), t);
}