Ejemplo n.º 1
0
        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);
        }
Ejemplo n.º 2
0
        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()));
                }
            }
        }