win32.Win32.POINT CalculatePos(float pitch, float yaw)
{
win32.Win32.POINT pos = new win32.Win32.POINT();
if (pitch >= PITCH_CENTER)
{
pos.x = (RES_Y / 2) + (int)((((pitch - PITCH_CENTER) / (RANGE))) * (RES_Y / 2));
}
else
{
pos.x = (int)((((pitch - (MIN_PITCH)) / (RANGE))) * (RES_Y / 2));
}
if (pos.x == 0)
{
pos.x = 1 * (RES_Y / 2);
}
if (flip == 0)
{
if (YAW_CENTER + RANGE > 360) // Case YAW_CENTER is at left of 360
{
if ((Math.Abs(yaw - YAW_CENTER) > RANGE) || (yaw > YAW_CENTER))
{
if (yaw - YAW_CENTER < 0)
{
float temp = 360 - YAW_CENTER + yaw;
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += RES_X / 2;
}
else
{
float temp = yaw - YAW_CENTER;
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += RES_X / 2;
}
}
else
{
float temp = YAW_CENTER - yaw;
pos.y = (int)((1 - (temp / RANGE)) * (RES_X / 2));
}
}
else
{
if ((yaw + YAW_CENTER > 360) || (yaw < YAW_CENTER))
{
if (YAW_CENTER - yaw < 0)
{
float temp = yaw - MIN_YAW;
pos.y = (int)((temp / RANGE) * (RES_X / 2));
}
else
{
float temp = YAW_CENTER - yaw;
pos.y = (int)((1 - (temp / RANGE)) * (RES_X / 2));
}
}
else
{
float temp = yaw - YAW_CENTER;
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += (RES_X / 2);
}
}
}
else if (flip == 1)
{
if ((YAW_CENTER < 360) && (YAW_CENTER > 180)) // In the half left
{
if (YAW_CENTER + RANGE < 360) // Normal
{
if (yaw > YAW_CENTER) // Turn left
{
float temp = Math.Abs(yaw - MAX_YAW);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
}
else if ((yaw < YAW_CENTER) && (yaw > MIN_YAW)) // turn right
{
float temp = Math.Abs(yaw - YAW_CENTER);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += (RES_X / 2);
}
}
else if (YAW_CENTER + RANGE > 360) // out of RANGE
{
if ((yaw < YAW_CENTER) && (yaw > MIN_YAW)) // turn right
{
float temp = Math.Abs(yaw - YAW_CENTER);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += (RES_X / 2);
}
else // turn left
{
if ((yaw < 360) && (yaw > YAW_CENTER)) // still inside [YAW_CENTER, 360]
{
float temp = Math.Abs(yaw - YAW_CENTER);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
}
else // outside [YAW_CENTER, 360]
{
float temp = Math.Abs(360 - YAW_CENTER) + yaw;
pos.y = (int)((temp / RANGE) * (RES_X / 2));
}
}
}
}
else if ((YAW_CENTER > 0) && (YAW_CENTER < 180)) // In the half right
{
if (YAW_CENTER - RANGE > 0) // normal
{
if (yaw > YAW_CENTER) // Turn left
{
float temp = Math.Abs(yaw - MAX_YAW);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
}
else if ((yaw < YAW_CENTER) && (yaw > MIN_YAW)) // turn right
{
float temp = Math.Abs(yaw - YAW_CENTER);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += (RES_X / 2);
}
}
else if (YAW_CENTER - RANGE < 0) // out of RANGE
{
if (yaw > YAW_CENTER) // Turn left
{
float temp = Math.Abs(yaw - YAW_CENTER);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
}
else // Turn right
{
if ((yaw > 0) && (yaw < YAW_CENTER)) // still inside [0, YAW_CENTER]
{
float temp = Math.Abs(yaw - YAW_CENTER);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += (RES_X / 2);
}
else // outside [0, YAW_CENTER]
{
float temp = YAW_CENTER + Math.Abs(360 - yaw);
pos.y = (int)((temp / RANGE) * (RES_X / 2));
pos.y += (RES_X / 2);
}
}
}
}
}
else if (flip == 2)
{
}
return(pos);
}
void m_oWorker_ProgressChanged(object sender, ProgressChangedEventArgs e)
{
if ((data != null))
{
if (data.IndexOf("*") != -1)
{
try
{
pitch = Get_Pitch_val(data);
yaw = Get_Yaw_val(data);
//Invoke(new Action(() => Bankinh.Text = pitch.ToString() + yaw.ToString()));
if ((pitch != -1) && (yaw != -1))
{
got_data = 1;
}
else
{
got_data = 0;
}
}
catch
{
//Invoke(new Action(() => label1.Text = pitch.ToString()));
}
//Data_Output.Text = i.ToString();
if (received_flag == 1)
{
Invoke(new Action(new Action(() => Bankinh.Text = YAW_CENTER.ToString())));
if ((pitch != -1) && (yaw != -1) && (count > 0))
{
win32.Win32.POINT pos = new win32.Win32.POINT();
pos = CalculatePos(pitch, yaw);
Invoke(new Action(new Action(() => Bankinh.Text = pos.y.ToString())));
if (((pos.x - sp.X) > Radius) || ((pos.y - sp.Y) > Radius))
{
if (flip == 1)
{
win32.Win32.ClientToScreen(desktop, ref pos);
win32.Win32.SetCursorPos(pos.y, pos.x);
}
else
{
win32.Win32.ClientToScreen(desktop, ref pos);
win32.Win32.SetCursorPos(RES_Y - pos.y, pos.x);
}
}
ep.X = pos.x;
ep.Y = pos.y;
//Invoke(new Action(new Action(() => mouseLog.Text = pitch.ToString())));
//Invoke(new Action(new Action(() => keyboardLog.Text = pos.x.ToString())));
//xPosLabel.Text = (1 - ((pitch - (MIN_PITCH)) / (PITCH_CENTER - MIN_PITCH))).ToString();
//yPosLabel.Text = (1 - ((pitch - (MIN_PITCH)) / (PITCH_CENTER - MIN_PITCH))).ToString();
//g_desktop.DrawLine(p, sp, ep);
sp = ep;
/**************************************
* Put to Graph
**************************************/
chart1.Series["Series1"].Points.Add(pitch);
chart1.Series["Series2"].Points.Add(yaw);
chart1.Series["Series1"].ChartType = SeriesChartType.Spline;
chart1.Series["Series1"].Color = Color.Red;
chart1.Series["Series2"].ChartType = SeriesChartType.Spline;
chart1.Series["Series2"].Color = Color.Blue;
chart1.Series["Series3"].ChartType = SeriesChartType.Spline;
chart1.Series["Series3"].Color = Color.Yellow;
/**************************************
* Render cube
**************************************/
Render(pitch - PITCH_CENTER, yaw - YAW_CENTER, 0);
}
else if ((count == 0) && (pitch != -1) && (yaw != -1) && (pitch < 90) && (yaw < 300))
{
getMAX_MIN(pitch, yaw);
count = 1;
}
//received_flag = 0;
}
else if ((received_flag == 2))
{
Symbol.axis p = new Symbol.axis();
if (symbol.current_pos == 0)
{
float radius = (float)Math.Sqrt((pitch - PITCH_CENTER) * (pitch - PITCH_CENTER) + (yaw - YAW_CENTER) * (yaw - YAW_CENTER));
symbol.setRADIUS(radius);
p.x = YAW_CENTER;
p.y = PITCH_CENTER;
symbol.setCENTER(p);
Invoke(new Action(() => label1.Text = symbol.point[0].x.ToString()));
}
p.x = yaw;
p.y = pitch;
symbol.add(p);
/******* Check Circle *******/
if (symbol.Check_Circle())
{
//Invoke(new Action(() => Percentage.Text ="Circle " + symbol.PERCENTAGE_CIRCLE.ToString()));
//Invoke(new Action(() => Percentage.Text = "TRUE"));
}
else
{
float a = (float)Math.Pow(symbol.point[symbol.current_pos - 1].x - symbol.CENTRAL_POINT.x, 2);
float b = (float)Math.Pow(symbol.point[symbol.current_pos - 1].y - symbol.CENTRAL_POINT.y, 2);
//Invoke(new Action(() => Percentage.Text = "OUT SIDE"));
//Invoke(new Action(() => Bankinh.Text = (a + b).ToString()));
}
/******* Check Vertical *******/
if (symbol.Check_Vertical() == true)
{
//Invoke(new Action(() => Bankinh.Text = "Vertical " + symbol.PERCENTAGE_VERTICAL.ToString()));
Invoke(new Action(() => label2.Text = "Vertical "));
}
else
{
Invoke(new Action(() => label2.Text = " "));
}
/******* Check Horizontal *******/
if (symbol.Check_Horizontal() == true)
{
//Invoke(new Action(() => Bankinh.Text = "Horizontal " + symbol.PERCENTAGE_HORIZONTAL.ToString()));
Invoke(new Action(() => label3.Text = "Horizontal "));
}
else
{
Invoke(new Action(() => label3.Text = " "));
}
}
else if (received_flag == 3)
{
float max = symbol.PERCENTAGE_CIRCLE;
if (max < symbol.PERCENTAGE_HORIZONTAL)
{
max = symbol.PERCENTAGE_HORIZONTAL;
Invoke(new Action(() => label2.Text = "horizontal detected"));
}
if (max < symbol.PERCENTAGE_VERTICAL)
{
max = symbol.PERCENTAGE_VERTICAL;
Invoke(new Action(() => label3.Text = "vertical detected"));
}
Invoke(new Action(() => label1.Text = max.ToString()));
received_flag = 1;
}
data = "";
Invoke(new Action(() => label1.Text = symbol.point[0].x.ToString()));
}
}
}
