TRestPhysics.cxx

/******************** REST disclaimer ***********************************
 * This file is part of the REST software framework.                     *
 *                                                                       *
 * Copyright (C) 2016 GIFNA/TREX (University of Zaragoza)                *
 * For more information see http://gifna.unizar.es/trex                  *
 *                                                                       *
 * REST is free software: you can redistribute it and/or modify          *
 * it under the terms of the GNU General Public License as published by  *
 * the Free Software Foundation, either version 3 of the License, or     *
 * (at your option) any later version.                                   *
 *                                                                       *
 * REST is distributed in the hope that it will be useful,               *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the          *
 * GNU General Public License for more details.                          *
 *                                                                       *
 * You should have a copy of the GNU General Public License along with   *
 * REST in $REST_PATH/LICENSE.                                           *
 * If not, see http://www.gnu.org/licenses/.                             *
 * For the list of contributors see $REST_PATH/CREDITS.                  *
 *************************************************************************/
 
#include "TRestPhysics.h"
 
#include <iostream>
#include <limits>
 
using namespace std;
 
namespace REST_Physics {
 
TVector3 MoveToPlane(const TVector3& pos, const TVector3& dir, const TVector3& n, const TVector3& a) {
    if (n * dir != 0) {
        Double_t t = (n * a - n * pos) / (n * dir);
 
        return pos + t * dir;
    }
    return pos;
}
 
Double_t DistanceToAxis(const TVector3& axisPoint, const TVector3& axisVector, const TVector3& point) {
    TVector3 a = axisVector.Cross(axisPoint - point);
    return a.Mag() / axisVector.Mag();
}
 
TVector3 GetPlaneVectorIntersection(const TVector3& pos, const TVector3& dir, const TVector3& n,
                                    const TVector3& a) {
    return MoveToPlane(pos, dir, n, a);
}
 
TVector3 GetParabolicVectorIntersection(const TVector3& pos, const TVector3& dir, const Double_t alpha,
                                        const Double_t R3) {
    Double_t e = 2 * R3 * TMath::Tan(alpha);
    Double_t a = dir.X() * dir.X() + dir.Y() * dir.Y();
    Double_t b = 2 * (pos.X() * dir.X() + pos.Y() * dir.Y()) + e * dir.Z();
    Double_t half_b = b / 2;
    Double_t c = pos.X() * pos.X() + pos.Y() * pos.Y() - R3 * R3 + e * pos.Z();
    if (a != 0) {
        Double_t root1 = (-half_b - TMath::Sqrt(half_b * half_b - a * c)) / a;
        Double_t root2 = (-half_b + TMath::Sqrt(half_b * half_b - a * c)) / a;
        Double_t int1 = pos.Z() + root1 * dir.Z();
        Double_t int2 = pos.Z() + root2 * dir.Z();
        if (int1 < 0 and int2 < 0 and int1 > int2) {
            return pos + root1 * dir;
        } else if (int1 < 0 and int2 < 0 and int1 < int2) {
            return pos + root2 * dir;
        }
        return pos;
    }
    return pos - c / b * dir;
}
 
TVector3 GetHyperbolicVectorIntersection(const TVector3& pos, const TVector3& dir, const Double_t beta,
                                         const Double_t R3, const Double_t focal) {
    Double_t e = 2 * R3 * TMath::Tan(beta);
    Double_t alpha = beta / 3;
    Double_t g = 2 * R3 * TMath::Tan(beta) / (focal + R3 / TMath::Tan(2 * alpha));
    Double_t a = dir.X() * dir.X() + dir.Y() * dir.Y() - g * dir.Z() * dir.Z();
    Double_t b = 2 * (pos.X() * dir.X() + pos.Y() * dir.Y() - g * dir.Z() * pos.Z()) + e * dir.Z();
    Double_t half_b = b / 2;
    Double_t c = pos.X() * pos.X() + pos.Y() * pos.Y() - R3 * R3 + e * pos.Z() - g * pos.Z() * pos.Z();
    Double_t root1 = (-half_b - TMath::Sqrt(half_b * half_b - a * c)) / a;
    Double_t root2 = (-half_b + TMath::Sqrt(half_b * half_b - a * c)) / a;
    Double_t int1 = pos.Z() + root1 * dir.Z();
    Double_t int2 = pos.Z() + root2 * dir.Z();
    if (int1 > 0 and int2 > 0 and int1 < int2) {
        return pos + root1 * dir;
    } else if (int1 > 0 and int2 > 0 and int1 > int2) {
        return pos + root2 * dir;
    }
    return pos;
}
 
TMatrixD GetConeMatrix(const TVector3& d, const Double_t cosTheta) {
    double cAxis[3];
    d.GetXYZ(cAxis);
 
    TVectorD coneAxis(3, cAxis);
 
    TMatrixD M(3, 3);
    M.Rank1Update(coneAxis, coneAxis);
 
    double cT2 = cosTheta * cosTheta;
    TMatrixD gamma(3, 3);
    gamma.UnitMatrix();
    gamma *= cT2;
 
    M -= gamma;
    return M;
}
 
TVector3 GetVectorReflection(const TVector3& dir, const TVector3& n) { return dir - 2 * dir.Dot(n) * n; }
 
Double_t GetVectorsAngle(const TVector3& v1, const TVector3& v2) { return TMath::ACos(v1.Dot(v2)); }
 
TVector3 GetConeNormal(const TVector3& pos, const Double_t alpha, const Double_t R) {
    Double_t r = 0;
    if (R == 0)
        r = TMath::Sqrt(pos.X() * pos.X() + pos.Y() * pos.Y());
    else
        r = R;
 
    Double_t cosA = TMath::Cos(alpha);
    Double_t sinA = TMath::Sin(alpha);
 
    return -TVector3(cosA * pos.X() / r, cosA * pos.Y() / r, sinA);
}
 
TVector3 GetParabolicNormal(const TVector3& pos, const Double_t alpha, const Double_t R3) {
    TVector3 normalVec = pos;
    Double_t m =
        1 / (R3 * TMath::Tan(alpha) / TMath::Sqrt(R3 * R3 + R3 * 2 * TMath::Tan(alpha) * (-pos.Z())));
    Double_t n = TMath::Sqrt(pos.X() * pos.X() + pos.Y() * pos.Y()) - m * pos.Z();
    normalVec.SetZ(pos.Z() - (-n / m));
    return normalVec.Unit();
}
 
TVector3 GetHyperbolicNormal(const TVector3& pos, const Double_t beta, const Double_t R3,
                             const Double_t focal) {
    TVector3 normalVec = pos;
    Double_t alpha = beta / 3;
    Double_t m = 1 / (R3 * TMath::Tan(beta) * (1 - 2 * pos.Z() / (focal + R3 / TMath::Tan(2 * alpha))) /
                      TMath::Sqrt(R3 * R3 - R3 * 2 * TMath::Tan(beta) * pos.Z() *
                                                (1.0 - pos.Z() / (focal + R3 / TMath::Tan(2 * alpha)))));
    Double_t n = TMath::Sqrt(pos.X() * pos.X() + pos.Y() * pos.Y()) - m * pos.Z();
    normalVec.SetZ(pos.Z() - (-n / m));
    return normalVec.Unit();
}
 
//
Double_t GetConeVectorIntersection(const TVector3& pos, const TVector3& dir, const TVector3& d,
                                   const TVector3& v, const Double_t cosTheta) {
    TMatrixD M = GetConeMatrix(d, cosTheta);
    return GetConeVectorIntersection(pos, dir, M, d, v);
}
 
Double_t GetConeVectorIntersection(const TVector3& pos, const TVector3& dir, const TMatrixD& M,
                                   const TVector3& axis, const TVector3& v) {
    double u[3];
    dir.GetXYZ(u);
    TMatrixD U(3, 1, u);
    TMatrixD Ut(1, 3, u);
 
    double delta[3];
    TVector3 deltaV = pos - v;
    deltaV.GetXYZ(delta);
    TMatrixD D(3, 1, delta);
    TMatrixD Dt(1, 3, delta);
 
    TMatrixD C2 = Ut * M * U;
    Double_t c2 = C2[0][0];
 
    TMatrixD C1 = Ut * M * D;
    Double_t c1 = C1[0][0];
 
    TMatrixD C0 = Dt * M * D;
    Double_t c0 = C0[0][0];
 
    Double_t root = c1 * c1 - c0 * c2;
    if (root < 0) return 0;
 
    Double_t t1 = (-c1 + TMath::Sqrt(root)) / c2;
    Double_t t2 = (-c1 - TMath::Sqrt(root)) / c2;
 
    // The projections along the cone axis. If positive then the solution
    // gives the cone intersection with the side defined by `axis`
    // Double_t h1 = t1 * dir.Dot(axis) + axis.Dot(deltaV);
    Double_t h2 = t2 * dir.Dot(axis) + axis.Dot(deltaV);
 
    // We use it to select the root we are interested in
    if (h2 > 0)
        return t2;
    else
        return t1;
}
 
TVector3 MoveByDistance(const TVector3& pos, const TVector3& dir, Double_t d) { return pos + d * dir.Unit(); }
 
TVector3 MoveByDistanceFast(const TVector3& pos, const TVector3& dir, Double_t d) { return pos + d * dir; }
 
Double_t GetDistance(const TVector3& v1, const TVector3& v2) { return (v2 - v1).Mag(); }
 
Double_t GetDistance2(const TVector3& v1, const TVector3& v2) { return (v2 - v1).Mag2(); }
}  // namespace REST_Physics