public static Point3 ChangePoint3(Point3 p, double[,] M)
{
List<List<double>> temp = new List<List<double>>();
temp.Add(new List<double>());
temp[0].Add(p.X);
temp[0].Add(p.Y);
temp[0].Add(p.Z);
temp[0].Add(1);
double[,] _temp = { { p.X, p.Y, p.Z, 1 } };
var res = multMatrix(_temp, M);
return new Point3(res[0, 0], res[0, 1], res[0, 2]);
}
public static List<Point3> Get3DPoints(Bitmap OriginalImage, Bitmap DepthScene, Point3 CameraLoc, Point3 CameraVect)
{
List<Point3> Result = new List<Point3>();
for (int i = 0; i < OriginalImage.Height; i++)
{
for (int j = 0; j < OriginalImage.Width; j++)
{
//Point3 p = new Point3(DepthScene.GetPixel(j,i).R, j,i);
Point3 p = new Point3(0, j, i);
p.Move(-CameraLoc.X, -CameraLoc.Y, -CameraLoc.Z);
//p.RotateY(j / OriginalImage.Width * 90 - 45);
//p.RotateZ(-CameraVect.Z);
//p.RotateX(CameraVect.X);
//p.RotateY(CameraVect.Y);
p.SetOriginalCoord(j,i);
Result.Add(p);
}
}
return Result;
}