public static MyPoint FindPointWithAngleRight(List <MyPoint> data, int step = 30, int count = 5)
{
List <ArrData> RightData = new List <ArrData>();
for (int i = data.Count - 1; i > 0; i -= step)
{
RightData.Add(new ArrData()
{
FirstIndex = data.Count - 1,
SecondIndex = i,
Difference = Math.Atan2(data[i].Z - data[data.Count - 1].Z, data[i].X - data[data.Count - 1].X) * 180 / Math.PI
});
}
if (RightData.Count == 0)
{
return(new MyPoint()
{
X = 0, Z = 0
});
}
int tempCount = 0;
ArrData tempAngle = RightData[0];
int leftIndex = 0;
int rightIndex = 0;
for (int i = 0; i < RightData.Count; i++)
{
//if (LeftData[i].Difference * tempAngle.Difference < 0) {
if (RightData[i].Difference < 0)
{
tempCount++;
}
else
{
if (tempCount >= count)
{
leftIndex = tempAngle.SecondIndex;
rightIndex = RightData[i].SecondIndex;
break;
}
else
{
tempAngle = RightData[i];
tempCount = 0;
}
}
}
MyPoint result = new MyPoint()
{
X = (data[leftIndex].X + data[rightIndex].X) / 2, Z = (data[leftIndex].Z + data[rightIndex].Z) / 2
};
int pCountOnLine = 0;
for (int i = 0; i < data.Count; i++)
{
if (data[i].X <data[rightIndex].X && data[i].X> data[leftIndex].X)
{
if (Math.Abs(data[i].Z - result.Z) < 2)
{
pCountOnLine++;
}
}
}
if (pCountOnLine < 20)
{
return(result);
}
//Console.WriteLine($"2 {data[rightIndex].Z} {data[leftIndex].Z}");
return(new MyPoint()
{
X = 0, Z = 0
});
// return new MyPoint() { X = (data[leftIndex].X + data[rightIndex].X) / 2, Z = (data[leftIndex].Z + data[rightIndex].Z) / 2 };
}
public static MyPoint FindPointWithAngle(List <MyPoint> data, int step = 30, int count = 5)
{
//первая точка с последующими по углу
List <ArrData> LeftData = new List <ArrData>();
List <ArrData> RightData = new List <ArrData>();
for (int i = 0; i < data.Count; i += step)
{
LeftData.Add(new ArrData()
{
FirstIndex = 0,
SecondIndex = i,
Difference = Math.Atan2(data[i].Z - data[0].Z, data[i].X - data[0].X) * 180 / Math.PI
});
}
//for (int i = data.Count - 1; i > 0; i -= step) {
// RightData.Add(new ArrData() {
// FirstIndex = data.Count - 1,
// SecondIndex = i,
// Difference = -Math.Atan2(data[i].Z - data[data.Count - 1].Z, data[i].X - data[data.Count - 1].X) * 180 / Math.PI
// });
//}
//Console.Clear();
//Console.WriteLine($"LeftData Count {LeftData.Count} :");
//foreach (ArrData item in LeftData) {
// Console.WriteLine($"{item.FirstIndex} \t {item.SecondIndex} \t {item.Difference}");
//}
//Console.WriteLine($"||||||||||||||||||||||||||||");
//Console.WriteLine($"RightData Count {RightData.Count} :");
//foreach (ArrData item in RightData) {
// Console.WriteLine($"{item.FirstIndex} \t {item.SecondIndex} \t {item.Difference}");
//}
//Console.WriteLine($"||||||||||||||||||||||||||||");
int tempCount = 0;
ArrData tempAngle = LeftData[0];
int leftIndex = 0;
int rightIndex = 0;
for (int i = 0; i < LeftData.Count; i++)
{
//if (LeftData[i].Difference * tempAngle.Difference < 0) {
if (LeftData[i].Difference < 0)
{
tempCount++;
}
else
{
if (tempCount >= count)
{
leftIndex = tempAngle.SecondIndex;
rightIndex = LeftData[i].SecondIndex;
break;
}
else
{
tempAngle = LeftData[i];
tempCount = 0;
}
}
}
if (leftIndex == 0 && rightIndex == 0)
{
//Console.WriteLine($"1 {data[rightIndex].Z} {data[leftIndex].Z}");
return(FindPointWithAngleRight(data, step, count));
}
MyPoint result = new MyPoint()
{
X = (data[leftIndex].X + data[rightIndex].X) / 2, Z = (data[leftIndex].Z + data[rightIndex].Z) / 2
};
int pCountOnLine = 0;
for (int i = 0; i < data.Count; i++)
{
if (data[i].X <data[leftIndex].X && data[i].X> data[rightIndex].X)
{
if (Math.Abs(data[i].Z - result.Z) < 2)
{
pCountOnLine++;
}
}
}
if (pCountOnLine < 20)
{
return(result);
}
//else {
// // Console.WriteLine($"11 {data[rightIndex].Z} {data[leftIndex].Z}");
// return FindPointWithAngleRight(data, step, count);
//}
return(new MyPoint()
{
X = 0, Z = 0
});
}
//относительная
public void ShowInfo3()
{
_Point tempPoint = new _Point();
bool yes = false;
while (true)
{
double[] X; // = new double[1];
double[] Z; // = new double[1];
Laser.GetProfile(out X, out Z);
List <MyPoint> data = Calculate.ZeroZ(X, Z);
if (_R.Recive_data != "" && _R.Recive_data != null)
{
// Dispatcher.Invoke(() => tb_ReciveData.Text = _R.Recive_data);
MyPoint p = new MyPoint()
{
X = 0, Z = 0
};
if (data.Count != 0)
{
p = Calculate.FindPointWithAngle(data, 10, 5);
}
_R.RX = MyXML.GetValues(_R.Recive_data, "X");
_R.RY = MyXML.GetValues(_R.Recive_data, "Y");
_R.RZ = MyXML.GetValues(_R.Recive_data, "Z");
_R.RA = MyXML.GetValues(_R.Recive_data, "A");
_R.RB = MyXML.GetValues(_R.Recive_data, "B");
_R.RC = MyXML.GetValues(_R.Recive_data, "C");
_Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 76.97, p.X + 7.51, p.Z - 350 - 5.87);
//if (pTrans.X != 0) {
// if (CurData.Count == 0) {
// CurData.Add(new _Point() {
// X = pTrans.X,
// Y = pTrans.Y,
// Z = pTrans.Z
// });
// Dispatcher.Invoke(() => map.LPoint(pTrans.X, pTrans.Y));
// } else {
// double yDif = 2; // разница по игреку меду соседними точками при записи траэктории
// if (Math.Abs(pTrans.Y - CurData[CurData.Count - 1].Y) < yDif) {
// //Console.WriteLine($"{Math.Abs(pTrans.Y - CurData[CurData.Count - 1].Y)}");
// CurData.Add(new _Point() {
// X = pTrans.X,
// Y = pTrans.Y,
// Z = pTrans.Z
// });
// Dispatcher.Invoke(() => map.LPoint(pTrans.X, pTrans.Y));
// }
// }
if (pTrans.X != 0)
{
if (!yes)
{
tempPoint = new _Point()
{
X = pTrans.X,
Y = pTrans.Y,
Z = pTrans.Z
};
yes = !yes;
}
else
{
double yDif = 5; // разница по игреку меду соседними точками при записи траэктории
if (Math.Abs(pTrans.Y - tempPoint.Y) < yDif && Math.Abs(pTrans.X - tempPoint.X) >= 1)
{
//Console.WriteLine($"{Math.Abs(pTrans.Y - CurData[CurData.Count - 1].Y)}");
_Point temp = Calculate.CalcPoint(tempPoint, pTrans);
CurData.Add(temp);
Dispatcher.Invoke(() => map.LPoint(temp.X, temp.Y));
tempPoint = pTrans;
Console.WriteLine($"X = {temp.X} Y = {temp.Y} Z = {temp.Z} ");
}
}
//Dispatcher.Invoke(() => map.LPoint(pTrans.X, pTrans.Y));
CurData.OrderBy(x => x.X);
if (CurData.Count > 2)
{
CurData = Calculate.UsredMap(CurData);
}
}
#region
//double rx = MyXML.GetValues(_R.Recive_data, "X");
//double ry = MyXML.GetValues(_R.Recive_data, "Y");
//double rz = MyXML.GetValues(_R.Recive_data, "Z");
//Dispatcher.Invoke(() => map.RPoint(rx, ry));
//#region
////Console.WriteLine($"X = {rx} Y = {ry} Z = {rz} ");
//int index = 0;
////for (int i = 0; i < CurData.Count; i++) {
//// if (CurData[i].X > rx) {
//// index = i;
//// break;
//// }
////}
//double curY = 0;
//double curZ = 0;
////шаг - максимальная корекция за один пакет
//double step = 0.08;
//if (CurData.Count > 0) {
// while (index < CurData.Count && rx > CurData[index].X) {
// index++;
// }
// if (index < CurData.Count) {
// curY = CurData[index].Y - ry;
// curZ = CurData[index].Z - rz;
// }
// // Console.WriteLine($"Index = {index}");
//}
//if (Math.Abs(curY) > step) {
// if (curY * -1 < 0) {
// curY = step;
// } else {
// curY = -step;
// }
//}
//if (Math.Abs(curZ) > step) {
// if (curZ * -1 < 0) {
// curZ = step/2;
// } else {
// curZ = -step/2;
// }
//}
//_R.CurY = curY;
//_R.CurZ = curZ;
//// _R.CurX = 0.2;
////Console.WriteLine($"Y = {_R.CurY } Z = {_R.CurZ }");
//if (_INDX < 100) {
// _R.CurX = 1;
// _INDX++;
//}
//_R.isCur = true;
//#endregion
#endregion
}
}
}
//абсолютная
public void ShowInfo4()
{
while (true)
{
double[] X; // = new double[1];
double[] Z; // = new double[1];
Laser.GetProfile(out X, out Z);
List <MyPoint> data = Calculate.ZeroZ(X, Z);
MyPoint p = new MyPoint()
{
X = 0, Z = 0
};
if (data.Count != 0)
{
p = Calculate.FindPointWithAngle(data, 10, 5);
}
_Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 0, p.X, p.Z - 350);
if (_R.Recive_data != "" && _R.Recive_data != null)
{
//double rx = MyXML.GetValues(_R.Recive_data, "X");
//double ry = MyXML.GetValues(_R.Recive_data, "Y");
//double rz = MyXML.GetValues(_R.Recive_data, "Z");
_R.RX = MyXML.GetValues(_R.Recive_data, "X");
_R.RY = MyXML.GetValues(_R.Recive_data, "Y");
_R.RZ = MyXML.GetValues(_R.Recive_data, "Z");
_R.RA = MyXML.GetValues(_R.Recive_data, "A");
_R.RB = MyXML.GetValues(_R.Recive_data, "B");
_R.RC = MyXML.GetValues(_R.Recive_data, "C");
_R.CurY = _R.RY;
_R.CurZ = _R.RZ;
_R.CurX = _R.RX;
_R.CurA = _R.RA;
_R.CurB = _R.RB;
_R.CurC = _R.RC;
if (prevIpoc != _R.Ipoc)
{
_INDX++;
prevIpoc = _R.Ipoc;
}
if (_INDX >= 10)
{
_INDX = 0;
int index = 0;
while (_R.RX > CurData[index].X && index < CurData.Count)
{
index++;
}
_R.CurY = CurData[index].Y;
_R.CurZ = CurData[index].Z;
_R.CurX = CurData[index].X;
}
}
}
#region
//_R.RX = MyXML.GetValues(_R.Recive_data, "X");
//_R.RY = MyXML.GetValues(_R.Recive_data, "Y");
//_R.RZ = MyXML.GetValues(_R.Recive_data, "Z");
//_R.RA = MyXML.GetValues(_R.Recive_data, "A");
//_R.RB = MyXML.GetValues(_R.Recive_data, "B");
//_R.RC = MyXML.GetValues(_R.Recive_data, "C");
//CurData.OrderBy(x => x.X);
//if (_INDX < CurData.Count) {
// if (_R.isCur == false) {
// Console.WriteLine($"Index = {_INDX }");
// _R.CurY = CurData[_INDX].Y;
// _R.CurZ = CurData[_INDX].Z;
// _R.CurX = CurData[_INDX].X;
// _R.CurA = _R.RA;
// _R.CurB = _R.RB;
// _R.CurC = _R.RC;
// _R.isCur = true;
// _INDX++;
// }
//}
//if (_R.isCur == false) {
// _R.CurY = _R.RY;
// _R.CurZ = _R.RZ;
// _R.CurX = _R.RX;
// _R.CurA = _R.RA;
// _R.CurB = _R.RB;
// _R.CurC = _R.RC;
// _R.isCur = true;
//}
#endregion
}
//вывод данных на экран
public void ShowInfo()
{
while (true)
{
Dispatcher.Invoke(() => {
double[] X; // = new double[1];
double[] Z; // = new double[1];
Laser.GetProfile(out X, out Z);
List <MyPoint> data = Calculate.ZeroZ(X, Z);
_V.SetData(data);
_V.ReDraw();
MyPoint p = new MyPoint()
{
X = 0, Z = 0
};
if (data.Count != 0)
{
p = Calculate.FindPointWithAngle(data, 10, 5);
//MyPoint p2 = Calculate.FindPointWithAngleRight(data, 10, 5);
//_V.DrawPoint(p2, new SolidColorBrush(Colors.Black));
}
_V.DrawPoint(p);
tb_ReciveData.Text = _R.Recive_data;
tb_SendData.Text = _R.Send_data;
if (_R.Recive_data != "" && _R.Recive_data != null)
{
_R.RX = MyXML.GetValues(_R.Recive_data, "X");
_R.RY = MyXML.GetValues(_R.Recive_data, "Y");
_R.RZ = MyXML.GetValues(_R.Recive_data, "Z");
_R.RA = MyXML.GetValues(_R.Recive_data, "A");
_R.RB = MyXML.GetValues(_R.Recive_data, "B");
_R.RC = MyXML.GetValues(_R.Recive_data, "C");
}
//Console.Clear();
//Console.WriteLine(p.Z);
//_Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 0, p.X, p.Z - 350);
//Console.WriteLine($"X = {pTrans.X} Y = {pTrans.Y} Z = {pTrans.Z}");
//if (_R.Ipoc != null && Convert.ToInt64(_R.Ipoc) % 10 == 0 ) {
// //Currection.WriteToFile("temp.txt", pTrans);
// //Console.Clear();
// //Console.WriteLine(_R.Ipoc);
//}
// Console.WriteLine()
#region
//data = Calculate.Usred(data);
//MyPoint p = Calculate.FindSpad(data);
//if (p.Z != 0) {
// MessageBox.Show("dsad");
//}
//_V.SetData(data);
//_V.ReDraw();
//_V.DrawPoint(p);
//MyPoint p1 = Calculate.FindSpad3(data, 0);
//MyPoint p2 = Calculate.FindSpad3(data, 0);
//MyPoint p3 = Calculate.FindSpad3(data, 0);
//MyPoint p4 = Calculate.FindSpad3(data, 0);
//_V.DrawPoint(p1 ,new SolidColorBrush(Colors.Black));
//_V.DrawPoint(p2, new SolidColorBrush(Colors.DarkBlue));
//_V.DrawPoint(p3, new SolidColorBrush(Colors.Green));
//_V.DrawPoint(p4, new SolidColorBrush(Colors.Pink));
#endregion
});
Thread.Sleep(100);
}
}