Пример #1
0
        private void SendControlParameters(List <Point3D> ViewPoint, byte[, ,] ImgDataToFlash)
        {
            List <Point3D> ViewDescriptor = new List <Point3D>();

            if (GlobalProperties.ViewPointMode)
            {
                ViewDescriptor.Add(ViewPoint[0]);
                ViewDescriptor.Add(ViewPoint[1]);
                ViewDescriptor.Add(ViewPoint[2]);
            }

            Visualization.ProjectDescriptor(ViewPoint, CenterProjXY, imgR, FrameRows);
            List <Point3D> currentOrientation = calibrateModule.RecalcOrientation(ViewPoint);

            if (GlobalProperties.ViewPointMode)
            {
                if (checkBoxPan.Checked)
                {
                    ViewDescriptor.Add(currentOrientation[0].Clone());
                }
                else
                {
                    ViewDescriptor.Add(new Point3D(0, 0, 0));
                }

                ViewDescriptor.Add(currentOrientation[1]);
                ViewDescriptor.Add(currentOrientation[2]);
                ViewDescriptor.Add(currentOrientation[3]);
            }

            // Recalibrate every frame
            if (calibrateModule.p0translation != null)
            {
                // Move current frame with translation of calibration frame
                Geometry.p_add(currentOrientation, calibrateModule.p0translation, -1);

                // Rotation of whole current coordinate system with parameters of calibration
                Geometry.p_rotY(currentOrientation, calibrateModule.ySinCosAxis, -1);
                Geometry.p_rotZ(currentOrientation, calibrateModule.zSinCosAxis, -1);
                Geometry.p_rotX(currentOrientation, calibrateModule.xSinCosAxis, -1);

                // Move back current frame with translation of calibration frame
                Geometry.p_add(currentOrientation, calibrateModule.p0translation, 1);
                CalibrationCopy = Geometry.CloneList(calibrateModule.CalibrationOrientation[0]);


                // Move current frame to origin and calculate align parameters
                TranslationCurr = currentOrientation[0].Clone();
                Geometry.p_add(currentOrientation, currentOrientation[0].Clone(), -1);
                currParam = Geometry.align_calc_pars(
                    currentOrientation[0],
                    currentOrientation[1],
                    currentOrientation[2]);

                // Rotate Current frame coordinate system to coincide with XYZ
                ySinCosCurr = Geometry.calc_rotY(currentOrientation[1]);
                Geometry.p_rotY(currentOrientation, ySinCosCurr, -1);

                zSinCosCurr = Geometry.calc_rotZ(currentOrientation[1]);
                Geometry.p_rotZ(currentOrientation, zSinCosCurr, -1);

                xSinCosCurr = Geometry.calc_rotX(currentOrientation[2]);
                Geometry.p_rotX(currentOrientation, xSinCosCurr, -1);

                //Align current orientation with parameters of calibration and current frame parameters before rotation
                Geometry.align(currentOrientation, currParam, calibrateModule.AlignParamCalibration);

                // Translate back with current frame translation
                Geometry.p_add(currentOrientation, TranslationCurr.Clone(), 1);

                //VisualizeCoordinateSystem(currentOrientation, true);
            }

            OnSendParameters(ViewDescriptor, currentOrientation);

            if (calibrateModule.IsCalibrate)
            {
                calibrateModule.CalcCalibration(ViewPoint, ImgDataToFlash);
            }

            if ((checkBoxCharts.Checked) && (!chartPoint.IsDisposed))
            {
                //    chartPt1.UpdateChart(ViewPoint[1].X, ViewPoint[1].Y, ViewPoint[1].Z);
                //    chartPtD1.UpdateChart(dArr1.X, dArr1.Y, dArr1.Z);
            }

            // Call subscribers for this event
            //if (checkBoxDrawViewPoint.Checked && (DrawViewPoint != null))
            //    DrawViewPoint(this, new DrawViewPointEventArgs(ViewPoint, true));
        }