/// <summary>
/// Check 3 sensor are in area
/// </summary>
/// <param name="listSensor">input list sensor for check</param>
/// <param name="sensors">return sensor that are in currect position</param>
/// <returns>true if in area</returns>
private bool CheckThreeSensorArea(List<SensorPosition> listSensor, out SensorPosition[] sensors)
{
try
{
int[] zone = new int[4];
List<SensorPosition> sensorlist = new List<SensorPosition>();
//sensors = new SensorPosition[4];
for (int i = 0; i < listSensor.Count; i++)
{
if (listSensor[i].X > 0 && listSensor[i].Z > 0)
{
zone[0] = 1;
//sensors[0] = listSensor[i];
sensorlist.Add(listSensor[i]);
}
else if (listSensor[i].X < 0 && listSensor[i].Z > 0)
{
//sensors[1] = listSensor[i];
sensorlist.Add(listSensor[i]);
zone[1] = 1;
}
else if (listSensor[i].X < 0 && listSensor[i].Z < 0)
{
//sensors[2] = listSensor[i];
sensorlist.Add(listSensor[i]);
zone[2] = 1;
}
else if (listSensor[i].X > 0 && listSensor[i].Z < 0)
{
//sensors[3] = listSensor[i];
sensorlist.Add(listSensor[i]);
zone[3] = 1;
}
}
if (zone[0] + zone[1] + zone[2] + zone[3] >= 3)
{
sensors = sensorlist.ToArray();
return true;
}
else
{
sensors = null;
return false;
}
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
/// Calculate 2 sensor axis z
/// </summary>
/// <param name="sensor1"></param>
/// <param name="sensor2"></param>
/// <param name="inpData"></param>
/// <returns></returns>
private TranslateData CalcTwoSensorAxisZ(SensorPosition sensor1, SensorPosition sensor2, double[] inpData)
{
try
{
TranslateData trandata = new TranslateData();
WayType waytype1 = sensor1.Way;
WayType waytype2 = sensor2.Way;
if (waytype1 == WayType.LEFT || waytype1 == WayType.RIGHT || waytype1 == WayType.INVALID)
{
waytype1 = WayType.UP;
}
if (waytype2 == WayType.LEFT || waytype2 == WayType.RIGHT || waytype2 == WayType.INVALID)
{
waytype2 = WayType.UP;
}
int datapos1 = sensor1.ChNo;
int datapos2 = sensor2.ChNo;
double datavalue1 = DataToRealAxisPlot("Z", inpData[datapos1], waytype1, sensor1.ChNo - 1, sensor1.Target);
double datavalue2 = DataToRealAxisPlot("Z", inpData[datapos2], waytype2, sensor2.ChNo - 1, sensor2.Target);
Vector3D vectorbase = new Vector3D(datavalue2 - datavalue1, sensor1.BaseVector.Y, sensor1.BaseVector.Z);
double anglez = CalcAngleFromAxis("X", vectorbase);
//trandata.OffsetZ = Math.Max(datavalue1, datavalue2);
if (sensor1.Way == WayType.LEFT)
trandata.OffsetZ = datavalue1;
else if (sensor2.Way == WayType.LEFT)
trandata.OffsetZ = datavalue2;
else
trandata.OffsetZ = (datavalue1 + datavalue2) / 2;
trandata.RotationY = anglez - 90;
return trandata;
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
/// Calculate two sensor axis Y
/// </summary>
/// <param name="sensor1"></param>
/// <param name="sensor2"></param>
/// <param name="inpData"></param>
/// <returns></returns>
private TranslateData CalcTwoSensorAxisY(SensorPosition sensor1, SensorPosition sensor2, double[] inpData)
{
try
{
TranslateData trandata = new TranslateData();
int datapos1 = sensor1.ChNo;
int datapos2 = sensor2.ChNo;
double datavalue1 = DataToRealAxisPlot("Y", inpData[datapos1], WayType.INVALID, sensor1.ChNo - 1, sensor1.Target);
double datavalue2 = DataToRealAxisPlot("Y", inpData[datapos2], WayType.INVALID, sensor2.ChNo - 1, sensor2.Target);
Vector3D vectorbase = new Vector3D(sensor1.BaseVector.X, datavalue2 - datavalue1, sensor1.BaseVector.Z);
Vector3D vectorpos = Vector3D.CrossProduct(vectorbase, sensor1.RotationAxis);
double anglex = CalcAngleFromAxis("X", vectorpos);
double anglez = CalcAngleFromAxis("Z", vectorpos);
trandata.OffsetY = Math.Max(datavalue1, datavalue2);
trandata.RotationX = anglez - 90;
trandata.RotationZ = anglex - 90;
return trandata;
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
/// Calculate Translate Plot Z
/// </summary>
/// <param name="senserPos"></param>
/// <param name="inputData"></param>
/// <param name="sensorNo"></param>
/// <returns></returns>
private double CalcTranslatePlotZ(SensorPosition senserPos, double inputData, int sensorNo, int sensorIndex)
{
WayType waytype = senserPos.Way;
if (waytype == WayType.LEFT || waytype == WayType.RIGHT || waytype == WayType.INVALID)
{
waytype = WayType.UP;
}
double outputdata = 0;
if (sensorNo == 1)
{
outputdata = DataToRealAxisPlot("Z", inputData, waytype, sensorIndex, senserPos.Target);
}
return outputdata;
}
/// <summary>
/// Calculate 3 sensor axis Y
/// </summary>
/// <param name="sensors"></param>
/// <param name="inpData"></param>
/// <param name="target"></param>
/// <returns></returns>
private TranslateData CalcThreeSensorAxisY(SensorPosition[] sensors, double[] inpData, TargetType target)
{
try
{
TranslateData trandata = new TranslateData();
int datapos1 = sensors[0].ChNo;
int datapos2 = sensors[1].ChNo;
int datapos3 = sensors[2].ChNo;
double[] datavalues = new double[3];
datavalues[0] = DataToRealAxisPlot("Y", inpData[datapos1], WayType.INVALID, sensors[0].ChNo - 1, sensors[0].Target);
datavalues[1] = DataToRealAxisPlot("Y", inpData[datapos2], WayType.INVALID, sensors[1].ChNo - 1, sensors[1].Target);
datavalues[2] = DataToRealAxisPlot("Y", inpData[datapos3], WayType.INVALID, sensors[2].ChNo - 1, sensors[2].Target);
double datascale = _DataScaleforEachObject[(int)target];
Point3D p1 = new Point3D(sensors[0].X * datascale, datavalues[0], sensors[0].Z * datascale);
Point3D p2 = new Point3D(sensors[1].X * datascale, datavalues[1], sensors[1].Z * datascale);
Point3D p3 = new Point3D(sensors[2].X * datascale, datavalues[2], sensors[2].Z * datascale);
Vector3D v1 = new Vector3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
Vector3D v2 = new Vector3D(p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z);
Vector3D crosssum = Vector3D.CrossProduct(v1, v2);
double anglex = CalcAngleFromAxis("X", crosssum);
double anglez = CalcAngleFromAxis("Z", crosssum);
//trandata.OffsetY = Math.Max(Math.Max(datavalues[0], datavalues[1]), datavalues[2]);
trandata.OffsetY = Math.Min(Math.Min(datavalues[0], datavalues[1]), datavalues[2]);
trandata.RotationX = anglez - 90;
trandata.RotationZ = anglex - 90;
return trandata;
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
/// Create Vector for 2 sensor
/// </summary>
/// <param name="type"></param>
/// <param name="sensor1"></param>
/// <param name="sensor2"></param>
/// <param name="baseVector"></param>
/// <param name="vector90"></param>
private void CreateVectorTwoSensor(string type, SensorPosition sensor1, SensorPosition sensor2, out Vector3D baseVector, out Vector3D vector90)
{
Point3D p1 = new Point3D(sensor1.X, 0, sensor1.Z);
Point3D p2 = new Point3D(sensor2.X, 0, sensor2.Z);
baseVector = new Vector3D();
vector90 = new Vector3D();
if (type == "Y")
{
baseVector = new Vector3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
vector90 = new Vector3D(baseVector.Z, 0, -baseVector.X);
}
else if (type == "X")
{
baseVector = new Vector3D(p2.X - p1.X, 0, 0);
vector90 = new Vector3D(baseVector.Y, -baseVector.X, 0);
}
else if (type == "Z")
{
baseVector = new Vector3D(0, 0, p2.Z - p1.Z);
vector90 = new Vector3D(baseVector.Y, -baseVector.Z, 0);
}
}
