public static Point3D _transformPoint3D(Point3D p, Point3D origin, EuclideanTransform transform)
{
var vectorRotationFocus = p - origin;
var transformedPoint = transform.Apply(Parser3DPoint.FromPoint3DToDataPoint(new Point3D(vectorRotationFocus.X, vectorRotationFocus.Y, vectorRotationFocus.Z)).point);
return(new Point3D(transformedPoint.X + origin.X, transformedPoint.Y + origin.Y, transformedPoint.Z + origin.Z));
}
public CameraSpacePoint Filter(CameraSpacePoint x)
{
if (dummy)
{
return(x);
}
return(Parser3DPoint.FromPoint3DToCameraSpace(Filter(Parser3DPoint.FromCameraSpaceToPoint3D(x), rate)));
}
public static Point3DCollection TransformPoint3DCollection(Point3DCollection p, EuclideanTransform t)
{
Point3DCollection result = new Point3DCollection();
Point3D origin = new Point3D(0, 0, 0);
for (int i = 0; i < p.Count; i++)
{
result.Add(Parser3DPoint._transformPoint3D(p[i], origin, t));
}
return(result);
}
public static Point3DCollection GetPopulatedPointCloud(Point3D p, Point3D q, Point3D a, Point3D b, EuclideanTransform transformation)
{
Point3DCollection result = new Point3DCollection();
// only keep rotation of initaltransformation matrix
transformation = transformation.Inverse();
transformation.translation = System.Numerics.Vector3.Zero;
Point3DCollection collection = Parser3DPoint.GetPopulatedPointCloud(
p, transformation
);
Point3D k = collection[0];
for (int i = 0; i < collection.Count; i++)
{
result.Add(collection[i]);
}
collection = Parser3DPoint.GetPopulatedPointCloud(
q, transformation
);
for (int i = 0; i < collection.Count; i++)
{
result.Add(collection[i]);
}
collection = Parser3DPoint.GetPopulatedPointCloud(
a, transformation
);
for (int i = 0; i < collection.Count; i++)
{
result.Add(collection[i]);
}
collection = Parser3DPoint.GetPopulatedPointCloud(
b, transformation
);
for (int i = 0; i < collection.Count; i++)
{
result.Add(collection[i]);
}
return(result);
}
