void LateUpdate()
{
int x = 0;
int y = 0;
short[] rawDepthhMap = ZigInput.Depth.data;
short[] rawLabelMap = ZigInput.LabelMap.data;
for (int i = cycle * emitterCount; i < (cycle + 1) * emitterCount; i++)
{
particleEmitters[i].ClearParticles();
for (int particleIndex = 0; particleIndex < MAX_PARTICLES_PER_PE; particleIndex++)
{
if (y >= YScaled)
{
break;
}
Vector3 scale = transform.localScale;
int index = x * factorX + XRes * factorY * y;
Vector3 vec = new Vector3(x * factorX, y * factorY, rawDepthhMap[index]);
vec = worldSpace ? ZigInput.ConvertImageToWorldSpace(vec) : vec;
vec = Vector3.Scale(vec, scale);
if (onlyUsers)
{
if (rawLabelMap[index] != 0)
{
particleEmitters[i].Emit(transform.rotation * vec + transform.position, velocity, size, energy, color);
}
}
else
{
particleEmitters[i].Emit(transform.rotation * vec + transform.position, velocity, size, energy, color);
}
x = (x + 1) % XScaled;
y = (x == 0) ? y + 1 : y;
}
if (y >= YScaled)
{
break;
}
}
cycle = (cycle + 1) % cycles;
}
internal void CalculateNormal(C5.IPriorityQueue <CloudPoint> neighbors)
{
Vector avg = neighbors.Aggregate(new Vector(3), (sum, val) => sum + val.location) / (double)neighbors.Count;
Matrix cov = ((double)1 / neighbors.Count) *
neighbors.Aggregate(new Matrix(3, 3),
(sum, val) => sum +
(val.location - avg).ToColumnMatrix() *
(val.location - avg).ToRowMatrix());
// that dude says the smallest eigenvalue is the normal
// first column of eigenvectors is the smallest eigenvalued one
normal = cov.EigenVectors.GetColumnVector(0);
// orient normal toward viewpoint
// to do this we have to push the origin into the world frame
//v = ZigInput.ConvertImageToWorldSpace(v);
var viewp = ZigInput.ConvertImageToWorldSpace(Vector3.zero);
normal = ((normal * (viewp.ToVector() - this.location)) > 0 ? normal : -normal);
normal.Normalize();
}
public PointCloud(short[] depth, int dWidth, int dHeight,
Color32[] color, int cWidth, int cHeight)
{
PointList = new List <CloudPoint>();
// these are different sizes. use the depth image as the native index
// we want to get as much depth data as possible
this.rawColor = color;
this.rawDepth = depth;
this.depthX = dWidth;
this.depthY = dHeight;
this.colorX = cWidth;
this.colorY = cHeight;
// express scaling factors to take x,y positions in depth to color
// also we are assuming that the color image is larger than the depth one
int factorX = cWidth / dWidth;
int factorY = cHeight / dHeight;
for (int i = 0; i < depth.Length; i++)
{
// the transform that the Zig thing provides expects coordinates in the image plane though
if ((depth[i] == 0) || (depth[i] == -1))
{
continue; // this is a garbage point
}
int x = (i % dWidth) * factorX;
int y = (i / dWidth) * factorY;
int z = depth[i];
Vector3 v = new Vector3(x, y, z);
v = ZigInput.ConvertImageToWorldSpace(v);
int cIndex = x + y * cWidth;
PointList.Add(new CloudPoint(v.ToVector(), color[cIndex], Vector.Zeros(3)));
}
depthHistogramMap = new float[MaxDepth];
depthToColor = new Color32[MaxDepth];
colorizedDepth = new Color32[depthX * depthY];
UpdateHistogram();
this.R = Matrix.Create(new double[, ] {
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
}); // identity rotation
this.T = Vector.Zeros(3); // zero translation
//Transform = new Matrix(new double[,]{{1, 0, 0, 0},
// {0, 1, 0, 0},
// {0, 0, 1, 0},
// {0, 0, 0, 1}}); // homogeneous identity
}
