public void WritePoint(CloudPoint point)
{
_pointCounter++;
var data = point.BinarySerialized();
_binaryWriter.Write(data);
}
private CloudPoint ReadAsciiPoint()
{
var p = new CloudPoint();
var line = _streamReader.ReadLine();
var fields = line?.Split(" ");
if (fields == null)
{
return(null);
}
for (var i = 0; i < fields.Length; i++)
{
p[i] = (Header.Type[i], Header.Size[i]) switch
{
(PcdHeader.EType.I, PcdHeader.ESize.Byte) => char.Parse(fields[i]) - 128,
(PcdHeader.EType.I, PcdHeader.ESize.Short) => short.Parse(fields[i]),
(PcdHeader.EType.I, PcdHeader.ESize.Single) => int.Parse(fields[i]),
(PcdHeader.EType.I, PcdHeader.ESize.Double) => long.Parse(fields[i]),
(PcdHeader.EType.U, PcdHeader.ESize.Byte) => char.Parse(fields[i]),
(PcdHeader.EType.U, PcdHeader.ESize.Short) => ushort.Parse(fields[i]),
(PcdHeader.EType.U, PcdHeader.ESize.Single) => uint.Parse(fields[i]),
(PcdHeader.EType.U, PcdHeader.ESize.Double) => ulong.Parse(fields[i]),
(PcdHeader.EType.F, PcdHeader.ESize.Byte) => float.Parse(fields[i], CultureInfo.InvariantCulture),
(PcdHeader.EType.F, PcdHeader.ESize.Short) => float.Parse(fields[i], CultureInfo.InvariantCulture),
(PcdHeader.EType.F, PcdHeader.ESize.Single) => float.Parse(fields[i], CultureInfo.InvariantCulture),
(PcdHeader.EType.F, PcdHeader.ESize.Double) => double.Parse(fields[i], CultureInfo.InvariantCulture),
_ => p[i]
};
}
return(p);
}
private CloudPoint ReadBinaryPoint()
{
var p = new CloudPoint();
for (var i = 0; i < Header.Fields.Length; i++)
{
var bytes = _binaryReader.ReadBytes((int)Header.Size[i]);
if (i >= 3)
{
continue;
}
p[i] = (Header.Type[i], Header.Size[i]) switch
{
(PcdHeader.EType.I, PcdHeader.ESize.Byte) => bytes[0] - 128,
(PcdHeader.EType.I, PcdHeader.ESize.Short) => BitConverter.ToInt16(bytes),
(PcdHeader.EType.I, PcdHeader.ESize.Single) => BitConverter.ToInt32(bytes),
(PcdHeader.EType.I, PcdHeader.ESize.Double) => BitConverter.ToInt64(bytes),
(PcdHeader.EType.U, PcdHeader.ESize.Byte) => bytes[0],
(PcdHeader.EType.U, PcdHeader.ESize.Short) => BitConverter.ToUInt16(bytes),
(PcdHeader.EType.U, PcdHeader.ESize.Single) => BitConverter.ToUInt32(bytes),
(PcdHeader.EType.U, PcdHeader.ESize.Double) => BitConverter.ToUInt64(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Byte) => BitConverter.ToSingle(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Short) => BitConverter.ToSingle(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Single) => BitConverter.ToSingle(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Double) => BitConverter.ToDouble(bytes),
_ => p[i]
};
}
return(p);
}
/// <summary>
/// Reads the points of the node
/// </summary>
/// <param name="buffer">The buffer to contains the points</param>
/// <param name="quota">The quota of which the number of the points that the invocation of this function should provide</param>
/// <returns>The number of points that are read</returns>
public int ReadPoints(IntPtrCloudPointBuffer buffer, int quota)
{
int count = 0;
if (m_partially_accepted) // gives the points that pass the test.
{
m_filter.PrepareForCell(Access.LowerLeft, Access.UpperRight, quota);
while (quota > 0)
{
CloudPoint cp = Access.Points[m_halton_sequence[m_current_index++]];
if (m_filter.TestPoint(cp))
{
buffer.AddCloudPoint(cp); count++;
}
quota--;
}
}
else // gives all the points.
{
while (quota > 0)
{
buffer.AddCloudPoint(Access.Points[m_halton_sequence[m_current_index++]]);
quota--; count++;
}
}
return(count);
}
//Adapta la informacion de una nube de puntos a la malla
public void FillGrid(CloudPoint cloudPoint)
{
//Calcula el tamaño de la caja de limites de la nube de puntos
/*float cloudLength = (float)(cloudPoint.GetBoundingBoxMaxPoint().x - cloudPoint.GetBoundingBoxMinPoint().x);
* float cloudHeight = (float)(cloudPoint.GetBoundingBoxMaxPoint().y - cloudPoint.GetBoundingBoxMinPoint().y);
* float cloudWidth = (float)(cloudPoint.GetBoundingBoxMaxPoint().z - cloudPoint.GetBoundingBoxMinPoint().z);*/
}
public PointMatch(CloudPoint A, CloudPoint B)
{
this.A = A;
this.B = B;
var D = A.ColorLocation() - B.ColorLocation();
Distance = D.Norm();
}
public Sensor()
{
Radius = 2;
Origin = new Vector2f(2, 2);
FillColor = Color.Yellow;
Angle = 0;
Distance = 0;
Line = new DrawableLine(Simulation.Robot.Position, Position);
ID = 0;
HasFoundPoint = false;
PreviousCloudPoint = CloudPoint = new CloudPoint();
}
private void AddModifiedPoint(XYZ point, XYZ modification, double transverseDelta, int pointNumber)
{
XYZ cloudPointXYZ = point + modification * Math.Pow(transverseDelta * pointNumber, 4.0);
CloudPoint cp = new CloudPoint((float)cloudPointXYZ.X, (float)cloudPointXYZ.Y, (float)cloudPointXYZ.Z, m_color);
m_pointsBuffer[m_numberOfPoints] = cp;
m_numberOfPoints++;
if (m_numberOfPoints == s_maxNumberOfPoints)
{
TaskDialog.Show("Point cloud engine", "A single cell is requiring more than the maximum hardcoded number of points for one cell: " + s_maxNumberOfPoints);
throw new Exception("Reached maximum number of points.");
}
}
public void getCloudPoints(CloudPoint[] cloudPoints)
{
p = new float[cloudPoints.Length,3];
c = new Color[cloudPoints.Length];
// Convert CloudPoint array to 2-D location array and 1-D color array.
for (int i = 0; i < cloudPoints.Length; i++) {
p[i,0] = (float) cloudPoints[i].location[0];
p[i,1] = (float) cloudPoints[i].location[1];
p[i,2] = (float) cloudPoints[i].location[2];
c[i] = cloudPoints[i].color;
}
updatePoints = true;
}
public static void Load(string path)
{
Loaded = true;
Trajectory = new Trajectory();
PointCloud = new PointCloud();
string[] lines = File.ReadAllLines(path);
foreach (string line in lines)
{
if (!line.Contains(";") && line.Length != 0) // Ignores comments
{
string[] pointLine = line.Split(' ');
// Point coordinates
string[] coordinates = pointLine[0].Split('_');
float x = float.Parse(coordinates[0]);
float y = float.Parse(coordinates[1]);
// Theta
float theta = float.Parse(pointLine[1]);
// Adds a trajectory point object to the trajectory
TrajectoryPoint trajectoryPoint = new TrajectoryPoint(new Vector2f(x, y), theta)
{
FillColor = Color.Red
};
Trajectory.Points.Add(trajectoryPoint);
// Points found by sensors
for (byte pointFoundIndex = 0; pointFoundIndex < pointLine.Length - 2; pointFoundIndex++)
{
string[] pointFoundCoordinates = pointLine[pointFoundIndex + 2].Split('_');
float pointFoundX = float.Parse(pointFoundCoordinates[0]);
float pointFoundY = float.Parse(pointFoundCoordinates[1]);
// Adds a point object to the point cloud
CloudPoint point = new CloudPoint(new Vector2f(pointFoundX, pointFoundY))
{
FillColor = Color.Red
};
PointCloud.Points.Add(point);
}
}
}
}
/// <summary>
/// Converts CloudPoint array to float array of position coordinates and int array of color.
/// The length of <code>xyz_array</code> will be exactly 3 times longer than the one of <code>point_array</code> and
/// the length of <code>color_array</code> will be the same as the one of <code>point_array</code>.
/// </summary>
/// <param name="point_array">The input array of CloudPoint instances</param>
/// <param name="xyz_array">The output array of position coordinates</param>
/// <param name="color_array">The output array of color</param>
public static void Unpack(CloudPoint[] point_array, out float[] xyz_array, out int[] color_array)
{
xyz_array = new float[point_array.Length * 3];
color_array = new int[point_array.Length];
for (int k = 0; k < point_array.Length; k++)
{
int index = k * 3;
CloudPoint cp = point_array[k];
xyz_array[index] = cp.X;
xyz_array[index + 1] = cp.Y;
xyz_array[index + 2] = cp.Z;
color_array[k] = cp.Color;
}
}
/// <summary>
/// Converts float array of position coordinates and int array of color to an array of CloudPoint.
/// The length of <code>xyz_array</code> should be exactly 3 times longer than the one of <code>color_array</code>,
/// otherwise IndexOutOfRangeException will occur.
/// The length of <code>point_array</code> will be exactly the same as the one of color_array.
/// </summary>
/// <param name="xyz_array">The input array of position coordinates</param>
/// <param name="color_array">The input array of color</param>
/// <param name="point_array">The output array of CloudPoint instances</param>
/// <exception cref="IndexOutOfRangeException"></exception>
public static void Pack(float[] xyz_array, int[] color_array, out CloudPoint[] point_array)
{
int num_points = xyz_array.Length / 3;
point_array = new CloudPoint[num_points];
for (int k = 0; k < num_points; k++)
{
int index = k * 3;
float x = xyz_array[index];
float y = xyz_array[index + 1];
float z = xyz_array[index + 2];
int color = color_array[k];
point_array[k] = new CloudPoint(x, y, z, color);
}
}
public void LoadPointsFromPly(string f)
{
string[] lines = f.Split('\n');
int startLine = 0;
int numPoints = 0;
for(int i = 0; !lines[i].Equals("end_header"); i++){
//read header
if( lines[i].StartsWith("element vertex") ){
numPoints = int.Parse(lines[i].Substring(15, lines[i].Length-15));
}
startLine = i+2;
}
points = new CloudPoint[numPoints];
particleSystem.maxParticles = numPoints;
if(numPoints <= 0){
Debug.LogWarning("no points in the file");
return;
}
//find the bounds and set transform to the center
float minX=float.PositiveInfinity, maxX=float.NegativeInfinity, minY=float.PositiveInfinity, maxY=float.NegativeInfinity, minZ=float.PositiveInfinity, maxZ=float.NegativeInfinity;
for(int i=0; i<numPoints; i++){
string [] values = lines[i+startLine].Split(' ');
points[i]=new CloudPoint();
points[i].pos = new Vector3( -float.Parse(values[0]), float.Parse(values[1]), float.Parse(values[2]) );
points[i].col = new Color32(byte.Parse(values[3]), byte.Parse(values[4]), byte.Parse(values[5]), byte.MaxValue);
//set min max
if(points[i].pos.x > maxX) maxX = points[i].pos.x;
if(points[i].pos.x < minX) minX = points[i].pos.x;
if(points[i].pos.y > maxY) maxY = points[i].pos.y;
if(points[i].pos.y < minY) minY = points[i].pos.y;
if(points[i].pos.z > maxZ) maxZ = points[i].pos.z;
if(points[i].pos.z < minZ) minZ = points[i].pos.z;
}
cloudDimensions = new Vector3(maxX - minX, maxY - minY, maxZ - minZ);
Vector3 centerPos = new Vector3((minX+maxX)/2f, (minY+maxY)/2f, (minZ+maxZ)/2f);
//Debug.Log("center:"+centerPos);
foreach(CloudPoint cp in points){
cp.pos -= centerPos;
}
transform.position = centerPos;
}
CloudPoint[] ConvertingProcess(Vector3[] vertices, Color[] colors)
{
CloudPoint[] cloudPoints = new CloudPoint[vertices.Length];
Parallel.For(0, vertices.Length, options, (i, loopState) =>
{
lock (Thread.CurrentContext)
cloudPoints[i] = new CloudPoint(vertices[i], 1, colors[i]);
if (destroyed)
{
loopState.Stop();
return;
}
});
return(cloudPoints);
}
private void correctNegativePoints(CloudPoint[] cp)
{
// Find the most negative value for each coordinate and subtract it from every one of those resepective coordinates.
// For example, if -300 was the most negative x-coordinate in cp[], this would subtract -300 from every x-coordinate in cp[].
Vector3 negaXYZ = Vector3.zero;
for (int i = 0; i < cp.Length; i++) {
if (cp[i].location[0] < negaXYZ.x) negaXYZ.x = (float) cp[i].location[0];
if (cp[i].location[1] < negaXYZ.y) negaXYZ.y = (float) cp[i].location[1];
if (cp[i].location[2] < negaXYZ.z) negaXYZ.z = (float) cp[i].location[2];
}
for (int i = 0; i < cp.Length; i++) {
cp[i].location[0] -= negaXYZ.x;
cp[i].location[1] -= negaXYZ.y;
cp[i].location[2] -= negaXYZ.z;
}
}
// Use this for initialization
void Start()
{
meshFilter = gameObject.GetComponent <MeshFilter>();
meshFilter.mesh = mesh = new Mesh();
// Tetrahedron Test
/*StartCoroutine(Testrahedron());*/
CloudPoint p0 = new CloudPoint(new Vector(new double[] { 0, 0, 0 }), Color.red, new Vector(new double[] { 0, 0, 0 }));
CloudPoint p1 = new CloudPoint(new Vector(new double[] { 1, 0, 0 }), Color.green, new Vector(new double[] { 0, 0, 0 }));
CloudPoint p2 = new CloudPoint(new Vector(new double[] { 0.5f, 0, Mathf.Sqrt(0.75f) }), Color.blue, new Vector(new double[] { 0, 0, 0 }));
CloudPoint p3 = new CloudPoint(new Vector(new double[] { 0.5f, Mathf.Sqrt(0.75f), Mathf.Sqrt(0.75f) / 3 }), Color.yellow, new Vector(new double[] { 0, 0, 0 }));
addTriangle(p0, p1, p2);
addTriangle(p0, p2, p3);
addTriangle(p2, p1, p3);
addTriangle(p0, p3, p1);
}
public void LoadPointsFromMesh(Mesh m)
{
points = new CloudPoint[m.vertexCount];
particleSystem.maxParticles = m.vertexCount;
Debug.Log(m.vertices.Length+" Verts; "+m.colors.Length+" colors; "+m.colors32.Length+" color32");
for(int i=0; i<m.vertexCount; i++){
points[i] = new CloudPoint();
points[i].pos = m.vertices[i];
if(m.colors.Length > i){
points[i].col = m.colors[i];
}
else{
points[i].col = Color.red;
}
}
}
private CloudPoint ReadBinaryPointFromOrdered()
{
var p = new CloudPoint();
// Points are stored as XXXYYYZZZIII instead of XYZIXYZIXYZI
if (_pointIndex / (long)Header.Size[2] >= (long)Header.Points)
{
return(null);
}
for (var i = 0; i < Header.Fields.Length; i++)
{
var position = (long)Header.Points * (long)Header.Size[i] * i; // skip all points for previous dimensions
position += _pointIndex;
_binaryReader.BaseStream.Seek(position, SeekOrigin.Begin);
var bytes = _binaryReader.ReadBytes((int)Header.Size[i]);
switch (i)
{
case 2:
_pointIndex += (long)Header.Size[i];
break;
case >= 3:
continue;
}
p[i] = (Header.Type[i], Header.Size[i]) switch
{
(PcdHeader.EType.I, PcdHeader.ESize.Byte) => bytes[0] - 128,
(PcdHeader.EType.I, PcdHeader.ESize.Short) => BitConverter.ToInt16(bytes),
(PcdHeader.EType.I, PcdHeader.ESize.Single) => BitConverter.ToInt32(bytes),
(PcdHeader.EType.I, PcdHeader.ESize.Double) => BitConverter.ToInt64(bytes),
(PcdHeader.EType.U, PcdHeader.ESize.Byte) => bytes[0],
(PcdHeader.EType.U, PcdHeader.ESize.Short) => BitConverter.ToUInt16(bytes),
(PcdHeader.EType.U, PcdHeader.ESize.Single) => BitConverter.ToUInt32(bytes),
(PcdHeader.EType.U, PcdHeader.ESize.Double) => BitConverter.ToUInt64(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Byte) => BitConverter.ToSingle(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Short) => BitConverter.ToSingle(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Single) => BitConverter.ToSingle(bytes),
(PcdHeader.EType.F, PcdHeader.ESize.Double) => BitConverter.ToDouble(bytes),
_ => p[i]
};
}
return(p);
}
public CloudPoint ReadPoint()
{
var data = _streamReader.ReadLine()?.Split(' ');
if (data == null)
{
return(null);
}
var p = new CloudPoint
{
X = Convert.ToSingle(data[0], CultureInfo.InvariantCulture),
Y = Convert.ToSingle(data[1], CultureInfo.InvariantCulture),
Z = Convert.ToSingle(data[2], CultureInfo.InvariantCulture)
};
return(p);
}
private void addTriangle(CloudPoint p1, CloudPoint p2, CloudPoint p3)
{
// Add triangle to the Mesh using inputted CloudPoints to use as colored vertices.
vertices.Add(new Vector3((float) p1.location[0], (float) p1.location[1], (float) p1.location[2]));
vertices.Add(new Vector3((float) p2.location[0], (float) p2.location[1], (float) p2.location[2]));
vertices.Add(new Vector3((float) p3.location[0], (float) p3.location[1], (float) p3.location[2]));
colors.Add(p1.color);
colors.Add(p2.color);
colors.Add(p3.color);
indices.Add(indices.Count);
indices.Add(indices.Count);
indices.Add(indices.Count);
//uvs.Add(new Vector2((float) p1.location[0], (float) p1.location[2]));
//uvs.Add(new Vector2((float) p2.location[0], (float) p2.location[2]));
//uvs.Add(new Vector2((float) p3.location[0], (float) p3.location[2]));
updateMesh = true;
}
private void addTriangle(CloudPoint[] cp, int i1, int i2, int i3)
{
// Add triangle to the Mesh using inputted CloudPoint[] array and indices of points to use as colored vertices.
vertices.Add(new Vector3((float) cp[i1].location[0], (float) cp[i1].location[1], (float) cp[i1].location[2]));
vertices.Add(new Vector3((float) cp[i2].location[0], (float) cp[i2].location[1], (float) cp[i2].location[2]));
vertices.Add(new Vector3((float) cp[i3].location[0], (float) cp[i3].location[1], (float) cp[i3].location[2]));
colors.Add(cp[i1].color);
colors.Add(cp[i2].color);
colors.Add(cp[i3].color);
indices.Add(indices.Count);
indices.Add(indices.Count);
indices.Add(indices.Count);
//uvs.Add(new Vector2((float) cp[i1].location[0], (float) cp[i1].location[2]));
//uvs.Add(new Vector2((float) cp[i2].location[0], (float) cp[i2].location[2]));
//uvs.Add(new Vector2((float) cp[i3].location[0], (float) cp[i3].location[2]));
//normals.Add()
updateMesh = true;
}
public static void ExportToStream(this Ellipsis ellipsis, ICloudStreamWriter writer, int size = 50, float resolution = 0.01f)
{
for (var x = -size; x < size; x++)
{
for (var y = -size; y < size; y++)
{
var p = new CloudPoint(ellipsis.Center.X + x * resolution, ellipsis.Center.Y + y * resolution, ellipsis.Center.Z)
{
Intensity = (float)ellipsis.Intensity
};
if (ellipsis.Contains(p))
{
writer.WritePoint(p);
}
}
}
}
private void addTriangle(CloudPoint p1, CloudPoint p2, CloudPoint p3)
{
// Add triangle to the Mesh using inputted CloudPoints to use as colored vertices.
vertices.Add(new Vector3((float)p1.location[0], (float)p1.location[1], (float)p1.location[2]));
vertices.Add(new Vector3((float)p2.location[0], (float)p2.location[1], (float)p2.location[2]));
vertices.Add(new Vector3((float)p3.location[0], (float)p3.location[1], (float)p3.location[2]));
colors.Add(p1.color);
colors.Add(p2.color);
colors.Add(p3.color);
indices.Add(indices.Count);
indices.Add(indices.Count);
indices.Add(indices.Count);
//uvs.Add(new Vector2((float) p1.location[0], (float) p1.location[2]));
//uvs.Add(new Vector2((float) p2.location[0], (float) p2.location[2]));
//uvs.Add(new Vector2((float) p3.location[0], (float) p3.location[2]));
updateMesh = true;
}
void Start()
{
// Set the control camera to be the Main Camera.
cam = Camera.mainCamera.camera;
// Get new CloudPoint data.
CloudPoint[] cloudP = new CloudPoint[4];
cloudP[0] = new CloudPoint(new Vector(new double[] { -562.1205, 562.1205, -2230 }), Color.red, new Vector(new double[] { 0, 0, 0 }));
cloudP[1] = new CloudPoint(new Vector(new double[] { -549.9371, 565.6496, -2244 }), Color.red, new Vector(new double[] { 0, 0, 0 }));
cloudP[2] = new CloudPoint(new Vector(new double[] { -534.2246, 565.6496, -2244 }), Color.red, new Vector(new double[] { 0, 0, 0 }));
cloudP[3] = new CloudPoint(new Vector(new double[] { -518.5121, 565.6496, -2244 }), Color.red, new Vector(new double[] { 0, 0, 0 }));
// Convert the CloudPoint data and fill the p locations matrix and c colors matrix.
getCloudPoints(cloudP);
// Fix all negative points and force them into the unit cube.
normalizePoints();
// Reset Camera to focus at center of unit cube with original viewing angle.
//resetCamera();
// Initial point cloud rendering.
updatePoints = true;
}
public CloudPoint ReadPoint()
{
if (_pointId >= _pointsInCurrentGroup)
{
return(null);
}
var p = new CloudPoint();
for (var i = 0; i < Header.Fields.Length; i++)
{
var bytes = _binaryReader.ReadBytes((int)Header.Size[i]);
if (i >= 3)
{
continue;
}
p[i] = (Header.Type[i], Header.Size[i]) switch
{
(GpdHeader.EType.I, GpdHeader.ESize.Byte) => bytes[0] - 128,
(GpdHeader.EType.I, GpdHeader.ESize.Short) => BitConverter.ToInt16(bytes),
(GpdHeader.EType.I, GpdHeader.ESize.Single) => BitConverter.ToInt32(bytes),
(GpdHeader.EType.I, GpdHeader.ESize.Double) => BitConverter.ToInt64(bytes),
(GpdHeader.EType.U, GpdHeader.ESize.Byte) => bytes[0],
(GpdHeader.EType.U, GpdHeader.ESize.Short) => BitConverter.ToUInt16(bytes),
(GpdHeader.EType.U, GpdHeader.ESize.Single) => BitConverter.ToUInt32(bytes),
(GpdHeader.EType.U, GpdHeader.ESize.Double) => BitConverter.ToUInt64(bytes),
(GpdHeader.EType.F, GpdHeader.ESize.Byte) => BitConverter.ToSingle(bytes),
(GpdHeader.EType.F, GpdHeader.ESize.Short) => BitConverter.ToSingle(bytes),
(GpdHeader.EType.F, GpdHeader.ESize.Single) => BitConverter.ToSingle(bytes),
(GpdHeader.EType.F, GpdHeader.ESize.Double) => BitConverter.ToDouble(bytes),
_ => p[i]
};
}
_pointId++;
return(p);
}
/// <summary>
/// Performs ray casting to the point cloud with a given ray.
/// The ray sweeps the points and collects the points
/// of which distance to the ray is less than the ray radius.
/// Among the points, the one that is nearest to the ray origin will be picked.
/// </summary>
/// <param name="ray_origin">The ray origin</param>
/// <param name="ray_direction">The ray direction</param>
/// <param name="ray_radius">The radius of ray sweeping</param>
/// <param name="idx">The index of the point that is picked</param>
/// <returns>true if there is a point picked, false otherwise</returns>
public bool Cast(XYZ ray_origin, XYZ ray_direction, float ray_radius, out uint idx)
{
Transform t = this.Transform.Inverse;
ray_origin = t.OfPoint(ray_origin);
ray_direction = t.OfVector(ray_direction);
idx = 0;
Func <XYZ, XYZ, XYZ, double> Distance = (o, d, p) =>
{
XYZ AB = p - o;
return(((d.DotProduct(AB) / d.DotProduct(d)) * d - AB).GetLength());
};
List <uint> indices_of_the_points_swept_by_the_ray = new List <uint>();
// collect indices of the points of which distance
// to the ray is less than ray_radius.
for (int k = 0; k < m_points.Length; k++)
{
CloudPoint cp = m_points[k];
XYZ cpos = new XYZ(cp.X, cp.Y, cp.Z);
if (Distance(ray_origin, ray_direction, cpos) < ray_radius)
{
indices_of_the_points_swept_by_the_ray.Add((uint)k);
}
}
if (indices_of_the_points_swept_by_the_ray.Any() == false)
{
return(false);
}
// find index of the point having minimum distance to viewer (ray_origin).
return(FindSurfaceRevitPluginUtils.MinElement(indices_of_the_points_swept_by_the_ray, i => ray_origin.DistanceTo(new XYZ(m_points[(int)i].X, m_points[(int)i].Y, m_points[(int)i].Z)), out idx));
}
void Update()
{
if (freeze)
{
return;
}
MaterialPropertyBlock props = new MaterialPropertyBlock();
IList <CloudPoint> cloudpoint = null;
cloudpoint = skeletonMode ? iRemoteService.GetSkeleton() : iRemoteService.GetCloudpoints();
pointColor.a = 0;
float max = 0;
int i;
int bodyPointIndex;
int stepRange = 1;
if (detailedFace)
{
stepRange = cloudpoint.Count / bodyPoints.Length;
}
for (bodyPointIndex = 0, i = 0;
i < cloudpoint.Count && bodyPointIndex < bodyPoints.Length;
i += stepRange, bodyPointIndex++)
{
CloudPoint item = cloudpoint[i];
float x, y, z;
float secondFactor;
pointColor.r = item.GetR();
pointColor.g = item.GetG();
pointColor.b = item.GetB();
bodyPoints[bodyPointIndex].SetActive(true);
if (skeletonMode)
{
z = item.GetZ() * 10;
x = (item.GetX() * 10);
y = (item.GetY() * 10);
}
else
{
z = item.GetZ() / 100;
secondFactor = z * factor;
x = (item.GetX() - width / 2.0f) * secondFactor; // x und y mal z genommen
y = -(item.GetY() - height / 2.0f) * secondFactor; //180° Drehung
}
// y - 2 für Cameraanpassung bei reset
bodyPoints[bodyPointIndex].transform.localPosition = new Vector3(x, y - 2, z - distanceToHead);
bodyPoints[bodyPointIndex].transform.localScale = new Vector3(cubeScale, cubeScale, cubeScale);
props.SetColor("_Color", pointColor);
bodyPoints[bodyPointIndex].GetComponent <MeshRenderer>().SetPropertyBlock(props);
if (max < y)
{
max = y;
currentDistanceToHead = z;
}
}
pointColor.a = 1;
for (int j = bodyPointIndex; j < bodyPoints.Length; j++)
{
bodyPoints[j].SetActive(false);
}
}
/// <summary>
/// Copies the cloud point to the buffer.
/// </summary>
/// <param name="cp">The cloud point to copy</param>
public void AddCloudPoint(CloudPoint cp)
{
*(m_buffer + m_point_count) = cp;
m_point_count++;
}
private float Max(CloudPoint[] cp)
{
float max = (float) cp[0].location[0];
// Search through points[] array and return the max recorded value within the array.
for (int i = 1; i < cp.Length; i++)
for (int j = 0; j < 3; j++)
if (cp[i].location[j] > max)
max = (float) cp[i].location[j];
return max;
}
/// <summary>
/// Start listening on specified port and adding data to the BlockingCollection
/// </summary>
/// <param name="threadInfos">Thread informations - connection params</param>
override protected void StartListening(object threadInfos)
{
UdpThreadInfos <Cloud> ti = (UdpThreadInfos <Cloud>)threadInfos;
Console.WriteLine("UdpCloudListener thread started");
_udp = new UdpClient(ti.Port);
IPEndPoint remoteEP = new IPEndPoint(IPAddress.Any, ti.Port);
Cloud aggregateCloud = null;
while (true)
{
byte[] data = null;
try
{
data = _udp.Receive(ref remoteEP);
}
catch (Exception ex)
{
}
var cloud = new Cloud
{
Timestamp = BitConverter.ToInt64(data, 0),
Points = new List <CloudPoint>()
};
int currentByte = 8;
while (currentByte < data.Length)
{
var point = new CloudPoint();
point.X = BitConverter.ToSingle(data, currentByte);
currentByte += 4;
point.Y = BitConverter.ToSingle(data, currentByte);
currentByte += 4;
point.Z = BitConverter.ToSingle(data, currentByte);
currentByte += 4;
point.R = data.ElementAt(currentByte);
currentByte++;
point.G = data.ElementAt(currentByte);
currentByte++;
point.B = data.ElementAt(currentByte);
currentByte++;
point.Tag = data.ElementAt(currentByte);
currentByte++;
// Filter points (0,0,0)
if (point.X != 0 && point.Y != 0 && point.Z != 0)
{
cloud.Points.Add(point);
}
}
// First frame
if (aggregateCloud == null)
{
aggregateCloud = cloud;
}
// If changing frame, send last complete received one
else if (aggregateCloud.Timestamp != cloud.Timestamp)
{
ti.DataTransferer.Add(aggregateCloud);
aggregateCloud = cloud;
}
// If same frame, aggregate
else
{
aggregateCloud.Points.AddRange(cloud.Points);
}
}
}
// clone constructor so that we can do the iterative stuff without messing up the underlying cloud
public CloudPoint(CloudPoint o)
{
this.color = o.color;
this.location = o.location;
this.normal = o.normal;
}
// clone constructor so that we can do the iterative stuff without messing up the underlying cloud
public CloudPoint(CloudPoint o)
{
this.color = o.color;
this.location = o.location;
this.normal = o.normal;
}
public void LoadPointsFromPts(string f)
{
string[] lines = f.Split('\n');
int startLine = 0;
int numPoints = lines.Length - startLine;
if(lines[0].Trim().Split(' ').Length == 1){
//parse numPoints from first line
numPoints = int.Parse(lines[0].Trim());
startLine = 1;
}
//Debug.Log("num points: "+numPoints);
points = new CloudPoint[numPoints];
particleSystem.maxParticles = numPoints;
if(numPoints <= 0){
Debug.LogWarning("no points in the file");
return;
}
//find the bounds and set transform to the center
float minX=float.PositiveInfinity, maxX=float.NegativeInfinity, minY=float.PositiveInfinity, maxY=float.NegativeInfinity, minZ=float.PositiveInfinity, maxZ=float.NegativeInfinity;
for(int i=0; i<numPoints; i++){
//for(int i=startLine; i<lines.Length; i++){
string [] values = lines[i+startLine].Split(' ');
//Debug.Log("values = "+lines[i+startLine]);
points[i]=new CloudPoint();
//rotate 90
points[i].pos = new Vector3( float.Parse(values[0]), float.Parse(values[2]), -float.Parse(values[1]) );
points[i].col = new Color32(byte.Parse(values[values.Length-3]), byte.Parse(values[values.Length-2]), byte.Parse(values[values.Length-1]), byte.MaxValue);
//aha! http://www.las-vegas.uni-osnabrueck.de/index.php/tutorials2/8-understanding-file-formats-pts-and-3d-files
//set min max
if(points[i].pos.x > maxX) maxX = points[i].pos.x;
if(points[i].pos.x < minX) minX = points[i].pos.x;
if(points[i].pos.y > maxY) maxY = points[i].pos.y;
if(points[i].pos.y < minY) minY = points[i].pos.y;
if(points[i].pos.z > maxZ) maxZ = points[i].pos.z;
if(points[i].pos.z < minZ) minZ = points[i].pos.z;
}
cloudDimensions = new Vector3(maxX - minX, maxY - minY, maxZ - minZ);
Vector3 centerPos = new Vector3((minX+maxX)/2f, (minY+maxY)/2f, (minZ+maxZ)/2f);
//Debug.Log("center:"+centerPos);
foreach(CloudPoint cp in points){
cp.pos -= centerPos;
}
transform.position = centerPos;
}
private void AddModifiedPoint(XYZ point, XYZ modification, double transverseDelta, int pointNumber)
{
XYZ cloudPointXYZ = point + modification * Math.Pow(transverseDelta * pointNumber, 4.0);
CloudPoint cp = new CloudPoint((float)cloudPointXYZ.X, (float)cloudPointXYZ.Y, (float)cloudPointXYZ.Z, m_color);
m_pointsBuffer[m_numberOfPoints] = cp;
m_numberOfPoints++;
if (m_numberOfPoints == s_maxNumberOfPoints)
{
TaskDialog.Show("Point cloud engine", "A single cell is requiring more than the maximum hardcoded number of points for one cell: " + s_maxNumberOfPoints);
throw new Exception("Reached maximum number of points.");
}
}
void Start()
{
// Set the control camera to be the Main Camera.
cam = Camera.mainCamera.camera;
// Get new CloudPoint data.
CloudPoint[] cloudP = new CloudPoint[4];
cloudP[0] = new CloudPoint(new Vector(new double[] {-562.1205,562.1205,-2230}), Color.red, new Vector(new double[] {0,0,0}));
cloudP[1] = new CloudPoint(new Vector(new double[] {-549.9371,565.6496,-2244}), Color.red, new Vector(new double[] {0,0,0}));
cloudP[2] = new CloudPoint(new Vector(new double[] {-534.2246,565.6496,-2244}), Color.red, new Vector(new double[] {0,0,0}));
cloudP[3] = new CloudPoint(new Vector(new double[] {-518.5121,565.6496,-2244}), Color.red, new Vector(new double[] {0,0,0}));
// Convert the CloudPoint data and fill the p locations matrix and c colors matrix.
getCloudPoints(cloudP);
// Fix all negative points and force them into the unit cube.
normalizePoints();
// Reset Camera to focus at center of unit cube with original viewing angle.
//resetCamera();
// Initial point cloud rendering.
updatePoints = true;
}
public double PushOntoCloud(PointCloud other, int iterations, int matchLength, double threshold)
{
// we are going to update our transform to place us on the other cloud with the ICP
double error = 0;
#region Correspondence
// make the list of matches
// NOTE this cannot be a SortedList because that will complain if you give multiple pairs with the same dist
List <PointMatch> matches = new List <PointMatch>();
// go over our list of features
foreach (CloudPoint p in this.FeatureTree)
{
// find nearest in their list of features after our transform
var pT = p.ApplyTransform(R, T);
CloudPoint o = other.FeatureTree.FindNearestNeighbor(pT.ColorLocation());
PointMatch match = new PointMatch(pT, o);
matches.Add(match);
}
// select the top ni matches based on how close they are
var topMatches = matches.OrderBy(x => x.Distance).Take(matchLength);
#endregion
#region find normals for matchpoints
var ourQueryPoints = topMatches.Select(x => x.A.UnApplyTransform(R, T));
this.CalculateNormals(ourQueryPoints);
var theirQueryPoints = topMatches.Select(x => x.B);
other.CalculateNormals(theirQueryPoints);
#endregion
// iterative part
for (int i = 0; i < iterations; i++)
{
#region refine ICP estimate
Matrix cov = new Matrix(6, 6);
Vector b = new Vector(6);
var r = Vector.Create(new double[] { R[2, 1], R[0, 2], R[1, 0] });
foreach (PointMatch pm in topMatches)
{
// moving A onto B
var A = pm.A;
var B = pm.B;
Vector ni = B.normal;
Vector ci = Vector.CrossProduct(A.location, ni);
Vector CN = Vector.Create(ci.Concat(ni).ToArray());
cov = cov + (CN.ToColumnMatrix() * CN.ToRowMatrix());
var diffDot = (A.location - B.location) * ni; // this is a dot product
b = b - (diffDot * Vector.Ones(6)).ArrayMultiply(CN);
// also accumulate error for this RT since we're here already
error = error + Math.Pow((diffDot + (T * ni) + (r * ci)), 2);
}
// done accumulating cov and B
// we're done here if our thing put us close enough
if (error < threshold)
{
break;
}
// solve cov * inc_transform = b
// compute decomp
var chol = cov.CholeskyDecomposition;
// solve LL' * inc_transform = b
var L = chol.TriangularFactor;
Vector inc_transform = LLTSolve(L, b);
R = Matrix.Create(new double[, ] {
{ 1, -inc_transform[2], inc_transform[1] },
{ inc_transform[2], 1, -inc_transform[0] },
{ -inc_transform[1], inc_transform[0], 1 }
});
// last three components are translation
T = Vector.Create(inc_transform.Skip(3).ToArray());
#endregion
}
// apply transform to whole cloud
ApplyTransform();
return(error);
}
/// <summary>
/// Finds the point on an obstacle, if there's one.
/// </summary>
public void FindPoint()
{
PreviousCloudPoint = CloudPoint;
// Updates the sensor vertices
Line.A = Simulation.Robot.Position; // First vertex: robot's center
Line.B = Position; // Second vertex: sensor's center
// Contains cloud point objects and their distance from the sensor.
Dictionary <CloudPoint, float> CloudPoints = new Dictionary <CloudPoint, float>();
foreach (DrawableLine wallLine in Simulation.Obstacles) // Determines the intersection point for each wall
{
if (Line.M != wallLine.M) // If the lines aren't parallel, let's solve the equation system
{
CloudPoint cloudPoint = GeometryTools.SolveEquationSystem(Line, wallLine);
Distance = (float)Math.Sqrt(Math.Pow(cloudPoint.Position.X - Simulation.Robot.Position.X, 2) + Math.Pow(cloudPoint.Position.Y - Simulation.Robot.Position.Y, 2));
// Determines if the point is on the segment
if (wallLine.A.X != wallLine.B.X) // Non-vertical wall
{
if (cloudPoint.Position.X >= wallLine.A.X && cloudPoint.Position.X <= wallLine.B.X)
{
if (Line.A.X != Line.B.X) // Non-vertical sensor line
{
if (Position.X >= Simulation.Robot.Position.X && cloudPoint.Position.X >= Position.X) // Right
{
CloudPoints.Add(cloudPoint, Distance);
}
else if (Position.X < Simulation.Robot.Position.X && cloudPoint.Position.X <= Position.X) // left
{
CloudPoints.Add(cloudPoint, Distance);
}
}
else // Vertical sensor line
{
if (Position.Y < Simulation.Robot.Position.Y && cloudPoint.Position.Y < Position.Y) // Up
{
CloudPoints.Add(cloudPoint, Distance);
}
else if (Position.Y > Simulation.Robot.Position.Y && cloudPoint.Position.Y > Position.Y) // Down
{
CloudPoints.Add(cloudPoint, Distance);
}
}
}
}
else // Vertical wall
{
if (cloudPoint.Position.Y >= wallLine.A.Y && cloudPoint.Position.Y <= wallLine.B.Y)
{
if (Position.X >= Simulation.Robot.Position.X && cloudPoint.Position.X >= Position.X)
{
CloudPoints.Add(cloudPoint, Distance);
}
else if (Position.X < Simulation.Robot.Position.X && cloudPoint.Position.X <= Position.X)
{
CloudPoints.Add(cloudPoint, Distance);
}
}
}
}
}
// PointDistances now has points which are pointed by the sensor and located on obstacles (drawable line segments)
// Finds the nearest point
if (CloudPoints.Count != 0)
{
var nearestPoint = CloudPoints.OrderBy(pair => pair.Value).Take(1); // Sorts by ascending distances
HasFoundPoint = true;
CloudPoint = nearestPoint.ElementAt(0).Key;
CloudPoint.FillColor = Color.Green;
CloudPoint.DistanceFromSensor = Distance;
Simulation.Robot.PointCloud.Points.Add(CloudPoint); // Selects the first point (the nearest one)
}
}
public void WritePoint(CloudPoint point)
{
throw new NotImplementedException();
}
public void WritePoint(CloudPoint point)
{
_streamWriter.WriteLine($"{point.X:0.########} {point.Y:0.########} {point.Z:0.########} {point.Intensity:0.########}");
}
public PointMatch(CloudPoint A, CloudPoint B)
{
this.A = A;
this.B = B;
var D = A.ColorLocation() - B.ColorLocation();
Distance = D.Norm();
}
public void LoadPointsFromXyz(string f)
{
string[] lines = f.Split('\n');
int startLine = 0;
int numPoints = lines.Length-1; //expects last line to be empty
points = new CloudPoint[numPoints];
particleSystem.maxParticles = numPoints;
if(numPoints <= 0){
Debug.LogWarning("no points in the file");
return;
}
//find the bounds and set transform to the center
float minX=float.PositiveInfinity, maxX=float.NegativeInfinity, minY=float.PositiveInfinity, maxY=float.NegativeInfinity, minZ=float.PositiveInfinity, maxZ=float.NegativeInfinity;
for(int i=0; i<numPoints; i++){
string [] values = lines[i+startLine].Split(' ');
points[i]=new CloudPoint();
if(values.Length >= 6){
points[i].pos = new Vector3( float.Parse(values[0]), float.Parse(values[1]), float.Parse(values[2]) );
points[i].col = new Color32(byte.Parse(values[3]), byte.Parse(values[4]), byte.Parse(values[5]), byte.MaxValue);
//set min max
if(points[i].pos.x > maxX) maxX = points[i].pos.x;
if(points[i].pos.x < minX) minX = points[i].pos.x;
if(points[i].pos.y > maxY) maxY = points[i].pos.y;
if(points[i].pos.y < minY) minY = points[i].pos.y;
if(points[i].pos.z > maxZ) maxZ = points[i].pos.z;
if(points[i].pos.z < minZ) minZ = points[i].pos.z;
}
else{
Debug.LogWarning("couldn't parse "+lines[i+startLine]);
}
}
cloudDimensions = new Vector3(maxX - minX, maxY - minY, maxZ - minZ);
Vector3 centerPos = new Vector3((minX+maxX)/2f, (minY+maxY)/2f, (minZ+maxZ)/2f);
//Debug.Log("center:"+centerPos);
foreach(CloudPoint cp in points){
cp.pos -= centerPos;
}
transform.position = centerPos;
}
// Use this for initialization
void Start()
{
meshFilter = gameObject.GetComponent<MeshFilter>();
meshFilter.mesh = mesh = new Mesh();
// Tetrahedron Test
/*StartCoroutine(Testrahedron());*/
CloudPoint p0 = new CloudPoint(new Vector(new double[] {0,0,0}), Color.red, new Vector(new double[] {0,0,0}));
CloudPoint p1 = new CloudPoint(new Vector(new double[] {1,0,0}), Color.green, new Vector(new double[] {0,0,0}));
CloudPoint p2 = new CloudPoint(new Vector(new double[] {0.5f,0,Mathf.Sqrt(0.75f)}), Color.blue, new Vector(new double[] {0,0,0}));
CloudPoint p3 = new CloudPoint(new Vector(new double[] {0.5f,Mathf.Sqrt(0.75f),Mathf.Sqrt(0.75f)/3}), Color.yellow, new Vector(new double[] {0,0,0}));
addTriangle(p0,p1,p2);
addTriangle(p0,p2,p3);
addTriangle(p2,p1,p3);
addTriangle(p0,p3,p1);
}
void SetPointsAsync()
{
Debug.Log("Scan start!");
int newPointsArrayCount = Mathf.Min(maxPointsNumInAProcess, totalPointCount - maxPointsNumInAProcess * processedSectionCount);
Debug.Log("new points array count : " + newPointsArrayCount);
continuos = (newPointsArrayCount >= maxPointsNumInAProcess);
CloudPoint[] points = new CloudPoint[newPointsArrayCount];
int currentCount = 0;
Parallel.For(0, newPointsArrayCount, options, (i, loopState) =>
{
try
{
string _read;
lock (Thread.CurrentContext)
_read = reader.ReadLine();
if (_read == null || _read == "")
{
Debug.LogError("reading failed!");
}
else
{
string[] data = _read.Split(' ');
points[i] = new CloudPoint(
PointConvertWithAxis(new Vector3(
float.Parse(data[0]) * sizeScale,
float.Parse(data[1]) * sizeScale,
float.Parse(data[2]) * sizeScale
)),
Int32.Parse(data[3]),
new Color(
float.Parse(data[4]) / 255f,
float.Parse(data[5]) / 255f,
float.Parse(data[6]) / 255f
));
}
}
catch (Exception e)
{
Debug.LogError(e);
Debug.LogException(e);
Debug.LogError("Dead!!!!!!!!!!!!!");
}
processedPointCount++;
currentCount++;
if (destroy)
{
loopState.Stop();
return;
}
});
Debug.Log("process up! : " + currentCount + "/" + points.Length);
processUp?.Invoke(this, new ProcessUpArgs(points));
processedSectionCount++;
}
private void normalizePoints(CloudPoint[] cp)
{
correctNegativePoints(cp);
float max = Max(cp);
// Force every point to be in the unit cube.
for (int i = 0; i < cp.Length; i++) {
cp[i].location[0] /= max;
cp[i].location[1] /= max;
cp[i].location[2] /= max;
}
}
