private static void TestPointCloud()
{
System.IO.Directory.CreateDirectory(@"..\..\testData\Output");
// TEST DotNet3d.ply from DotNet3d.Pointcloud
{
var ply = new DotNet3d.Ply(@"..\..\testData\Frame_00600.ply"); // ~200ms
var pc = ply.ToPointCloud(); // ~1ms
var moveToOrigin = DotNet3d.Matrix.Translation(-722.58, 123.87, -2805.85); // ~1ms
var rotateX90 = DotNet3d.Matrix.RotationX(90); // ~1ms
var rotateY180 = DotNet3d.Matrix.RotationY(180); // ~1ms
var rotateZ30 = DotNet3d.Matrix.RotationZ(30); // ~1ms
var superMatrix = rotateZ30 * rotateY180 * rotateX90 * moveToOrigin; // ~1ms
pc.Multiply(superMatrix); // ~1400ms
var ply2 = new DotNet3d.Ply(pc); // ~1ms
ply2.SaveBinary(@"..\..\testData\Output\Frame_00600-ONFLOOR-FACINGZ.ply"); // ~1100ms
}
// Test writing matrix as ply
{
var mat = new DotNet3d.Matrix();
var ply = new DotNet3d.Ply(mat);
ply.SaveAscii(@"..\..\testData\Output\IdentityMatrix.ply");
}
// Test writing array of matrices as ply
{
var mat1 = DotNet3d.Matrix.Identity();
var mat2 = DotNet3d.Matrix.Identity();
mat2.Translate(1000, 1000, 1000);
var matArr = new DotNet3d.Matrix[2];
matArr[0] = mat1;
matArr[1] = mat2;
var ply = new DotNet3d.Ply(matArr);
ply.SaveBinary(@"..\..\testData\Output\2Matices.ply");
}
// Test seting, getting
{
float x = 1;
float y = 2;
float z = 3;
var mat = DotNet3d.Matrix.Translation(x, y, z);
var ply = new DotNet3d.Ply(mat);
var pc = ply.ToPointCloud();
Trace.Assert((float)pc.Get("x", 0) == x);
Trace.Assert((float)pc.Get("y", 0) == y);
Trace.Assert((float)pc.Get("z", 0) == z);
Trace.Assert((float)pc.Get("x", 1) == (x + 500));
Trace.Assert((float)pc.Get("y", 1) == y);
Trace.Assert((float)pc.Get("z", 1) == z);
Trace.Assert((float)pc.Get("x", 2) == x);
Trace.Assert((float)pc.Get("y", 2) == (y + 500));
Trace.Assert((float)pc.Get("z", 2) == z);
Trace.Assert((float)pc.Get("x", 3) == x);
Trace.Assert((float)pc.Get("y", 3) == y);
Trace.Assert((float)pc.Get("z", 3) == (z + 1000));
float dummy = 9;
pc.Set("x", 0, dummy);
Trace.Assert((float)pc.Get("x", 0) == dummy);
byte dummy2 = 32;
pc.Set("red", 1, dummy2);
Trace.Assert((byte)pc.Get("red", 1) == dummy2);
}
// Test dividing by z
{
float x = 2;
float y = 4;
float z = 8;
var mat = DotNet3d.Matrix.Translation(x, y, z);
var ply = new DotNet3d.Ply(mat);
var pc = ply.ToPointCloud();
Trace.Assert((float)pc.Get("x", 0) == x);
Trace.Assert((float)pc.Get("y", 0) == y);
Trace.Assert((float)pc.Get("z", 0) == z);
pc.DividePositionByZ();
Trace.Assert((float)pc.Get("x", 0) == x / z);
Trace.Assert((float)pc.Get("y", 0) == y / z);
Trace.Assert((float)pc.Get("z", 0) == z / z);
}
// Test drawing on image
{
int size = 256;
var pointArr = new DotNet3d.Point[size];
for (int i = 0; i < size; i++)
{
pointArr[i] = new DotNet3d.Point();
pointArr[i].X = i;
pointArr[i].Y = i;
pointArr[i].Z = i;
}
var pc = new DotNet3d.PointCloud(pointArr);
var newImg = new System.Drawing.Bitmap(size, size);
var src = System.IO.Path.GetFullPath(@"..\..\testData\Output\TestDrawOnImage-EMPTY.png");
var dst = System.IO.Path.GetFullPath(@"..\..\testData\Output\TestDrawOnImage-LINE.png");
newImg.Save(src); // src: black 256x256 image
pc.DrawOnImage(src, dst); // dst: black 256x256 with red line from upper left to lower right
}
// Test drawing on image, 8-bit index
{
int size = 3840;
var pointArr = new DotNet3d.Point[size];
for (int i = 0; i < size; i++)
{
pointArr[i] = new DotNet3d.Point();
pointArr[i].X = i;
pointArr[i].Y = i;
pointArr[i].Z = i;
}
var pc = new DotNet3d.PointCloud(pointArr);
var src = System.IO.Path.GetFullPath(@"..\..\testData\Mask-Cam0000-F00001-8bitIndexed.png");
var dst = System.IO.Path.GetFullPath(@"..\..\testData\Output\Mask-Cam0000-F00001-8bitIndexed-LINE.png");
pc.DrawOnImage(src, dst); // dst: 5120x3840 mask with white image of person, with red line that starts at upper left and ends at bottom right (if bottom right were square)
}
// Test adding point clouds
{
float x1 = 2;
float y1 = 4;
float z1 = 8;
var mat1 = DotNet3d.Matrix.Translation(x1, y1, z1);
var ply1 = new DotNet3d.Ply(mat1);
var one = ply1.ToPointCloud();
float x2 = 3;
float y2 = 6;
float z2 = 9;
var mat2 = DotNet3d.Matrix.Translation(x2, y2, z2);
var ply2 = new DotNet3d.Ply(mat2);
var two = ply2.ToPointCloud();
DotNet3d.PointCloud three = one + two;
Trace.Assert(three.ToString() == (one.ToString() + two.ToString()));
Trace.Assert(three.Data.Length == (one.Data.Length + two.Data.Length));
for (int i = 0; i < one.Data.Length; i++)
{
Trace.Assert(three.Data[i] == one.Data[i]);
int j = i + one.Data.Length; // second half of data
Trace.Assert(three.Data[j] == two.Data[i]);
}
var ply3 = new DotNet3d.Ply(three);
ply3.SaveAscii(@"..\..\testData\Output\AddingPointClouds-Ascii.ply");
}
// Test multiply point cloud by a matrix
{
DotNet3d.Ply ply = new DotNet3d.Ply(DotNet3d.Matrix.Identity());
DotNet3d.PointCloud pc = ply.ToPointCloud();
Trace.Assert(pc.ToString() == "0 0 0 0 0 0 255\n500 0 0 255 0 0 255\n0 500 0 0 255 0 255\n0 0 1000 0 0 255 255\n");
pc.Multiply(DotNet3d.Matrix.Scaling(2, 2, 2));
Trace.Assert(pc.ToString() == "0 0 0 0 0 0 255\n1000 0 0 255 0 0 255\n0 1000 0 0 255 0 255\n0 0 2000 0 0 255 255\n");
// test point cloud with normals
int numPoints = 1;
var dotNetTypes = new List <string> {
"System.Single", "System.Single", "System.Single", "System.Single", "System.Single", "System.Single"
};
var names = new List <string> {
"x", "y", "z", "nx", "ny", "nz"
};
int numBytes = DotNet3d.Utilities.TotalBytes(dotNetTypes);
var data = new byte[numBytes];
int index = 0;
index += DotNet3d.Utilities.NumToBytes((float)1, data, index);
index += DotNet3d.Utilities.NumToBytes((float)2, data, index);
index += DotNet3d.Utilities.NumToBytes((float)3, data, index);
index += DotNet3d.Utilities.NumToBytes((float)4, data, index);
index += DotNet3d.Utilities.NumToBytes((float)5, data, index);
index += DotNet3d.Utilities.NumToBytes((float)6, data, index);
var pcWithNormals = new DotNet3d.PointCloud(numPoints, dotNetTypes, names, data);
var trans = DotNet3d.Matrix.Translation(10, 10, 10);
var scale = DotNet3d.Matrix.Scaling(2, 2, 2);
var super = scale * trans;
pcWithNormals.Multiply(super);
Trace.Assert(pcWithNormals.ToString() == "22 24 26 8 10 12\n");
}
}
private static void TestMatrix()
{
{
var txt = "";
DotNet3d.Matrix m0 = new DotNet3d.Matrix();
txt = " 1 0 0 0\n" +
" 0 1 0 0\n" +
" 0 0 1 0\n" +
" 0 0 0 1\n";
var str = m0.ToString();
Trace.Assert(str == txt);
DotNet3d.Matrix m1 = new DotNet3d.Matrix(1, 2, 3, 4,
5, 6, 7, 8,
9, 10, 11, 12,
13, 14, 15, 16);
txt = " 1 2 3 4\n" +
" 5 6 7 8\n" +
" 9 10 11 12\n" +
" 13 14 15 16\n";
Trace.Assert(m1.ToString() == txt);
{
double[] _array = new double[9] {
1, 2, 3, 4, 5, 6, 7, 8, 9
};
DotNet3d.Matrix _m = new DotNet3d.Matrix(_array);
var _txt = " 1 2 3 0\n" +
" 4 5 6 0\n" +
" 7 8 9 0\n" +
" 0 0 0 1\n";
Trace.Assert(_m.ToString() == _txt);
}
DotNet3d.Matrix m2 = new DotNet3d.Matrix(m1);
Trace.Assert(m2.ToString() == txt);
double[] array = new double[16] {
16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1
};
DotNet3d.Matrix m3 = new DotNet3d.Matrix(array);
txt = " 16 15 14 13\n" +
" 12 11 10 9\n" +
" 8 7 6 5\n" +
" 4 3 2 1\n";
Trace.Assert(m3.ToString() == txt);
DotNet3d.Vector vx = new DotNet3d.Vector(1, 1, 1);
DotNet3d.Vector vy = new DotNet3d.Vector(2, 2, 2);
DotNet3d.Vector vz = new DotNet3d.Vector(3, 3, 3);
DotNet3d.Matrix m4 = new DotNet3d.Matrix(vx, vy, vz);
txt = " 1 2 3 0\n" +
" 1 2 3 0\n" +
" 1 2 3 0\n" +
" 0 0 0 1\n";
Trace.Assert(m4.ToString() == txt);
DotNet3d.Matrix m5 = new DotNet3d.Matrix();
m5.Translate(1, 2, 3);
txt = " 1 0 0 1\n" +
" 0 1 0 2\n" +
" 0 0 1 3\n" +
" 0 0 0 1\n";
Trace.Assert(m5.ToString() == txt);
DotNet3d.Matrix m6 = new DotNet3d.Matrix();
m6.Translate(vx);
txt = " 1 0 0 1\n" +
" 0 1 0 1\n" +
" 0 0 1 1\n" +
" 0 0 0 1\n";
Trace.Assert(m6.ToString() == txt);
DotNet3d.Matrix m7 = new DotNet3d.Matrix();
m7.Scale(2, 4, 8);
txt = " 2 0 0 0\n" +
" 0 4 0 0\n" +
" 0 0 8 0\n" +
" 0 0 0 1\n";
Trace.Assert(m7.ToString() == txt);
DotNet3d.Matrix m8 = new DotNet3d.Matrix();
m8.RotateX(90);
str = m8.ToString();
txt = " 1 0 0 0\n" +
" 0 0 -1 0\n" +
" 0 1 0 0\n" +
" 0 0 0 1\n";
Trace.Assert(str == txt);
DotNet3d.Matrix m9 = new DotNet3d.Matrix();
m9.RotateY(90);
txt = " 0 0 1 0\n" +
" 0 1 0 0\n" +
" -1 0 0 0\n" +
" 0 0 0 1\n";
Trace.Assert(m9.ToString() == txt);
DotNet3d.Matrix m10 = new DotNet3d.Matrix();
m10.RotateZ(90);
txt = " 0 -1 0 0\n" +
" 1 0 0 0\n" +
" 0 0 1 0\n" +
" 0 0 0 1\n";
Trace.Assert(m10.ToString() == txt);
DotNet3d.Matrix m11 = new DotNet3d.Matrix(1, 2, 3, 4,
5, 6, 7, 8,
9, 10, 11, 12,
13, 14, 15, 16);
Trace.Assert(m11.Determinate() == 0);
DotNet3d.Matrix m12 = new DotNet3d.Matrix(7, 6, 5, 4,
1, 2, 3, 4,
9, 8, 7, 6,
2, 4, 6, 8);
m12.Invert(); // determinate is zero (ie not invertable), divide by zero gives NaN
txt = " NaN NaN NaN NaN\n" +
" NaN NaN NaN NaN\n" +
" NaN NaN NaN NaN\n" +
" NaN NaN NaN NaN\n";
str = m12.ToString();
Trace.Assert(str == txt);
DotNet3d.Matrix m13 = new DotNet3d.Matrix(7, 3, 5, 4,
3, 2, 3, 1,
9, 3, 7, 6,
1, 4, 6, 8);
m13.Invert(); // Verified numbers are correct via http://matrix.reshish.com/inverse.php
Trace.Assert(m13.M11 == 0.34090909090909094);
Trace.Assert(m13.M12 == -0.22727272727272729);
Trace.Assert(m13.M13 == -0.068181818181818177);
Trace.Assert(m13.M14 == -0.090909090909090912);
Trace.Assert(m13.M21 == 1.0340909090909092);
Trace.Assert(m13.M22 == -0.022727272727272728);
Trace.Assert(m13.M23 == -0.80681818181818188);
Trace.Assert(m13.M24 == 0.090909090909090912);
Trace.Assert(m13.M31 == -1.125);
Trace.Assert(m13.M32 == 0.75);
Trace.Assert(m13.M33 == 0.625);
Trace.Assert(m13.M34 == 0);
Trace.Assert(m13.M41 == 0.28409090909090912);
Trace.Assert(m13.M42 == -0.52272727272727271);
Trace.Assert(m13.M43 == -0.056818181818181823);
Trace.Assert(m13.M44 == 0.090909090909090912);
DotNet3d.Matrix m14 = DotNet3d.Matrix.Identity();
m14 = 3.0 * m14;
txt = " 3 0 0 0\n" +
" 0 3 0 0\n" +
" 0 0 3 0\n" +
" 0 0 0 3\n";
Trace.Assert(m14.ToString() == txt);
DotNet3d.Matrix m15 = new DotNet3d.Matrix(1, 2, 3, 4,
5, 6, 7, 8,
9, 10, 11, 12,
13, 14, 15, 16);
DotNet3d.Matrix m16 = new DotNet3d.Matrix(3, 11, -3, 2,
-2, 2, 99, 18,
59, 17, 121, 112,
3, 4, -5, 106);
// answer verified by http://matrix.reshish.com/multCalculation.php
txt = " 188 82 538 798\n" +
" 440 218 1,386 1,750\n" +
" 692 354 2,234 2,702\n" +
" 944 490 3,082 3,654\n";
str = (m15 * m16).ToString();
Trace.Assert(str == txt);
DotNet3d.Vector v1 = new DotNet3d.Vector(1, 2, 3);
DotNet3d.Matrix m17 = DotNet3d.Matrix.Identity();
txt = "[1 2 3]\n";
str = (m17 * v1).ToString();
Trace.Assert(str == txt);
DotNet3d.Point p1 = new DotNet3d.Point(3, 2, 1);
txt = "(3 2 1)\n";
Trace.Assert((m17 * p1).ToString() == txt);
txt = " 0 0 0 0\n" +
" 0 0 0 0\n" +
" 0 0 0 0\n" +
" 0 0 0 0\n";
Trace.Assert(DotNet3d.Matrix.Zero().ToString() == txt);
txt = " 1 0 0 1\n" +
" 0 1 0 2\n" +
" 0 0 1 3\n" +
" 0 0 0 1\n";
Trace.Assert(DotNet3d.Matrix.Translation(1, 2, 3).ToString() == txt);
Trace.Assert(DotNet3d.Matrix.Translation(new DotNet3d.Vector(1, 2, 3)).ToString() == txt);
txt = " 2 0 0 0\n" +
" 0 3 0 0\n" +
" 0 0 4 0\n" +
" 0 0 0 1\n";
Trace.Assert(DotNet3d.Matrix.Scaling(2, 3, 4).ToString() == txt);
txt = " 1 0 0 0\n" +
" 0 0 -1 0\n" +
" 0 1 0 0\n" +
" 0 0 0 1\n";
Trace.Assert(DotNet3d.Matrix.RotationX(90).ToString() == txt);
txt = " 0 0 1 0\n" +
" 0 1 0 0\n" +
" -1 0 0 0\n" +
" 0 0 0 1\n";
Trace.Assert(DotNet3d.Matrix.RotationY(90).ToString() == txt);
txt = " 0 -1 0 0\n" +
" 1 0 0 0\n" +
" 0 0 1 0\n" +
" 0 0 0 1\n";
Trace.Assert(DotNet3d.Matrix.RotationZ(90).ToString() == txt);
DotNet3d.Matrix m18 = new DotNet3d.Matrix(16, 15, 14, 13,
12, 11, 10, 9,
8, 7, 6, 5,
4, 3, 2, 1);
txt = " 16 12 8 4\n" +
" 15 11 7 3\n" +
" 14 10 6 2\n" +
" 13 9 5 1\n";
Trace.Assert(DotNet3d.Matrix.Transpose(m18).ToString() == txt);
DotNet3d.Matrix m19 = new DotNet3d.Matrix(7, 3, 5, 4,
3, 2, 3, 1,
9, 3, 7, 6,
1, 4, 6, 8);
// Verified numbers are correct via http://matrix.reshish.com/inverse.php
txt = " 0.34 -0.23 -0.07 -0.09\n" +
" 1.03 -0.02 -0.81 0.09\n" +
" -1.13 0.75 0.63 0\n" +
" 0.28 -0.52 -0.06 0.09\n";
var m19i = DotNet3d.Matrix.Invert(m19);
str = m19i.ToString();
Trace.Assert(str == txt);
Trace.Assert(m19i.M11 == 0.34090909090909094);
Trace.Assert(m19i.M12 == -0.22727272727272729);
Trace.Assert(m19i.M13 == -0.068181818181818177);
Trace.Assert(m19i.M14 == -0.090909090909090912);
Trace.Assert(m19i.M21 == 1.0340909090909092);
Trace.Assert(m19i.M22 == -0.022727272727272728);
Trace.Assert(m19i.M23 == -0.80681818181818188);
Trace.Assert(m19i.M24 == 0.090909090909090912);
Trace.Assert(m19i.M31 == -1.125);
Trace.Assert(m19i.M32 == 0.75);
Trace.Assert(m19i.M33 == 0.625);
Trace.Assert(m19i.M34 == 0);
Trace.Assert(m19i.M41 == 0.28409090909090912);
Trace.Assert(m19i.M42 == -0.52272727272727271);
Trace.Assert(m19i.M43 == -0.056818181818181823);
Trace.Assert(m19i.M44 == 0.090909090909090912);
}
// Create matrix from 3x3 matrix
{
var m3x3 = new DotNet3d.Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9);
var txt = " 1 2 3 0\n" +
" 4 5 6 0\n" +
" 7 8 9 0\n" +
" 0 0 0 1\n";
Trace.Assert(m3x3.ToString() == txt);
}
// Add two matrices
{
var a = new DotNet3d.Matrix(1, 2, 3, 4,
5, 6, 7, 8,
9, 10, 11, 12,
13, 14, 15, 16);
var b = new DotNet3d.Matrix(2, 4, 6, 8,
10, 12, 14, 16,
18, 20, 22, 24,
26, 28, 30, 32);
var txt = " 3 6 9 12\n" +
" 15 18 21 24\n" +
" 27 30 33 36\n" +
" 39 42 45 48\n";
Trace.Assert((a + b).ToString() == txt);
}
}
