std::vector<vgl_point_2d<double> > ScannerImage(const vector<vgl_point_3d<double> > &Points, const vgl_point_3d<double> &ScannerLocation)
{
std::vector<vgl_point_3d<double> > Intersections(Points.size());
//get all directions
std::vector<vgl_vector_3d<double> > Directions(Points.size());
for(unsigned int i = 0; i < Points.size(); i++)
{
Directions[i] = Points[i] - ScannerLocation;
}
vgl_vector_3d<double> ProjectionPlaneNormal = VXLHelpers::normalize(AverageDirection(Directions));
vgl_point_3d<double> ProjectionPlanePoint = ScannerLocation + ProjectionPlaneNormal;
//find the 3d coords of the points projected on the plane
for(unsigned int i = 0; i < Points.size(); i++)
{
vgl_line_3d_2_points<double> Line(ScannerLocation, Points[i]);
//vgl_plane_3d<double> Plane(ScannerForward, ScannerLocation + ScannerForward);
vgl_plane_3d<double> Plane(ProjectionPlaneNormal, ProjectionPlanePoint);
vgl_point_3d<double> Intersection = vgl_intersection(Line, Plane);
Intersections[i] = Intersection;
}
vgl_vector_3d<double> b = VXLHelpers::normalize(Intersections[0] - ProjectionPlanePoint);
//WriteAxisFile(vgl_point_3d<double> (0,0,0), cross_product(ProjectionPlaneNormal, b), b, ProjectionPlaneNormal, "BeforeAxis.vtp");
vnl_double_3x3 R = FindAligningRotation(cross_product(ProjectionPlaneNormal, b), b, ProjectionPlaneNormal);
/*
//write out axes after transformation
vnl_double_3 a1 = vnl_inverse(R) * vgl_vector_to_vnl_vector(cross_product(ProjectionPlaneNormal, b));
vnl_double_3 a2 = vnl_inverse(R) * vgl_vector_to_vnl_vector(b);
vnl_double_3 a3 = vnl_inverse(R) * vgl_vector_to_vnl_vector(ProjectionPlaneNormal);
WriteAxisFile(vgl_point_3d<double> (0,0,0), vnl_vector_to_vgl_vector(a1), vnl_vector_to_vgl_vector(a2), vnl_vector_to_vgl_vector(a3), "AfterAxis.vtp");
*/
std::vector<vgl_point_2d<double> > Points2d(Points.size());
for(unsigned int i = 0; i < Points.size(); i++)
{
vnl_double_3 temp = vnl_inverse(R) * VXLHelpers::vgl_point_to_vnl_vector(Intersections[i]);
Points2d[i] = vgl_point_2d<double> (temp(0), temp(1));
}
return Points2d;
}
int main()
{
SetHeadPtr set_1 = CreateEmptySet();
AddElementToSet(set_1, CreateElement(10));
AddElementToSet(set_1, CreateElement(6));
AddElementToSet(set_1, CreateElement(2));
AddElementToSet(set_1, CreateElement(7));
AddElementToSet(set_1, CreateElement(9));
SetHeadPtr set_2 = CreateEmptySet();
AddElementToSet(set_2, CreateElement(9));
AddElementToSet(set_2, CreateElement(1));
AddElementToSet(set_2, CreateElement(2));
AddElementToSet(set_2, CreateElement(-5));
//SetHeadPtr set_1 = ReadSetFromFile("Set1.txt");
//SetHeadPtr set_2 = ReadSetFromFile("Set2.txt");
PrintSet(set_1);
PrintSet(set_2);
PrintSet(Union(set_1, set_2));
PrintSet(Intersections(set_1, set_2));
}
