bool VglPlus::intersectionFromLineSet(const vcl_vector<vgl_line_2d<double> > & lines, vgl_point_2d<double> & orthocenter)
{
assert(lines.size() >= 2);
int num = 0;
double px = 0.0;
double py = 0.0;
for (int i = 0; i<lines.size(); i++) {
for (int j = i+1; j<lines.size(); j++) {
vgl_point_2d<double> p;
bool isIntersect = vgl_intersection(lines[i], lines[j], p);
if (isIntersect) {
px += p.x();
py += p.y();
num++;
}
}
}
if (num >= 1) {
orthocenter = vgl_point_2d<double>(px/num, py/num);
return true;
}
else
{
return false;
}
}
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()
{
// Create a line segment
vgl_point_3d<double> Line0P0(-10,0,0);
vgl_point_3d<double> Line0P1(10,0,0);
vgl_line_3d_2_points<double> Line0(Line0P0, Line0P1);
// Create another line segment
vgl_point_3d<double> Line1P0(0,-10,0);
vgl_point_3d<double> Line1P1(0,10,0);
vgl_line_3d_2_points<double> Line1(Line1P0, Line1P1);
// Find the intersection, if there is one
vgl_point_3d<double> intersectionPoint = vgl_intersection(Line0, Line1);
return 0;
}
bool vgl_intersection(vgl_line_segment_2d<double> lineSegment0, vgl_line_segment_2d<double> lineSegment1, vgl_point_2d<double> &intersectionPoint)
{
vgl_point_3d<double> lineSegment03DP0(lineSegment0.point1().x(), lineSegment0.point1().y(), 0);
vgl_point_3d<double> lineSegment03DP1(lineSegment0.point2().x(), lineSegment0.point2().y(), 0);
vgl_line_segment_3d<double> lineSegment03D(lineSegment03DP0, lineSegment03DP1);
vgl_point_3d<double> lineSegment13DP0(lineSegment1.point1().x(), lineSegment1.point1().y(), 0);
vgl_point_3d<double> lineSegment13DP1(lineSegment1.point2().x(), lineSegment1.point2().y(), 0);
vgl_line_segment_3d<double> lineSegment13D(lineSegment13DP0, lineSegment13DP1);
vgl_point_3d<double> intersectionPoint3D;
bool isIntersect = vgl_intersection(lineSegment03D, lineSegment13D, intersectionPoint3D);
intersectionPoint.set(intersectionPoint3D.x(), intersectionPoint3D.y());
return isIntersect;
}
void TestIntersection2D()
{
// Create a line segment
vgl_point_2d<double> LineSegment0P0(-10,0);
vgl_point_2d<double> LineSegment0P1(10,0);
vgl_line_segment_2d<double> LineSegment0(LineSegment0P0, LineSegment0P1);
// Create another line segment
vgl_point_2d<double> LineSegment1P0(0,-10);
vgl_point_2d<double> LineSegment1P1(0,10);
vgl_line_segment_2d<double> LineSegment1(LineSegment1P0, LineSegment1P1);
vgl_point_2d<double> intersectionPoint;
bool isIntersect = vgl_intersection(LineSegment0, LineSegment1, intersectionPoint);
std::cout << "Intersect? " << isIntersect << std::endl;
if(isIntersect)
{
std::cout << intersectionPoint << std::endl;
}
}
void PlaneIntersectLine()
{
std::cout << std::endl << "PlaneIntersectLine()" << std::endl
<< "-------------------------" << std::endl;
//create a line (the x axis)
vgl_point_3d<double> LineP0(0.0, 0.0, 0.0);
vgl_point_3d<double> LineP1(1.0, 0.0, 0.0);
vgl_line_3d_2_points<double> Line(LineP0, LineP1);
//create a plane (parallel to the YZ plane at x=1)
vgl_point_3d<double> PlaneP0(1.0,0.0,0.0);
vgl_point_3d<double> PlaneP1(1.0,0.0,1.0);
vgl_point_3d<double> PlaneP2(1.0,1.0,0.0);
vgl_plane_3d<double> Plane(PlaneP0, PlaneP1, PlaneP2);
//intersect the line with the plane
vgl_point_3d<double> Intersection = vgl_intersection(Line, Plane);
std::cout << "Intersection: " << Intersection << std::endl;
}
int VglPlus::lineEllipseIntersection(const vgl_line_2d<double> & line, const vgl_ellipse_2d<double> & ellipse,
vgl_point_2d<double> & pt1, vgl_point_2d<double> & pt2, bool isMajorAxis)
{
vgl_polygon<double> poly = ellipse.polygon();
assert(poly.num_sheets() == 1);
int num = 0;
// printf("sheet number is %u\n", poly.num_sheets());
const vcl_vector< vgl_point_2d< double > > sheet = poly[0];
assert( sheet.size() > 1 );
for ( unsigned int v = 0; v < sheet.size() - 1; v++ )
{
vgl_line_segment_2d< double > edge( sheet[v], sheet[v+1] );
vgl_point_2d<double> p;
bool isIntersection = vgl_intersection(line, edge, &p);
if (isIntersection) {
if (num == 0) {
pt1 = p;
num++;
}
else if(num == 1)
{
pt2 = p;
num++;
}
}
if (num == 2) {
break;
}
}
// last line segment
if(num != 2)
{
vgl_line_segment_2d< double > edge( sheet.back(), sheet.front());
vgl_point_2d<double> p;
bool isIntersection = vgl_intersection(line, edge, &p);
if (isIntersection) {
if (num == 0) {
pt1 = p;
num++;
}
else if(num == 1)
{
pt2 = p;
num++;
}
}
}
double disDif = 20;
if (num == 2 && isMajorAxis) {
double dis = vgl_distance(pt1, pt2);
// distance of two points should be as the similar length of minor or major axis
double dis1 = vgl_distance(ellipse.major_diameter().point1(), ellipse.major_diameter().point2());
double dis2 = vgl_distance(ellipse.minor_diameter().point1(), ellipse.minor_diameter().point2());
if (fabs(dis - dis1) >= disDif && fabs(dis - dis2) >= disDif) {
num = 0;
}
}
return num;
}
