private static void FilterCenters(Point3D[] aCenterLine)
{
var newCenters = new Point3D[aCenterLine.Length];
for (var i = 0; i < aCenterLine.Length; ++i) {
var sumX = 0.0;
var sumY = 0.0;
var pointCount = 0;
for (var j = i - WINDOW_SIZE; j <= i + WINDOW_SIZE; ++j) {
if (j >= 0 && j < aCenterLine.Length) {
sumX += aCenterLine[j].X;
sumY += aCenterLine[j].Y;
pointCount++;
}
}
newCenters[i] = new Point3D {
X = sumX / pointCount,
Y = sumY / pointCount
};
}
for (var i = 0; i < aCenterLine.Length; ++i) {
aCenterLine[i].X = newCenters[i].X;
aCenterLine[i].Y = newCenters[i].Y;
}
}
public static Point3D[] RotateY(Point3D[] points3D, double degrees)
{
for (int i = 0; i < points3D.Length; i++) {
points3D[i] = RotateY(points3D[i], degrees);
}
return points3D;
}
private static void FilterLeftView(ref Point3D[] aLine)
{
if (aLine == null) {
return;
}
var startDeletetIndex = int.MinValue;
for (var i = MAX_POINT_COUNT; i > 0 && i < aLine.Length; --i) {
if (IsSplash(aLine[i - 1].Z, aLine[i - 1].Y, aLine[i].Z, aLine[i].Y)) {
startDeletetIndex = i;
break;
}
}
var endDeletetIndex = int.MaxValue;
for (var i = aLine.Length - MAX_POINT_COUNT; i > 0 && i < aLine.Length; ++i) {
if (IsSplash(aLine[i - 1].Z, aLine[i - 1].Y, aLine[i].Z, aLine[i].Y)) {
endDeletetIndex = i - 1;
break;
}
}
if (startDeletetIndex != int.MinValue || endDeletetIndex != int.MaxValue) {
var points = new List<Point3D>();
for (var i = 0; i < aLine.Length; ++i) {
if (i >= startDeletetIndex && i <= endDeletetIndex) {
points.Add(aLine[i]);
}
}
aLine = points.ToArray();
}
}
private static void FilterHorizontLine(Point3D[] aLine, int[] aSegments)
{
if (aLine == null) {
return;
}
for (var s = 0; s < aSegments.Length / 2; ++s) {
var from = aSegments[s * 2];
var to = aSegments[s * 2 + 1];
if (to - from >= WINDOW_SIZE * 2 + 1) {
var averages = new double[to - from + 1];
var index = 0;
for (var i = from; i <= to; ++i) {
var summed = 0.0;
var count = 0;
for (var j = i - WINDOW_SIZE; j <= i + WINDOW_SIZE; ++j) {
if (j >= 0 && j < aLine.Length && j >= from && j <= to) {
summed += aLine[j].X;
count++;
}
}
averages[index++] = summed / count;
}
index = 0;
for (var i = from; i <= to; ++i) {
aLine[i].X = averages[index++];
}
}
}
}
private static bool IsBump(Point3D aA, Point3D aB) {
var distance = Math.Sqrt((aA.X-aB.X)* (aA.X - aB.X) + (aA.Y - aB.Y) * (aA.Y - aB.Y));
if (distance > MAX_RADIUS) {
return true;
}
return false;
}
private static void FilterBumps(Point3D[] aCenterLine) {
for (var i = 0; i < aCenterLine.Length - 1; ++i) {
if (IsBump(aCenterLine[i], aCenterLine[i + 1])) {
var dx = aCenterLine[i].X - aCenterLine[i + 1].X;
var dy = aCenterLine[i].Y - aCenterLine[i + 1].Y;
SmoothBump(aCenterLine, i + 1, dx, dy);
}
}
}
public double CalculateValue(SlabModelImpl aSlabModel)
{
if (aSlabModel == null) {
throw new ArgumentNullException("TopCurvatureAlgorithm, CalculateValue: Модель слитка равна null.");
}
if (aSlabModel.CenterLine == null) {
throw new ArgumentNullException("TopCurvatureAlgorithm, CalculateValue: Центральные точки равны null.");
}
if (aSlabModel.Diameters == null) {
throw new ArgumentNullException("TopCurvatureAlgorithm, CalculateValue: Диаметры слитка равны null.");
}
if (aSlabModel.Diameters.Length != aSlabModel.CenterLine.Length) {
throw new ArgumentNullException("TopCurvatureAlgorithm, CalculateValue: Количество центральных точек не равно количеству точек диаметра.");
}
var xpoints = new double[aSlabModel.CenterLine.Length];
var ypoints = new double[aSlabModel.CenterLine.Length];
var points3d = new Point3D[aSlabModel.CenterLine.Length];
for (var i = 0; i < aSlabModel.CenterLine.Length; ++i) {
xpoints[i] = aSlabModel.CenterLine[i].Z;
ypoints[i] = aSlabModel.CenterLine[i].Y + aSlabModel.Diameters[i] / 2.0;
points3d[i] = new Point3D {
X = xpoints[i],
Y = ypoints[i],
Z = 0
};
}
// Строим выпуклую оболочку из имеющихся точек.
var saddlePoints = ConvexHull.Build(xpoints, ypoints, 1);
var leftSaddlePoint = 0;
var maxCurvature = double.MinValue;
for (var i = 1; i < points3d.Length - 1; ++i) {
if (i == saddlePoints[leftSaddlePoint + 1]) {
maxCurvature = Math.Max(maxCurvature, 0);
leftSaddlePoint++;
} else {
var distance = points3d[i].DistanceToLine(
points3d[saddlePoints[leftSaddlePoint]],
points3d[saddlePoints[leftSaddlePoint + 1]]);
maxCurvature = Math.Max(maxCurvature, distance);
}
}
return Math.Round(maxCurvature, 4, MidpointRounding.ToEven);
}
private static void FilterLeftView(ref Point3D[] aLine)
{
if (aLine == null) {
return;
}
for (var i = 0; i < aLine.Length - 2; ++i) {
var a = aLine[i];
var b = aLine[i + 1];
var c = aLine[i + 2];
if (IsPick(a.Y, b.Y, c.Y)) {
b.Y = (a.Y + c.Y) / 2;
}
}
}
private static void FilterTopView(ref Point3D[] aLine)
{
if (aLine == null) {
return;
}
for (var i = 0; i < aLine.Length - 2; ++i) {
var a = aLine[i];
var b = aLine[i + 1];
var c = aLine[i + 2];
if (IsPick(a.X, b.X, c.X)) {
b.X = (a.X + c.X) / 2;
}
}
}
public static PointF Convert3DTo2D(Point3D aPoint, Camera aCamera, double aZoom, PointF aSenter)
{
var resPoint = new PointF();
if (aPoint.Z >= aCamera.Position.Z) {
resPoint.X = (float)((aPoint.X - aCamera.Position.X) / (-0.1f) * aZoom) + aSenter.X;
resPoint.Y = (float)(-(aPoint.Y - aCamera.Position.Y) / (-0.1f) * aZoom) + aSenter.Y;
}
else {
resPoint.X = (float)(-(aPoint.X - aCamera.Position.X) /
(aPoint.Z - aCamera.Position.Z) * aZoom + aSenter.X);
resPoint.Y = (float)((aPoint.Y - aCamera.Position.Y) /
(aPoint.Z - aCamera.Position.Z) * aZoom + aSenter.Y);
}
return resPoint;
}
public static Point3D RotateZ(Point3D point3D, double degrees)
{
//Z-axis
//[ cos(x) sin(x) 0]
//[ -sin(x) cos(x) 0]
//[ 0 0 1]
double cDegrees = (Math.PI * degrees) / 180.0;
double cosDegrees = Math.Cos(cDegrees);
double sinDegrees = Math.Sin(cDegrees);
double x = (point3D.X * cosDegrees) + (point3D.Y * sinDegrees);
double y = (point3D.X * -sinDegrees) + (point3D.Y * cosDegrees);
return new Point3D(x, y, point3D.Z);
}
public static Point3D RotateY(Point3D point3D, double degrees)
{
//Y-axis
//[ cos(x) 0 sin(x)]
//[ 0 1 0 ]
//[-sin(x) 0 cos(x)]
double cDegrees = (Math.PI * degrees) / 180.0;
double cosDegrees = Math.Cos(cDegrees);
double sinDegrees = Math.Sin(cDegrees);
double x = (point3D.X * cosDegrees) + (point3D.Z * sinDegrees);
double z = (point3D.X * -sinDegrees) + (point3D.Z * cosDegrees);
return new Point3D(x, point3D.Y, z);
}
/// <summary>
/// Считает расстояние от прямой aP - aQ до текущей точки.
/// </summary>
/// <param name="aP"></param>
/// <param name="aQ"></param>
/// <returns></returns>
public double DistanceToLine(Point3D aP, Point3D aQ)
{
// A, B, c - коэфициенты прямой.
var a = aQ.X - aP.X;
var b = aQ.Y - aP.Y;
var c = aQ.Z - aP.Z;
var x0 = aP.X;
var y0 = aP.Y;
var z0 = aP.Z;
var numerator = Math.Pow(b * (X - x0) - a * (Y - y0), 2) +
Math.Pow(c * (Y - y0) - b * (Z - z0), 2) +
Math.Pow(c * (X - x0) - a * (Z - z0), 2);
var distance = Math.Sqrt(numerator) / Math.Sqrt(a * a + b * b + c * c);
return distance;
}
public static Point3D RotateX(Point3D point3D, double degrees)
{
//[ a b c ] [ x ] [ x*a + y*b + z*c ]
//[ d e f ] [ y ] = [ x*d + y*e + z*f ]
//[ g h i ] [ z ] [ x*g + y*h + z*i ]
//[ 1 0 0 ]
//[ 0 cos(x) sin(x)]
//[ 0 -sin(x) cos(x)]
double cDegrees = (Math.PI * degrees) / 180.0f;
double cosDegrees = Math.Cos(cDegrees);
double sinDegrees = Math.Sin(cDegrees);
double y = (point3D.Y * cosDegrees) + (point3D.Z * sinDegrees);
double z = (point3D.Y * -sinDegrees) + (point3D.Z * cosDegrees);
return new Point3D(point3D.X, y, z);
}
private int GetLowerBoundPoint(Point3D[] aMassive, double aZ)
{
if (aMassive == null) {
throw new ArgumentNullException("aMassive");
}
if (aZ < aMassive[0].Z) {
return 0;
}
if (aZ > aMassive[aMassive.Length - 1].Z) {
return aMassive.Length - 1;
}
// TODO: Сделать бинарный поиск.
for (var i = 0; i < aMassive.Length -1; ++i) {
if (aMassive[i].Z <= aZ && aMassive[i + 1].Z > aZ) {
return i;
}
}
throw new ArgumentException("Непонятная ситуация: не найдена точка.");
}
private static double DyDivDz(Point3D aPointA, Point3D aPointB)
{
return (aPointA.Y - aPointB.Y)/(aPointA.Z - aPointB.Z);
}
private static void SmoothBump(Point3D[] aCenterLine, int aStartPosition, double aDx, double aDy) {
for (var i = aStartPosition; i < aCenterLine.Length; ++i) {
aCenterLine[i].X += aDx;
aCenterLine[i].Y += aDy;
}
}
private int GetLowerBoundLineByY(Point3D[][] aLines, double aY)
{
if (aLines == null) {
throw new ArgumentNullException("aLines");
}
if (aLines[0][0].Y >= aY) {
return 0;
}
if (aLines[aLines.Length - 1][0].Y <= aY) {
return aLines.Length - 1;
}
for (var i = 0; i < aLines.Length - 1; ++i) {
if (aLines[i][0].Y <= aY && aLines[i + 1][0].Y > aY) {
return i;
}
}
throw new ArgumentException("Непонятная ситуация: нет подходящей линии.");
}
private Point3D[] BuildTopSideValues(
ISensorValueInfo[] aPositions,
ISensorValueInfo[] aSensorValues,
double aShift,
double aCenterDistance)
{
var result = new Point3D[aSensorValues.Length];
for (var i = 0; i < aSensorValues.Length; ++i) {
result[i] = new Point3D {
X = aShift,
Y = aCenterDistance - aSensorValues[i].GetValue(),
Z = GetPositionByTime(aPositions, aSensorValues[i].GetTime())
};
}
return result;
}
private static double DxDivDz(Point3D aPointA, Point3D aPointB)
{
return (aPointA.X - aPointB.X) / (aPointA.Z - aPointB.Z);
}
private void MoveToOnePlainZ(ref int[] aIndexes, Point3D[][] aLines)
{
while (!IsOnOnePalneZ(aLines[0][aIndexes[0]], aLines[1][aIndexes[1]], aLines[2][aIndexes[2]])) {
var a = aLines[0][aIndexes[0]];
var b = aLines[1][aIndexes[1]];
var c = aLines[2][aIndexes[2]];
if (a.Z < b.Z || a.Z < c.Z) {
aIndexes[0]++;
} else if (b.Z < a.Z || b.Z < c.Z) {
aIndexes[1]++;
} else if (c.Z < a.Z || c.Z < b.Z) {
aIndexes[2]++;
}
}
}
/// <summary>
/// Вычисляет диаметр описанной окружности.
/// </summary>
/// <param name="aA"></param>
/// <param name="aB"></param>
/// <param name="aC"></param>
/// <returns>Диаметр.</returns>
private double CalcCircleDiameter(Point3D aA, Point3D aB, Point3D aC)
{
// Формула:
// D = a*b*c / 2 * sqrt(p*(p-a)*(p-b)*(p-c)),
// где a,b,c - длины сторон треугольника, p - полупериметр треугольника.
var a = aA.DistanceToPoint(aB);
var b = aB.DistanceToPoint(aC);
var c = aC.DistanceToPoint(aA);
var p = (a + b + c) / 2.0;
return (a * b * c) / (2 * Math.Sqrt(p * (p - a) * (p - b) * (p - c)));
}
/// <summary>
/// Строит линию центров слитка.
/// </summary>
/// <param name="aModel"></param>
private void BuildCenters(SlabModelImpl aModel)
{
// теперь по 3-м точкам будем строить описанную окружность и вычислять ее центр и диаметр.
var centers = new List<Point3D>();
var diameters = new List<double>();
var lines = new Point3D[3][];
lines[0] = aModel.TopSensorLine;
lines[1] = aModel.BottomSensorLine;
lines[2] = aModel.LeftSensorLine;
var indexes = new int[3] { 0, 0, 0 };
MoveToOnePlainZ(ref indexes, lines); // двигает границы массивов до тех пор, пока все точки не станут в одной плоскости.
var pointsCount = int.MaxValue;
for (var i = 0; i < 3; ++i) {
pointsCount = Math.Min(pointsCount, lines[i].Length - indexes[i]);
}
for (var i = 0; i < pointsCount; ++i) {
var a = indexes[0];
var b = indexes[1];
var c = indexes[2];
var center = CalcCircleCenter(lines[0][a], lines[1][b], lines[2][c]);
var diameter = CalcCircleDiameter(lines[0][a], lines[1][b], lines[2][c]);
centers.Add(center);
diameters.Add(diameter);
indexes[0]++;
indexes[1]++;
indexes[2]++;
}
aModel.CenterLine = centers.ToArray();
aModel.Diameters = diameters.ToArray();
}
private static void FilterBump(Point3D[] aLine, int aStart, double aBump)
{
for (var i = aStart; i < aLine.Length; ++i) {
aLine[i].Y += aBump;
}
}
/// <summary>
/// Вычисляет центр описанной окружности.
/// </summary>
/// <returns></returns>
private Point3D CalcCircleCenter(Point3D aA, Point3D aB, Point3D aC)
{
// формула http://www.cyberforum.ru/geometry/thread1190053.html
double x1 = aA.X, x2 = aB.X, x3 = aC.X,
y1 = aA.Y, y2 = aB.Y, y3 = aC.Y;
double x12 = x1 - x2,
x23 = x2 - x3,
x31 = x3 - x1,
y12 = y1 - y2,
y23 = y2 - y3,
y31 = y3 - y1;
double z1 = x1 * x1 + y1 * y1,
z2 = x2 * x2 + y2 * y2,
z3 = x3 * x3 + y3 * y3;
double zx = y12 * z3 + y23 * z1 + y31 * z2,
zy = x12 * z3 + x23 * z1 + x31 * z2,
z = x12 * y31 - y12 * x31;
return new Point3D {
X = -zx / (2.0 * z),
Y = zy / (2.0 * z),
Z = aA.Z
};
}
private static double GetBump(Point3D aA, Point3D aB)
{
return aA.Y - aB.Y;
}
private bool IsOnOnePalneZ(Point3D aA, Point3D aB, Point3D aC)
{
var epsilon = 0.0001;
if (Math.Abs(aA.Z - aB.Z) > epsilon || Math.Abs(aA.Z - aC.Z) > epsilon) {
return false;
}
return true;
}
public double DistanceToPoint(Point3D aPoint)
{
return Math.Sqrt((aPoint.X - X) * (aPoint.X - X) +
(aPoint.Y - Y) * (aPoint.Y - Y) +
(aPoint.Z - Z) * (aPoint.Z - Z));
}
private void MoveToZeroOnZ(Point3D[] aValues)
{
var difference = aValues[0].Z;
for (var i = 0; i < aValues.Length; ++i) {
aValues[i].Z -= difference;
}
}
private static int[] FindVerticalSegments(Point3D[] aLine)
{
var segments = new List<int>();
for (var i = 0; i < aLine.Length; ++i) {
var difLeft = false;
var difRight = false;
if (i == 0 || Math.Abs(DyDivDz(aLine[i], aLine[i - 1])) >= MIN_SPEED) {
difLeft = true;
}
if (i == aLine.Length - 1 || Math.Abs(DyDivDz(aLine[i], aLine[i + 1])) >= MIN_SPEED) {
difRight = true;
}
if (difRight ^ difLeft) {
segments.Add(i);
}
}
return segments.ToArray();
}