} // End Property Slope
// https://stackoverflow.com/questions/17692922/check-is-a-point-x-y-is-between-two-points-drawn-on-a-straight-line
public bool IsPointOnLine(MyPoint2D <T> p)
{
T norm = this.Vector.MagnitudeSquared;
MyVector2D <T> vec1 = new MyVector2D <T>(this.m_start, p);
MyVector2D <T> vec2 = new MyVector2D <T>(this.m_end, p);
T dist = Arithmetics <T> .Add(vec1.MagnitudeSquared, vec2.MagnitudeSquared);
if (norm.Equals(dist))
{
return(true);
}
T delta = Arithmetics <T> .Subtract(vec1.MagnitudeSquared, vec2.MagnitudeSquared);
decimal decDelta = System.Convert.ToDecimal(delta);
decDelta = System.Math.Abs(decDelta);
// Greatest possible floating-point difference
decimal decFloatEpsilon = System.Convert.ToDecimal(float.Epsilon);
if (decDelta <= decFloatEpsilon)
{
return(true);
}
return(false);
} // End Function IsPointOnLine
} // End Property Normalized
// http://mathworld.wolfram.com/NormalVector.html
// https://stackoverflow.com/questions/1243614/how-do-i-calculate-the-normal-vector-of-a-line-segment
public static MyVector2D <T> GetNormalVector(MyVector2D <T> vec)
{
MyVector2D <T> normal1 = new MyVector2D <T>(Arithmetics <T> .Minus(vec.Y), vec.X);
// MyVector2D<T> normal2 = new MyVector2D<T>(vec.Y, Arithmetics<T>.Minus(vec.X));
return(normal1);
} // End Function GetNormalVector
} // End function Angle_Rad
public static T Angle_Degrees(MyVector2D <T> a, MyVector2D <T> b)
{
T nReturnValue = Angle_Rad(a, b);
decimal decReturnType = System.Convert.ToDecimal(nReturnValue);
decimal quotient = decReturnType * 180.0m / (decimal)System.Math.PI;
T ret = (T)System.Convert.ChangeType(quotient, typeof(T));
return(ret);
} // End function Angle_Degrees
} // End Operator *
public static MyVector2D <T> operator *(MyVector2D <T> a, T b)
{
MyVector2D <T> v = a.Clone();
v.X = Arithmetics <T> .Multiply(v.X, b);
v.Y = Arithmetics <T> .Multiply(v.Y, b);
return(v);
} // End Operator *
} // End Operator +
public static MyVector2D <T> operator -(MyVector2D <T> a, MyVector2D <T> b)
{
MyVector2D <T> v = a.Clone();
v.X = Arithmetics <T> .Subtract(v.X, b.X);
v.Y = Arithmetics <T> .Subtract(v.Y, b.Y);
return(v);
} // End Operator -
} // End function CrossP
// The dot product (also called the scalar product) is the magnitude of
// vector b multiplied by the size of the projection of a onto b.
// The size of the projection is a cosθ (where θ is the angle between the 2 vectors).
// https://coderwall.com/p/icvt-g/2d-vector-dot-product
// where theta is the angle between u and v, given by the dot product
// cos(theta) = u dotp v
public static T DotP(MyVector2D <T> a, MyVector2D <T> b)
{
T s1 = Arithmetics <T> .Multiply(a.X, b.X);
T s2 = Arithmetics <T> .Multiply(a.Y, b.Y);
//A * B = ax*bx+ay*by+az*bz
T retValue = Arithmetics <T> .Add(s1, s2);
return(retValue);
} // End function DotP
} // End Property NormalVector
// http://mathworld.wolfram.com/CrossProduct.html
// the cross product is a vector that is perpendicular to both
// a and b and thus normal to the plane containing them
// |u x v| = |u| x |v| * sin(phi)
public static T CrossP(MyVector2D <T> a, MyVector2D <T> b)
{
// crossp = det(a,b) = a.X*b.Y- a.Y*b.X
T s1 = Arithmetics <T> .Multiply(a.X, b.Y);
T s2 = Arithmetics <T> .Multiply(a.Y, b.X);
T retValue = Arithmetics <T> .Subtract(s1, s2);
return(retValue);
} // End function CrossP
} // End function Angle_Degrees
public MyPoint2D <T> Schnittpunktli(MyPoint2D <T> p1, MyVector2D <T> vec1, MyPoint2D <T> p2, MyVector2D <T> vec2)
{
T x1 = Arithmetics <T> .Add(p1.X, vec1.X);
T y1 = Arithmetics <T> .Add(p1.Y, vec1.Y);
T x2 = Arithmetics <T> .Add(p2.X, vec2.X);
T y2 = Arithmetics <T> .Add(p2.Y, vec2.Y);
return(Schnittpunktli(p1, new MyPoint2D <T>(x1, x1), p2, new MyPoint2D <T>(x2, y2)));
} // End Function Schnittpunktli
} // End Function ToString
/// <summary>
/// Returns a new Vector that is the linear blend of the 2 given Vectors
/// </summary>
/// <param name="a">First input vector</param>
/// <param name="b">Second input vector</param>
/// <param name="blend">The blend factor. a when blend=0, b when blend=1.</param>
/// <returns>a when blend=0, b when blend=1, and a linear combination otherwise</returns>
public static MyVector2D <T> Lerp(MyVector2D <T> a, MyVector2D <T> b, T blend)
{
T bxax = Arithmetics <T> .Subtract(b.X, a.X);
T byay = Arithmetics <T> .Subtract(b.Y, a.Y);
T f1 = Arithmetics <T> .Multiply(blend, bxax);
T f2 = Arithmetics <T> .Multiply(blend, byay);
T x = Arithmetics <T> .Add(f1, a.X);
T y = Arithmetics <T> .Add(f2, a.Y);
return(new MyVector2D <T>(x, y));
} // End Function Lerp
} // End function DotP
public static T Angle_Rad(MyVector2D <T> a, MyVector2D <T> b)
{
T axbx = Arithmetics <T> .Multiply(a.X, b.X);
T ayby = Arithmetics <T> .Multiply(a.Y, b.Y);
T azbz = Arithmetics <T> .ZERO;
T sumAB = Arithmetics <T> .Sum(axbx, ayby, azbz);
T ax2 = Arithmetics <T> .Pow(a.X, 2);
T ay2 = Arithmetics <T> .Pow(a.Y, 2);
T az2 = Arithmetics <T> .ZERO;
T bx2 = Arithmetics <T> .Pow(b.X, 2);
T by2 = Arithmetics <T> .Pow(b.Y, 2);
T bz2 = Arithmetics <T> .ZERO;
T aSquare = Arithmetics <T> .Sum(ax2, ay2, az2);
T bSquare = Arithmetics <T> .Sum(bx2, by2, bz2);
T val = Arithmetics <T> .Divide(sumAB,
(
Arithmetics <T> .Multiply(Arithmetics <T> .Sqrt(aSquare), Arithmetics <T> .Sqrt(bSquare))
)
);
T nReturnValue = Arithmetics <T> .Acos(val);
return(nReturnValue);
} // End function Angle_Rad
} // End Constructor
public MyVector2D(MyVector2D <T> vector)
: base(vector.X, vector.Y)
{
} // End Constructor
} // End Constructor
public MyLine2D(MyPoint2D <T> start, MyVector2D <T> vec)
{
this.Start = start;
this.End = start + vec;
} // End Constructor
} // End Function Schnittpunktli
public override bool Equals(object obj)
{
MyVector2D <T> tof = (MyVector2D <T>)obj;
return(this == tof);
} // End Function Equals
} // End function GetPolygonCenter
public uint FindNearestLineEndIndex <T>(MyPoint2D <T> cptPointToAdd)
{
if (this.aptDefinitionPoints.Length == 0)
{
return(0);
}
if (this.aptDefinitionPoints.Length > 1)
{
double nOldDistance = 1000000;
uint iOldIndex = 0;
double nDistance = 0;
MyVector2D <T> vec2_LineStart = null;
MyVector2D <T> vec2_LineEnd = null;
MyVector2D <T> vec2_Point = null;
MyVector2D <T> vec2_VecLine = null;
MyPoint2D <T> cptIntersectionPoint = null;
bool bHasFirst = false;
bool bHasLast = true;
uint iFirst = 0;
uint iLast = 0;
for (uint i = 0; i < this.aptDefinitionPoints.Length; ++i)
{
if (this.aptDefinitionPoints[i].bCurrentlyValid)
{
if (bHasFirst == false)
{
bHasFirst = true;
iFirst = i;
iLast = i;
continue;
}
bHasLast = true;
vec2_LineStart = cVector_2d.MakeVector(this.aptDefinitionPoints[iLast].x, this.aptDefinitionPoints[iLast].y);
//trace("vec2_LineStart: " + vec2_LineStart.toString() );
vec2_LineEnd = cVector_2d.MakeVector(this.aptDefinitionPoints[i].x, this.aptDefinitionPoints[i].y);
//trace("vec2_LineEnd: " + vec2_LineEnd.toString() );
vec2_Point = cVector_2d.MakeVector(cptPointToAdd.x, cptPointToAdd.y);
//trace("vec2_Point: " + vec2_Point.toString() );
nDistance = cVector_2d.DistanceOfPointToLine(vec2_Point, vec2_LineStart, vec2_LineEnd);
vec2_VecLine = cVector_2d.VectorSubtract(vec2_LineStart, vec2_LineEnd);
if (nDistance < nOldDistance)
{
cptIntersectionPoint = cVector_2d.GetPointVerticalIntersection(this.aptDefinitionPoints[i], vec2_VecLine, cptPointToAdd);
if (cptIntersectionPoint.bHasInterSection)
{
//trace("Has intersection");
if (cVector_2d.isPointOnLine(this.aptDefinitionPoints[i], this.aptDefinitionPoints[iLast], cptIntersectionPoint))
{
//trace("is on line");
nOldDistance = nDistance;
iOldIndex = i;
}
// else
// trace("is not on line.");
}
// else
// trace("has no intersection");
}
// trace("Length: " + aptDefinitionPoints.Length);
// trace("Pair["+i+"]: " + aptDefinitionPoints[iLast].toString() + " ; "+aptDefinitionPoints[i].toString() + "Distance: " + nDistance);
// trace("Dist: " + nDistance );
iLast = i;
} // End isvalid
} // End for
if (bHasLast)
{
// trace("Has Last...");
vec2_LineStart = cVector_2d.MakeVector(this.aptDefinitionPoints[iLast].x, this.aptDefinitionPoints[iLast].y);
vec2_LineEnd = cVector_2d.MakeVector(this.aptDefinitionPoints[iFirst].x, this.aptDefinitionPoints[iFirst].y);
vec2_Point = cVector_2d.MakeVector(cptPointToAdd.x, cptPointToAdd.y);
nDistance = cVector_2d.DistanceOfPointToLine(vec2_Point, vec2_LineStart, vec2_LineEnd);
vec2_VecLine = cVector_2d.VectorSubtract(vec2_LineStart, vec2_LineEnd);
//trace("Final Pair: " + aptDefinitionPoints[iFirst].toString()+" ; " + aptDefinitionPoints[iLast].toString() + "Distance: " + nDistance);
if (nDistance < nOldDistance)
{
// nOldDistance = nDistance;
// iOldIndex = aptDefinitionPoints.Length;
cptIntersectionPoint = cVector_2d.GetPointVerticalIntersection(this.aptDefinitionPoints[iLast], vec2_VecLine, cptPointToAdd);
if (cptIntersectionPoint.bHasInterSection)
{
//trace("Is point on this line? "+ aptDefinitionPoints[iLast].toString()+", "+ aptDefinitionPoints[iFirst].toString() );
if (cVector_2d.isPointOnLine(this.aptDefinitionPoints[iLast], this.aptDefinitionPoints[iFirst], cptIntersectionPoint))
{
//trace("isonline");
nOldDistance = nDistance;
iOldIndex = iLast + 1; //aptDefinitionPoints.Length;
}
//else
// trace("is not on line.");
}
//else
// trace("has not");
} // End if(nDistance<nOldDistance)
//trace("Selected pair: "+iOldIndex);
return(iOldIndex);
}// End if(bHasLast)
else
{
//trace("Selected pair: 1");
return(1);
}
}
else
{
return(1);
}
return((uint)this.aptDefinitionPoints.Length);
}
/// <summary>
/// Computes table as minimal bounding rectangle over table pixels and makes a mask of table and table items
/// </summary>
public bool[] ConvexHullAlgorithm()
{
// get candidates to convex hull -> those marked as table points that neighbors with different type of point
List <ConvexHullPoints> pts = new List <ConvexHullPoints>();
for (int y = 0; y < PlaneLocalizationConfig.DepthImageHeight; y++)
{
for (int x = 0; x < PlaneLocalizationConfig.DepthImageWidth; x++)
{
#region longIfs - optimalization - takes only pixels of table that are surrounded with different pixel type
if (DepthData[PosFromCoor(x, y)].Type == PixelType.Table)
{
bool left = false;
bool right = false;
bool up = false;
bool down = false;
if (x > 0)
{
if (DepthData[PosFromCoor(x - 1, y)].Type == PixelType.Table)
{
left = true;
}
}
else
{
left = true;
}
if (x < PlaneLocalizationConfig.DepthImageWidth - 1)
{
if (DepthData[PosFromCoor(x + 1, y)].Type == PixelType.Table)
{
right = true;
}
}
else
{
right = true;
}
if (y > 0)
{
if (DepthData[PosFromCoor(x, y - 1)].Type == PixelType.Table)
{
up = true;
}
}
else
{
up = true;
}
if (y < PlaneLocalizationConfig.DepthImageHeight - 1)
{
if (DepthData[PosFromCoor(x, y + 1)].Type == PixelType.Table)
{
down = true;
}
}
else
{
down = true;
}
if (!(left && right && up && down))
{
pts.Add(new ConvexHullPoints(ref DepthData[PosFromCoor(x, y)], PosFromCoor(x, y)));
}
#endregion
}
}
}
// make convex hull with algorithm
List <ConvexHullPoints> hullPts = (List <ConvexHullPoints>)ConvexHull.MakeHull(pts);
// make Minimal Bounding Rectangle using one edge of hull at a time
double minArea = double.PositiveInfinity;
double minAreaAngle = double.PositiveInfinity;
MyVector2D maxValues = new MyVector2D(0, 0);
MyVector2D minValues = new MyVector2D(0, 0);
for (int i = 0; i < hullPts.Count; i++)
{
// from which points will be the edge
int index1 = i;
int index2 = i + 1;
// special case of overflowing index
if (index1 == (hullPts.Count - 1))
{
index2 = 0;
}
// get vectors and angle
MyVector2D v = new MyVector2D(hullPts[index1].X - hullPts[index2].X, hullPts[index1].Y - hullPts[index2].Y);
MyVector2D n = new MyVector2D(1, 0);
double angle = MyVector2D.GetAngleBetweenTwoVectors(v, n);
// try this rotation on all points
double minX = double.PositiveInfinity;
double minY = double.PositiveInfinity;
double maxX = double.NegativeInfinity;
double maxY = double.NegativeInfinity;
for (int j = 0; j < hullPts.Count; j++)
{
double x = hullPts[j].X * Math.Cos(angle) - hullPts[j].Y * Math.Sin(angle);
double y = hullPts[j].X * Math.Sin(angle) + hullPts[j].Y * Math.Cos(angle);
if (x > maxX)
{
maxX = x;
}
if (y > maxY)
{
maxY = y;
}
if (x < minX)
{
minX = x;
}
if (y < minY)
{
minY = y;
}
}
// compute area of rectangle
double area = Math.Abs(maxX - minX) * Math.Abs(maxY - minY);
if (area < minArea)
{
minArea = area;
minAreaAngle = angle;
minValues = new MyVector2D(minX, minY);
maxValues = new MyVector2D(maxX, maxY);
}
}
// clipping of edges - for small corrections of table edge error - not necessary
minValues.X += 0.01f;
minValues.Y += 0.01f;
maxValues.X += -0.01f;
maxValues.Y += -0.01f;
// compute mask for given minimal bounding rectangle and some height limits
bool[] resultingMaskOfTable = new bool[512 * 424];
for (int i = 0; i < DepthData.Length; i++)
{
double x = DepthData[i].X * Math.Cos(minAreaAngle) - DepthData[i].Y * Math.Sin(minAreaAngle);
double y = DepthData[i].X * Math.Sin(minAreaAngle) + DepthData[i].Y * Math.Cos(minAreaAngle);
if (
(x > minValues.X) && (x < maxValues.X) &&
(y > minValues.Y) && (y < maxValues.Y) &&
DepthData[i].Z <0.2f && DepthData[i].Z> -0.3f
)
{
resultingMaskOfTable[i] = true;
}
else
{
resultingMaskOfTable[i] = false;
}
}
return(resultingMaskOfTable);
}
