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));
}