/// <summary>
/// Query distance between boundary and point
/// </summary>
/// <param name="point">point</param>
/// <returns>
/// Distance > 0.0, Inside the Boundary
/// Distance = 0.0, On the Boundary
/// Distance < 0.0, Outside the Boundary
/// -1 : Error
/// </returns>
public double QueryDistanceOfPoint(Vector3 point, bool isPlayArea, ref Vector3 closestPoint, ref Vector3 normalDir)
{
//point.Set(point.x * 1000, point.y * 1000, point.z * 1000); // m -> mm
//IntPtr pointPtr = Pvr_BoundaryAPI.StructToIntPtr<Vector3>(point);
//double distance = Pvr_SafeAreaAlgoAPI.Pvr_GSABoundaryDetector(pointPtr);
//Marshal.FreeHGlobal(pointPtr);
Pvr_SafeAreaAlgoAPI.GSAPoint3i point3i = new Pvr_SafeAreaAlgoAPI.GSAPoint3i();
point3i.x = (int)(point.x * 1000);
point3i.y = (int)(point.y * 1000);
point3i.z = (int)(point.z * 1000);
Pvr_SafeAreaAlgoAPI.GSAPoint3i closestPoint3i = new Pvr_SafeAreaAlgoAPI.GSAPoint3i();
Pvr_SafeAreaAlgoAPI.GSAPoint3i normalDir3i = new Pvr_SafeAreaAlgoAPI.GSAPoint3i();
double distance = Pvr_SafeAreaAlgoAPI.Pvr_GSABoundaryDetector2(ref point3i, isPlayArea, ref closestPoint3i, ref normalDir3i);
closestPoint.x = closestPoint3i.x / 1000.0f;
closestPoint.y = closestPoint3i.y / 1000.0f;
closestPoint.z = closestPoint3i.z / 1000.0f;
normalDir.x = normalDir3i.x / 1000.0f;
normalDir.y = normalDir3i.y / 1000.0f;
normalDir.z = normalDir3i.z / 1000.0f;
return(distance / 1000.0f); // mm -> m
}
/// <summary>
/// Commit new line
/// </summary>
/// <param name="isFistLine">init or reset</param>
/// <param name="points">point set</param>
/// <returns></returns>
public bool CommitNewLineData(bool isFistLine, Vector3[] points)
{
Pvr_SafeAreaAlgoAPI.GSALineCollection lineCollection = new Pvr_SafeAreaAlgoAPI.GSALineCollection();
lineCollection.lineCount = 2;
Pvr_SafeAreaAlgoAPI.GSALine[] lineArray = new Pvr_SafeAreaAlgoAPI.GSALine[2];
if (isFistLine)
{
lineArray[0].pointArray = IntPtr.Zero;
lineArray[0].pointCount = 0;
}
else
{
Pvr_SafeAreaAlgoAPI.GSAPoint3i[] tPoint = new Pvr_SafeAreaAlgoAPI.GSAPoint3i[1];
tPoint[0] = new Pvr_SafeAreaAlgoAPI.GSAPoint3i()
{
x = 0, y = 0, z = 0
};
lineArray[0].pointArray = Marshal.UnsafeAddrOfPinnedArrayElement(tPoint, 0); // unsafe
lineArray[0].pointCount = 1;
}
Pvr_SafeAreaAlgoAPI.GSAPoint3i[] pointArray = new Pvr_SafeAreaAlgoAPI.GSAPoint3i[points.Length];
for (int i = 0; i < points.Length; i++)
{
Pvr_SafeAreaAlgoAPI.GSAPoint3i newPoint = new Pvr_SafeAreaAlgoAPI.GSAPoint3i();
newPoint.x = (int)(points[i].x * 1000);
newPoint.y = (int)(points[i].y * 1000);
newPoint.z = (int)(points[i].z * 1000);
pointArray[i] = newPoint;
}
lineArray[1].pointArray = Marshal.UnsafeAddrOfPinnedArrayElement(pointArray, 0); // unsafe
lineArray[1].pointCount = pointArray.Length;
lineCollection.lineArray = Marshal.UnsafeAddrOfPinnedArrayElement(lineArray, 0);
IntPtr collectionPtr = Pvr_BoundaryAPI.StructToIntPtr <Pvr_SafeAreaAlgoAPI.GSALineCollection>(lineCollection);
int ret = Pvr_SafeAreaAlgoAPI.Pvr_GSAUpDateData(collectionPtr);
Marshal.FreeHGlobal(collectionPtr);
if (ret != 0)
{
Debug.LogError("BoundarySystem commit new line data failed!");
return(false);
}
return(true);
}
/// <summary>
/// Query distance between boundary and point
/// </summary>
/// <param name="point">point</param>
/// <returns>
/// Distance > 0.0, Inside the Boundary
/// Distance = 0.0, On the Boundary
/// Distance < 0.0, Outside the Boundary
/// -1 : Error
/// </returns>
public double QueryDistanceOfPoint(Vector3 point)
{
//point.Set(point.x * 1000, point.y * 1000, point.z * 1000); // m -> mm
//IntPtr pointPtr = Pvr_BoundaryAPI.StructToIntPtr<Vector3>(point);
//double distance = Pvr_SafeAreaAlgoAPI.Pvr_GSABoundaryDetector(pointPtr);
//Marshal.FreeHGlobal(pointPtr);
Pvr_SafeAreaAlgoAPI.GSAPoint3i point3i = new Pvr_SafeAreaAlgoAPI.GSAPoint3i();
point3i.x = (int)(point.x * 1000);
point3i.y = (int)(point.y * 1000);
point3i.z = (int)(point.z * 1000);
double distance = Pvr_SafeAreaAlgoAPI.Pvr_GSABoundaryDetector(ref point3i);
return(distance / 1000.0f); // mm -> m
}
private static void _SafeAreaCallbackShrink(IntPtr lineCollectionPtr, IntPtr safeAreaDetailRectPtr)
{
if (lineCollectionPtr == IntPtr.Zero || safeAreaDetailRectPtr == IntPtr.Zero)
{
Debug.LogError("BoundarySystem callback is inValid!");
return;
}
if (instance.boundarySystemCallbackShrink != null)
{
List <Vector3> boundaryPoints = new List <Vector3>();
List <List <Vector3> > unusedLines = new List <List <Vector3> >();
int byteNum = Marshal.SizeOf(typeof(Pvr_SafeAreaAlgoAPI.GSALine));
int byteNum2 = Marshal.SizeOf(typeof(Pvr_SafeAreaAlgoAPI.GSAPoint3i));
// boundary
Pvr_SafeAreaAlgoAPI.GSALineCollection collection = Pvr_BoundaryAPI.IntPtrToStruct <Pvr_SafeAreaAlgoAPI.GSALineCollection>(lineCollectionPtr);
IntPtr ptr;
for (int i = 0; i < collection.lineCount; i++)
{
ptr = new IntPtr(collection.lineArray.ToInt64() + (byteNum * i));
Pvr_SafeAreaAlgoAPI.GSALine line = (Pvr_SafeAreaAlgoAPI.GSALine)Marshal.PtrToStructure(ptr, typeof(Pvr_SafeAreaAlgoAPI.GSALine));
List <Vector3> newLine = new List <Vector3>();
for (int j = 0; j < line.pointCount; j++)
{
IntPtr tptr = new IntPtr(line.pointArray.ToInt64() + (byteNum2 * j));
Pvr_SafeAreaAlgoAPI.GSAPoint3i point = (Pvr_SafeAreaAlgoAPI.GSAPoint3i)Marshal.PtrToStructure(tptr, typeof(Pvr_SafeAreaAlgoAPI.GSAPoint3i));
newLine.Add(new Vector3(point.x / 1000f, point.y / 1000f, point.z / 1000f));
}
if (i == 0) // first line for colsed boundary
{
boundaryPoints = newLine;
}
else
{
unusedLines.Add(newLine);
}
}
// Rect Detail Points
List <Vector3> rectPoints = new List <Vector3>();
Pvr_SafeAreaAlgoAPI.GSALineCollection safeAreaRectLineCollection = Pvr_BoundaryAPI.IntPtrToStruct <Pvr_SafeAreaAlgoAPI.GSALineCollection>(safeAreaDetailRectPtr);
IntPtr safeAreaRectLinePtr;
for (int i = 0; i < safeAreaRectLineCollection.lineCount; i++)
{
safeAreaRectLinePtr = new IntPtr(safeAreaRectLineCollection.lineArray.ToInt64() + (byteNum * i));
Pvr_SafeAreaAlgoAPI.GSALine line = (Pvr_SafeAreaAlgoAPI.GSALine)Marshal.PtrToStructure(safeAreaRectLinePtr, typeof(Pvr_SafeAreaAlgoAPI.GSALine));
for (int j = 0; j < line.pointCount; j++)
{
IntPtr tptr = new IntPtr(line.pointArray.ToInt64() + (byteNum2 * j));
Pvr_SafeAreaAlgoAPI.GSAPoint3i point = (Pvr_SafeAreaAlgoAPI.GSAPoint3i)Marshal.PtrToStructure(tptr, typeof(Pvr_SafeAreaAlgoAPI.GSAPoint3i));
rectPoints.Add(new Vector3(point.x / 1000f, point.y / 1000f, point.z / 1000f));
}
}
instance.boundarySystemCallbackShrink(rectPoints, boundaryPoints, unusedLines);
}
}
private static void _SafeAreaCallback(IntPtr lineCollectionPtr, IntPtr safeAreaRectPtr)
{
if (lineCollectionPtr == IntPtr.Zero || safeAreaRectPtr == IntPtr.Zero)
{
Debug.LogError("BoundarySystem callback is inValid!");
return;
}
if (instance.boundarySystemCallback != null)
{
List <Vector3> boundaryPoints = new List <Vector3>();
List <List <Vector3> > unusedLines = new List <List <Vector3> >();
int byteNum = Marshal.SizeOf(typeof(Pvr_SafeAreaAlgoAPI.GSALine));
int byteNum2 = Marshal.SizeOf(typeof(Pvr_SafeAreaAlgoAPI.GSAPoint3i));
// boundary
Pvr_SafeAreaAlgoAPI.GSALineCollection collection = Pvr_BoundaryAPI.IntPtrToStruct <Pvr_SafeAreaAlgoAPI.GSALineCollection>(lineCollectionPtr);
IntPtr ptr;
for (int i = 0; i < collection.lineCount; i++)
{
ptr = new IntPtr(collection.lineArray.ToInt64() + (byteNum * i));
Pvr_SafeAreaAlgoAPI.GSALine line = (Pvr_SafeAreaAlgoAPI.GSALine)Marshal.PtrToStructure(ptr, typeof(Pvr_SafeAreaAlgoAPI.GSALine));
List <Vector3> newLine = new List <Vector3>();
for (int j = 0; j < line.pointCount; j++)
{
IntPtr tptr = new IntPtr(line.pointArray.ToInt64() + (byteNum2 * j));
Pvr_SafeAreaAlgoAPI.GSAPoint3i point = (Pvr_SafeAreaAlgoAPI.GSAPoint3i)Marshal.PtrToStructure(tptr, typeof(Pvr_SafeAreaAlgoAPI.GSAPoint3i));
newLine.Add(new Vector3(point.x / 1000f, point.y / 1000f, point.z / 1000f));
}
if (i == 0) // first line for colsed boundary
{
boundaryPoints = newLine;
}
else
{
unusedLines.Add(newLine);
}
}
// rect
Pvr_SafeAreaAlgoAPI.GSARect algoResult = Pvr_BoundaryAPI.IntPtrToStruct <Pvr_SafeAreaAlgoAPI.GSARect>(safeAreaRectPtr);
BoundaryPlayerArea playAreaInfo = new BoundaryPlayerArea();
playAreaInfo.lowerleft = new Vector3(algoResult.leftup.x / 1000f, algoResult.leftup.y / 1000f, algoResult.leftup.z / 1000f);
playAreaInfo.upperleft = new Vector3(algoResult.leftdown.x / 1000f, algoResult.leftdown.y / 1000f, algoResult.leftdown.z / 1000f);
playAreaInfo.upperRight = new Vector3(algoResult.rightdown.x / 1000f, algoResult.rightdown.y / 1000f, algoResult.rightdown.z / 1000f);
playAreaInfo.lowerRight = new Vector3(algoResult.rightup.x / 1000f, algoResult.rightup.y / 1000f, algoResult.rightup.z / 1000f);
playAreaInfo.center = new Vector3(algoResult.center.x / 1000f, algoResult.center.y / 1000f, algoResult.center.z / 1000f);
playAreaInfo.width = algoResult.width;
playAreaInfo.height = algoResult.height;
playAreaInfo.isLegal = algoResult.isLegal == 0 ? false : true;
// extra info
playAreaInfo.extraInfo.overMaxRange = algoResult.legalData.overMaxRange == 0 ? false : true;
playAreaInfo.extraInfo.includeBigGap = algoResult.legalData.includeBigGrap == 0 ? false : true;
playAreaInfo.extraInfo.bigGapNum = algoResult.legalData.bigGapNum;
playAreaInfo.extraInfo.centerOut = algoResult.legalData.centerOut == 0 ? false : true;
playAreaInfo.extraInfo.removeNarrow = algoResult.legalData.removeNarrow == 0 ? false : true;
playAreaInfo.extraInfo.overMaxRange_more = algoResult.legalData.overMaxRange_more == 0 ? false : true;
playAreaInfo.extraInfo.validShrinkArea = algoResult.legalData.validShrinkArea;
instance.boundarySystemCallback(playAreaInfo, boundaryPoints, unusedLines);
}
}
