public void LoadScene()
{
LoadedScene = GD.Load <PackedScene>(ScenePath);
if (!string.IsNullOrEmpty(ConvexShapePath))
{
LoadedConvexShape = GD.Load <ConvexPolygonShape>(ConvexShapePath);
}
}
public async void UpdateShapes()
{
// await System.Threading.Thread.SpinWait(1000);
await System.Threading.Tasks.Task.Delay(100);
try
{
using (var frames = pipe.WaitForFrames())
using (var depth = frames.DepthFrame)
using (var points = pc.Process(depth).As <Points>())
{
// CopyVertices is extensible, any of these will do:
float[] vertices;
vertices = new float[points.Count * 3];
points.CopyVertices(vertices);
Points = vertices;
}
// Remove all children from the detectbody
foreach (Node child in detectBody.GetChildren())
{
child.QueueFree();
}
if (detectBody.GetShapeOwners().Count > 0)
{
GD.Print(detectBody.GetShapeOwners().Count);
for (int l = 0; l < detectBody.GetShapeOwners().Count; l++)
{
detectBody.ShapeOwnerClearShapes((uint)l);
}
}
}
catch
{
}
detectedPoints.Clear();
sortedPoints.Clear();
Vector3 currentGroundPoint = new Vector3();
// allPoints = new Vector3[101760];
for (int i = 0; i < meshDataTool.GetVertexCount(); i++) // 101760
{
Vector3 vertex = meshDataTool.GetVertex(i);
int indexOfX = i * 3;
int indexOfY = i * 3 + 1;
int indexOfZ = i * 3 + 2;
float x = Points[indexOfX];
float y = Points[indexOfY];
float z = Points[indexOfZ];
bool shapeFoundForPoint = false;
// GD.Print(x);
if (z > distanceToGround || z == 0.0f || z < 0 || x < syncPointTL.x || y > syncPointTL.y || x > syncPointBR.x || y < syncPointBR.y)
{
z = 0;
}
else
{
z = (distanceToGround - z) * 2;
if (vertex.y > 0)
{
currentGroundPoint = vertex;
currentGroundPoint.y = 0;
//------------------------------------------------- For each point test --------------------------------------------------
if (detectedPoints.Count == 0)
{
// make a vector on the ground for the detection shape to get a height
List <Vector3> shapePoints = new List <Vector3> {
currentGroundPoint, vertex
};
detectedPoints.Add(shapePoints); // Add the vectors to a list with shapes
}
else
{
for (int j = 0; j < detectedPoints.Count; j++) // Run thrugh all the shapes
{
int lengthOfShape = detectedPoints[j].Count; // number of points in shape
// find the shape with a last added start point...
if (detectedPoints[j][lengthOfShape - 2].DistanceTo(currentGroundPoint) < 1) //4 distanceBetweenPoints
{
detectedPoints[j].Add(currentGroundPoint);
detectedPoints[j].Add(vertex);
shapeFoundForPoint = true;
break;
}
}
if (shapeFoundForPoint == false)
{
//... create a new shape and add the points
List <Vector3> shapePoints = new List <Vector3> {
currentGroundPoint, vertex
};
detectedPoints.Add(shapePoints);
}
}
shapeFoundForPoint = false;
//------------------------------------------------------------------------------------------------------------------------
}
}
vertex.y = z;
meshDataTool.SetVertex(i, vertex); // Draw shape
}
Vector3 prevPoint = new Vector3(10, 10, 10);
Vector3 prevprevPoint = new Vector3(10, 10, 10);
// ---------------------------- Add points to shapes --------------------------
for (int i = 0; i < detectedPoints.Count; i++)
{
// Point (000) removal
detectedPoints[i].RemoveAll(item => item == pointZero);
for (int j = 0; j < detectedPoints[i].Count; j++)
{
if (detectedPoints[i][j].z != prevprevPoint.z)
{
prevprevPoint = prevPoint;
prevPoint = detectedPoints[i][j];
}
else if (detectedPoints[i][j].z == prevprevPoint.z)
{
prevprevPoint = prevPoint;
prevPoint = detectedPoints[i][j];
detectedPoints[i].Remove(prevprevPoint);
}
}
int pointSets = 100000;
var sortOut = detectedPoints[i].Where((x, n) => n % pointSets == 0 || n - 1 % pointSets == 0);
Vector3[] convertedArr = sortOut.ToArray();
// -------------------------- Must be turned on ----------------------
convexPolygonShape = new ConvexPolygonShape();
convexPolygonShape.Points = convertedArr;
var areaShape = new Area();
areaShape.AddToGroup("cameraDetections");
DetectionCollisionShape = new CollisionShape();
DetectionCollisionShape.Shape = convexPolygonShape;
DetectionCollisionShape.Shape.Margin = 1;
areaShape.AddChild(DetectionCollisionShape);
detectBody.AddChild(areaShape);
// ----------------------------------------------------------------
}
detectedPoints.Clear();
sortedPoints.Clear();
// ---------------------------- Remove surface --------------------------
for (int j = 0; j < arrayPlane.GetSurfaceCount(); j++)
{
arrayPlane.SurfaceRemove(j); // Removes the drawing
}
meshDataTool.CommitToSurface(arrayPlane); // Resets the surface
UpdateShapes(); // Rerun function
}
