public void DrawEpl(PointFloat[] leftPts, PointSide side, PointFloat[] rightPts, Mat left, Mat right)
{
const int PTS_SIZE = 1;
for (var i = 0; i < leftPts.Length; i++)
{
var pts = leftPts[i];
var rpts = rightPts[i];
var clr = colors[i];
CvInvoke.Rectangle(left, new System.Drawing.Rectangle((int)(pts.X - PTS_SIZE), (int)(pts.Y + PTS_SIZE), PTS_SIZE * 2, PTS_SIZE * 2), clr, 2);
}
const int EPPTS_SIZE = 2;
for (var i = 0; i < leftPts.Length; i++)
{
if (i >= onChecks.Length)
{
continue;
}
if (!onChecks[i])
{
continue;
}
var pts = leftPts[i];
var rpts = rightPts[i];
var clr = colors[i];
CvInvoke.Rectangle(left, new System.Drawing.Rectangle((int)(pts.X - EPPTS_SIZE), (int)(pts.Y - EPPTS_SIZE), EPPTS_SIZE * 2, EPPTS_SIZE * 2), clr, 2);
var gm = GetEpLineABC(pts, side, F);
//var gm = F.dot(new GMatrix(new double[3, 1] { { pts.X }, { pts.Y }, { 1 } }));
DrawEpl(right, gm, clr, rpts);
}
}
//________________________________________________________________________
internal Point GetSidePoint(PointSide value)
{
Point ret = this;
switch (value)
{
case PointSide.Top:
--ret.Row;
return(ret);
case PointSide.Right:
++ret.Column;
return(ret);
case PointSide.Bottom:
++ret.Row;
return(ret);
case PointSide.Left:
--ret.Column;
return(ret);
default:
return(ret);
}
}
public static PointFloat FindEpipole(PointFloat[] pts, PointSide side, GMatrix f)
{
if (side == PointSide.Right)
{
f = f.tranpose();
}
GMatrix lines = new GMatrix(pts.Length, 3);
var storage = lines.storage;
for (int i = 0; i < pts.Length; i++)
{
var cur = storage[i];
var pt = pts[i];
var gm = f.dot(new GMatrix(new double[3, 1] {
{ pt.X }, { pt.Y }, { 1 }
}));
var ms = gm.storage;
for (int j = 0; j < 3; j++)
{
cur[j] = ms[j][0];
}
}
var svdA = JacobSvd.JacobiSVD(lines);
var res = svdA.Vt.storage[2];
//Console.WriteLine($"{res[0].ToString("0.00")},{res[1].ToString("0.00")},{res[2].ToString("0.0000000")}");
//Console.WriteLine($"{(res[0]/res[2]).ToString("0.00")},{(res[1]/res[2]).ToString("0.00")}");
return(new PointFloat((float)(res[0] / res[2]), (float)(res[1] / res[2])));
}
private LineSlop GetEpLineABC(PointFloat pts, PointSide side, GMatrix f)
{
if (side == PointSide.Right)
{
f = f.tranpose();
}
var gm = f.dot(new GMatrix(new double[3, 1] {
{ pts.X }, { pts.Y }, { 1 }
}));
var ms = gm.storage;
var a = ms[0][0];
var b = ms[1][0];
var c = ms[2][0];
return(new LineSlop(a, b, c));
}
/// <summary>
/// 测试给定点是否在三角形中
/// 点在三角形边上也算
/// </summary>
/// <param name="pt">指定点</param>
/// <returns>是否在三角形中</returns>
public bool IsPointIn(Vector2 pt)
{
if (this.m_cBoxCollider.xMin != this.m_cBoxCollider.xMax && !this.m_cBoxCollider.Contains(pt))
{
return(false);
}
PointSide resultA = GetSide(0).ClassifyPoint(pt);
PointSide resultB = GetSide(1).ClassifyPoint(pt);
PointSide resultC = GetSide(2).ClassifyPoint(pt);
if (resultA == PointSide.ON_LINE || resultB == PointSide.ON_LINE || resultC == PointSide.ON_LINE)
{
return(true);
}
else if (resultA == PointSide.RIGHT_SIDE && resultB == PointSide.RIGHT_SIDE && resultC == PointSide.RIGHT_SIDE)
{
return(true);
}
return(false);
}
//________________________________________________________________________
private static void RemoveClosePoints_Pass1(Matrix matrix)
{
//
// ###
// #
// ###
//
Point pt = new Point();
Point ptMax = new Point((byte)(matrix.m_Size.Row - 2), (byte)(matrix.m_Size.Column - 2));
PointSide side = PointSide.None;
bool bNestedRepeat = false;
Point ptTest = pt;
for (pt.Row = 1; pt.Row <= ptMax.Row; ++pt.Row)
{
for (pt.Column = 1; pt.Column <= ptMax.Column; ++pt.Column)
{
ptTest = pt;
do
{
bNestedRepeat = false;
side = PointSide.None;
if (matrix.m_Buffer[ptTest.Row, ptTest.Column] == CellType.Verde)
{
break;
}
if (matrix.m_Buffer[ptTest.Row - 1, ptTest.Column] != CellType.Verde) //Top test...
{
side = PointSide.Top;
}
if (matrix.m_Buffer[ptTest.Row, ptTest.Column + 1] != CellType.Verde) //Right test...
{
if (side > PointSide.None)
{
break;
}
side = PointSide.Right;
}
if (matrix.m_Buffer[ptTest.Row + 1, ptTest.Column] != CellType.Verde) //Bottom test...
{
if (side > PointSide.None)
{
break;
}
side = PointSide.Bottom;
}
if (matrix.m_Buffer[ptTest.Row, ptTest.Column - 1] != CellType.Verde) //Left test...
{
if (side > PointSide.None)
{
break;
}
side = PointSide.Left;
}
matrix.m_Buffer[ptTest.Row, ptTest.Column] = CellType.Verde;
bNestedRepeat = side > PointSide.None;
if (bNestedRepeat)
{
ptTest = ptTest.GetSidePoint(side);
}
}while (bNestedRepeat);
}
}
}
//________________________________________________________________________
private static int FindNextPoint(Matrix matrix, PointSpeedBuffer buffer, Point end, int index, bool first)
{
if ((index + 1) >= buffer.m_Capacity)
{
return(-1);
}
PointSide side = PointSide.None;
Point point = buffer.m_Buffer[index];
Point back = buffer.m_Buffer[index - 1];
Point next;
//-----
if (first)
{
//For speed.
next = point; --next.Row; //Top
if ((next != back) && (buffer.IndexOf(next) >= 0))
{
return(-1);
}
if (next == end)
{
buffer.m_Buffer[index + 1] = next;
return(+1);
}
next = point; ++next.Column; //Right
if ((next != back) && (buffer.IndexOf(next) >= 0))
{
return(-1);
}
if (next == end)
{
buffer.m_Buffer[index + 1] = next;
return(+1);
}
next = point; ++next.Row; //Bottom
if ((next != back) && (buffer.IndexOf(next) >= 0))
{
return(-1);
}
if (next == end)
{
buffer.m_Buffer[index + 1] = next;
return(+1);
}
next = point; --next.Column; //Left
if ((next != back) && (buffer.IndexOf(next) >= 0))
{
return(-1);
}
if (next == end)
{
buffer.m_Buffer[index + 1] = next;
return(+1);
}
}
else
{
next = buffer.m_Buffer[index + 1];
if (point.Column == next.Column)
{
if ((point.Row - 1) == next.Row)
{
side = PointSide.Top;
}
else if ((point.Row + 1) == next.Row)
{
side = PointSide.Bottom;
}
}
else if (point.Row == next.Row)
{
if ((point.Column - 1) == next.Column)
{
side = PointSide.Left;
}
else if ((point.Column + 1) == next.Column)
{
side = PointSide.Right;
}
}
#if (DEBUG)
if (side == PointSide.None)
{
System.Diagnostics.Debugger.Break();
}
#endif
}
//-----Goto next point ...
byte maxRow = (byte)(matrix.m_Size.Row - 2);
byte maxCol = (byte)(matrix.m_Size.Column - 2);
while (true)
{
++side;
next = point.GetSidePoint(side);
if (next == point)
{
return(-1); //side != Top|Right|Bottom|Left
}
//-----Range test ...
if ((next.Row <= 0) || (next.Column <= 0) || (next.Row > maxRow) || (next.Column > maxCol))
{
continue;
}
//-----Is fix ...
if (matrix.m_Buffer[next.Row, next.Column] == CellType.Verde)
{
continue;
}
//-----Exist test ...
if (next == back)
{
continue;
}
#if (DEBUG)
if (buffer.IndexOf(next) >= 0)
{
System.Diagnostics.Debugger.Break();
}
#endif
//----Next point finded ...
buffer.m_Buffer[index + 1] = next;
return(+1);
}
}
