public Trapeze(PointX bottomLeft, PointX topLeft, PointX topRight, PointX bottomRight)
{
this.bottomLeft = bottomLeft;
this.bottomRight = bottomRight;
this.topLeft = topLeft;
this.topRight = topRight;
}
public double getHeightCompensation(double d, double h)
{
if (h <= maxHeightSpeedPerTick)
{
return(h);
}
/* Вычисляем распределение для входных данных */
FuzzyDistribution deviationDistribution = this.deviationGraph.getFuzzyDistribution(Math.Abs(d));
FuzzyDistribution heightDistribution = this.heightGraph.getFuzzyDistribution(h);
/* Вычисление распределение методов возможной реакции */
FuzzyDistribution speedDistribution = new FuzzyDistribution(new double[Enum.GetNames(typeof(SpeedFuzzyTypes)).Length]);
for (int i = 0; i < deviationDistribution.Count; ++i)
{
for (int j = 0; j < heightDistribution.Count; ++j)
{
speedDistribution[rules[i, j]] =
Math.Max(speedDistribution[rules[i, j]], Math.Min(deviationDistribution[i], heightDistribution[j]));
}
}
/* и тут появляется Янушка */
List <PointX> polygon = speedGraph.getPolygon(speedDistribution);
PointX p = this.centerOfMass(polygon);
return(p.x);
}
private PointX centerOfMass(List <PointX> PointXs) // нахождение центра массы
{
PointX p = new PointX(0, 0); // произвольная точка для подсчета
double x = 0;
double y = 0;
double s = 0;
for (int i = 0; i < PointXs.Count - 1; i++)
{
double s1 = signSquare(p, PointXs[i], PointXs[i + 1]);
PointX r = centroid(p, PointXs[i], PointXs[i + 1]);
x += r.x * s1;
y += r.y * s1;
s += s1;
}
if (s != 0)
{
return(new PointX(x / s, y / s));
}
else
{
// если у нас прямая, то мы берем среднее по Х
x = 0;
for (int i = 0; i < PointXs.Count; i++)
{
x += PointXs[i].x;
}
return(new PointX(x / PointXs.Count, 0));
}
}
public static PointX getIntersection(PointX p11, PointX p12, PointX p21, PointX p22) // нахождение точки пересечения прямых заданных точками
{
double a1, b1, c1, a2, b2, c2;
a1 = p12.y - p11.y;
b1 = p11.x - p12.x;
c1 = -a1 * p11.x - b1 * p11.y;
a2 = p22.y - p21.y;
b2 = p21.x - p22.x;
c2 = -a2 * p21.x - b2 * p21.y;
double z1 = a1 * b2 - a2 * b1;
double z2 = a1 * b2 - a2 * b1;
if (z1 != 0 && z2 != 0)
{
return(new PointX(-(c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1), -(a1 * c2 - a2 * c1) / (a1 * b2 - a2 * b1)));
}
else
{
return(null);
}
}
public static PointX getPointXByTwoPointXsAndY(PointX p1, PointX p2, double y) // нахождение координаты точки на прямой по У
{
return(new PointX((y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y) - p1.x, y));
}
private PointX centroid(PointX p1, PointX p2, PointX p3) // центроид треугольника
{
return(new PointX((p1.x + p2.x + p3.x) / 3, (p1.y + p2.y + p3.y) / 3));
}
private double signSquare(PointX p1, PointX p2, PointX p3) //знаковая площадь треугольника
{
return(((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / 2);
}
public List <PointX> getPolygon(List <double> distribution)
{
List <PointX> res = new List <PointX>();
if (distribution.Count != this._list.Count)
{
return(null);
}
//добавил Вадик
int j = 0;
while (distribution[j] == 0)
{
j++;
}
Trapeze t1 = _list[j].Cut(distribution[j]);
res.Add(t1.bottomLeft);
res.Add(t1.topLeft);
for (int i = j + 1; i < _list.Count; i++)
{
if (distribution[i] == 0)
{
continue;
}
Trapeze t2 = _list[i].Cut(distribution[i]);
PointX intersection = PointX.getIntersection(t1.topRight, t1.bottomRight, t2.bottomLeft, t2.topLeft);
if (intersection != null)
{
if (t1.topRight.y >= intersection.y)
{
res.Add(t1.topRight);
if (t2.topLeft.y > intersection.y)
{
res.Add(t2.topLeft);
res.Add(intersection);
}
else
{
res.Add(PointX.getPointXByTwoPointXsAndY(t1.topRight, t1.bottomRight, distribution[i]));
}
}
else
{
if (t2.topLeft.y > intersection.y)
{
res.Add(PointX.getPointXByTwoPointXsAndY(t2.bottomLeft, t2.topLeft, distribution[i - 1]));
res.Add(t2.topLeft);
}
else
{
if (t1.topRight.y > t2.topLeft.y)
{
res.Add(t1.topRight);
res.Add(PointX.getPointXByTwoPointXsAndY(t1.topRight, t1.bottomRight, distribution[i]));
}
else if (t1.topRight.y < t2.topLeft.y)
{
res.Add(t2.topLeft);
res.Add(PointX.getPointXByTwoPointXsAndY(t2.bottomLeft, t2.topLeft, distribution[i - 1]));
}
}
}
}
t1 = t2;
}
res.Add(t1.topRight);
res.Add(t1.bottomRight);
return(res);
}
