264 lines
11 KiB
C#
264 lines
11 KiB
C#
using Aurora.Api;
|
|
using Aurora.Api.DataType;
|
|
using Matrox.MatroxImagingLibrary;
|
|
using MilCommon;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using VisionCalibrator;
|
|
using VisionCommon;
|
|
|
|
namespace MilUI {
|
|
internal enum eGridMode {
|
|
None, Narrow, Wide
|
|
}
|
|
|
|
public class GridGuideLine {
|
|
private Logger _Logger = new Logger("GridGuideLine");
|
|
private MIL_ID _diplayId;
|
|
private MIL_ID _graphicContext;
|
|
private MIL_ID _graphicListId;
|
|
private MilGraphics _milGraphics;
|
|
private FovInfo _fovInfo;
|
|
private eGridMode _gridMode = eGridMode.None;
|
|
private int COLOR_ORANGE = MIL.M_RGB888(255, 165, 0);
|
|
|
|
public GridGuideLine(MIL_ID diplayId, MIL_ID graphicContext, MIL_ID graphicListId, FovInfo fovInfo) {
|
|
_diplayId = diplayId;
|
|
_graphicContext = graphicContext;
|
|
_graphicListId = graphicListId;
|
|
_milGraphics = new MilGraphics(_diplayId, _graphicListId);
|
|
_fovInfo = fovInfo;
|
|
}
|
|
|
|
private CoordSystem GetCameraSetupDirection(FovInfo fovInfo) {
|
|
var camRecipe = VisionRecipeManager.GetCamRecipe(fovInfo);
|
|
var camConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(camRecipe.CamId.ToString()) as CamConfig;
|
|
if (camConfig != null) {
|
|
return camConfig.SetupDirection.ToCoordSystem();
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private CoordSystem GetStaticCoordSystem(SubSysInfo subSysInfo) {
|
|
var calRecipe = VisionRecipeManager.GetCalRecipe(subSysInfo);
|
|
return calRecipe?.CoordSystem;
|
|
}
|
|
|
|
public void Update(ImageSize imageSize) {
|
|
switch (_gridMode) {
|
|
case eGridMode.Narrow:
|
|
NarrowOn(imageSize);
|
|
return;
|
|
case eGridMode.Wide:
|
|
WideOn(imageSize);
|
|
return;
|
|
case eGridMode.None:
|
|
return;
|
|
}
|
|
}
|
|
|
|
public void NarrowOn(ImageSize imageSize, bool errRotation = false) {
|
|
Clear();
|
|
var calFovRecipe = VisionRecipeManager.GetCalFovRecipe(_fovInfo);
|
|
|
|
if (errRotation) {
|
|
DrawGridLines(eGridMode.Narrow, imageSize, calFovRecipe.ErrRotationX);
|
|
}
|
|
else {
|
|
DrawGridLines(eGridMode.Narrow, imageSize);
|
|
}
|
|
|
|
DrawRefLine(imageSize, calFovRecipe.ErrRotationX);
|
|
DrawCoordDirection(imageSize);
|
|
_gridMode = eGridMode.Narrow;
|
|
}
|
|
|
|
public void WideOn(ImageSize imageSize, bool errRotation = false) {
|
|
Clear();
|
|
var calFovRecipe = VisionRecipeManager.GetCalFovRecipe(_fovInfo);
|
|
|
|
if (errRotation) {
|
|
DrawGridLines(eGridMode.Wide, imageSize, calFovRecipe.ErrRotationX);
|
|
}
|
|
else {
|
|
DrawGridLines(eGridMode.Wide, imageSize);
|
|
}
|
|
|
|
DrawRefLine(imageSize, calFovRecipe.ErrRotationX);
|
|
DrawCoordDirection(imageSize);
|
|
_gridMode = eGridMode.Wide;
|
|
}
|
|
|
|
private void DrawGridLines(eGridMode gridMode, ImageSize imageSize, Angle errRotation = null) {
|
|
var centerPos = new ImagePoint(imageSize.Width / 2, imageSize.Height / 2);
|
|
|
|
if (gridMode == eGridMode.Narrow) {
|
|
DrawGuideLines(imageSize, centerPos, 11, MIL.M_COLOR_LIGHT_GRAY, errRotation);
|
|
DrawGuideLines(imageSize, centerPos, 1, COLOR_ORANGE, errRotation);
|
|
}
|
|
else {
|
|
DrawGuideLines(imageSize, centerPos, 5, MIL.M_COLOR_LIGHT_GRAY, errRotation);
|
|
DrawGuideLines(imageSize, centerPos, 1, COLOR_ORANGE, errRotation);
|
|
}
|
|
}
|
|
|
|
public void DrawRefLine(ImageSize imageSize, Angle errRotation) {
|
|
var refPointRecipe = VisionRecipeManager.GetRefPointRecipe(_fovInfo);
|
|
if (refPointRecipe == null) {
|
|
_Logger.E($"refPointRecipe is null");
|
|
return;
|
|
}
|
|
|
|
if (refPointRecipe.Offset.X == 0 && refPointRecipe.Offset.Y == 0) {
|
|
// 레퍼런스 위치가 센터이므로 또 그릴 필요가 없음.
|
|
return;
|
|
}
|
|
|
|
var refPos = GetRefPos(imageSize);
|
|
DrawGuideLines(imageSize, refPos, 1, MIL.M_COLOR_YELLOW, errRotation);
|
|
}
|
|
|
|
private ImagePoint GetRefPos(ImageSize imageSize) {
|
|
var calibrator = CalibratorManager.GetCalibrator(_fovInfo);
|
|
var staticRefPoint = VisionRecipeManager.GetRefPoint(_fovInfo);
|
|
var imageRefPoint = calibrator.TransformToImagePoint(staticRefPoint, imageSize);
|
|
|
|
return new ImagePoint(imageRefPoint.X, imageRefPoint.Y);
|
|
}
|
|
|
|
public void DrawGuideLines(
|
|
ImageSize imageSize,
|
|
ImagePoint crossPos,
|
|
int lineCount,
|
|
int color,
|
|
Angle errRotation = null) {
|
|
|
|
try {
|
|
int lineCnt = ConvertToOddNumber(lineCount);
|
|
|
|
double[] lineStartX = new double[lineCnt * 2];
|
|
double[] lineStartY = new double[lineCnt * 2];
|
|
double[] lineEndX = new double[lineCnt * 2];
|
|
double[] lineEndY = new double[lineCnt * 2];
|
|
|
|
var width = imageSize.Width;
|
|
var horizontalGap = width / (lineCnt + 1);
|
|
var height = imageSize.Height;
|
|
var verticalGap = height / (lineCnt + 1);
|
|
int index = (lineCnt / 2) * -1;
|
|
|
|
for (int i = 0; i < lineCnt; i++) {
|
|
// Vertical
|
|
var vLineStart = new ImagePoint(crossPos.X + (horizontalGap * index), 0);
|
|
var vLineEnd = new ImagePoint(vLineStart.X, height);
|
|
|
|
// Horizontal
|
|
var hLineStart = new ImagePoint(0, crossPos.Y + (verticalGap * index));
|
|
var hLineEnd = new ImagePoint(width, hLineStart.Y);
|
|
|
|
if (errRotation != null) { // 카메라 틀어진 각도를 고려한 그리드 라인
|
|
// Vertical
|
|
var vLineStartRotated = MathUtil.Rotate(vLineStart, errRotation, crossPos);
|
|
var vLineEndRotated = MathUtil.Rotate(vLineEnd, errRotation, crossPos);
|
|
lineStartX[i] = vLineStartRotated.X;
|
|
lineStartY[i] = vLineStartRotated.Y;
|
|
lineEndX[i] = vLineEndRotated.X;
|
|
lineEndY[i] = vLineEndRotated.Y;
|
|
|
|
// Horizontal
|
|
var hLineStartRotated = MathUtil.Rotate(hLineStart, errRotation, crossPos);
|
|
var hLineEndRotated = MathUtil.Rotate(hLineEnd, errRotation, crossPos);
|
|
lineStartX[lineCnt + i] = hLineStartRotated.X;
|
|
lineStartY[lineCnt + i] = hLineStartRotated.Y;
|
|
lineEndX[lineCnt + i] = hLineEndRotated.X;
|
|
lineEndY[lineCnt + i] = hLineEndRotated.Y;
|
|
}
|
|
else {
|
|
// Vertical
|
|
lineStartX[i] = vLineStart.X;
|
|
lineStartY[i] = vLineStart.Y;
|
|
lineEndX[i] = vLineEnd.X;
|
|
lineEndY[i] = vLineEnd.Y;
|
|
|
|
// Horizontal
|
|
lineStartX[lineCnt + i] = hLineStart.X;
|
|
lineStartY[lineCnt + i] = hLineStart.Y;
|
|
lineEndX[lineCnt + i] = hLineEnd.X;
|
|
lineEndY[lineCnt + i] = hLineEnd.Y;
|
|
}
|
|
|
|
index++;
|
|
}
|
|
MIL.MgraColor(_graphicContext, color);
|
|
MIL.MgraLines(_graphicContext, _graphicListId, lineCnt * 2, lineStartX, lineStartY, lineEndX, lineEndY, MIL.M_LINE_LIST);
|
|
|
|
_milGraphics.Add();
|
|
}
|
|
catch (Exception e) {
|
|
_Logger.FuncE(e.Message);
|
|
}
|
|
}
|
|
|
|
private int ConvertToOddNumber(int number) {
|
|
if (number % 2 == 0) {
|
|
return number + 1;
|
|
}
|
|
|
|
return number;
|
|
}
|
|
|
|
public void Clear() {
|
|
_milGraphics.Clear();
|
|
_gridMode = eGridMode.None;
|
|
}
|
|
|
|
public void DrawCoordDirection(ImageSize imageSize) {
|
|
DrawCoordDirection(
|
|
GetCameraSetupDirection(_fovInfo),
|
|
GetStaticCoordSystem(_fovInfo.ToSubSysInfo()),
|
|
new ImagePoint(imageSize.Width - 150, 150), "S");
|
|
}
|
|
|
|
private void DrawCoordDirection(CoordSystem imageCoordSystem, CoordSystem staticCoordSystem, ImagePoint drawPosition, string prefix) {
|
|
if (imageCoordSystem != null && staticCoordSystem != null) {
|
|
var corrCS = staticCoordSystem - imageCoordSystem;
|
|
var corrRotX = GetCorrectionRotationX(imageCoordSystem, staticCoordSystem);
|
|
var corrFlipY = (int)GetCorrectionFlipY(imageCoordSystem, staticCoordSystem);
|
|
|
|
var axisX = new ImagePoint(100, 0);
|
|
var axisY = new ImagePoint(0, 100);
|
|
axisX = MathUtil.Rotate(axisX, corrRotX) as ImagePoint;
|
|
axisY = MathUtil.Rotate(axisY, corrRotX) * corrFlipY as ImagePoint;
|
|
|
|
DisplayDrawer.AddVectors(_diplayId, _graphicContext, _graphicListId, COLOR_ORANGE, _milGraphics,
|
|
new List<Line<ImagePoint>>() {
|
|
new Line<ImagePoint>(drawPosition, drawPosition + axisX as ImagePoint),
|
|
new Line<ImagePoint>(drawPosition, drawPosition + axisY as ImagePoint)
|
|
},
|
|
false);
|
|
|
|
var textOffset = -20;
|
|
var textX = new ImagePoint(100 + textOffset, 0 - textOffset);
|
|
var textY = new ImagePoint(0 - textOffset, 100 + textOffset);
|
|
textX = (MathUtil.Rotate(textX, corrRotX) + drawPosition) as ImagePoint;
|
|
textY = (MathUtil.Rotate(textY, corrRotX) * corrFlipY + drawPosition) as ImagePoint;
|
|
|
|
DisplayDrawer.AddText(_graphicContext, _graphicListId, COLOR_ORANGE, _milGraphics, new ImageSize(), textX, prefix + "X");
|
|
DisplayDrawer.AddText(_graphicContext, _graphicListId, COLOR_ORANGE, _milGraphics, new ImageSize(), textY, prefix + "Y");
|
|
}
|
|
}
|
|
|
|
public Angle GetCorrectionRotationX(CoordSystem sourceCS, CoordSystem targetCS) {
|
|
return Angle.CreateWithDegree(
|
|
(targetCS.RotationX.Degree - sourceCS.RotationX.Degree) * (int)sourceCS.FlipY);
|
|
}
|
|
|
|
public eFlipY GetCorrectionFlipY(CoordSystem sourceCS, CoordSystem targetCS) {
|
|
return (eFlipY)((int)targetCS.FlipY * (int)sourceCS.FlipY);
|
|
}
|
|
}
|
|
}
|