/// <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 ;)
}
}