Intersection3D.h

Go to the documentation of this file.

#pragma once
 
#include <vector>
#include <cmath>
#include <iostream>
 
#include "TRDConstants.h"
#include "Line.h"
#include "Point.h"
 
using std::cerr;
using std::endl;
 
 
namespace Intersection3D
{    
    //____________________________________________________________________________
    struct IntersectionPoint3D
    {
        double x, y, z;
        std::string FitPlane;
        
        // Constructor to initialize values
        IntersectionPoint3D(const std::string& str, double xVal, double yVal, double zVal) : x(xVal), y(yVal), z(zVal), FitPlane(str) {}
    };
    
    
    //____________________________________________________________________________
    struct Intersection2DResult 
    {
        bool intersects;
        Point::Point2D point;
 
        Intersection2DResult()                               : intersects(false),      point({-99999, -99999}) {}
        Intersection2DResult(bool intersects, Point::Point2D point) : intersects(intersects), point(point)            {}
    };
 
    
    //____________________________________________________________________________
    Intersection2DResult findIntersection2D(const Line::FitLine2D& lineA,
                                            const Line::FitLine2D& lineB,
                                            const bool& debugMode = false
                                            )
    {
        if (fabs(lineA.m - lineB.m) <= 0.01*(lineA.err_m + lineB.err_m)) 
        {
            if (fabs(lineA.c - lineB.c) > (lineA.err_c + lineB.err_c)) 
            {
                if (debugMode)
                {
                    std::cerr<<"The lines are almost parallel (within given uncertainties) and don't intersect.\n";
                    std::cerr<<"fabs(mA - mB)   = "<<fabs(lineA.m - lineB.m)<<endl;
                    std::cerr<<"Err_mA + Err_mB = "<<lineA.err_m + lineB.err_m<<endl;
                }
                return Intersection2DResult(false, {});
            }
        }
        
        double x = (lineB.c - lineA.c) / (lineA.m - lineB.m);
        double y = lineA.m * x + lineA.c;
        return Intersection2DResult(true, {x, y});
    }
 
 
 
 
    //____________________________________________________________________________
    bool areLinesCloseEnough(const std::vector<Line::FitLine2D>& linesA,
                             const std::vector<Line::FitLine2D>& linesB,
                             const bool& debugMode = false)
    {
        std::vector<bool> vResults;
        std::vector<IntersectionPoint3D> vPoints;
        
        for (size_t i = 0; i < 3; ++i)
        {
            if (debugMode)
                printf("For %s:\n", vFitPlanes[i].c_str());
            
            Intersection2DResult result = findIntersection2D(linesA[i], linesB[i], debugMode);
            vResults.push_back(result.intersects);
            
            if (debugMode)
            {
                if (result.intersects == false)
                    printf("----> No intersection found.");
            
                else
                {
                    printf("----> Intersection point found at (%s, %s) = %8.3f, %8.3f\n",
                    components.at(i).first.c_str(), components.at(i).second.c_str(), result.point.x, result.point.y);
                }
            }
            
            if (i == 0)
                vPoints.push_back(IntersectionPoint3D(vFitPlanes.at(i), result.point.x,              0, result.point.y)); //XZ
            else if (i == 1)
                vPoints.push_back(IntersectionPoint3D(vFitPlanes.at(i),              0, result.point.x, result.point.y)); //YZ
            else if (i == 2)
                vPoints.push_back(IntersectionPoint3D(vFitPlanes.at(i), result.point.x, result.point.y,              0)); //XY
            else {}
        }
        
        
        // Displaying the points stored in the vector
        if (debugMode)
        {
            std::cout<<"Saved 3D intersection points:\n";
            printf("%s: %8.3f, %8.3f, %8.3f\n", vPoints[0].FitPlane.c_str(),      vPoints[0].x, vPoints[0].y, vPoints[0].z);
            printf("%s: %8.3f, %8.3f, %8.3f\n", vPoints[1].FitPlane.c_str(),      vPoints[1].x, vPoints[1].y, vPoints[1].z);
            printf("%s: %8.3f, %8.3f, %8.3f\n", vPoints[2].FitPlane.c_str(),      vPoints[2].x, vPoints[2].y, vPoints[2].z);
        }
        
        
        if (VectorUtilities::areAllElementsEqual(vResults) != true) 
            return false;
        
        
 
        if (fabs(vPoints.at(0).x - vPoints.at(2).x) < INTERSECTION_MAX_TOLERANCE  &&
            fabs(vPoints.at(1).y - vPoints.at(2).y) < INTERSECTION_MAX_TOLERANCE  &&
            fabs(vPoints.at(0).z - vPoints.at(1).z) < INTERSECTION_MAX_TOLERANCE)
        {
            if (debugMode)
            {
                printf("Checked for following conditions:\n");
                printf("%s: fabs(%8.3f - %8.3f) < %f cm:    TRUE\n", vPoints[0].FitPlane.c_str(), vPoints.at(0).x, vPoints.at(2).x, INTERSECTION_MAX_TOLERANCE);
                printf("%s: fabs(%8.3f - %8.3f) < %f cm:    TRUE\n", vPoints[1].FitPlane.c_str(), vPoints.at(1).y, vPoints.at(2).y, INTERSECTION_MAX_TOLERANCE);
                printf("%s: fabs(%8.3f - %8.3f) < %f cm:    TRUE\n", vPoints[2].FitPlane.c_str(), vPoints.at(0).z, vPoints.at(1).z, INTERSECTION_MAX_TOLERANCE);
                
                
                if (vPoints.at(0).z > 145 && vPoints.at(0).z < 150)
                    printf("\n\n\nFound interaction just above TRD: %8.3f cm\n", vPoints.at(0).z);
            }
 
 
            if (std::min(vPoints.at(0).z, vPoints.at(1).z) > TRD_ZFITCUTOFF_MAX)
            {
                return false;
            }
            
            else
                return true;
        }
        
        else
            return false;
    }    
    
    
 
 
    //____________________________________________________________________________
    int GetNIntersectingLinePairs(const std::vector<std::vector<Line::FitLine2D>>& vvFittedTracks, const bool& debugMode = false)
    {
        int count = 0;
        for (unsigned int i = 0; i < vvFittedTracks.size()-1; ++i) 
        {
            for (unsigned int j = i + 1; j < vvFittedTracks.size(); ++j)
            {
                if (debugMode)
                {
                    printf("\n\nChecking for fitted line pair (%d, %d), g3PID's (%d, %d), trkID's (%d, %d):\n",
                    i, j, vvFittedTracks[i][0].g3PID, vvFittedTracks[j][0].g3PID, vvFittedTracks[i][0].trkID, vvFittedTracks[j][0].trkID);
                }
                
                if (areLinesCloseEnough(vvFittedTracks[i], vvFittedTracks[j], debugMode))
                {
                    if (debugMode)
                        printf("----> Intersecting!\n");
                    count++;
                }
                
                else
                {
                    if (debugMode)
                    {
                        printf("!!!!!!!!!!!!!!!!!!!!!!!\n");
                        printf("----> Not intersecting!\n");
                        printf("!!!!!!!!!!!!!!!!!!!!!!!\n");
                    }
                }
            }
        }
        return count;
    }
}