/// <summary> /// Rotates the point cloud by a given angle /// </summary> /// <param name="axis">The axis of rotation</param> /// <param name="angle">The angle to which te point cloud is to be rotated</param> public void rotate(double[] axis, double angle) { if (!(axis.Length != 3)) { //centre of rotation Point3D centre = new Point3D(axis[0], axis[1], axis[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach (PARSE.ICP.PointRGB point in pts) { //create rot matrix Matrix3D mtx = new Matrix3D(); Quaternion q = new Quaternion(new Vector3D(0, 1, 0), angle); mtx.RotateAt(q, centre); //complete rotation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree hole double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); } //replace the old kd tree with the new one this.points = newPoints; } else { //throw an exception and annoy Bernie in the process ;) } }
/// <summary> /// Translate the point cloud by a given value /// </summary> /// <param name="tx">Up to three co-ords</param> public void translate(double[] tx) { if (tx.Length == 3) { //turn the transformation vector into and object Console.WriteLine("Translating"); TranslateTransform3D translation = new TranslateTransform3D(tx[0], tx[1], tx[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach(PARSE.ICP.PointRGB point in pts) { //perform the new translation which does appear to work. Matrix3D mtx = new Matrix3D(); mtx.Translate(new Vector3D(tx[0], tx[1], tx[2])); //complete translation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); //perform the old translation method which doesn't appear to work. //point.point.Offset(tx[0], tx[1], tx[2]); //double[] key = new double[3]{point.point.X, point.point.Y, point.point.Z}; //newPoints.insert(key, point); } //replace the old kd tree with the new one this.points = newPoints; } else { //probably want to throw an exception here } }
/// <summary> /// Translate the point cloud by a given value /// </summary> /// <param name="tx">Up to three co-ords</param> public void translate(double[] tx) { if (tx.Length == 3) { //turn the transformation vector into and object Console.WriteLine("Translating"); TranslateTransform3D translation = new TranslateTransform3D(tx[0], tx[1], tx[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach (PARSE.ICP.PointRGB point in pts) { //perform the new translation which does appear to work. Matrix3D mtx = new Matrix3D(); mtx.Translate(new Vector3D(tx[0], tx[1], tx[2])); //complete translation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); //perform the old translation method which doesn't appear to work. //point.point.Offset(tx[0], tx[1], tx[2]); //double[] key = new double[3]{point.point.X, point.point.Y, point.point.Z}; //newPoints.insert(key, point); } //replace the old kd tree with the new one this.points = newPoints; } else { //probably want to throw an exception here } }
/// <summary> /// Rotates the point cloud by a given angle /// </summary> /// <param name="axis">The axis of rotation</param> /// <param name="angle">The angle to which te point cloud is to be rotated</param> public void rotate(double[] axis, double angle) { if (!(axis.Length != 3)) { //centre of rotation Point3D centre = new Point3D(axis[0], axis[1], axis[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach (PARSE.ICP.PointRGB point in pts) { //create rot matrix Matrix3D mtx = new Matrix3D(); Quaternion q = new Quaternion(new Vector3D(0, 1, 0), angle); mtx.RotateAt(q, centre); //complete rotation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree hole double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); } //replace the old kd tree with the new one this.points = newPoints; } else{ //throw an exception and annoy Bernie in the process ;) } }