diff --git a/.github/workflows/build-test.yml b/.github/workflows/build-test.yml index 171a6d1b..a0344e74 100644 --- a/.github/workflows/build-test.yml +++ b/.github/workflows/build-test.yml @@ -14,48 +14,40 @@ jobs: strategy: fail-fast: false matrix: - os: ["ubuntu-latest", "macos-10.15"] + os: ["ubuntu-latest", "macos-latest"] python-version: ["3.9"] - root-version: ["6.24.6"] + root-version: ["6.30.2"] build-type: ["Debug", "Release"] steps: - - name: Cache conda - uses: actions/cache@v1 - env: - # Increase this value to reset cache if etc/example-environment.yml has not changed - # This will be necessary to update e.g. the version of cmake, cxx-compiler, or clang-tools that is used - CACHE_NUMBER: 0 - with: - path: ~/conda_pkgs_dir - key: - ${{ runner.os }}-conda-${{ env.CACHE_NUMBER }}-${{ matrix.root-version }}-${{ matrix.python-version }} - - uses: conda-incubator/setup-miniconda@v2 - with: - python-version: ${{ matrix.python-version }} - mamba-version: "*" - channels: conda-forge,defaults - channel-priority: strict - use-only-tar-bz2: true - - name: Conda info - run: conda info - - - name: Conda/Mamba Install - run: mamba install -c conda-forge cxx-compiler root=${{ matrix.root-version }} cmake # clang-tools - - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: fetch-depth: 0 + - name: Install conda environment with micromamba + uses: mamba-org/setup-micromamba@v1 + with: + micromamba-version: 'latest' + environment-name: "kinkal-test-env" + create-args: >- + python=${{ matrix.python-version }} + root=${{ matrix.root-version }} + cxx-compiler + cmake + condarc : | + channels: + - conda-forge + - defaults + channel-priority: strict - name: CMake (${{ matrix.build-type }}) run: | cd .. ENABLE_COVERAGE="" - if [ "${{ matrix.build-type }}" = "Debug" ]; then - if [ "${{ matrix.os }}" = "macos-10.15" ]; then - ENABLE_COVERAGE="-DCOVERAGE=ON" - fi - fi + # if [ "${{ matrix.build-type }}" = "Debug" ]; then + # if [ "${{ matrix.os }}" = "macos-latest" ]; then + # ENABLE_COVERAGE="-DCOVERAGE=ON" + # fi + #fi cmake -S KinKal -B KinKal_${{ matrix.build-type }} \ -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} \ @@ -71,24 +63,24 @@ jobs: cd ../KinKal_${{ matrix.build-type }} env CTEST_OUTPUT_ON_FAILURE=1 make test - - name: Create coverage report - run: | - cd ../KinKal_${{ matrix.build-type }} - mamba install -c conda-forge -y gcovr - - make coverage - gcovr -r ../KinKal . --xml -o coverage.xml --gcov-executable "llvm-cov gcov" --exclude-directories Tests - - if: ${{ matrix.build-type == 'Debug' && matrix.os == 'macos-10.15' }} - - - name: Upload coverage - uses: codecov/codecov-action@v1 - with: - files: ../KinKal_${{ matrix.build-type }}/coverage.xml - flags: unittests - name: codecov-umbrella - if: ${{ matrix.build-type == 'Debug' && matrix.os == 'macos-10.15' }} - + # - name: Create coverage report + # run: | + # cd ../KinKal_${{ matrix.build-type }} + # mamba install -c conda-forge -y gcovr + # + # make coverage + # gcovr -r ../KinKal . --xml -o coverage.xml --gcov-executable "llvm-cov gcov" --exclude-directories Tests + # + # if: ${{ matrix.build-type == 'Debug' && matrix.os == 'macos-latest' }} + # + # - name: Upload coverage + # uses: codecov/codecov-action@v1 + # with: + # files: ../KinKal_${{ matrix.build-type }}/coverage.xml + # flags: unittests + # name: codecov-umbrella + # if: ${{ matrix.build-type == 'Debug' && matrix.os == 'macos-latest' }} + # # - name: Run clang-tidy # if: ${{ matrix.build-type == 'Release' && matrix.os == 'ubuntu-latest' }} # run: | diff --git a/.gitignore b/.gitignore index a3f9fb4b..17f6ff19 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,8 @@ KinKal/UnitTests/Dict.cc debug/* *.root .DS_Store -.vscode \ No newline at end of file +.vscode/* +.*.swp +spack-* +build-* +.spack* diff --git a/CMakeLists.txt b/CMakeLists.txt index c0e79333..e8782766 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,33 +6,33 @@ set(default_build_type "Release") if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) message(STATUS "Setting build type to '${default_build_type}' as none was specified.") set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE - STRING "Choose the type of build." FORCE) + STRING "Choose the type of build." FORCE) # Set the possible values of build type for cmake-gui set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release") endif() -if (CMAKE_BUILD_TYPE MATCHES "^((p|P)(rofile|rof))|(release)$") - message(STATUS "Setting build type to 'Release' instead of '${CMAKE_BUILD_TYPE}'.") - set(CMAKE_BUILD_TYPE "Release" CACHE - STRING "Choose the type of build." FORCE) +if (CMAKE_BUILD_TYPE MATCHES "^((p|P)(rofile|rof))|(release)|(RelWithDebInfo)$") + message(STATUS "Setting build type to 'Release' instead of '${CMAKE_BUILD_TYPE}'.") + set(CMAKE_BUILD_TYPE "Release" CACHE + STRING "Choose the type of build." FORCE) endif() if (CMAKE_BUILD_TYPE MATCHES "^debug$") - message(STATUS "Setting build type to 'Debug' instead of '${CMAKE_BUILD_TYPE}'.") - set(CMAKE_BUILD_TYPE "Debug" CACHE - STRING "Choose the type of build." FORCE) + message(STATUS "Setting build type to 'Debug' instead of '${CMAKE_BUILD_TYPE}'.") + set(CMAKE_BUILD_TYPE "Debug" CACHE + STRING "Choose the type of build." FORCE) endif() if (NOT CMAKE_BUILD_TYPE MATCHES "^(Debug|Release)$") - message(FATAL_ERROR "'${CMAKE_BUILD_TYPE}' is not a valid build type. Please choose either 'Release' or 'Debug'.") + message(FATAL_ERROR "'${CMAKE_BUILD_TYPE}' is not a valid build type. Please choose either 'Release' or 'Debug'.") endif() message(STATUS "Build Type: ${CMAKE_BUILD_TYPE}" ) -# enable c++17 standard -set(CMAKE_CXX_STANDARD 17) +# enable c++20 standard +set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED on) # make sure CMake is able to find ROOT @@ -55,22 +55,22 @@ find_package(Git) if(GIT_FOUND) execute_process( COMMAND ${GIT_EXECUTABLE} describe --tags - WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" OUTPUT_VARIABLE _build_version ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE - ) + ) message( STATUS "git describes this revision as: ${_build_version}") if (_build_version MATCHES "^[vV]?([0-9]+)[._]([0-9]+)[._]([0-9]+)(.*)$") set (_build_ver_maj ${CMAKE_MATCH_1}) set (_build_ver_min ${CMAKE_MATCH_2}) set (_build_ver_patch ${CMAKE_MATCH_3}) - set (_build_ver_commits_since_tag ${CMAKE_MATCH_4}) -# set (_build_ver_commit_short ${CMAKE_MATCH_5}) + set (_build_ver_commits_since_tag ${CMAKE_MATCH_4}) + # set (_build_ver_commit_short ${CMAKE_MATCH_5}) message( STATUS "Version string correctly matched: major= ${_build_ver_maj}, minor= ${_build_ver_min}, patch= ${_build_ver_patch}, other=${_build_ver_commits_since_tag}") else() - message ("Version not parsed!") + message (STATUS "Version not parsed!") endif() else() message( STATUS "Git not found!") @@ -79,10 +79,10 @@ endif() # Set project name and version project (KinKal - LANGUAGES CXX - VERSION ${_build_ver_maj}.${_build_ver_min}.${_build_ver_patch} - DESCRIPTION "Kinematic Kalman filter track fit code package" -) + LANGUAGES CXX + VERSION ${_build_ver_maj}.${_build_ver_min}.${_build_ver_patch} + DESCRIPTION "Kinematic Kalman filter track fit code package" + ) message( STATUS "Project Version: ${CMAKE_PROJECT_VERSION}") message( STATUS "Commits made since last tag: ${_build_ver_commits_since_tag}") @@ -94,17 +94,18 @@ set(CMAKE_SHARED_LIBRARY_PREFIX "libKinKal_") # find the ROOT dependency find_package(ROOT REQUIRED COMPONENTS - Core RIO Net Hist GenVector MLP Graf Graf3d Gpad Tree - Rint Postscript Matrix Physics MathCore Thread Gui) + Core RIO Net Hist GenVector MLP Graf Graf3d Gpad Tree + Rint Postscript Matrix Physics MathCore Thread Gui) # include "useful" ROOT cmake functions # which are useful for building dictionaries include(${ROOT_USE_FILE}) # set RPATH options -set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/lib) -set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - +#set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/lib) +#set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) +set(CMAKE_SKIP_INSTALL_RPATH ON) +# # output shared libraries and executables in lib/ and bin/ set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib) @@ -115,26 +116,31 @@ enable_testing() # compiler flags add_compile_options( - # flags applied to ALL build types - "-Wall" - "-Wno-unused-parameter" - "-Wno-unused-local-typedefs" - "-Werror" - "-gdwarf-2" - "-Werror=return-type" - "-Winit-self" - "-Woverloaded-virtual" - "-fdiagnostics-color=always" - "-Werror=sign-compare" -# "-Wshadow" - - # debug flags - "$<$:-O0;-g>" - - # release flags - "$<$:-O3;-DNDEBUG;-fno-omit-frame-pointer>" -) - + # flags applied to ALL build types + "-Wall" + "-Wno-unused-parameter" + "-Wno-unused-local-typedefs" + "-Werror" + # "-Wpedantic" + "-gdwarf-2" + "-Werror=return-type" + "-Winit-self" + "-Woverloaded-virtual" + "-fdiagnostics-color=always" + "-Werror=sign-compare" + "-Wdeprecated-copy" + # "-Wshadow" + # debug flags + "$<$:-O0;-g;-ftrapping-math>" + # release flags + "$<$:-O3;-DNDEBUG;-fno-omit-frame-pointer>" + ) +if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + # gcc doesn't like the lazy memory evaluation in root SMatrix, This turns a fatal error into a warning (the code runs fine) + add_compile_options( + "-Wno-error=maybe-uninitialized" + ) +endif() # install rules include(GNUInstallDirs) @@ -142,14 +148,15 @@ include(GNUInstallDirs) # clang tidy if(ENABLE_CLANG_TIDY) - set(CMAKE_CXX_CLANG_TIDY "clang-tidy") + set(CMAKE_CXX_CLANG_TIDY "clang-tidy") endif() if(ENABLE_CLANG_TIDY_WITH_FIXES) - set(CMAKE_CXX_CLANG_TIDY "clang-tidy;-fix") + set(CMAKE_CXX_CLANG_TIDY "clang-tidy;-fix") endif() if (CMAKE_BUILD_TYPE MATCHES "^Debug$") + message(STATUS "debug build") if (COVERAGE) add_compile_options("-fprofile-arcs" "-ftest-coverage") if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") @@ -158,20 +165,30 @@ if (CMAKE_BUILD_TYPE MATCHES "^Debug$") add_custom_target(coverage COMMAND gcovr --gcov-executable "llvm-cov gcov" -r ${CMAKE_SOURCE_DIR} ${PROJECT_BINARY_DIR} -s --exclude-directories Tests WORKING_DIRECTORY ${PROJECT_BINARY_DIR} - ) + ) elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - link_libraries(-lgcov) # gcov symbols + # gcc doesn't like the lazy memory evaluation in root SMatrix, This turns a fatal error into a warning (the code runs fine) + add_compile_options( + "-Wno-error=maybe-uninitialized" + ) + link_libraries(-lgcov) # gcov symbols add_custom_target(coverage COMMAND gcovr -r ${CMAKE_SOURCE_DIR} ${PROJECT_BINARY_DIR} -s --exclude-directories Tests WORKING_DIRECTORY ${PROJECT_BINARY_DIR} - ) + ) endif() endif() + if (SANITIZE) + message(STATUS "Sanitize build") + add_compile_options("-fsanitize=address" "-fno-omit-frame-pointer") + add_link_options("-fsanitize=address") + endif() endif() # add shared library targets add_subdirectory(MatEnv) add_subdirectory(General) +add_subdirectory(Geometry) add_subdirectory(Detector) add_subdirectory(Trajectory) add_subdirectory(Fit) @@ -179,17 +196,22 @@ add_subdirectory(Examples) add_subdirectory(Tests) -install(TARGETS General Trajectory Detector Fit MatEnv Examples - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +install(TARGETS General Trajectory Geometry Detector Fit MatEnv Examples + EXPORT KinKalTargets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) # install headers # Globbing here is fine because it does not influence build behaviour install(DIRECTORY "${CMAKE_SOURCE_DIR}/" - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/KinKal - FILES_MATCHING PATTERN "*.hh" - PATTERN ".git*" EXCLUDE -) + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/KinKal + FILES_MATCHING PATTERN "*.hh" PATTERN "*.h" + PATTERN "LinkDef.h" EXCLUDE + PATTERN ".git*" EXCLUDE + ) message (STATUS "Writing setup.sh...") @@ -199,8 +221,8 @@ file(WRITE ${PROJECT_BINARY_DIR}/setup.sh " # (return 0 2>/dev/null) && sourced=1 || sourced=0 if [ \"\$sourced\" = \"0\" ];then - echo \"You should be sourcing this file, not executing it.\" - exit 1 +echo \"You should be sourcing this file, not executing it.\" +exit 1 fi export DEBUG_LEVEL=${CMAKE_BUILD_TYPE} @@ -216,4 +238,30 @@ export KINKAL_LIBRARY_DIR=${CMAKE_CURRENT_BINARY_DIR}/lib ") - +# Set up CMake exports for find_package +set(KinKal_IN_TREE TRUE CACHE BOOL "Whether KinKal is in the current tree") +include(CMakePackageConfigHelpers) +install(EXPORT KinKalTargets + FILE KinKalTargets.cmake + NAMESPACE KinKal:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME}/cmake +) + + write_basic_package_version_file("${PROJECT_NAME}ConfigVersion.cmake" + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion) + + configure_package_config_file( + "${PROJECT_SOURCE_DIR}/cmake/${PROJECT_NAME}Config.cmake.in" + "${PROJECT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION + ${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME}/cmake) + +install(FILES "${PROJECT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${PROJECT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION ${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME}/cmake) + +export(EXPORT KinKalTargets + FILE "${PROJECT_BINARY_DIR}/KinKalTargets.cmake" + NAMESPACE KinKal:: +) diff --git a/Detector/CMakeLists.txt b/Detector/CMakeLists.txt index 78929b08..d3897073 100644 --- a/Detector/CMakeLists.txt +++ b/Detector/CMakeLists.txt @@ -6,13 +6,14 @@ # you can regenerate this list easily by running in this directory: ls -1 *.cc add_library(Detector SHARED - StrawMaterial.cc Residual.cc ) #message( "source dir detector " ${CMAKE_SOURCE_DIR}/..) # set top-level directory as include root -target_include_directories(Detector PRIVATE ${PROJECT_SOURCE_DIR}/..) +target_include_directories(Detector PUBLIC + $ + $) # link this library with ROOT libraries and other KinKal libraries target_link_libraries(Detector Trajectory General ${ROOT_LIBRARIES}) diff --git a/Detector/CylindricalShell.hh b/Detector/CylindricalShell.hh deleted file mode 100644 index 0b1021be..00000000 --- a/Detector/CylindricalShell.hh +++ /dev/null @@ -1,95 +0,0 @@ -// -// 2-dimensional cylindrial shell, used to model thin cylinders of passive material. -// This also computes the intersection of a particle trajectory with the cylinder -// -#ifndef KinKal_Detector_CylindricalShell_hh -#define KinKal_Detector_CylindricalShell_hh -#include "KinKal/General/TimeRange.hh" -#include -#include -namespace KinKal { - using TimeRanges = std::vector; - class CylindricalShell { - public: - CylindricalShell(): radius_(-1.0), rhalf_(-1.0), zpos_(0.0), zhalf_(-1.0) {} - CylindricalShell(double radius, double rhalf, double zpos, double zhalf) : radius_(radius), rhalf_(rhalf), zpos_(zpos), zhalf_(zhalf) {} - double radius() const { return radius_;} - double rhalf() const { return rhalf_;} - double zmin() const { return zpos_ - zhalf_;} - double zmax() const { return zpos_ + zhalf_;} - double zpos() const { return zpos_;} - double zhalf() const { return zhalf_;} - // find the 1st intersection of the trajectory with this cylinder, starting from the given time - template KinKal::TimeRange intersect(PKTRAJ const& pktraj, double tstart, double tstep) const; - // find all intersections of a trajectory with this cylinder. - template void intersect(KTRAJ const& ktraj, TimeRanges& tranges,double tstart, double tstep) const; - private: - double radius_, rhalf_, zpos_, zhalf_; - }; - - template void CylindricalShell::intersect(PKTRAJ const& pktraj, TimeRanges& tranges, double tstart, double tstep) const { - using KinKal::TimeRange; - tranges.clear(); - TimeRange trange(tstart,tstart); - do { - trange = intersect(pktraj,trange.end(),tstep); - if(!trange.null())tranges.push_back(trange); - } while( (!trange.null()) && trange.end() < pktraj.range().end()); - } - - template KinKal::TimeRange CylindricalShell::intersect(PKTRAJ const& pktraj, double tstart, double tstep) const { - using KinKal::TimeRange; - double ttest = tstart; - auto pos = pktraj.position3(ttest); - double dr = pos.Rho() - radius(); - double olddr = dr; - TimeRange trange(ttest,ttest); - while(ttest < pktraj.range().end()){ - // cout << "particle enters at " << pos << endl; - auto vel = pktraj.velocity(ttest); - double dz = fabs(tstep*vel.Z()); - if(pos.Z() > zmin()-dz && pos.Z()< zmax()+dz ){ - // we're in the z range of the cylinder. Step until (and if) we cross the radius` - ttest+= tstep; - auto oldpos = pos; - pos = pktraj.position3(ttest); - dr = pos.Rho() - radius(); - if(olddr*dr < 0 - && ( (pos.Z() > zmin() && pos.Z() < zmax()) || - (oldpos.Z() > zmin() && oldpos.Z() < zmax()) ) ) { - // we've crossed the shell. Interpolate to the exact crossing - double tx = ttest - tstep*fabs(dr/(dr-olddr)); - // compute the crossing time range - auto vel = pktraj.velocity(tx); - double vr = vel.Rho(); - double dt = vr > 1e-8 ? rhalf()/vr : 2.0*sqrt(2.0*radius()*rhalf())/vel.R(); - trange = TimeRange(tx-dt,tx+dt); - break; - } - olddr = dr; - } else { - // If we are heading in the wrong direction, step by piece until (and if) the trajectory reverses - vel = pktraj.velocity(ttest); - double dt = (zpos() - pos.Z())/vel.Z(); - while(dt < 0.0 && ttest < pktraj.range().end()){ - // advance to the end of this piece - ttest = pktraj.nearestPiece(ttest).range().end()+tstep; - pos = pktraj.position3(ttest); - vel = pktraj.velocity(ttest); - dt = (zpos() - pos.Z())/vel.Z(); - } - // now advance to a piece that is within 1 step of the z range - dt = (vel.Z() > 0) ? (zmin() - pos.Z())/vel.Z() : (zmax() - pos.Z())/vel.Z(); - while( dt > tstep && ttest < pktraj.range().end()){ - ttest = std::min(ttest+dt, pktraj.nearestPiece(ttest).range().end()+tstep); - pos = pktraj.position3(ttest); - vel = pktraj.velocity(ttest); - dt = (vel.Z() > 0) ? (zmin() - pos.Z())/vel.Z() : (zmax() - pos.Z())/vel.Z(); - } - } - } - return trange; - } -} -#endif - diff --git a/Detector/ElementXing.hh b/Detector/ElementXing.hh index 2ba6338a..afcb4cb0 100644 --- a/Detector/ElementXing.hh +++ b/Detector/ElementXing.hh @@ -1,67 +1,122 @@ #ifndef KinKal_ElementXing_hh #define KinKal_ElementXing_hh // -// Describe the material effects of a kinematic trajectory crossing a detector element (piece of the detector) +// Describe the material effects of a particle crossing a detector element (piece of the detector) // Used in the kinematic Kalman fit // +#include "KinKal/General/CloneContext.hh" #include "KinKal/General/MomBasis.hh" #include "KinKal/Detector/MaterialXing.hh" #include "KinKal/Trajectory/ParticleTrajectory.hh" -#include "KinKal/General/TimeDir.hh" -#include "KinKal/Detector/Hit.hh" #include "KinKal/Fit/MetaIterConfig.hh" #include -#include #include -#include #include +#include +#include namespace KinKal { template class ElementXing { public: - using PKTRAJ = ParticleTrajectory; - // construct from a time + using PTRAJ = ParticleTrajectory; + using KTRAJPTR = std::shared_ptr; ElementXing() {} virtual ~ElementXing() {} - virtual void update(PKTRAJ const& pktraj,MetaIterConfig const& miconfig) =0; - virtual double crossingTime() const=0; + // clone op for reinstantiation + ElementXing(ElementXing const& rhs) = default; + virtual std::shared_ptr< ElementXing > clone(CloneContext&) const; + virtual void updateReference(PTRAJ const& ptraj) = 0; // update the trajectory reference + virtual void updateState(MetaIterConfig const& config,bool first) =0; // update the state according to this meta-config + virtual Parameters params() const =0; // parameter change induced by this element crossing WRT the reference parameters going forwards in time + virtual double time() const=0; // time the particle crosses thie element + virtual double transitTime() const=0; // time to cross this element + virtual KTRAJ const& referenceTrajectory() const =0; // trajectory WRT which the xing is defined + virtual std::vectorconst& matXings() const =0; // Effect of each physical material component of this detector element on this trajectory virtual void print(std::ostream& ost=std::cout,int detail=0) const =0; - // crossings without material are inactive - bool active() const { return mxings_.size() > 0; } - std::vectorconst& matXings() const { return mxings_; } - std::vector& matXings() { return mxings_; } - // calculate the cumulative material effect from these crossings - void materialEffects(PKTRAJ const& pktraj, TimeDir tdir, std::array& dmom, std::array& momvar) const; + virtual bool active() const =0; + // momentum change and variance increase associated with crossing this element forwards in time, in spatial basis + void momentumChange(SVEC3& dmom, SMAT& dmomvar) const; + // parameter change associated with crossing this element forwards in time + Parameters parameterChange(double varscale=1.0) const; + // cumulative material effect from all the materials in this element crossing going forwards in time, expressed as scalars + void materialEffects(double& dmom, double& paramomvar, double& perpmomvar) const; // sum radiation fraction double radiationFraction() const; private: - std::vector mxings_; // Effect of each physical material component of this detector element on this trajectory }; - template void ElementXing::materialEffects(PKTRAJ const& pktraj, TimeDir tdir, std::array& dmom, std::array& momvar) const { - // compute the derivative of momentum to energy - double mom = pktraj.momentum(crossingTime()); - double mass = pktraj.mass(); - double dmFdE = sqrt(mom*mom+mass*mass)/(mom*mom); // dimension of 1/E - if(tdir == TimeDir::backwards)dmFdE *= -1.0; - // loop over crossings for this detector piece - for(auto const& mxing : mxings_){ - // compute FRACTIONAL momentum change and variance on that in the given direction - momvar[MomBasis::momdir_] += mxing.dmat_.energyLossVar(mom,mxing.plen_,mass)*dmFdE*dmFdE; - dmom [MomBasis::momdir_]+= mxing.dmat_.energyLoss(mom,mxing.plen_,mass)*dmFdE; - double scatvar = mxing.dmat_.scatterAngleVar(mom,mxing.plen_,mass); - // scattering is the same in each direction and has no net effect, it only adds noise - momvar[MomBasis::perpdir_] += scatvar; - momvar[MomBasis::phidir_] += scatvar; + // cloning requires domain knowledge of pointer members of the cloned object, + // which must be reassigned explicitly; the default action is thus to throw + // an error if a clone routine has not been explicitly provided. + template std::shared_ptr< ElementXing > ElementXing::clone(CloneContext&) const{ + std::string msg = "Attempt to clone KinKal::Hit subclass with no clone() implementation"; + throw std::runtime_error(msg); + } + + template void ElementXing::momentumChange(SVEC3& dmom, SMAT& dmomvar) const { + // compute the momentum change moving forwards time in global cartesian basis + // first, compute the change and variance in the momentum basis (along and perp to the momentum direction) + double dm, paramomvar, perpmomvar; + materialEffects(dm, paramomvar,perpmomvar); + // momentum change in forward time direction is due to energy loss; this is along the momentum + auto momdir = referenceTrajectory().direction(time()); + dmom = dm*SVEC3(momdir.X(),momdir.Y(),momdir.Z()); + // now compute the covariance; this includes smearing from energy straggling and multiple scattering. + // This is a diagonal matrix in momentum basis; no physical correlation between stragling and scattering, or orthogonal scattering. + ROOT::Math::SVector varvec(paramomvar, 0, perpmomvar, 0, 0, perpmomvar); + SMAT mmvar(varvec); // construct matrix from upper-diagonal elements + // loop over the momentum change basis directions and create the transform matrix between that and global Cartesian basis + SSMAT dmdxyz; // momentum basis -> Cartesian conversion matrix + for(int idir=0;idir(idir)); + SVEC3 vmdir(mdir.X(), mdir.Y(), mdir.Z()); + dmdxyz.Place_in_col(vmdir,0,idir); + } + // return covariance in global Cartesian coordinates + dmomvar = ROOT::Math::Similarity(dmdxyz,mmvar); + } + + template Parameters ElementXing::parameterChange(double varscale) const { + // compute this xing's effect on parameters and covariance. First compute the momentum change and variance + SVEC3 dmom; + SMAT dmomvar; + momentumChange(dmom,dmomvar); + // convert that to parameter space + DPDV dPdM = referenceTrajectory().dPardM(time()); + auto dmomp = dPdM*dmom; + // scale covariance as needed + dmomvar*= varscale; + // jacobean transform of covariance into parameter space + auto dmompvar = ROOT::Math::Similarity(dPdM,dmomvar); + return Parameters(dmomp,dmompvar); + } + + template void ElementXing::materialEffects(double& dmom, double& paramomvar, double& perpmomvar) const { + // accumulate the change in energy and scattering angle variance from the material components + double E = referenceTrajectory().energy(); + double mom = referenceTrajectory().momentum(); + double mass = referenceTrajectory().mass(); + // loop over individual materials and accumulate their effects + double dE(0.0), dEVar(0.0), scatvar(0.0); + for(auto const& mxing : matXings()){ + dE += mxing.dmat_.energyLoss(mom,mxing.plen_,mass); + dEVar += mxing.dmat_.energyLossVar(mom,mxing.plen_,mass); + scatvar += mxing.dmat_.scatterAngleVar(mom,mxing.plen_,mass); } - // correct for time direction + // convert energy change to change in momentum + double dmdE = E/mom; + dmom = dE*dmdE; + paramomvar = dEVar*dmdE*dmdE; + // scattering applies directly to momentum (1st order) + perpmomvar = scatvar*mom*mom; } template double ElementXing::radiationFraction() const { double retval(0.0); - for(auto const& mxing : mxings_) - retval += mxing.dmat_.radiationFraction(mxing.plen_/10.0); // Ugly conversion to cm FIXME!! + for(auto const& mxing : matXings()) + retval += mxing.dmat_.radiationFraction(mxing.plen_); return retval; } + } #endif diff --git a/Detector/Hit.hh b/Detector/Hit.hh index 55644077..56c5aa8b 100644 --- a/Detector/Hit.hh +++ b/Detector/Hit.hh @@ -2,40 +2,80 @@ #define KinKal_Hit_hh // // Base class to describe a measurement that constrains some aspect of the track fit -// The hit may be associated with a piece of detector material as well -// Used as part of the kinematic Kalman fit +// The constraint is expressed as a weight WRT a set of reference parameters. // +#include "KinKal/General/CloneContext.hh" #include "KinKal/General/Weights.hh" #include "KinKal/General/Parameters.hh" #include "KinKal/General/Chisq.hh" #include "KinKal/Trajectory/ParticleTrajectory.hh" #include "KinKal/Fit/MetaIterConfig.hh" +#include #include +#include +#include namespace KinKal { template class Hit { public: - using PKTRAJ = ParticleTrajectory; - // default - Hit(){} + using PTRAJ = ParticleTrajectory; + using KTRAJPTR = std::shared_ptr; + Hit() {} virtual ~Hit(){} // disallow copy and equivalence Hit(Hit const& ) = delete; Hit& operator =(Hit const& ) = delete; - // the constraint this hit implies WRT the current reference, expressed as a weight - virtual Weights weight() const =0; - // hits may be active (used in the fit) or inactive; this is a pattern recognition feature + // clone op for reinstantiation + virtual std::shared_ptr< Hit > clone(CloneContext&) const; + // hits may be active (used in the fit) or inactive; this is a pattern recognition feature virtual bool active() const =0; - virtual Chisq chisq() const =0; // least-squares distance to reference parameters + virtual unsigned nDOF() const=0; virtual Chisq chisq(Parameters const& params) const =0; // least-squares distance to given parameters virtual double time() const = 0; // time of this hit: this is WRT the reference trajectory - // update to a new reference, without changing state - virtual void update(PKTRAJ const& pktraj) = 0; - // update the internals of the hit, specific to this meta-iteraion - virtual void update(PKTRAJ const& pktraj, MetaIterConfig const& config) = 0; virtual void print(std::ostream& ost=std::cout,int detail=0) const = 0; + // update to a new reference, without changing internal state + virtual void updateReference(PTRAJ const& ptraj) = 0; + virtual KTRAJPTR const& refTrajPtr() const = 0; + // update the internals of the hit, specific to this meta-iteraion + virtual void updateState(MetaIterConfig const& config,bool first) = 0; + // The following provides the constraint/information content of this hit in the trajectory weight space + virtual Weights const& weight() const = 0; + KTRAJ const& referenceTrajectory() const { return *refTrajPtr(); } // trajectory WRT which the weight etc is defined + // parameters WRT which this hit's residual and weights are set. These are generally biased + // in that they contain the information of this hit + Parameters const& referenceParameters() const { return referenceTrajectory().params(); } + // Unbiased parameters, taking out this hit's effect from the reference + Parameters unbiasedParameters() const; + // unbiased least-squares distance to reference parameters + Chisq chisquared() const; }; + // cloning requires domain knowledge of pointer members of the cloned object, + // which must be reassigned explicitly; the default action is thus to throw + // an error if a clone routine has not been explicitly provided. + template std::shared_ptr< Hit > Hit::clone(CloneContext& context) const{ + std::string msg = "Attempt to clone KinKal::Hit subclass with no clone() implementation"; + throw std::runtime_error(msg); + } + + template Parameters Hit::unbiasedParameters() const { + if(active()){ + // convert the parameters to a weight, and subtract this hit's weight + Weights wt(referenceParameters()); + // subtract out the effect of this hit's reference weight from the reference parameters + wt -= weight(); + return Parameters(wt); + } else + return referenceParameters(); + } + + template Chisq Hit::chisquared() const { + if(active()){ + return chisq(unbiasedParameters()); + } else + return Chisq(); + } + template std::ostream& operator <<(std::ostream& ost, Hit const& thit) { thit.print(ost,0); return ost; diff --git a/Detector/ParameterHit.hh b/Detector/ParameterHit.hh index 0b6517af..39831f05 100644 --- a/Detector/ParameterHit.hh +++ b/Detector/ParameterHit.hh @@ -5,7 +5,6 @@ // external information to be added to the fit. // #include "KinKal/Detector/Hit.hh" -#include "KinKal/General/Vectors.hh" #include namespace KinKal { @@ -13,54 +12,78 @@ namespace KinKal { public: using PMASK = std::array; // parameter mask using HIT = Hit; - using PKTRAJ = ParticleTrajectory; + using PTRAJ = ParticleTrajectory; + using KTRAJPTR = std::shared_ptr; + // copy constructor + ParameterHit(ParameterHit const& rhs): + time_(rhs.time()), + params_(rhs.constraintParameters()), + pweight_(rhs.parameterWeights()), + weight_(rhs.weight()), + pmask_(rhs.constraintMask()), + mask_(rhs.maskMatrix()), + ncons_(rhs.numConstrainedParameters()){ + /**/ + }; + // clone op for reinstantiation + std::shared_ptr< Hit > clone(CloneContext& context) const override{ + auto rv = std::make_shared< ParameterHit >(*this); + return rv; + }; // Hit interface overrrides - Weights weight() const override { return weight_; } bool active() const override { return ncons_ > 0; } - Chisq chisq() const override { return chisq(refparams_); } // this is the BIASED chisquared, should take out the weight of this constraint, FIXME! Chisq chisq(Parameters const& pdata) const override; double time() const override { return time_; } + void updateState(MetaIterConfig const& config,bool first) override; + Weights const& weight() const override { return weight_; } // parameter constraints are absolute and can't be updated - void update(PKTRAJ const& pktraj) override { refparams_ = pktraj.nearestPiece(time()).params(); } - void update(PKTRAJ const& pktraj, MetaIterConfig const& config) override { update(pktraj); } void print(std::ostream& ost=std::cout,int detail=0) const override; + void updateReference(PTRAJ const& ptraj) override { reftraj_ = ptraj.nearestTraj(time()); } + KTRAJPTR const& refTrajPtr() const override { return reftraj_; } // ParameterHit-specfic interface // construct from constraint values, time, and mask of which parameters to constrain - ParameterHit(double time, Parameters const& params, PMASK const& pmask); + ParameterHit(double time, PTRAJ const& ptraj, Parameters const& params, PMASK const& pmask); virtual ~ParameterHit(){} - unsigned nDOF() const { return ncons_; } + unsigned nDOF() const override { return ncons_; } Parameters const& constraintParameters() const { return params_; } PMASK const& constraintMask() const { return pmask_; } + Weights parameterWeights() const { return pweight_; }; + DMAT maskMatrix() const { return mask_; } + unsigned numConstrainedParameters() const { return ncons_; }; private: - double time_; // time of this constraint: must be supplied on construction + double time_; // time of this constraint: must be supplied on construction and does not change + KTRAJPTR reftraj_; // reference WRT this hits weight was calculated Parameters params_; // constraint parameters with covariance - Weights weight_; // weight from these parameters - Parameters refparams_; // reference parameters + Weights pweight_; // weight from these (masked) parameters + Weights weight_; // current weight, including temp effects PMASK pmask_; // subset of parmeters to constrain DMAT mask_; // matrix to mask off unconstrainted parameters unsigned ncons_; // number of parameters constrained }; - template ParameterHit::ParameterHit(double time, Parameters const& params, PMASK const& pmask) : - time_(time), params_(params), weight_(params), pmask_(pmask), mask_(ROOT::Math::SMatrixIdentity()), ncons_(0) { + template ParameterHit::ParameterHit(double time, PTRAJ const& ptraj, Parameters const& params, PMASK const& pmask) : + time_(time), reftraj_(ptraj.nearestTraj(time)), params_(params), pweight_(params), pmask_(pmask), ncons_(0) { + // create the mask matrix; Use a temporary, not the data member, as root has caching problems with that (??) + mask_ = ROOT::Math::SMatrixIdentity(); // count constrained parameters, and mask off unused parameters for(size_t ipar=0;ipar < NParams(); ipar++){ - if(pmask_[ipar]){ - ncons_++; - } else { - // zero unconstrained values - mask_(ipar,ipar) = 0.0; - } + if(pmask_[ipar]){ + ncons_++; + } else { + mask_(ipar,ipar) = 0.0; + } } // Mask Off unused parameters - DMAT wmat = ROOT::Math::Similarity(mask_,weight_.weightMat()); - // 2 steps needed her, as otherwise root caching results in incomplete objects - DVEC wvec = weight_.weightVec(); - DVEC wreduced = wvec*mask_; - weight_ = Weights(wreduced, wmat); + pweight_.weightMat() = ROOT::Math::Similarity(mask_,pweight_.weightMat()); + pweight_.weightVec() = mask_*pweight_.weightVec(); } + template void ParameterHit::updateState(MetaIterConfig const& miconfig,bool first) { + weight_ = pweight_; // do this in 2 steps to avoid SMatrix caching issue + weight_ *= 1.0/miconfig.varianceScale(); // weight is inverse of variance + } + template Chisq ParameterHit::chisq(Parameters const& pdata) const { // chi measures the dimensionless tension between this constraint and the given parameters, including uncertainty // on both the measurement and the trajectory estimate. @@ -70,7 +93,7 @@ namespace KinKal { pdiff += params_; // this is now the difference of parameters but sum of covariances // invert the covariance matrix DMAT wmat = pdiff.covariance(); - if(!wmat.Invert()) throw std::runtime_error("Inversion failure"); + if(!wmat.Invert()) throw std::runtime_error("ParameterHit inversion failure"); // zero out unconstrainted parts wmat = ROOT::Math::Similarity(mask_,wmat); double chisq = ROOT::Math::Similarity(pdiff.parameters(),wmat); @@ -86,10 +109,10 @@ namespace KinKal { ost << " ParameterHit Hit" << std::endl; if(detail > 0){ for(size_t ipar=0;ipar < NParams(); ipar++){ - auto tpar = static_cast(ipar); - if (pmask_[ipar]) { - ost << " constraining parameter " << KTRAJ::paramName(tpar) << " to value " << params_.parameters()[ipar] << " +- " << sqrt(params_.covariance()(ipar,ipar)) << std::endl; - } + auto tpar = static_cast(ipar); + if (pmask_[ipar]) { + ost << " constraining parameter " << KTRAJ::paramName(tpar) << " to value " << params_.parameters()[ipar] << " +- " << sqrt(params_.covariance()(ipar,ipar)) << std::endl; + } } } } diff --git a/Detector/Residual.cc b/Detector/Residual.cc index b1e9723a..72105835 100644 --- a/Detector/Residual.cc +++ b/Detector/Residual.cc @@ -1,5 +1,27 @@ #include "KinKal/Detector/Residual.hh" namespace KinKal { + + Weights Residual::weight(DVEC const& params, double varscale) const { + if(active()){ + // convert derivatives vector to a Nx1 matrix + ROOT::Math::SMatrix dRdPM; + dRdPM.Place_in_col(dRdP(),0,0); + // convert the variance into a 1X1 matrix + ROOT::Math::SMatrix> RVarM; + // weight by inverse variance + double mvar = measurementVariance()*varscale; + RVarM(0,0) = 1.0/mvar; + // expand these into the weight matrix + DMAT wmat = ROOT::Math::Similarity(dRdPM,RVarM); + // translate residual value into weight vector WRT the reference parameters + // sign convention reflects resid = measurement - prediction + DVEC wvec = wmat*params + dRdP()*value()/mvar; + // weights are linearly additive + return Weights(wvec,wmat); + } else + return Weights(); + } + std::ostream& operator <<(std::ostream& ost, Residual const& res) { ost << " residual value " << res.value() << " variance " << res.variance() << " dRdP " << res.dRdP(); return ost; diff --git a/Detector/Residual.hh b/Detector/Residual.hh index 8730a5a8..f21fe60f 100644 --- a/Detector/Residual.hh +++ b/Detector/Residual.hh @@ -7,20 +7,33 @@ // #include #include "KinKal/General/Vectors.hh" +#include "KinKal/General/Weights.hh" namespace KinKal { class Residual { public: // accessors double value() const { return value_; } // residual value - double variance() const { return var_; } // variance on value, based on measurement uncertainty + double measurementVariance() const { return mvar_; } + double parameterVariance() const { return pvar_; } + double variance() const { return mvar_ + pvar_; } DVEC const& dRdP() const { return dRdP_; } // derivative of this residual WRT parameters - Residual(double value, double var, DVEC const& dRdP) : value_(value), var_(var), dRdP_(dRdP){} - Residual() : value_(0.0), var_(-1.0) {} + bool active() const { return active_; } + double chisq() const { return active() ? (value_*value_)/variance() : 0.0; } + double chi() const { return active() ? value_/sqrt(variance()): 0.0; } + double pull() const { return chi(); } + unsigned nDOF() const { return active() ? 1 : 0; } + // calculate the weight WRT some parameters implied by this residual. Optionally scale the variance + Weights weight(DVEC const& params, double varscale=1.0) const; + Residual(double value, double mvar, double pvar, DVEC const& dRdP,bool active=true) : + value_(value), mvar_(mvar), pvar_(pvar), dRdP_(dRdP), active_(active){} + Residual() : value_(0.0), mvar_(-1.0), pvar_(-1.0), active_(false) {} private: double value_; // value for this residual - double var_; // estimated variance of the residual due to sensor measurement uncertainty ONLY + double mvar_; // estimated variance due to measurement uncertainty + double pvar_; // estimated variance due to parameter uncertainty DVEC dRdP_; // derivative of this residual WRT the trajectory parameters, evaluated at the reference parameters + bool active_; }; std::ostream& operator <<(std::ostream& ost, Residual const& res); } diff --git a/Detector/ResidualHit.hh b/Detector/ResidualHit.hh index 0c64af5e..68236746 100644 --- a/Detector/ResidualHit.hh +++ b/Detector/ResidualHit.hh @@ -2,7 +2,7 @@ #define KinKal_ResidualHit_hh // // class representing a hit based on a set of uncorrelated but related (same sensor) measurements, expressed as residuals. -// Each residual esimates the 1-dimenaional tension between the measurement and the value predicted by a reference trajectory +// Each residual esimates the 1-dimenaional tension between the measurement and the value predicted by the reference trajectory // #include "KinKal/Detector/Hit.hh" #include "KinKal/Detector/Residual.hh" @@ -10,72 +10,68 @@ namespace KinKal { template class ResidualHit : public Hit { public: + // copy constructor + ResidualHit(ResidualHit const& rhs): weight_(rhs.weight()){ + /**/ + }; // override of some Hit interface. Subclasses must still implement update and material methods - Weights weight() const override; + using HIT = Hit; + using PTRAJ = ParticleTrajectory; bool active() const override { return nDOF() > 0; } - Chisq chisq() const override; + unsigned nDOF() const override; Chisq chisq(Parameters const& params) const override; - // ResidualHit specific interface. - unsigned nDOF() const; + Weights const& weight() const override { return weight_; } // describe residuals associated with this hit virtual unsigned nResid() const = 0; - // individual residuals may be active or inactive - virtual bool activeRes(unsigned ires) const = 0; - // reference residuals for this hit. iDOF indexs the measurement and is hit-specific, outside the range will throw - virtual Residual const& residual(unsigned ires) const = 0; - // residuals corrected to refer to the given set of parameters (1st-order) + // reference residuals for this hit. ires indexs the measurement and is hit-specific, outside the range will throw + // this is generally biased as the refefence includes the effect of this hit + virtual Residual const& refResidual(unsigned ires) const = 0; + // residuals corrected to refer to the given set of parameters (1st-order) Residual residual(Parameters const& params, unsigned ires) const; - + // unbiased residuals WRT the reference parameters; computed from the reference + Residual residual(unsigned ires) const; + // unbiased pull of this residual (including the uncertainty on the reference parameters) + double pull(unsigned ires) const; protected: - // allow subclasses to overwrite these during update - void setRefParams(KTRAJ const& reftraj) { refparams_ = reftraj.params(); } + ResidualHit() {} + // ResidualHit specific interface + void updateWeight(MetaIterConfig const& config); private: - Parameters refparams_; // reference parameters, used to compute reference residuals + Weights weight_; // weight of this hit computed from the residuals }; - template Residual ResidualHit::residual(Parameters const& pdata,unsigned ires) const { - auto const& resid = residual(ires); + template Residual ResidualHit::residual(Parameters const& params,unsigned ires) const { + auto const& resid = refResidual(ires); // compute the difference between these parameters and the reference parameters - DVEC dpvec = pdata.parameters() - refparams_.parameters(); + DVEC dpvec = params.parameters() - HIT::referenceParameters().parameters(); // project the parameter differnce to residual space and 'correct' the reference residual to be WRT these parameters double uresid = resid.value() - ROOT::Math::Dot(dpvec,resid.dRdP()); - return Residual(uresid,resid.variance(),resid.dRdP()); + double pvar = ROOT::Math::Similarity(resid.dRdP(),params.covariance()); + bool active = resid.active(); + if(pvar<0.0){ + active = false; + pvar = resid.variance(); + } + return Residual(uresid,resid.variance(),pvar,resid.dRdP(),active); + } - template Chisq ResidualHit::chisq() const { - double chisq(0.0); - unsigned ndof(0); - for(unsigned ires=0; ires< nResid(); ires++) { - if(activeRes(ires)) { - ndof++; - // find the reference residual - auto const& res = residual(ires); - // project the parameter covariance into a residual space variance - double rvar = ROOT::Math::Similarity(res.dRdP(),refparams_.covariance()); - // add the measurement variance - rvar += res.variance(); - // add chisq for this DOF - chisq += (res.value()*res.value())/rvar; - } - } - return Chisq(chisq, ndof); + template Residual ResidualHit::residual(unsigned ires) const { + return residual(HIT::unbiasedParameters(),ires); + } + + template double ResidualHit::pull(unsigned ires) const { + auto ures = residual(ires); + return ures.pull(); } template Chisq ResidualHit::chisq(Parameters const& params) const { double chisq(0.0); unsigned ndof(0); for(unsigned ires=0; ires< nResid(); ires++) { - if(activeRes(ires)) { - ndof++; - // compute the residual WRT the given parameters - auto res = residual(params,ires); - // project the parameter covariance into a residual space variance - double rvar = ROOT::Math::Similarity(res.dRdP(),params.covariance()); - // add the measurement variance - rvar += res.variance(); - // add chisq for this DOF - chisq += (res.value()*res.value())/rvar; - } + auto resid = residual(params,ires); + chisq += resid.chisq(); + ndof += resid.nDOF(); } return Chisq(chisq,ndof); } @@ -85,36 +81,18 @@ namespace KinKal { // into a single residual unsigned retval(0); for(unsigned ires=0; ires< nResid(); ires++) - if(activeRes(ires)) retval++; + retval += refResidual(ires).nDOF(); return retval; } - template Weights ResidualHit::weight() const { - // start with a null weight - Weights weight; + template void ResidualHit::updateWeight(MetaIterConfig const& miconfig) { + // start by zeroing the weight, then augment with each residual's weight + weight_ = Weights(); for(unsigned ires=0; ires< nResid(); ires++) { - if(activeRes(ires)) { - auto const& res = residual(ires); - // convert derivatives vector to a Nx1 matrix - ROOT::Math::SMatrix dRdPM; - dRdPM.Place_in_col(res.dRdP(),0,0); - // convert the variance into a 1X1 matrix - ROOT::Math::SMatrix> RVarM; - // weight by inverse variance - double tvar = res.variance(); - RVarM(0,0) = 1.0/tvar; - // expand these into the weight matrix - DMAT wmat = ROOT::Math::Similarity(dRdPM,RVarM); - // translate residual value into weight vector WRT the reference parameters - // sign convention reflects resid = measurement - prediction - DVEC wvec = wmat*refparams_.parameters() + res.dRdP()*res.value()/tvar; - // weights are linearly additive - weight += Weights(wvec,wmat); - } + auto const& resid = refResidual(ires); + if(resid.active())weight_ += resid.weight(HIT::referenceParameters().parameters(),miconfig.varianceScale()); } - return weight; } - } #endif diff --git a/Detector/StrawMaterial.cc b/Detector/StrawMaterial.cc deleted file mode 100644 index 01f5d512..00000000 --- a/Detector/StrawMaterial.cc +++ /dev/null @@ -1,48 +0,0 @@ -#include "KinKal/Detector/StrawMaterial.hh" -#include -#include -#include - -namespace KinKal { - void StrawMaterial::pathLengths(ClosestApproachData const& cadata,StrawXingConfig const& caconfig, - double& wallpath, double& gaspath, double& wirepath) const { - wallpath = gaspath = wirepath = 0.0; - double doca = std::min(fabs(cadata.doca()),srad_); - double sigdoca = sqrt(cadata.docaVar()); - doca = std::min(fabs(doca),srad_); - if (sigdoca < caconfig.minsigdoca_) { // small error: don't integrate - double rad = std::min(doca,srad_-thick_); - wallpath = 2.0*thick_*srad_/sqrt(srad2_-rad*rad); - gaspath = 2.0*sqrt(srad2_-rad*rad); - } else if (sigdoca*caconfig.nsig_ > srad_ ) { - // errors are large WRT the size of the straw, or DOCA is very far from the wire: just take the average over all impact parameters - wallpath = M_PI*thick_; - gaspath = 0.5*M_PI*srad_; - } else { - // integrate +- N sigma from DOCA. Restrict rmax to physical values - // Note that negative rmin is handled naturally - double rmax = std::min(srad_-thick_,(doca+caconfig.nsig_*sigdoca))/srad_; - double rmin = std::max(-srad_+thick_,doca-caconfig.nsig_*sigdoca)/srad_; - wallpath = 2.0*thick_*(asin(rmax) - asin(rmin))/(rmax-rmin); - gaspath = srad_*(asin(rmax) - asin(rmin) + - rmax*sqrt(1.0-rmax*rmax) - rmin*sqrt(1.0-rmin*rmin) )/(rmax-rmin); - if(isnan(wallpath) || isnan(gaspath))throw std::runtime_error("Invalid pathlength"); - } - // correct for the angle WRT the axis - double adot = cadata.dirDot(); - double afac = 1.0/sqrt(1.0-adot*adot); // angle factor, =1.0/sin(theta) - wallpath *= afac; - gaspath *= afac; - // Model the wire as a diffuse gas, density constrained by DOCA TODO - } - - void StrawMaterial::findXings(ClosestApproachData const& cadata,StrawXingConfig const& caconfig, std::vector& mxings) const { - mxings.clear(); - double wallpath, gaspath, wirepath; - pathLengths(cadata,caconfig,wallpath, gaspath, wirepath); - if(wallpath > 0.0) mxings.push_back(MaterialXing(*wallmat_,wallpath)); - if(gaspath > 0.0) mxings.push_back(MaterialXing(*gasmat_,gaspath)); - if(wirepath > 0.0) mxings.push_back(MaterialXing(*wiremat_,wirepath)); - } - -} diff --git a/Detector/StrawXing.hh b/Detector/StrawXing.hh deleted file mode 100644 index f6e8c1dd..00000000 --- a/Detector/StrawXing.hh +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef KinKal_StrawXing_hh -#define KinKal_StrawXing_hh -// -// Describe the material effects of a kinematic trajectory crossing a straw -// Used in the kinematic Kalman fit -// -#include "KinKal/Detector/ElementXing.hh" -#include "KinKal/Detector/StrawMaterial.hh" -#include "KinKal/Detector/StrawXingConfig.hh" -#include "KinKal/Detector/WireHit.hh" -#include "KinKal/Trajectory/Line.hh" -#include "KinKal/Trajectory/PiecewiseClosestApproach.hh" - -namespace KinKal { - template class StrawXing : public ElementXing { - public: - using PKTRAJ = ParticleTrajectory; - using EXING = ElementXing; - using PTCA = PiecewiseClosestApproach; - // construct from PTCA - StrawXing(PTCA const& tpoca, StrawMaterial const& smat) : tpdata_(tpoca.tpData()), tprec_(tpoca.precision()), smat_(smat), - sxconfig_(0.05*smat.strawRadius(),1.0), - axis_(tpoca.sensorTraj()) { - update(tpoca); } - virtual ~StrawXing() {} - // ElementXing interface - void update(PKTRAJ const& pktraj,MetaIterConfig const& miconfig) override; - double crossingTime() const override { return tpdata_.particleToca(); } - void print(std::ostream& ost=std::cout,int detail=0) const override; - // specific interface: this xing is based on PTCA - void update(PTCA const& tpoca); - // accessors - ClosestApproachData const& closestApproach() const { return tpdata_; } - double tpocaPrecision() const { return tprec_; } - StrawMaterial const& strawMaterial() const { return smat_; } - StrawXingConfig const& config() const { return sxconfig_; } - private: - ClosestApproachData tpdata_; // result of most recent TPOCA - double tprec_; // precision of TPOCA - StrawMaterial const& smat_; - StrawXingConfig sxconfig_; - Line axis_; // straw axis, expressed as a timeline - - // should add state for displace wire TODO - }; - - template void StrawXing::update(PTCA const& tpoca) { - if(tpoca.usable()){ - EXING::matXings().clear(); - tpdata_ = tpoca.tpData(); - smat_.findXings(tpdata_,sxconfig_,EXING::matXings()); - } else - throw std::runtime_error("CA failure"); - } - - template void StrawXing::update(PKTRAJ const& pktraj,MetaIterConfig const& miconfig) { - // search for an update to the xing configuration among this meta-iteration payload - auto sxconfig = miconfig.findUpdater(); - if(sxconfig != 0) sxconfig_ = *sxconfig; - // use current xing time create a hint to the CA calculation: this speeds it up - CAHint tphint(this->crossingTime(), this->crossingTime()); - PTCA tpoca(pktraj,axis_,tphint,tprec_); - update(tpoca); - } - - template void StrawXing::print(std::ostream& ost,int detail) const { - ost <<"Straw Xing time " << this->crossingTime(); - if(detail > 0){ - for(auto const& mxing : this->matXings()){ - ost << " " << mxing.dmat_.name() << " pathLen " << mxing.plen_; - } - } - if(detail > 1){ - ost << " Axis "; - axis_.print(ost,0); - } - ost << std::endl; - } - -} -#endif diff --git a/Detector/StrawXingConfig.hh b/Detector/StrawXingConfig.hh deleted file mode 100644 index 62714121..00000000 --- a/Detector/StrawXingConfig.hh +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef KinKal_StrawXingConfig_hh -#define KinKal_StrawXingConfig_hh -namespace KinKal { - // simple struct to hold crossing calculation configuration parameters - struct StrawXingConfig { - double minsigdoca_; // minimum doca sigma to integrate - double nsig_; // number of sigma past wall to consider 'inside' the straw - StrawXingConfig(double ddmax, double nsig) : minsigdoca_(ddmax), nsig_(nsig) {} - }; -} -#endif - diff --git a/Detector/WireHit.hh b/Detector/WireHit.hh deleted file mode 100644 index 829a9e35..00000000 --- a/Detector/WireHit.hh +++ /dev/null @@ -1,150 +0,0 @@ -#ifndef KinKal_WireHit_hh -#define KinKal_WireHit_hh -// -// class representing a drift wire measurement. Implemented using PTCA between the particle traj and the wire -// Used as part of the kinematic Kalman fit -// -#include "KinKal/Detector/ResidualHit.hh" -#include "KinKal/Detector/WireHitStructs.hh" -#include "KinKal/Trajectory/Line.hh" -#include "KinKal/Trajectory/PiecewiseClosestApproach.hh" -#include "KinKal/General/BFieldMap.hh" -#include -#include -namespace KinKal { - - - template class WireHit : public ResidualHit { - public: - using PKTRAJ = ParticleTrajectory; - using PTCA = PiecewiseClosestApproach; - enum Dimension { tresid=0, dresid=1}; // residual dimensions - // Hit interface overrrides; subclass still needs to implement state change update - unsigned nResid() const override { return 2; } // potentially 2 residuals - bool activeRes(unsigned ires) const override; - Residual const& residual(unsigned ires=tresid) const override; - double time() const override { return tpdata_.particleToca(); } - void update(PKTRAJ const& pktraj) override; - void update(PKTRAJ const& pktraj, MetaIterConfig const& config) override; - void print(std::ostream& ost=std::cout,int detail=0) const override; - // virtual interface that must be implemented by concrete WireHit subclasses - // given a drift DOCA and direction in the cell, compute drift time and velocity - virtual void distanceToTime(POL2 const& drift, DriftInfo& dinfo) const = 0; - // define null ambiguity hit properties - virtual double nullVariance(Dimension dim,DriftInfo const& dinfo) const = 0; - virtual double nullOffset(Dimension dim,DriftInfo const& dinfo) const = 0; - // WireHit specific functions - ClosestApproachData const& closestApproach() const { return tpdata_; } - WireHitState const& hitState() const { return whstate_; } - Residual const& timeResidual() const { return rresid_[tresid]; } - Residual const& spaceResidual() const { return rresid_[dresid]; } - Line const& wire() const { return wire_; } - BFieldMap const& bfield() const { return bfield_; } - double precision() const { return precision_; } - // constructor - WireHit(BFieldMap const& bfield, PTCA const& ptca, WireHitState const& whs); - virtual ~WireHit(){} - protected: - void setState(WireHitState::State state) { whstate_.state_ = state; } - private: - WireHitState whstate_; // current state - ClosestApproachData tpdata_; // reference time and distance of closest approach to the wire - BFieldMap const& bfield_; // drift calculation requires the BField for ExB effects - Line wire_; // local linear approximation to the wire of this hit. - // the start time is the measurement time, the direction is from - // the physical source of the signal (particle) towards the measurement location, the vector magnitude - // is the effective signal propagation velocity, and the range describes the active wire length - // (when multiplied by the propagation velocity). - std::array rresid_; // residuals WRT most recent reference - double precision_; // precision for PTCA calculation; can change during processing schedule - }; - - template WireHit::WireHit(BFieldMap const& bfield, PTCA const& ptca, WireHitState const& wstate) : - whstate_(wstate), tpdata_(ptca.tpData()), bfield_(bfield), wire_(ptca.sensorTraj()), precision_(ptca.precision()) {} - - template void WireHit::update(PKTRAJ const& pktraj,MetaIterConfig const& miconfig) { - update(pktraj); - } - - template bool WireHit::activeRes(unsigned ires) const { - if(ires ==0 && whstate_.active()) - return true; - else if(ires ==1 && whstate_.state_ == WireHitState::null) - return true; - else - return false; - } - - template void WireHit::update(PKTRAJ const& pktraj) { - CAHint tphint(wire_.range().mid(),wire_.range().mid()); - // if we already computed PTCA in the previous iteration, use that to set the hint. This speeds convergence - if(tpdata_.usable()) tphint = CAHint(tpdata_.particleToca(),tpdata_.sensorToca()); - PTCA tpoca(pktraj,wire_,tphint,precision_); - if(!tpoca.usable())throw std::runtime_error("PTCA failure"); - tpdata_ = tpoca.tpData(); - this->setRefParams(pktraj.nearestPiece(tpoca.particleToca())); - // compute drift parameters. These are used even for null-ambiguity hits - VEC3 bvec = bfield_.fieldVect(tpoca.particlePoca().Vect()); - auto pdir = bvec.Cross(wire_.direction()).Unit(); // direction perp to wire and BFieldMap - VEC3 dvec = tpoca.delta().Vect(); - double phi = asin(double(dvec.Unit().Dot(pdir))); // azimuth around the wire WRT the BField - POL2 drift(fabs(tpoca.doca()), phi); - DriftInfo dinfo; - distanceToTime(drift, dinfo); - if(whstate_.useDrift()){ - // translate PTCA to residual. Use ambiguity to convert drift time to a time difference. - double dsign = whstate_.lrSign()*tpoca.lSign(); // overall sign is the product of assigned ambiguity and doca (angular momentum) sign - double dt = tpoca.deltaT()-dinfo.tdrift_*dsign; - DVEC dRdP = tpoca.dDdP()*dsign/dinfo.vdrift_ - tpoca.dTdP(); - rresid_[tresid] = Residual(dt,dinfo.tdriftvar_,dRdP); - } else { - // interpret DOCA against the wire directly as a residuals. We have to take the DOCA sign out of the derivatives - DVEC dRdP = -tpoca.lSign()*tpoca.dDdP(); - double dd = tpoca.doca() + nullOffset(dresid,dinfo); - double nulldvar = nullVariance(dresid,dinfo); - rresid_[dresid] = Residual(dd,nulldvar,dRdP); - // interpret TOCA as a residual - double dt = tpoca.deltaT() + nullOffset(tresid,dinfo); - // the time constraint variance is the sum of the variance from maxdoca and from the intrinsic measurement variance - double nulltvar = dinfo.tdriftvar_ + nullVariance(tresid,dinfo); - rresid_[tresid] = Residual(dt,nulltvar,-tpoca.dTdP()); - // Note there is no correlation between distance and time residuals; the former is just from the wire position, the latter from the time measurement - } - } - - template Residual const& WireHit::residual(unsigned ires) const { - if(ires >=2)throw std::invalid_argument("Invalid residual"); - return rresid_[ires]; - } - - template void WireHit::print(std::ostream& ost, int detail) const { - ost << " WireHit state "; - switch(whstate_.state_) { - case WireHitState::inactive: - ost << "inactive"; - break; - case WireHitState::left: - ost << "left"; - break; - case WireHitState::right: - ost << "right"; - break; - case WireHitState::null: default: - ost << "null"; - break; - } - if(detail > 0){ - if(activeRes(tresid)) - ost << " Time Residual " << rresid_[tresid]; - if(activeRes(dresid)) - ost << " Distance Residual " << rresid_[dresid]; - ost << std::endl; - } - if(detail > 1) { - ost << "Propagation speed " << wire_.speed() << " TPOCA " << tpdata_ << std::endl; - } - } - - -} // KinKal namespace -#endif diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt index ae561ecd..f85aca48 100644 --- a/Examples/CMakeLists.txt +++ b/Examples/CMakeLists.txt @@ -1,18 +1,26 @@ +# Explicitly list source files in this shared library +# When a new source file is added, it must be added to this list. + # generate root dictionary ROOT_GENERATE_DICTIONARY(ExamplesDict # headers to include in ROOT dictionary HitInfo.hh - BFieldInfo.hh + DomainWallInfo.hh ParticleTrajectoryInfo.hh MaterialInfo.hh LINKDEF LinkDef.h MODULE Examples ) # create shared library with ROOT dict -add_library(Examples SHARED ExamplesDict) - -target_include_directories(Examples PRIVATE ${PROJECT_SOURCE_DIR}/..) +add_library(Examples SHARED + CaloDistanceToTime.cc + StrawMaterial.cc + ExamplesDict +) target_include_directories(Examples PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) +target_include_directories(Examples PUBLIC + $ + $) # link ROOT libraries target_link_libraries(Examples ${ROOT_LIBRARIES}) diff --git a/Examples/CaloDistanceToTime.cc b/Examples/CaloDistanceToTime.cc new file mode 100644 index 00000000..034198a5 --- /dev/null +++ b/Examples/CaloDistanceToTime.cc @@ -0,0 +1,50 @@ +#include "KinKal/Examples/CaloDistanceToTime.hh" +#include +#include +#include +#include + +CaloDistanceToTime::CaloDistanceToTime(double asymptoticSpeed, double distanceOffset, double flatSlope) : + asymptoticSpeed_(asymptoticSpeed), distanceOffset_(distanceOffset), + timeOffset_(sqrt(1+pow(distanceOffset/asymptoticSpeed, 2))), flatSlope_(flatSlope), + slopeRoot_(sqrt(1-pow(flatSlope * asymptoticSpeed, 2))) {} + + double CaloDistanceToTime::distance(double deltaT) { + if (deltaT > timeOffset_ - 1/slopeRoot_) { + return ((deltaT - timeOffset_ + 1/slopeRoot_) / flatSlope_) + distanceOffset_ - flatSlope_*pow(asymptoticSpeed_, 2)/slopeRoot_; + } + return distanceOffset_ - asymptoticSpeed_ * sqrt(pow(deltaT - timeOffset_, 2) - 1); +} + +double CaloDistanceToTime::time(double distance) { + //double static const calorimeterLength = 200; + double cutoff = distanceOffset_ - flatSlope_*pow(asymptoticSpeed_, 2) / slopeRoot_; + if (distance >= cutoff) { + return (distance - cutoff) * flatSlope_ + timeOffset_-1/slopeRoot_; + } + return timeOffset_ - evaluate_root(distance); +} + +double CaloDistanceToTime::speed(double distance) { + // double static const speedOfLight = 299.792458; // mm/ns + /*double actualSpeed = 1 / inverseSpeed(distance); + if (abs(actualSpeed) > speedOfLight) { + return speedOfLight; + } */ + double invSpeed = inverseSpeed(distance); + /*if (invSpeed < (1/speedOfLight)) { + return speedOfLight; + }*/ + return 1/invSpeed; +} + +double CaloDistanceToTime::inverseSpeed(double distance) { + if (distance >= distanceOffset_ - flatSlope_*pow(asymptoticSpeed_, 2) / slopeRoot_) { + return flatSlope_; + } + return (distanceOffset_ - distance) / (pow(asymptoticSpeed_, 2) * evaluate_root(distance)); +} + +double CaloDistanceToTime::evaluate_root(double distance) { + return sqrt(1 + pow((distance - distanceOffset_) / asymptoticSpeed_, 2)); +} \ No newline at end of file diff --git a/Examples/CaloDistanceToTime.hh b/Examples/CaloDistanceToTime.hh new file mode 100644 index 00000000..374096f3 --- /dev/null +++ b/Examples/CaloDistanceToTime.hh @@ -0,0 +1,20 @@ +#include "KinKal/Trajectory/DistanceToTime.hh" +#ifndef KinKal_CaloDistanceToTime_hh +#define KinKal_CaloDistanceToTime_hh + +class CaloDistanceToTime : public DistanceToTime { + public: + CaloDistanceToTime(double asymptoticSpeed, double distanceOffset, double flatSlope); + double distance(double deltaT) override; + double time(double distance) override; + double speed(double distance) override; + double inverseSpeed(double distance) override; + private: + double evaluate_root(double distance); + double asymptoticSpeed_; + double distanceOffset_; + double timeOffset_; + double flatSlope_; + double slopeRoot_; +}; +#endif diff --git a/Examples/DOCAWireHitUpdater.hh b/Examples/DOCAWireHitUpdater.hh new file mode 100644 index 00000000..01ee0f02 --- /dev/null +++ b/Examples/DOCAWireHitUpdater.hh @@ -0,0 +1,31 @@ +#ifndef KinKal_DOCAWireHitUpdater_hh +#define KinKal_DOCAWireHitUpdater_hh +// simple WireHit updater based on DOCA +#include "KinKal/Examples/WireHitStructs.hh" +namespace KinKal { + class DOCAWireHitUpdater { + public: + DOCAWireHitUpdater(double mindoca,double maxdoca ) : mindoca_(mindoca), maxdoca_(maxdoca) {} + // define the state given the (presumably unbiased) distance of closest approach + WireHitState wireHitState(double doca) const; + double minDOCA() const { return mindoca_; } + double maxDOCA() const { return maxdoca_; } + private: + double mindoca_; // minimum DOCA value to sign LR ambiguity + double maxdoca_; // maximum DOCA to still use a hit + }; + + WireHitState DOCAWireHitUpdater::wireHitState(double doca) const { + WireHitState state; + if(fabs(doca) > maxdoca_ ) { + state = WireHitState::inactive; // disable the hit if it's an outlier + } else if(fabs(doca) > mindoca_ ) { + state = doca > 0.0 ? WireHitState::right : WireHitState::left; + } else { + // too close to the wire: don't try to disambiguate LR sign + state = WireHitState::null; + } + return state; + } +} +#endif diff --git a/Examples/BFieldInfo.hh b/Examples/DomainWallInfo.hh similarity index 53% rename from Examples/BFieldInfo.hh rename to Examples/DomainWallInfo.hh index 6cfb8986..6dca403b 100644 --- a/Examples/BFieldInfo.hh +++ b/Examples/DomainWallInfo.hh @@ -1,15 +1,14 @@ -#ifndef KinKal_BFieldInfo_hh -#define KinKal_BFieldInfo_hh +#ifndef KinKal_DomainWallInfo_hh +#define KinKal_DomainWallInfo_hh #include namespace KinKal { - struct BFieldInfo { - BFieldInfo(){}; - ~BFieldInfo(){}; + struct DomainWallInfo { + DomainWallInfo(){}; Int_t active_; Float_t time_, range_; static std::string leafnames() { return std::string("active/i:time/f:range/f"); } }; - typedef std::vector KKBFIV; + typedef std::vector KKBFIV; } #endif diff --git a/Examples/HitInfo.hh b/Examples/HitInfo.hh index f5d9e6e5..41ae32ae 100644 --- a/Examples/HitInfo.hh +++ b/Examples/HitInfo.hh @@ -4,15 +4,18 @@ #include "Math/Vector3D.h" namespace KinKal { struct HitInfo { - enum htype{straw=0, scint,constraint,unknown}; + enum htype{straw=0, scint, parcon,unknown}; HitInfo(): - active_(-1), type_(-1), state_(-1), ndof_(-1), - time_(0.0), tresid_(0.0), tresidvar_(0.0), dresid_(0.0), dresidvar_(0.0), chisq_(0.0), - doca_(0.0), deltat_(0.0), docavar_(0.0), tocavar_(0.0), t0_(0.0) {} + active_(-1), type_(-1), state_(-1), ndof_(-1), id_(-1), + time_(0.0), tresid_(0.0), tresidvar_(0.0), tresidpull_(0.0), + dresid_(0.0), dresidvar_(0.0), dresidpull_(0.0), chisq_(0.0), prob_(0.0), + doca_(0.0), deltat_(0.0), docavar_(0.0), tocavar_(0.0), t0_(0.0), dirdot_(0.0) {} - Int_t active_, type_, state_, ndof_; - Float_t time_, tresid_, tresidvar_, dresid_, dresidvar_, chisq_; - Float_t doca_, deltat_, docavar_, tocavar_, t0_; + Int_t active_, type_, state_, ndof_, id_; + Float_t time_; + Float_t tresid_, tresidvar_, tresidpull_, dresid_, dresidvar_, dresidpull_; + Float_t chisq_, prob_; + Float_t doca_, deltat_, docavar_, tocavar_, t0_, dirdot_; ROOT::Math::XYZVectorF pos_; }; typedef std::vector KKHIV; diff --git a/Examples/LinkDef.h b/Examples/LinkDef.h index 6f4ac83f..54116fb8 100644 --- a/Examples/LinkDef.h +++ b/Examples/LinkDef.h @@ -2,8 +2,8 @@ #pragma link C++ class KinKal::HitInfo+; #pragma link C++ class vector+; -#pragma link C++ class KinKal::BFieldInfo+; -#pragma link C++ class vector+; +#pragma link C++ class KinKal::DomainWallInfo+; +#pragma link C++ class vector+; #pragma link C++ class KinKal::MaterialInfo+; #pragma link C++ class vector+; #pragma link C++ class KinKal::ParticleTrajectoryInfo+; diff --git a/Examples/MaterialInfo.hh b/Examples/MaterialInfo.hh index 9e835219..77213a29 100644 --- a/Examples/MaterialInfo.hh +++ b/Examples/MaterialInfo.hh @@ -4,11 +4,11 @@ #include namespace KinKal { struct MaterialInfo { - MaterialInfo(){}; - ~MaterialInfo(){}; + MaterialInfo(): active_(false), nxing_(-1), time_(0.0), dmomf_(0.0), + momvar_(0.0), perpvar_(0.0), doca_(0.0), docavar_(0.0), dirdot_(0.0){}; Int_t active_, nxing_; Float_t time_, dmomf_, momvar_, perpvar_; - static std::string leafnames() { return std::string("active/i:nxing/i:time/f:dmomf/f:momvar/f:perpvar/f"); } + Float_t doca_, docavar_, dirdot_; }; typedef std::vector KKMIV; } diff --git a/Examples/ParticleTrajectoryInfo.hh b/Examples/ParticleTrajectoryInfo.hh index 83a02e48..fb59ec60 100644 --- a/Examples/ParticleTrajectoryInfo.hh +++ b/Examples/ParticleTrajectoryInfo.hh @@ -5,7 +5,6 @@ namespace KinKal { struct ParticleTrajectoryInfo { ParticleTrajectoryInfo(){}; - ~ParticleTrajectoryInfo(){}; Float_t time_, dperp_, dt_; static std::string leafnames() { return std::string("time/f:dperp/f:dt/f"); } }; diff --git a/Examples/ScintHit.hh b/Examples/ScintHit.hh index 41b78495..8a370e8e 100644 --- a/Examples/ScintHit.hh +++ b/Examples/ScintHit.hh @@ -1,82 +1,119 @@ #ifndef KinKal_ScintHit_hh #define KinKal_ScintHit_hh // -// simple example hit subclass representing a time measurement using scintillator light from a crystal or plastic scintillator +// simple hit subclass representing a time measurement using scintillator light from a crystal or plastic scintillator // #include "KinKal/Detector/ResidualHit.hh" -#include "KinKal/Trajectory/Line.hh" +#include "KinKal/Trajectory/SensorLine.hh" #include "KinKal/Trajectory/PiecewiseClosestApproach.hh" #include namespace KinKal { template class ScintHit : public ResidualHit { public: - using PKTRAJ = ParticleTrajectory; - using PTCA = PiecewiseClosestApproach; - - // Hit interface implementation + using PTRAJ = ParticleTrajectory; + using PCA = PiecewiseClosestApproach; + using CA = ClosestApproach; + using RESIDHIT = ResidualHit; + using HIT = Hit; + using KTRAJPTR = std::shared_ptr; + // copy constructor + ScintHit(ScintHit const& rhs): + ResidualHit(rhs), + saxis_(rhs.sensorAxis()), + tvar_(rhs.timeVariance()), + wvar_(rhs.widthVariance()), + tpca_( + rhs.closestApproach().particleTraj(), + saxis_, + rhs.closestApproach().hint(), + rhs.closestApproach().precision() + ), + rresid_(rhs.refResidual()){ + /**/ + }; + // clone op for reinstantiation + std::shared_ptr< Hit > clone(CloneContext& context) const override{ + auto rv = std::make_shared< ScintHit >(*this); + auto ca = rv->closestApproach(); + auto trajectory = std::make_shared(ca.particleTraj()); + ca.setTrajectory(trajectory); + rv->setClosestApproach(ca); + return rv; + }; + // ResidualHit interface implementation unsigned nResid() const override { return 1; } // 1 time residual - bool activeRes(unsigned ires=0) const override; - Residual const& residual(unsigned ires=0) const override; - double time() const override { return tpdata_.particleToca(); } - void update(PKTRAJ const& pktraj) override; - void update(PKTRAJ const& pktraj, MetaIterConfig const& config) override; + Residual const& refResidual(unsigned ires=0) const override; + double time() const override { return tpca_.particleToca(); } + void updateReference(PTRAJ const& ptraj) override; + KTRAJPTR const& refTrajPtr() const override { return tpca_.particleTrajPtr(); } + void updateState(MetaIterConfig const& config,bool first) override; void print(std::ostream& ost=std::cout,int detail=0) const override; - // scintHit explicit interface - ScintHit(PTCA const& ptca, double tvar, double wvar, double precision=1e-8) : - saxis_(ptca.sensorTraj()), tvar_(tvar), wvar_(wvar), active_(true), tpdata_(ptca.tpData()), precision_(precision) {} + // scintHit explicit interface + ScintHit(PCA const& pca, double tvar, double wvar); virtual ~ScintHit(){} - Residual const& timeResidual() const { return rresid_; } // the line encapsulates both the measurement value (through t0), and the light propagation model (through the velocity) - Line const& sensorAxis() const { return saxis_; } - ClosestApproachData const& closestApproach() const { return tpdata_; } + auto const& sensorAxis() const { return saxis_; } + auto const& closestApproach() const { return tpca_; } double timeVariance() const { return tvar_; } double widthVariance() const { return wvar_; } + auto precision() const { return tpca_.precision(); } private: - Line saxis_; // symmetry axis of this sensor + SensorLine saxis_; // symmetry axis of this sensor double tvar_; // variance in the time measurement: assumed independent of propagation distance/time double wvar_; // variance in transverse position of the sensor/measurement in mm. Assumes cylindrical error, could be more general - bool active_; // active or not - ClosestApproachData tpdata_; // reference time and distance of closest approach to the axis. - // caches + CA tpca_; // reference time and position of closest approach to the axis Residual rresid_; // residual WRT most recent reference parameters - double precision_; // current precision + + // modifiers to support cloning + void setClosestApproach(const CA& ca){ tpca_ = ca; } }; - template bool ScintHit::activeRes(unsigned ires) const { - if(ires == 0 && active_) - return true; - else - return false; - } + template ScintHit::ScintHit(PCA const& pca, double tvar, double wvar) : + saxis_(pca.sensorTraj()), tvar_(tvar), wvar_(wvar), + tpca_(pca.localTraj(),saxis_,pca.precision(),pca.tpData(),pca.dDdP(),pca.dTdP()) + {} - template Residual const& ScintHit::residual(unsigned ires) const { + template Residual const& ScintHit::refResidual(unsigned ires) const { if(ires !=0)throw std::invalid_argument("Invalid residual"); return rresid_; } - template void ScintHit::update(PKTRAJ const& pktraj) { - // compute PTCA - CAHint tphint( saxis_.t0(), saxis_.t0()); - // don't update the hint: initial T0 values can be very poor, which can push the CA calculation onto the wrong helix loop, - // from which it's impossible to ever get back to the correct one. Active loop checking might be useful eventually too TODO - // if(tpdata_.usable()) tphint = CAHint(tpdata_.particleToca(),tpdata_.sensorToca()); - PTCA tpoca(pktraj,saxis_,tphint,precision_); - if(tpoca.usable()){ - tpdata_ = tpoca.tpData(); - // residual is just delta-T at CA. - // the variance includes the measurement variance and the tranvserse size (which couples to the relative direction) - double dd2 = tpoca.dirDot()*tpoca.dirDot(); - double totvar = tvar_ + wvar_*dd2/(saxis_.speed()*saxis_.speed()*(1.0-dd2)); - rresid_ = Residual(tpoca.deltaT(),totvar,-tpoca.dTdP()); - this->setRefParams(pktraj.nearestPiece(tpoca.particleToca())); - } else - throw std::runtime_error("PTCA failure"); + template void ScintHit::updateReference(PTRAJ const& ptraj) { + // use previous hint, or initialize from the sensor time + CAHint tphint = tpca_.usable() ? tpca_.hint() : CAHint(saxis_.measurementTime(), saxis_.measurementTime()); + PCA pca(ptraj,saxis_,tphint,precision()); + tpca_ = pca.localClosestApproach(); + if(!tpca_.usable())throw std::runtime_error("ScintHit TPOCA failure"); } - template void ScintHit::update(PKTRAJ const& pktraj, MetaIterConfig const& miconfig) { - // for now, no updates are needed. Eventually could test for consistency, update errors, etc - update(pktraj); + template void ScintHit::updateState(MetaIterConfig const& config,bool first) { + // check that TPCA position is consistent with the physical sensor. This can be off if the CA algorithm finds the wrong helix branch + // early in the fit when t0 has very large errors. + // If it is unphysical try to adjust it back using a better hint. + auto ppos = tpca_.particlePoca().Vect(); + auto const& sstart = saxis_.start(); + auto const& send = saxis_.end(); + // tolerance should come from the config. Should also test relative to the error. FIXME + double tol = saxis_.length()*1.0; + double sdist = (ppos - saxis_.middle()).Dot(saxis_.direction()); + if( (ppos-sstart).Dot(saxis_.direction()) < -tol || (ppos-send).Dot(saxis_.direction()) > tol) { + // adjust hint to the middle and try agian + double sspeed = tpca_.particleTraj().velocity(tpca_.particleToca()).Dot(saxis_.direction()); + auto tphint = tpca_.hint(); + tphint.particleToca_ -= sdist/sspeed; + tpca_ = CA(tpca_.particleTrajPtr(),saxis_,tphint,precision()); + // should check if this is still unphysical and disable the hit if so FIXME + sdist = (tpca_.particlePoca().Vect() - saxis_.middle()).Dot(saxis_.direction()); + } + + // residual is just delta-T at CA. + // the variance includes the measurement variance and the tranvserse size (which couples to the relative direction) + // Might want to do more updating (set activity) based on DOCA in future: TODO + double dd2 = tpca_.dirDot()*tpca_.dirDot(); + double totvar = tvar_ + wvar_*dd2/(saxis_.speed(sdist)*saxis_.speed(sdist)*(1.0-dd2)); + rresid_ = Residual(tpca_.deltaT(),totvar,0.0,tpca_.dTdP()); + this->updateWeight(config); } template void ScintHit::print(std::ostream& ost, int detail) const { diff --git a/Examples/ShellXing.hh b/Examples/ShellXing.hh new file mode 100644 index 00000000..bb803b4b --- /dev/null +++ b/Examples/ShellXing.hh @@ -0,0 +1,98 @@ +#ifndef KinKal_ShellXing_hh +#define KinKal_ShellXing_hh +// +// Describe the effects of a kinematic trajectory crossing a thin shell of material defined by a surface +// Used in the kinematic Kalman fit +// +#include "KinKal/Detector/ElementXing.hh" +#include "KinKal/Geometry/Surface.hh" +#include "KinKal/Geometry/ParticleTrajectoryIntersect.hh" +#include "Offline/Mu2eKinKal/inc/ShellXingUpdater.hh" +#include "KinKal/MatEnv/DetMaterial.hh" + +namespace KinKal { + template class ShellXing : public ElementXing { + public: + using PTRAJ = ParticleTrajectory; + using KTRAJPTR = std::shared_ptr; + using EXING = ElementXing; + using PCA = PiecewiseClosestApproach; + using CA = ClosestApproach; + using SURFPTR = std::shared_ptr; + // construct from a surface, material, intersection, and transverse thickness + ShellXing(SURFPTR surface, MatEnv::DetMaterial const& mat, Intersection inter, double thickness) + virtual ~ShellXing() {} + // ElementXing interface + void updateReference(PTRAJ const& ptraj) override; + void updateState(MetaIterConfig const& config,bool first) override; + Parameters params() const override; + double time() const override { return inter_.time(); } + KTRAJ const& referenceTrajectory() const override { return *reftrajptr_; } + std::vectorconst& matXings() const override { return mxings_; } + void print(std::ostream& ost=std::cout,int detail=0) const override; + // accessors + private: + SURFPTR surf_; // surface + MatEnv::DetMaterial const& mat_; + Intersection inter_; // most recent intersection + KTRAJPTR reftrajptr_; // reference trajectory + std::vector mxings_; // material xing + double thick_; // shell thickness + double tol_; // tolerance for intersection + Parameters fparams_; // 1st-order parameter change for forwards time + ShellXingUpdater sxconfig_; // note this must come from an updater during processing + double varscale_; // cache + }; + + template ShellXing::ShellXing(SURFPTR surface, MatEnv::DetMaterial const& mat, Intersection inter, + KTTRAJPTR reftrajptr, double thickness, double tol) : + surf_(surface), mat_(mat), inter_(inter), reftrajptr_(reftrajptr), thick_(thick),tol_(tol), varscale_(1.0) + {} + + template void ShellXing::updateReference(PTRAJ const& ptraj) { + // re-intersect with the surface, taking the current time as start and range from the current piece (symmetrized) + double delta = 0.5*reftraj->range().range(); // this factor may need tuning: too small and it may miss the intersection, too large and it may jump to a different intersection TODO + TimeRange irange(inter_.time_-delta, inter_.time_+delta); + inter_ = intersect(ptraj, &surf_, irange,tol_); + reftrajptr_ = ptraj.nearestTraj(inter_.time_); // I may need to protect against the case the time is crazy (failed intersection) TODO + } + + template void ShellXing::updateState(MetaIterConfig const& miconfig,bool first) { + if(first) { + // search for an update to the xing configuration among this meta-iteration payload + auto sxconfig = miconfig.findUpdater(); + if(sxconfig != 0) sxconfig_ = *sxconfig; + if(sxconfig_.scalevar_) varscale_ = miconfig.varianceScale(); + // find the material xings from gas, straw wall, and wire + auto cad = ca_.tpData(); + if(shptr_ && shptr_->hitState().active()){ + // if we have an associated hit, overwrite the DOCA and DOCAVAR using the drift info, which is much more accurate + auto dinfo = shptr_->fillDriftInfo(); + cad.doca_ = dinfo.rDrift_; + cad.docavar_ = dinfo.unsignedDriftVar(); + } + mxings_.clear(); + // check if we are on the surface + if(inter_.good()){ + // compute the material + } + } + if(mxings_.size() > 0){ + fparams_ = this->parameterChange(varscale_); + } else { + // reset + fparams_ = Parameters(); + } + } + + template Parameters ShellXing::params() const { + return fparams_; + } + + template void ShellXing::print(std::ostream& ost,int detail) const { + ost <<"Shell Xing time " << this->time(); + ost << std::endl; + } + +} +#endif diff --git a/Examples/SimpleWireHit.hh b/Examples/SimpleWireHit.hh index b786bae9..e5657db7 100644 --- a/Examples/SimpleWireHit.hh +++ b/Examples/SimpleWireHit.hh @@ -3,111 +3,218 @@ // // Simple implementation of a wire hit, for testing purpopses // -#include "KinKal/Detector/WireHit.hh" +#include "KinKal/Detector/ResidualHit.hh" +#include "KinKal/Examples/DOCAWireHitUpdater.hh" +#include "KinKal/Examples/WireHitStructs.hh" +#include "KinKal/Trajectory/ParticleTrajectory.hh" +#include "KinKal/Trajectory/SensorLine.hh" +#include "KinKal/Trajectory/PiecewiseClosestApproach.hh" +#include "KinKal/Trajectory/ClosestApproach.hh" +#include "KinKal/General/BFieldMap.hh" +#include +#include namespace KinKal { - template class SimpleWireHit : public WireHit { + template class SimpleWireHit : public ResidualHit { public: - using WIREHIT = WireHit; - using Dimension = typename WireHit::Dimension; - using PKTRAJ = ParticleTrajectory; - using PTCA = PiecewiseClosestApproach; + using HIT = Hit; + using PCA = PiecewiseClosestApproach; + using CA = ClosestApproach; + using KTRAJPTR = std::shared_ptr; + using PTRAJ = ParticleTrajectory; + enum Dimension { dresid=0, tresid=1}; // residual dimensions - SimpleWireHit(BFieldMap const& bfield, PTCA const& ptca, WireHitState const& whstate, double mindoca, - double driftspeed, double tvar, double rcell); - // override updating. I have to override both since they have the same name - void update(PKTRAJ const& pktraj) override; - void update(PKTRAJ const& pktraj, MetaIterConfig const& config) override; - // WireHit interface implementations - void distanceToTime(POL2 const& drift, DriftInfo& dinfo) const override; - double cellRadius() const { return rcell_; } - double nullVariance(Dimension dim,DriftInfo const& dinfo) const override; - double nullOffset(Dimension dim,DriftInfo const& dinfo) const override; + SimpleWireHit(BFieldMap const& bfield, PCA const& pca, WireHitState const& whstate, double mindoca, + double driftspeed, double tvar, double tot, double totvar, double rcell,int id); + unsigned nResid() const override { return 2; } // 2 residuals + // clone op for reinstantiation + SimpleWireHit(SimpleWireHit const& rhs): + ResidualHit(rhs), + bfield_(rhs.bfield()), + whstate_(rhs.hitState()), + wire_(rhs.wire()), + ca_( + rhs.closestApproach().particleTraj(), + wire_, + rhs.closestApproach().hint(), + rhs.closestApproach().precision() + ), + rresid_(rhs.residuals()), + mindoca_(rhs.minDOCA()), + dvel_(driftVelocity()), + tvar_(timeVariance()), + tot_(rhs.tot()), + totvar_(rhs.totVariance()), + rcell_(rhs.cellRadius()), + id_(rhs.id()){ + /**/ + }; + std::shared_ptr< Hit > clone(CloneContext& context) const override{ + auto rv = std::make_shared< SimpleWireHit >(*this); + auto ca = rv->closestApproach(); + auto trajectory = std::make_shared(ca.particleTraj()); + ca.setTrajectory(trajectory); + rv->setClosestApproach(ca); + return rv; + }; + double time() const override { return ca_.particleToca(); } + VEC3 dRdX(unsigned ires) const; + Residual const& refResidual(unsigned ires=dresid) const override; + void updateReference(PTRAJ const& ptraj) override; + KTRAJPTR const& refTrajPtr() const override { return ca_.particleTrajPtr(); } + void print(std::ostream& ost=std::cout,int detail=0) const override; + // Use dedicated updater + void updateState(MetaIterConfig const& config,bool first) override; // specific to SimpleWireHit: this has a constant drift speed + double cellRadius() const { return rcell_; } virtual ~SimpleWireHit(){} double driftVelocity() const { return dvel_; } double timeVariance() const { return tvar_; } double minDOCA() const { return mindoca_; } + int id() const { return id_; } + CA unbiasedClosestApproach() const; + auto const& closestApproach() const { return ca_; } + auto const& hitState() const { return whstate_; } + auto const& wire() const { return wire_; } + auto const& bfield() const { return bfield_; } + auto precision() const { return ca_.precision(); } + auto const& residuals() const { return rresid_; } + double tot() const { return tot_; } + double totVariance() const { return totvar_; } + private: + BFieldMap const& bfield_; // drift calculation requires the BField for ExB effects + WireHitState whstate_; // current state + SensorLine wire_; // local linear approximation to the wire of this hit, encoding all (local) position and time information. + // the start time is the measurement time, the direction is from + // the physical source of the signal (particle) to the measurement recording location (electronics), the direction magnitude + // is the effective signal propagation velocity along the wire, and the time range describes the active wire length + // (when multiplied by the propagation velocity). + CA ca_; // reference time and position of closest approach to the wire; this is generally biased by the hit + std::array rresid_; // residuals WRT most recent reference double mindoca_; // effective minimum DOCA used when assigning LR ambiguity, used to define null hit properties double dvel_; // constant drift speed double tvar_; // constant time variance + double tot_, totvar_; // TimeOverThreshold and variance double rcell_; // straw radius - // allow the updater access - friend class SimpleWireHitUpdater; + int id_; // id + void updateResiduals(); + + // modifiers to support cloning + void setClosestApproach(const CA& ca){ ca_ = ca; } }; - template double SimpleWireHit::nullVariance(Dimension dim,DriftInfo const& dinfo) const { - switch (dim) { - case WIREHIT::dresid: default: - return (mindoca_*mindoca_)/3.0; // doca is signed - case WIREHIT::tresid: - return (mindoca_*mindoca_)/(dinfo.vdrift_*dinfo.vdrift_*12.0); // TOCA is always larger than the crossing time - } - } + //trivial 'updater' that sets the wire hit state to null + class NullWireHitUpdater { + public: + WireHitState wireHitState() const { return WireHitState(WireHitState::null); } + }; - template double SimpleWireHit::nullOffset(Dimension dim,DriftInfo const& dinfo) const { - switch (dim) { - case WIREHIT::dresid: default: - return 0.0; // not sure if there's a better answer - case WIREHIT::tresid: - return -0.5*mindoca_/dinfo.vdrift_; + template SimpleWireHit::SimpleWireHit(BFieldMap const& bfield, PCA const& pca, WireHitState const& whstate, + double mindoca, double driftspeed, double tvar, double tot, double totvar, double rcell, int id) : + bfield_(bfield), + whstate_(whstate), wire_(pca.sensorTraj()), + ca_(pca.localTraj(),wire_,pca.precision(),pca.tpData(),pca.dDdP(),pca.dTdP()), // must be explicit to get the right sensor traj reference + mindoca_(mindoca), dvel_(driftspeed), tvar_(tvar), tot_(tot), totvar_(totvar), rcell_(rcell), id_(id) { } - } - // simple updater based on DOCA - class SimpleWireHitUpdater { - public: - SimpleWireHitUpdater(double mindoca,double maxdoca, double minprob ) : mindoca_(mindoca), maxdoca_(maxdoca), minprob_(minprob) {} - template void update(SimpleWireHit& swh) const; - private: - double mindoca_; // minimum DOCA value to sign LR ambiguity - double maxdoca_; // maximum DOCA to still use a hit - double minprob_; // minimum residual probability to keep using a hit - }; + template void SimpleWireHit::updateReference(PTRAJ const& ptraj) { + // if we already computed PCA in the previous iteration, use that to set the hint. This speeds convergence + // otherwise use the time at the center of the wire + CAHint tphint = ca_.usable() ? ca_.hint() : CAHint(wire_.timeAtMidpoint(),wire_.timeAtMidpoint()); + PCA pca(ptraj,wire_,tphint,precision()); + ca_ = pca.localClosestApproach(); + if(!ca_.usable())throw std::runtime_error("WireHit TPOCA failure"); + } - template void SimpleWireHitUpdater::update(SimpleWireHit& swh) const { - swh.mindoca_ = std::min(mindoca_,swh.cellRadius()); - WireHitState::State state; - if(swh.closestApproach().usable()){ - double doca = swh.closestApproach().doca(); - auto chisq = swh.chisq(); - if(fabs(doca) > maxdoca_ || chisq.probability() < minprob_ ) { - state = WireHitState::inactive; // disable the hit if it's an outlier - } else if(fabs(doca) > mindoca_ ) { - state = doca > 0.0 ? WireHitState::right : WireHitState::left; + template void SimpleWireHit::updateState(MetaIterConfig const& miconfig, bool first) { + if(first){ + // look for an updater; if found, use it to update the state + auto nwhu = miconfig.findUpdater(); + auto dwhu = miconfig.findUpdater(); + if(nwhu != 0 && dwhu != 0)throw std::invalid_argument(">1 SimpleWireHit updater specified"); + if(nwhu != 0){ + mindoca_ = cellRadius(); + whstate_ = nwhu->wireHitState(); + // set the residuals based on this state + } else if(dwhu != 0){ + // update minDoca (for null ambiguity error estimate) + mindoca_ = std::min(dwhu->minDOCA(),cellRadius()); + // compute the unbiased closest approach. This is brute-force + // a more clever solution is to linearly correct the residuals for the change in parameters + auto uca = this->unbiasedClosestApproach(); + whstate_ = uca.usable() ? dwhu->wireHitState(uca.doca()) : WireHitState(WireHitState::inactive); + } + } + rresid_[tresid] = rresid_[dresid] = Residual(); + if(whstate_.active()){ + rresid_[tresid] = Residual(ca_.deltaT() - tot_, totvar_,0.0,ca_.dTdP()); // always constrain to TOT; this stabilizes the fit + if(whstate_.useDrift()){ + // translate PCA to residual. Use ambiguity assignment to convert drift time to a drift radius + double dr = dvel_*whstate_.lrSign()*ca_.deltaT() -ca_.doca(); + DVEC dRdP = dvel_*whstate_.lrSign()*ca_.dTdP() -ca_.dDdP(); + rresid_[dresid] = Residual(dr,tvar_*dvel_*dvel_,0.0,dRdP); } else { - // too close to the wire: don't try to disambiguate LR sign - state = WireHitState::null; + // interpret DOCA against the wire directly as a residuals + double nulldvar = dvel_*dvel_*(ca_.deltaT()*ca_.deltaT()+0.8); + rresid_[dresid] = Residual(ca_.doca(),nulldvar,0.0,ca_.dDdP()); } - } else { - state = WireHitState::inactive; } - swh.setState(state); - }; + // now update the weight + this->updateWeight(miconfig); + } - template SimpleWireHit::SimpleWireHit(BFieldMap const& bfield, PTCA const& ptca, WireHitState const& whstate, - double mindoca, double driftspeed, double tvar, double rcell) : - WIREHIT(bfield,ptca,whstate), mindoca_(mindoca), dvel_(driftspeed), tvar_(tvar), rcell_(rcell) {} + template VEC3 SimpleWireHit::dRdX(unsigned ires) const { + if (whstate_.active()){ + if (ires == dresid){ + if (whstate_.useDrift()){ + return ca_.lSign()*ca_.delta().Vect().Unit(); + }else{ + return -1*ca_.lSign()*ca_.delta().Vect().Unit(); + } + } + } + return VEC3(0,0,0); + } - template void SimpleWireHit::distanceToTime(POL2 const& drift, DriftInfo& dinfo) const { - // simply translate distance to time using the fixed velocity - dinfo.tdrift_ = drift.R()/dvel_; - dinfo.vdrift_ = dvel_; - dinfo.tdriftvar_ = tvar_; + template Residual const& SimpleWireHit::refResidual(unsigned ires) const { + if(ires >tresid)throw std::invalid_argument("Invalid residual"); + return rresid_[ires]; } - template void SimpleWireHit::update(PKTRAJ const& pktraj) { - WIREHIT::update(pktraj); + template ClosestApproach SimpleWireHit::unbiasedClosestApproach() const { + // compute the unbiased closest approach; this is brute force, but works + auto const& ca = this->closestApproach(); + auto uparams = HIT::unbiasedParameters(); + KTRAJ utraj(uparams,ca.particleTraj()); + return CA(utraj,this->wire(),ca.hint(),ca.precision()); } - template void SimpleWireHit::update(PKTRAJ const& pktraj,MetaIterConfig const& miconfig) { - WIREHIT::update(pktraj,miconfig); - // look for an updater; if it's there, update the state - auto swhu = miconfig.findUpdater(); - if(swhu != 0){swhu->update(*this); - // update again - WIREHIT::update(pktraj,miconfig); + template void SimpleWireHit::print(std::ostream& ost, int detail) const { + ost << " WireHit state "; + switch(whstate_.state_) { + case WireHitState::inactive: + ost << "inactive"; + break; + case WireHitState::left: + ost << "left"; + break; + case WireHitState::right: + ost << "right"; + break; + case WireHitState::null: default: + ost << "null"; + break; + } + if(detail > 0){ + if(rresid_[tresid].active()) + ost << " Active Time Residual " << rresid_[tresid]; + if(rresid_[dresid].active()) + ost << " Active Distance Residual " << rresid_[dresid]; + ost << std::endl; + } + if(detail > 1) { + ost << "Approximate Propagation speed " << wire_.speed(100) << " TPOCA " << ca_.tpData() << std::endl; } } diff --git a/Examples/StrawMaterial.cc b/Examples/StrawMaterial.cc new file mode 100644 index 00000000..5be8c206 --- /dev/null +++ b/Examples/StrawMaterial.cc @@ -0,0 +1,58 @@ +#include "KinKal/Examples/StrawMaterial.hh" +#include +#include +#include + +namespace KinKal { + void StrawMaterial::pathLengths(ClosestApproachData const& cadata,StrawXingConfig const& caconfig, + double& wallpath, double& gaspath, double& wirepath) const { + wallpath = gaspath = wirepath = 0.0; + double adoca = fabs(cadata.doca()); + double sigdoca = sqrt(cadata.docaVar()); + if(adoca < caconfig.maxdoca_){ + if ((!caconfig.average_) && sigdoca < caconfig.minsigdoca_ && adoca < caconfig.maxddoca_) { // use exact calculation based on DOCA + double doca = std::min(adoca, grad_); // truncate + double ddoca = doca*doca; + gaspath = 2.0*sqrt(grad2_- ddoca); + wallpath = 2.0*thick_*srad_/sqrt(srad2_-ddoca); + } else { + // errors are large WRT the size of the straw, or DOCA is very far from the wire: just take the average over all impact parameters + gaspath = M_PI_2*srad_; + wallpath = M_PI*thick_; + } + if(isnan(wallpath) || isnan(gaspath))throw std::runtime_error("Invalid StrawMaterial pathlength"); + // Model the wire as a diffuse gas, density constrained by DOCA TODO + // correct for the angle WRT the axis + double afac = angleFactor(cadata.dirDot()); + wallpath *= afac; + gaspath *= afac; + } + } + + double StrawMaterial::transitLength(ClosestApproachData const& cadata) const { + double doca = std::min(fabs(cadata.doca()),grad_); + double tlen = 2.0*sqrt(srad2_-doca*doca); + // correct for the angle WRT the axis + double afac = angleFactor(cadata.dirDot()); + tlen *= afac; + return tlen; + } + + double StrawMaterial::angleFactor(double dirdot) const { + // protect against nearly parallel angles + static const double maxfac(10.0); // beyond this the straw irregularities dominate and the estimate is unreliable + static const double minsin2(1.0/(maxfac*maxfac)); // minimum sin^2(theta) this implies + double sin2 = std::max(1.0 -dirdot*dirdot,minsin2); + return 1.0/sqrt(sin2); + } + + void StrawMaterial::findXings(ClosestApproachData const& cadata,StrawXingConfig const& caconfig, std::vector& mxings) const { + mxings.clear(); + double wallpath, gaspath, wirepath; + pathLengths(cadata,caconfig,wallpath, gaspath, wirepath); + if(wallpath > 0.0) mxings.push_back(MaterialXing(*wallmat_,wallpath)); + if(gaspath > 0.0) mxings.push_back(MaterialXing(*gasmat_,gaspath)); + if(wirepath > 0.0) mxings.push_back(MaterialXing(*wiremat_,wirepath)); + } + +} diff --git a/Detector/StrawMaterial.hh b/Examples/StrawMaterial.hh similarity index 53% rename from Detector/StrawMaterial.hh rename to Examples/StrawMaterial.hh index a1fcec8b..311d0296 100644 --- a/Detector/StrawMaterial.hh +++ b/Examples/StrawMaterial.hh @@ -6,7 +6,7 @@ // #include "KinKal/MatEnv/DetMaterial.hh" #include "KinKal/Detector/MaterialXing.hh" -#include "KinKal/Detector/StrawXingConfig.hh" +#include "KinKal/Examples/StrawXingConfig.hh" #include "KinKal/MatEnv/MatDBInfo.hh" #include "KinKal/Trajectory/ClosestApproachData.hh" @@ -14,19 +14,23 @@ namespace KinKal { class StrawMaterial { public: // explicit constructor from geometry and materials - StrawMaterial(double srad, double thick, double wrad, - const MatEnv::DetMaterial *wallmat, const MatEnv::DetMaterial *gasmat, const MatEnv::DetMaterial *wiremat) : - srad_(srad), thick_(thick), wrad_(wrad), wallmat_(wallmat), gasmat_(gasmat), wiremat_(wiremat) { + StrawMaterial(double srad, double thick, double sradsig, double wrad, + const std::shared_ptr wallmat, const std::shared_ptr gasmat, const std::shared_ptr wiremat) : + srad_(srad), thick_(thick), sradsig_(sradsig), wrad_(wrad), wallmat_(wallmat), gasmat_(gasmat), wiremat_(wiremat) { srad2_ = srad_*srad_; + grad_ = srad_-sradsig_; // count the gas volume inside 1 sigma. This smooths discontinuities at the edge + grad2_ = grad_*grad_; } // construct using default materials - StrawMaterial(MatEnv::MatDBInfo const& matdbinfo,double srad, double thick, double wrad, + StrawMaterial(MatEnv::MatDBInfo const& matdbinfo,double srad, double thick, double sradsig, double wrad, const char* wallmat="straw-wall", const char* gasmat="straw-gas", const char* wiremat="straw-wire") : - StrawMaterial(srad,thick,wrad, matdbinfo.findDetMaterial(wallmat), + StrawMaterial(srad,thick,sradsig, wrad,matdbinfo.findDetMaterial(wallmat), matdbinfo.findDetMaterial(gasmat), matdbinfo.findDetMaterial(wiremat)) {} // pathlength through straw components, given closest approach void pathLengths(ClosestApproachData const& cadata,StrawXingConfig const& caconfig, double& wallpath, double& gaspath, double& wirepath) const; + // transit length given closest approach + double transitLength(ClosestApproachData const& cadata) const; // find the material crossings given doca and error on doca. Should allow for straw and wire to have different axes TODO void findXings(ClosestApproachData const& cadata,StrawXingConfig const& caconfig, std::vector& mxings) const; double strawRadius() const { return srad_; } @@ -36,13 +40,19 @@ namespace KinKal { MatEnv::DetMaterial const& gasMaterial() const { return *gasmat_; } MatEnv::DetMaterial const& wireMaterial() const { return *wiremat_; } private: - double srad_; // outer transverse radius of the straw - double srad2_; // outer transverse radius of the straw squared + double srad_; // average outer transverse radius of the straw + double srad2_; // average outer transverse radius of the straw squared double thick_; // straw wall thickness + double sradsig_; // sqrt(variance) of straw radius + double grad_; // effective gas volume radius + double grad2_; // effective gas volume radius squared double wrad_; // transverse radius of the wire - const MatEnv::DetMaterial* wallmat_; // material of the straw wall - const MatEnv::DetMaterial* gasmat_; // material of the straw gas - const MatEnv::DetMaterial* wiremat_; // material of the wire + const std::shared_ptr wallmat_; // material of the straw wall + const std::shared_ptr gasmat_; // material of the straw gas + const std::shared_ptr wiremat_; // material of the wire + // utility to calculate material factor given the cosine of the angle of the particle WRT the straw + double angleFactor(double dirdot) const; + // maximum DOCA given straw irregularities }; } #endif diff --git a/Examples/StrawXing.hh b/Examples/StrawXing.hh new file mode 100644 index 00000000..5c5d4b16 --- /dev/null +++ b/Examples/StrawXing.hh @@ -0,0 +1,140 @@ +#ifndef KinKal_StrawXing_hh +#define KinKal_StrawXing_hh +// +// Describe the material effects of a kinematic trajectory crossing a straw +// Used in the kinematic Kalman fit +// +#include "KinKal/Detector/ElementXing.hh" +#include "KinKal/Examples/StrawMaterial.hh" +#include "KinKal/Examples/StrawXingConfig.hh" +#include "KinKal/Trajectory/SensorLine.hh" +#include "KinKal/Trajectory/PiecewiseClosestApproach.hh" + +namespace KinKal { + template class StrawXing : public ElementXing { + public: + using PTRAJ = ParticleTrajectory; + using KTRAJPTR = std::shared_ptr; + using EXING = ElementXing; + using PCA = PiecewiseClosestApproach; + using CA = ClosestApproach; + // construct from PCA and material + StrawXing(PCA const& pca, StrawMaterial const& smat); + virtual ~StrawXing() {} + // copy constructor + StrawXing(StrawXing const& rhs): + ElementXing(rhs), + axis_(rhs.axis_), + smat_(rhs.smat_), + tpca_( + rhs.closestApproach().particleTraj(), + axis_, + rhs.closestApproach().hint(), + rhs.closestApproach().precision() + ), + toff_(rhs.toff_), + sxconfig_(rhs.sxconfig_), + varscale_(rhs.varscale_), + mxings_(rhs.mxings_), + fparams_(rhs.fparams_){ + /**/ + } + // clone op for reinstantiation + std::shared_ptr< ElementXing > clone(CloneContext& context) const override{ + auto rv = std::make_shared< StrawXing >(*this); + auto ca = rv->closestApproach(); + auto trajectory = std::make_shared(ca.particleTraj()); + ca.setTrajectory(trajectory); + rv->setClosestApproach(ca); + return rv; + } + // ElementXing interface + void updateReference(PTRAJ const& ptraj) override; + void updateState(MetaIterConfig const& config,bool first) override; + Parameters params() const override; + double time() const override { return tpca_.particleToca() + toff_; } // offset time WRT TOCA to avoid exact overlapp with the wire hit + double transitTime() const override; // time to cross this element + KTRAJ const& referenceTrajectory() const override { return tpca_.particleTraj(); } + std::vectorconst& matXings() const override { return mxings_; } + void print(std::ostream& ost=std::cout,int detail=0) const override; + bool active() const override { return mxings_.size() > 0; } + // accessors + auto const& closestApproach() const { return tpca_; } + auto const& strawMaterial() const { return smat_; } + auto const& config() const { return sxconfig_; } + auto precision() const { return tpca_.precision(); } + + private: + SensorLine axis_; // straw axis, expressed as a timeline + StrawMaterial const& smat_; + CA tpca_; // result of most recent TPOCA + double toff_; // small time offset + StrawXingConfig sxconfig_; + double varscale_; // variance scale + std::vector mxings_; + Parameters fparams_; // parameter change for forwards time + + // modifiers to support cloning + void setClosestApproach(const CA& ca){ tpca_ = ca; } + }; + + template StrawXing::StrawXing(PCA const& pca, StrawMaterial const& smat) : + axis_(pca.sensorTraj()), + smat_(smat), + tpca_(pca.localTraj(),axis_,pca.precision(),pca.tpData(),pca.dDdP(),pca.dTdP()), + toff_(smat.wireRadius()/pca.particleTraj().speed(pca.particleToca())), // locate the effect to 1 side of the wire to avoid overlap with hits + varscale_(1.0) + {} + + template void StrawXing::updateReference(PTRAJ const& ptraj) { + CAHint tphint = tpca_.usable() ? tpca_.hint() : CAHint(axis_.timeAtMidpoint(),axis_.timeAtMidpoint()); + PCA pca(ptraj,axis_,tphint,precision()); + tpca_ = pca.localClosestApproach(); + if(!tpca_.usable())throw std::runtime_error("StrawXing TPOCA failure"); + } + + template void StrawXing::updateState(MetaIterConfig const& miconfig,bool first) { + if(first) { + // search for an update to the xing configuration among this meta-iteration payload + auto sxconfig = miconfig.findUpdater(); + if(sxconfig != 0){ + sxconfig_ = *sxconfig; + } + if(sxconfig_.scalevar_) + varscale_ = miconfig.varianceScale(); + else + varscale_ = 1.0; + } + smat_.findXings(tpca_.tpData(),sxconfig_,mxings_); + if(mxings_.size() > 0){ + fparams_ = this->parameterChange(varscale_); + } else { + // reset + fparams_ = Parameters(); + } + } + + template Parameters StrawXing::params() const { + return fparams_; + } + + template double StrawXing::transitTime() const { + return smat_.transitLength(tpca_.tpData())/tpca_.particleTraj().speed(tpca_.particleToca()); + } + + template void StrawXing::print(std::ostream& ost,int detail) const { + ost <<"Straw Xing time " << this->time(); + if(detail > 0){ + for(auto const& mxing : mxings_){ + ost << " " << mxing.dmat_.name() << " pathLen " << mxing.plen_; + } + } + if(detail > 1){ + ost << " Axis "; + axis_.print(ost,0); + } + ost << std::endl; + } + +} +#endif diff --git a/Examples/StrawXingConfig.hh b/Examples/StrawXingConfig.hh new file mode 100644 index 00000000..dac3d7c1 --- /dev/null +++ b/Examples/StrawXingConfig.hh @@ -0,0 +1,18 @@ +#ifndef KinKal_StrawXingConfig_hh +#define KinKal_StrawXingConfig_hh +namespace KinKal { + // simple struct to hold crossing calculation configuration parameters + struct StrawXingConfig { + bool average_; // use the average effect no matter what DOCA value is + bool scalevar_; // scale variance by temperature + double minsigdoca_; // minimum doca error to use non-averaged value on + double maxdoca_; // maximum DOCA to consider this straw hit: otherwise set no path + double maxddoca_; // maximum DOCA to use 'exact' calculation, otherwise average + // default constructor is functional but will always use the average correction + StrawXingConfig() : average_(true), scalevar_(true), minsigdoca_(-1.0), maxdoca_(0.0), maxddoca_(0.0) {} + StrawXingConfig(double minsigdoca, double maxdoca, double maxddoca, bool scalevar) : average_(false), scalevar_(scalevar), minsigdoca_(minsigdoca), + maxdoca_(maxdoca), maxddoca_(maxddoca){} + }; +} +#endif + diff --git a/Detector/WireHitStructs.hh b/Examples/WireHitStructs.hh similarity index 85% rename from Detector/WireHitStructs.hh rename to Examples/WireHitStructs.hh index 889e2d38..024e4e05 100644 --- a/Detector/WireHitStructs.hh +++ b/Examples/WireHitStructs.hh @@ -16,6 +16,8 @@ namespace KinKal { State state_; // left-right ambiguity bool useDrift() const { return state_ == left || state_ == right; } bool active() const { return state_ != inactive; } + bool operator == (WireHitState::State state) const { return state_ == state; } + bool operator != (WireHitState::State state) const { return state_ != state; } double lrSign() const { switch (state_) { case left: diff --git a/Fit/BField.hh b/Fit/BField.hh deleted file mode 100644 index df1f05b1..00000000 --- a/Fit/BField.hh +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef KinKal_BField_hh -#define KinKal_BField_hh -// -// Effect to correct the fit parameters for the change in BField along a small piece of the trajectory. -// This effect adds no information content or noise (presently), just transports the parameters -// -#include "KinKal/Fit/Effect.hh" -#include "KinKal/General/TimeDir.hh" -#include "KinKal/General/BFieldMap.hh" -#include "KinKal/Fit/Config.hh" -#include -#include -#include -#include - -namespace KinKal { - template class BField : public Effect { - public: - using KKEFF = Effect; - using PKTRAJ = ParticleTrajectory; - - double time() const override { return drange_.mid(); } // apply the correction at the middle of the range - bool active() const override { return bfcorr_; } - void update(PKTRAJ const& ref) override; - void update(PKTRAJ const& ref, MetaIterConfig const& miconfig) override; - void update(Config const& config) override { bfcorr_ = config.bfcorr_; } - void print(std::ostream& ost=std::cout,int detail=0) const override; - void process(FitState& kkdata,TimeDir tdir) override; - void append(PKTRAJ& fit) override; - Parameters const& effect() const { return dbforw_; } - virtual ~BField(){} - // disallow copy and equivalence - BField(BField const& ) = delete; - BField& operator =(BField const& ) = delete; - // create from the domain range, the effect, and the - BField(Config const& config, BFieldMap const& bfield,TimeRange const& drange) : - bfield_(bfield), drange_(drange), bfcorr_(config.bfcorr_) {} - TimeRange const& range() const { return drange_; } - - private: - BFieldMap const& bfield_; // bfield - TimeRange drange_; // extent of this effect. The middle is at the transition point between 2 bfield domains (domain transition) - Parameters dbforw_; // aggregate effect in parameter space of BFieldMap change in the forwards direction - bool bfcorr_; // apply correction or not - }; - - template void BField::process(FitState& kkdata,TimeDir tdir) { - if(bfcorr_){ - // forwards; just append the effect's parameter change - if(tdir == TimeDir::forwards) { - kkdata.append(dbforw_); - } else { - // SUBTRACT the effect going backwards: covariance change is sign-independent - Parameters reverse(dbforw_); - reverse.parameters() *= -1.0; - kkdata.append(reverse); - } - } - KKEFF::setState(tdir,KKEFF::processed); - } - - template void BField::update(PKTRAJ const& ref) { - double etime = time(); - // compute parameter change due to changing BNom across this domain - if(bfcorr_){ - auto const& begtraj = ref.nearestPiece(drange_.begin()); - auto const& endtraj = ref.nearestPiece(drange_.end()); - dbforw_.parameters() = begtraj.dPardB(etime,endtraj.bnom()); - } - // eventually include field map uncertainties in dbforw_ covariance TODO! - KKEFF::updateState(); - } - - template void BField::update(PKTRAJ const& ref, MetaIterConfig const& miconfig) { - update(ref); - } - - template void BField::append(PKTRAJ& fit) { - if(bfcorr_){ - double etime = time(); - // make sure the piece is appendable - if(fit.back().range().begin() > etime) throw std::invalid_argument("BField: Can't append piece"); - TimeRange newrange(etime,std::max(fit.range().end(),drange_.end())); - // copy the back piece of fit and set its range - KTRAJ newpiece(fit.back()); - newpiece.range() = newrange; - // update the parameters according to the change in bnom across this domain - VEC3 newbnom = bfield_.fieldVect(fit.position3(drange_.end())); - newpiece.setBNom(etime,newbnom); - fit.append(newpiece); - } - } - - template void BField::print(std::ostream& ost,int detail) const { - ost << "BField " << static_castconst&>(*this); - ost << " effect " << dbforw_.parameters() << " domain range " << drange_ << std::endl; - } - - template std::ostream& operator <<(std::ostream& ost, BField const& kkmat) { - kkmat.print(ost,0); - return ost; - } - -} -#endif diff --git a/Fit/CMakeLists.txt b/Fit/CMakeLists.txt index ca69611b..60299016 100644 --- a/Fit/CMakeLists.txt +++ b/Fit/CMakeLists.txt @@ -12,7 +12,9 @@ add_library(Fit SHARED ) # set top-level directory as include root -target_include_directories(Fit PRIVATE ${PROJECT_SOURCE_DIR}/..) +target_include_directories(Fit PUBLIC + $ + $) # link this library with ROOT libraries target_link_libraries(Fit Detector Trajectory General ${ROOT_LIBRARIES}) diff --git a/Fit/Config.cc b/Fit/Config.cc index 808052a4..4b3370ca 100644 --- a/Fit/Config.cc +++ b/Fit/Config.cc @@ -5,8 +5,8 @@ namespace KinKal { << " dweight " << kkconfig.dwt_ << " converge dchisq/dof " << kkconfig.convdchisq_ << " diverge dchisq/dof " << kkconfig.divdchisq_ - << " dpar chisq " << kkconfig.pdchi2_ - << " time buffer " << kkconfig.tbuff_ + << " diverge dpar chisq " << kkconfig.pdchisq_ + << " diverge traj gap (mm) " << kkconfig.divgap_ << " fractional momentum tolerance " << kkconfig.tol_ << " min NDOF " << kkconfig.minndof_ << " BField correction " << kkconfig.bfcorr_ diff --git a/Fit/Config.hh b/Fit/Config.hh index 2b261b39..a73ff62a 100644 --- a/Fit/Config.hh +++ b/Fit/Config.hh @@ -17,23 +17,22 @@ namespace KinKal { struct Config { enum printLevel{none=0,minimal, basic, complete, detailed, extreme}; - using Schedule = std::vector; + using Schedule = std::vector; + explicit Config() {} explicit Config(Schedule const& schedule) : Config() { schedule_ = schedule; } - Config() : maxniter_(10), dwt_(1.0e6), convdchisq_(0.01), divdchisq_(10.0), pdchi2_(1.0e6), tbuff_(0.0), tol_(1.0e-4), minndof_(5), bfcorr_(true), plevel_(none) {} - Schedule& schedule() { return schedule_; } Schedule const& schedule() const { return schedule_; } - // algebraic iteration parameters - int maxniter_; // maximum number of algebraic iterations for this config - double dwt_; // dweighting of initial seed covariance - double convdchisq_; // maximum change in chisquared/dof for convergence - double divdchisq_; // minimum change in chisquared/dof for divergence - double pdchi2_; // maximum allowed parameter change (units of chisqred) WRT previous reference - double tbuff_; // time buffer for final fit (ns) - double tol_; // tolerance on fractional momentum accuracy due to BField domain steps - unsigned minndof_; // minimum number of DOFs to continue fit - bool bfcorr_; // whether to make BFieldMap corrections in the fit - printLevel plevel_; // print level + unsigned maxniter_ = 10; // maximum number of algebraic iterations for this config + double dwt_ = 1.0e6; // dweighting of initial seed covariance + double convdchisq_ = 1.0e-2; // maximum change in chisquared/dof for convergence + double divdchisq_ = 1.0e1; // minimum change in chisquared/dof for divergence + double pdchisq_ = 1.0e6; // maximum allowed parameter change (units of chisqred) WRT previous reference + double divgap_ = 1.0e2; // maximum average gap of trajectory before calling it diverged (mm) + double tol_ = 1.0e-4; // tolerance on fractional momentum accuracy due to BField domain steps + unsigned minndof_ = 5; // minimum number of DOFs to continue fit + bool bfcorr_ = true; // whether to make BFieldMap corrections in the fit + bool ends_ = true; // process the passive effects at each end of the track after schedule completion + printLevel plevel_ = none; // print level // schedule of meta-iterations. These will be executed sequentially until completion or failure Schedule schedule_; }; diff --git a/Fit/Domain.hh b/Fit/Domain.hh new file mode 100644 index 00000000..9fde2100 --- /dev/null +++ b/Fit/Domain.hh @@ -0,0 +1,44 @@ +#ifndef KinKal_Domain_hh +#define KinKal_Domain_hh +// +// domain used to compute magnetic field corrections. Magnetic bending not described by the intrinsic parameterization is assumed +// to be negligible over the domain +// +#include +#include "KinKal/General/CloneContext.hh" +#include "KinKal/General/TimeRange.hh" +#include "KinKal/General/Vectors.hh" +namespace KinKal { + class Domain : public TimeRange { + public: + Domain(double lowtime, double range, VEC3 const& bnom) : range_(lowtime,lowtime+range), bnom_(bnom) {} + Domain(TimeRange const& range, VEC3 const& bnom) : range_(range), bnom_(bnom) {} + // clone op for reinstantiation + Domain(Domain const&); + std::shared_ptr< Domain > clone(CloneContext&) const; + bool operator < (Domain const& other) const {return begin() < other.begin(); } + auto const& range() const { return range_; } + // forward range functions + double begin() const { return range_.begin();} + double end() const { return range_.end();} + double mid() const { return range_.mid();} + auto const& bnom() const { return bnom_; } + void updateBNom( VEC3 const& bnom) { bnom_ = bnom; }; // used in DomainWall updating + private: + TimeRange range_; // range of this domain + VEC3 bnom_; // nominal BField for this domain + }; + + // clone op for reinstantiation + Domain::Domain(Domain const& rhs): + range_(rhs.range()), + bnom_(rhs.bnom()){ + } + + std::shared_ptr Domain::clone(CloneContext& context) const{ + auto rv = std::make_shared(*this); + return rv; + } +} +#endif + diff --git a/Fit/DomainWall.hh b/Fit/DomainWall.hh new file mode 100644 index 00000000..25ad797f --- /dev/null +++ b/Fit/DomainWall.hh @@ -0,0 +1,170 @@ +#ifndef KinKal_DomainWall_hh +#define KinKal_DomainWall_hh +// +// Effect describing the change in fit parameters for the change in BField crossing between 2 domains +// This effect adds no information content or noise (presently), just transports the parameters +// +#include "KinKal/Fit/Effect.hh" +#include "KinKal/Fit/Domain.hh" +#include "KinKal/Fit/Config.hh" +#include "KinKal/Fit/FitState.hh" +#include "KinKal/General/TimeDir.hh" +#include "KinKal/General/BFieldMap.hh" +#include +#include +#include +#include + +namespace KinKal { + template class DomainWall : public Effect { + public: + using KKEFF = Effect; + using PTRAJ = ParticleTrajectory; + using DOMAINPTR = std::shared_ptr; + double time() const override { return prev_->end(); } + bool active() const override { return true; } // always active + void process(FitState& kkdata,TimeDir tdir) override; + void updateState(MetaIterConfig const& miconfig,bool first) override; + void updateConfig(Config const& config) override {} + void updateReference(PTRAJ const& ptraj) override; + void print(std::ostream& ost=std::cout,int detail=0) const override; + void append(PTRAJ& fit,TimeDir tdir) override; + void extrapolate(PTRAJ& fit,TimeDir tdir) override; + Chisq chisq(Parameters const& pdata) const override { return Chisq();} // no information added + auto const& parameterChange() const { return dpfwd_; } + virtual ~DomainWall(){} + // disallow copy and equivalence +// DomainWall(DomainWall const& ) = delete; + DomainWall& operator =(DomainWall const& ) = delete; + // clone op for reinstantiation + DomainWall(DomainWall const&); + std::unique_ptr< Effect > clone(CloneContext&) const override; + // specific DomainWall interface + DomainWall(DOMAINPTR const& prevdomain,DOMAINPTR const& nextdomain, PTRAJ const& ptraj); + // previous and next domains + auto const& prevDomain() const { return *prev_; } + auto const& nextDomain() const { return *next_; } + // other accessors + auto const& prevPtr() const { return prev_; } + auto const& nextPtr() const { return next_; } + auto const& fwdChange() const { return dpfwd_; } + auto const& prevWeight() const { return prevwt_; } + auto const& nextWeight() const { return nextwt_; } + auto const& fwdCovarianceRotation() const { return dpdpdb_; } + + private: + DOMAINPTR prev_, next_; // pointers to previous and next domains + DVEC dpfwd_; // parameter change across this domain wall in the forwards time direction + Weights prevwt_, nextwt_; // cache of weights + PSMAT dpdpdb_; // forward rotation of covariance matrix going in the forwards direction + + // modifiers to support cloning + void setPrevPtr(DOMAINPTR const& ptr){ prev_ = ptr; } + void setNextPtr(DOMAINPTR const& ptr){ next_ = ptr; } + }; + + template DomainWall::DomainWall( + DOMAINPTR const& prevdomain, DOMAINPTR const& nextdomain, PTRAJ const& ptraj) : + prev_(prevdomain), next_(nextdomain) { + updateReference(ptraj); + } + + template void DomainWall::process(FitState& fstate,TimeDir tdir) { + if(tdir == TimeDir::forwards) { + prevwt_ += fstate.wData(); + fstate.append(dpfwd_,tdir); + // fstate.pData().covariance() = ROOT::Math::Similarity(dpdpdb_,fstate.pData().covariance()); Not tested TODO + nextwt_ += fstate.wData(); + } else { + nextwt_ += fstate.wData(); + fstate.append(dpfwd_,tdir); + // fstate.pData().covariance() = ROOT::Math::SimilarityT(dpdpdb_,fstate.pData().covariance()); + prevwt_ += fstate.wData(); + } + } + + template void DomainWall::updateState(MetaIterConfig const& miconfig,bool first) { + // reset the cached weights + prevwt_ = nextwt_ = Weights(); + } + + template void DomainWall::updateReference(PTRAJ const& ptraj) { + // update change in parameters for passage through this DW + auto const& refpiece = ptraj.nearestPiece(time()-1e-5); // disambiguate derivativates + auto db = next_->bnom() - prev_->bnom(); + dpfwd_ = refpiece.dPardB(this->time(),db); + dpdpdb_ = refpiece.dPardPardB(this->time(),db); + } + + template void DomainWall::append(PTRAJ& ptraj,TimeDir tdir) { + // make sure the piece is appendable + if((tdir == TimeDir::forwards && ptraj.back().range().begin() > time()) || + (tdir == TimeDir::backwards && ptraj.front().range().end() < time()) ) + throw std::invalid_argument("DomainWall: Can't append piece"); + auto const& oldpiece = (tdir == TimeDir::forwards) ? ptraj.back() : ptraj.front(); + KTRAJ newpiece(oldpiece); + if( tdir == TimeDir::forwards){ + newpiece.range() = TimeRange(next_->begin(),std::max(ptraj.range().end(),next_->end())); + newpiece.params() = Parameters(nextwt_); + newpiece.resetBNom(next_->bnom()); + ptraj.append(newpiece); + } else { + newpiece.range() = TimeRange(std::min(ptraj.range().begin(),prev_->begin()),prev_->end()); + newpiece.params() = Parameters(prevwt_); + newpiece.resetBNom(prev_->bnom()); + ptraj.prepend(newpiece); + } + } + + template void DomainWall::extrapolate(PTRAJ& ptraj,TimeDir tdir) { + // make sure the piece is appendable + if((tdir == TimeDir::forwards && ptraj.back().range().begin() > time()) || + (tdir == TimeDir::backwards && ptraj.front().range().end() < time()) ) + throw std::invalid_argument("DomainWall: Can't append piece"); + // sample the particle state at this domain wall + auto pstate = ptraj.stateEstimate(time()); + if( tdir == TimeDir::forwards){ + // re-intepret that as a trajectory, using the next domain's magnetic field. This gives exact continuity in position and momentum + KTRAJ newpiece(pstate, nextDomain().bnom(),nextDomain().range()); + ptraj.append(newpiece); + } else { + // same as above, in the opposite direction + KTRAJ newpiece(pstate, prevDomain().bnom(),prevDomain().range()); + ptraj.prepend(newpiece); + } + } + + template void DomainWall::print(std::ostream& ost,int detail) const { + ost << "DomainWall " << static_castconst&>(*this); + ost << " previous domain " << *prev_ << " next domain " << *next_; + ost << " effect " << dpfwd_ << std::endl; + } + + template std::ostream& operator <<(std::ostream& ost, DomainWall const& kkmat) { + kkmat.print(ost,0); + return ost; + } + + // clone op for reinstantiation + template + DomainWall::DomainWall(DomainWall const& rhs): + dpfwd_(rhs.fwdChange()), + prevwt_(rhs.prevWeight()), + nextwt_(rhs.nextWeight()), + dpdpdb_(rhs.fwdCovarianceRotation()){ + /**/ + } + + template + std::unique_ptr< Effect > DomainWall::clone(CloneContext& context) const{ + auto casted = std::make_unique< DomainWall >(*this); + DOMAINPTR prev = context.get(prev_); + casted->setPrevPtr(prev); + DOMAINPTR next = context.get(next_); + casted->setNextPtr(next); + //auto rv = std::make_unique< Effect >(casted); + auto rv = std::move(casted); + return rv; + } +} +#endif diff --git a/Fit/Effect.hh b/Fit/Effect.hh index fba21d86..e234ed4a 100644 --- a/Fit/Effect.hh +++ b/Fit/Effect.hh @@ -7,74 +7,61 @@ // #include "KinKal/Trajectory/ParticleTrajectory.hh" #include "KinKal/General/Chisq.hh" +#include "KinKal/General/CloneContext.hh" #include "KinKal/Fit/FitState.hh" #include "KinKal/Fit/Config.hh" #include "KinKal/General/TimeRange.hh" #include #include #include +#include +#include namespace KinKal { template class Effect { public: - enum State{unprocessed=-1,processed,updated,failed}; - static std::string const& stateName(State state); // type of the data payload used for processing the fit - using PKTRAJ = ParticleTrajectory; - // properties common to all effects + using PTRAJ = ParticleTrajectory; + Effect() {} + virtual ~Effect(){} + // clone op for reinstantiation + virtual std::unique_ptr< Effect > clone(CloneContext&) const; + // Effect interface virtual double time() const = 0; // time of this effect virtual bool active() const = 0; // whether this effect is/was used in the fit // Add this effect to the ongoing fit in the given direction. virtual void process(FitState& kkdata,TimeDir tdir) = 0; - // update this effect for a new reference trajectory within the existing algebraic iteration sequence - virtual void update(PKTRAJ const& ref) = 0; - // update this effect to start a new algebraic iteration squence using the new reference trajectory and configuration - virtual void update(PKTRAJ const& ref, MetaIterConfig const& miconfig) = 0; + // update this effect for a new algebraic iteration, perhaps with a new config + virtual void updateState(MetaIterConfig const& miconfig,bool first) = 0; // update this effect for a new configuration - virtual void update(Config const& config) =0; + virtual void updateConfig(Config const& config) =0; + // add this effect to a trajectory in the given direction. This uses 1st order approximations + virtual void append(PTRAJ& fit,TimeDir tdir) =0; + // extrapolate a trajectory in the given direction through this effect, using dead-reckoning calculation (not a fit!) + virtual void extrapolate(PTRAJ& fit,TimeDir tdir) =0; + // update the reference trajectory for this effect + virtual void updateReference(PTRAJ const& ptraj) =0; + // chisquared WRT a given local parameter set, assumed uncorrelatedd This is used for convergence testing + virtual Chisq chisq(Parameters const& pdata) const = 0; // diagnostic printout virtual void print(std::ostream& ost=std::cout,int detail=0) const =0; - // the following only has a non-trivial implementation for effects which (potentially) add information content to the fit - // chisquared (quality) associated with the most recent processing and current reference. This is used to determine - // fit convergence - virtual Chisq chisq() const { return Chisq();} - // chisquared WRT a given local parameter set. This is a purely diagnostic function - virtual Chisq chisq(Parameters const& pdata) const { return Chisq();} // chisq contribution WRT parameters - // The following only has a non-trivial implemetation for effects which (potentially) alter the physical particle trajectory - virtual void append(PKTRAJ& fit) {}; // disallow copy and equivalence Effect(Effect const& ) = delete; Effect& operator =(Effect const& ) = delete; - State state(TimeDir tdir) const { return state_[static_cast::type>(tdir)]; } - void setState(TimeDir tdir, State state) { state_[static_cast::type>(tdir)] = state; } - bool wasProcessed(TimeDir tdir) const { return state(tdir) == processed; } - void updateState() { state_[0] = state_[1] = updated; } - Effect() : state_{{unprocessed,unprocessed}} {} - virtual ~Effect(){} - private: - std::array state_; // state of processing in each direction }; - template std::ostream& operator <<(std::ostream& ost, Effect const& eff) { - ost << (eff.active() ? "Active " : "Inactive ") << "time " << eff.time() << " state " << - TimeDir::forwards << " " << eff.stateName(eff.state(TimeDir::forwards)) << " : " << - TimeDir::backwards << " " << eff.stateName(eff.state(TimeDir::backwards)); - return ost; + // cloning requires domain knowledge of pointer members of the cloned object, + // which must be reassigned explicitly; the default action is thus to throw + // an error if a clone routine has not been explicitly provided. + template std::unique_ptr< Effect > Effect::clone(CloneContext& context) const{ + std::string msg = "Attempt to clone KinKal::Effect subclass with no clone() implementation"; + throw std::runtime_error(msg); } - template std::string const& Effect::stateName(Effect::State state) { - const static std::vector stateNames_ = { "Unprocessed", "Processed", "Updated", "Failed" }; - switch (state) { - case unprocessed: default: - return stateNames_[0]; - case processed: - return stateNames_[1]; - case updated: - return stateNames_[2]; - case failed: - return stateNames_[3]; - } + template std::ostream& operator <<(std::ostream& ost, Effect const& eff) { + ost << (eff.active() ? "Active " : "Inactive ") << "time " << eff.time(); + return ost; } } diff --git a/Fit/FitState.hh b/Fit/FitState.hh index 3be875bf..747ad10a 100644 --- a/Fit/FitState.hh +++ b/Fit/FitState.hh @@ -7,6 +7,7 @@ // #include "KinKal/General/Weights.hh" #include "KinKal/General/Parameters.hh" +#include "KinKal/General/TimeDir.hh" namespace KinKal { class FitState { public: @@ -16,19 +17,33 @@ namespace KinKal { // accessors bool hasParameters() const { return hasParameters_; } bool hasWeights() const { return hasWeights_; } - // add to either parameters or weights - void append(Parameters const& pdata) { - pData() += pdata; + // add to either parameters or weights. Parameters can have a direction. Covariance is always additivie + void append(Parameters const& pdata,TimeDir tdir) { + if(tdir==TimeDir::forwards) + pData().parameters() += pdata.parameters(); + else + pData().parameters() -= pdata.parameters(); + pData().covariance() += pdata.covariance(); // this invalidates the weight information hasParameters_ = true; hasWeights_ = false; } +// append parameter vector, leaving covariance as-is + void append(DVEC const& pvec,TimeDir tdir=TimeDir::forwards) { + if(tdir==TimeDir::forwards) + pData().parameters() += pvec; + else + pData().parameters() -= pvec; + hasParameters_ = true; + hasWeights_ = false; + } void append(Weights const& wdata) { wData() += wdata; // this invalidates the parameter information hasWeights_ = true; hasParameters_ = false; } + Parameters& pData() { if(!hasParameters_ && hasWeights_ ){ // invert the weight diff --git a/Fit/Material.hh b/Fit/Material.hh index 16dfdb2b..a4aa205b 100644 --- a/Fit/Material.hh +++ b/Fit/Material.hh @@ -16,133 +16,133 @@ namespace KinKal { template class Material : public Effect { public: using KKEFF = Effect; - using PKTRAJ = ParticleTrajectory; + using PTRAJ = ParticleTrajectory; using EXING = ElementXing; using EXINGPTR = std::shared_ptr; - double time() const override { return dxing_->crossingTime() + tbuff_ ;} - bool active() const override { return dxing_->active(); } - void update(PKTRAJ const& ref) override; - void update(PKTRAJ const& ref, MetaIterConfig const& miconfig) override; - void update(Config const& config) override {} - void print(std::ostream& ost=std::cout,int detail=0) const override; + double time() const override { return exingptr_->time();} + bool active() const override { return exingptr_->active(); } void process(FitState& kkdata,TimeDir tdir) override; - void append(PKTRAJ& fit) override; + void updateState(MetaIterConfig const& miconfig,bool first) override; + void updateConfig(Config const& config) override {} + void append(PTRAJ& fit,TimeDir tdir) override; + void extrapolate(PTRAJ& fit,TimeDir tdir) override; + void updateReference(PTRAJ const& ptraj) override; + Chisq chisq(Parameters const& pdata) const override { return Chisq();} + void print(std::ostream& ost=std::cout,int detail=0) const override; virtual ~Material(){} // create from the material and a trajectory - Material(EXINGPTR const& dxing, PKTRAJ const& pktraj); + Material(EXINGPTR const& exingptr, PTRAJ const& ptraj); + // clone op for reinstantiation + Material(Material const&); + std::unique_ptr< Effect > clone(CloneContext&) const override; // accessors - Parameters const& effect() const { return mateff_; } - Weights const& cache() const { return cache_; } - EXING const& detXing() const { return *dxing_; } - KTRAJ const& refKTraj() const { return ref_; } + auto const& elementXing() const { return *exingptr_; } + auto const& elementXingPtr() const { return exingptr_; } + auto const& referenceTrajectory() const { return exingptr_->referenceTrajectory(); } + auto const& weights() const { return nextwt_; } private: - // update the local cache - void updateCache(); - EXINGPTR dxing_; // detector piece crossing for this effect - KTRAJ ref_; // reference to local trajectory - Parameters mateff_; // parameter space description of this effect - Weights cache_; // cache of weight processing in opposite directions, used to build the fit trajectory - double vscale_; // variance factor due to annealing 'temperature' - static double tbuff_; // small time buffer to avoid ambiguity - }; + EXINGPTR exingptr_; // element crossing for this effect + Weights nextwt_; // cache of weight forwards of this effect. - template double Material::tbuff_ = 1.0e-6; // small buffer to disambiguate this effect + // modifiers to support cloning + void setElementXingPtr(EXINGPTR const& ptr){ exingptr_ = ptr; } + }; - template Material::Material(EXINGPTR const& dxing, PKTRAJ const& pktraj) : dxing_(dxing), - ref_(pktraj.nearestPiece(dxing->crossingTime())), vscale_(1.0) { - update(pktraj); + template Material::Material(EXINGPTR const& exingptr, PTRAJ const& ptraj) : exingptr_(exingptr) { + updateReference(ptraj); } template void Material::process(FitState& kkdata,TimeDir tdir) { - if(dxing_->active()){ - // forwards, set the cache AFTER processing this effect + // The assymetry in the cache processing WRT fit update implements the convention that the cache is forwards in time of this effect, and insures the effect is only included once + if(exingptr_->active()){ if(tdir == TimeDir::forwards) { - kkdata.append(mateff_); - cache_ += kkdata.wData(); + kkdata.append(exingptr_->params(),tdir); + nextwt_ += kkdata.wData(); } else { - // backwards, set the cache BEFORE processing this effect, to avoid double-counting it - cache_ += kkdata.wData(); - // SUBTRACT the effect going backwards: covariance change is sign-independent - Parameters reverse(mateff_); - reverse.parameters() *= -1.0; - kkdata.append(reverse); + nextwt_ += kkdata.wData(); + kkdata.append(exingptr_->params(),tdir); } } - KKEFF::setState(tdir,KKEFF::processed); } - template void Material::update(PKTRAJ const& ref) { - cache_ = Weights(); - ref_ = ref.nearestPiece(dxing_->crossingTime()); - updateCache(); - KKEFF::updateState(); + template void Material::updateState(MetaIterConfig const& miconfig,bool first) { + // update the ElementXing + exingptr_->updateState(miconfig,first); + // reset the cached weights + nextwt_ = Weights(); } - template void Material::update(PKTRAJ const& ref, MetaIterConfig const& miconfig) { - vscale_ = miconfig.varianceScale(); - // update the detector Xings for this effect - dxing_->update(ref,miconfig); - update(ref); - } - - template void Material::updateCache() { - mateff_ = Parameters(); - if(dxing_->active()){ - // loop over the momentum change basis directions, adding up the effects on parameters from each - std::array dmom = {0.0,0.0,0.0}, momvar = {0.0,0.0,0.0}; - dxing_->materialEffects(ref_,TimeDir::forwards, dmom, momvar); - // get the parameter derivative WRT momentum - DPDV dPdM = ref_.dPardM(time()); - double mommag = ref_.momentum(time()); - for(int idir=0;idir(idir); - auto dir = ref_.direction(time(),mdir); - // project the momentum derivatives onto this direction - DVEC pder = mommag*(dPdM*SVEC3(dir.X(), dir.Y(), dir.Z())); - // convert derivative vector to a Nx1 matrix - ROOT::Math::SMatrix dPdm; - dPdm.Place_in_col(pder,0,0); - // update the transport for this effect; first the parameters. Note these are for forwards time propagation (ie energy loss) - mateff_.parameters() += pder*dmom[idir]; - // now the variance: this doesn't depend on time direction - ROOT::Math::SMatrix> MVar; - MVar(0,0) = momvar[idir]*vscale_; - mateff_.covariance() += ROOT::Math::Similarity(dPdm,MVar); + template void Material::append(PTRAJ& ptraj,TimeDir tdir) { + if(exingptr_->active()){ + // create a trajectory piece from the cached weight + double etime = this->time(); + // make sure this effect is appendable + if( (tdir == TimeDir::forwards && etime < ptraj.back().range().begin()) || + (tdir == TimeDir::backwards && etime > ptraj.front().range().end()) ) + throw std::invalid_argument("New piece overlaps existing"); + KTRAJ newpiece = (tdir == TimeDir::forwards) ? ptraj.back() : ptraj.front(); + // make sure the range includes the transit time + newpiece.range() = (tdir == TimeDir::forwards) ? TimeRange(etime,std::max(ptraj.range().end(),etime+exingptr_->transitTime())) : + TimeRange(std::min(ptraj.range().begin(),etime-exingptr_->transitTime()),etime); + // invert the cached weight to define the parameters + newpiece.params() = Parameters(nextwt_); + if( tdir == TimeDir::forwards){ + ptraj.append(newpiece); + } else { + // Since the cache was forwards of this site, we have to apply the effect of this material xing to the parameters. + auto temp = exingptr_->params(); + temp.parameters() *= -1; // reverse the sign of the parameter change + newpiece.params() += temp; + ptraj.prepend(newpiece); } } } - template void Material::append(PKTRAJ& fit) { - if(dxing_->active()){ - // create a trajectory piece from the cached weight - double time = this->time(); - KTRAJ newpiece(ref_); - newpiece.params() = Parameters(cache_); - // extend as necessary: absolute time can shift during iterations - newpiece.range() = TimeRange(time,std::max(time+tbuff_,fit.range().end())); - // make sure the piece is appendable; if not, adjust - if(time < fit.back().range().begin()){ - // if this is the first piece, simply extend it back - if(fit.pieces().size() ==1){ - fit.front().setRange(TimeRange(newpiece.range().begin()-tbuff_,fit.range().end())); - } else { - newpiece.setRange(TimeRange(fit.back().range().begin()+tbuff_,fit.range().end())); - } - } - fit.append(newpiece); + template void Material::extrapolate(PTRAJ& ptraj,TimeDir tdir) { + // make sure the traj can be appended + if((tdir == TimeDir::forwards && ptraj.back().range().begin() > time()) || + (tdir == TimeDir::backwards && ptraj.front().range().end() < time()) ) + throw std::invalid_argument("Material: Can't append piece"); + auto ktrajptr = ptraj.nearestTraj(time()); + // convert parameters at this material to momentum space representation + auto pstate = ktrajptr->stateEstimate(time()); + auto bnom = ktrajptr->bnom(); + // change in momentum due to the associated element Xing + SVEC3 dmom; + SMAT dmomvar; + exingptr_->momentumChange(dmom,dmomvar); + // expand these to the full particle state, with null position and cross-covariance + DMAT dstatevar; dstatevar.Place_at(dmomvar,3,3); // lower right corner for momentum + auto psvar = pstate.stateCovariance() + dstatevar; + SVEC6 dstate; dstate.Place_at(dmom,3); + // update the pstate accordingly. momentum vector change depends on the time direction, but not the covariance + if( tdir == TimeDir::forwards){ + // add the material effect + auto psvec = pstate.state() + dstate; + ParticleStateEstimate newpstate(psvec,psvar,time(),pstate.mass(),pstate.charge()); + TimeRange range(time(),ptraj.range().end()); + KTRAJ newpiece(newpstate, bnom, range); + ptraj.append(newpiece); + } else { + auto psvec = pstate.state() - dstate; + ParticleStateEstimate newpstate(psvec,psvar,time(),pstate.mass(),pstate.charge()); + TimeRange range(ptraj.range().begin(),time()); + KTRAJ newpiece(newpstate, bnom, range); + ptraj.prepend(newpiece); } } + template void Material::updateReference(PTRAJ const& ptraj) { + exingptr_->updateReference(ptraj); + } + template void Material::print(std::ostream& ost,int detail) const { ost << "Material " << static_castconst&>(*this); - ost << " effect "; - effect().print(ost,detail-2); ost << " ElementXing "; - dxing_->print(ost,detail); + exingptr_->print(ost,detail); if(detail >3){ - ost << " cache "; - cache().print(ost,detail); - ost << "Reference " << ref_ << std::endl; + ost << " forward cache "; + nextwt_.print(ost,detail); } } @@ -151,5 +151,21 @@ namespace KinKal { return ost; } + // clone op for reinstantiation + template + Material::Material(Material const& rhs): + nextwt_(rhs.weights()){ + /**/ + } + + template + std::unique_ptr< Effect > Material::clone(CloneContext& context) const{ + auto casted = std::make_unique< Material >(*this); + EXINGPTR ptr = context.get(exingptr_); + casted->setElementXingPtr(ptr); + //auto rv = std::make_unique< Effect >(casted); + auto rv = std::move(casted); + return rv; + } } #endif diff --git a/Fit/Measurement.hh b/Fit/Measurement.hh index 26bf9056..e5b3f3e1 100644 --- a/Fit/Measurement.hh +++ b/Fit/Measurement.hh @@ -14,107 +14,91 @@ namespace KinKal { template class Measurement : public Effect { public: using KKEFF = Effect; - using PKTRAJ = ParticleTrajectory; + using PTRAJ = ParticleTrajectory; using HIT = Hit; using HITPTR = std::shared_ptr; - - Chisq chisq(Parameters const& pdata) const override; - Chisq chisq() const override; - void update(PKTRAJ const& pktraj) override; - void update(PKTRAJ const& pktraj, MetaIterConfig const& miconfig) override; - void update(Config const& config) override {} - void process(FitState& kkdata,TimeDir tdir) override; - bool active() const override { return hit_->active(); } + // Effect Interface double time() const override { return hit_->time(); } + bool active() const override { return hit_->active(); } + void process(FitState& kkdata,TimeDir tdir) override; + void updateState(MetaIterConfig const& miconfig,bool first) override; + void updateConfig(Config const& config) override {} + void updateReference(PTRAJ const& ptraj) override; + void append(PTRAJ& fit,TimeDir tdir) override; + void extrapolate(PTRAJ& fit,TimeDir tdir) override; + Chisq chisq(Parameters const& pdata) const override; void print(std::ostream& ost=std::cout,int detail=0) const override; virtual ~Measurement(){} // local functions // construct from a hit and reference trajectory - Measurement(HITPTR const& hit, PKTRAJ const& reftraj,double precision=1e-6); - // the unbiased parameters are the fit parameters not including the information content of this effect - Parameters unbiasedParameters() const; - // access the contents + Measurement(HITPTR const& hit,PTRAJ const& ptraj); + // clone op for reinstantiation + Measurement(Measurement const&); + std::unique_ptr< Effect > clone(CloneContext&) const override; + // access the underlying hit HITPTR const& hit() const { return hit_; } - Weights const& weightCache() const { return wcache_; } - Weights const& hitWeight() const { return hitwt_; } - double precision() const { return precision_; } private: - HITPTR hit_ ; // hit used for this constraint - Weights wcache_; // sum of processing weights in opposite directions, excluding this hit's information. used to compute unbiased parameters and chisquared - Weights hitwt_; // weight representation of the hits constraint - double vscale_; // variance factor due to annealing 'temperature' - double precision_; // precision used in TCA calcuation + HITPTR hit_ ; // hit used for this measurement + + // modifiers to support cloning + void setHitPtr(HITPTR const& ptr){ hit_ = ptr; } }; - template Measurement::Measurement(HITPTR const& hit, PKTRAJ const& reftraj,double precision) : hit_(hit), vscale_(1.0), precision_(precision) { - update(reftraj); + template Measurement::Measurement(HITPTR const& hit,PTRAJ const& ptraj) : hit_(hit) { + this->updateReference(ptraj); } template void Measurement::process(FitState& kkdata,TimeDir tdir) { - // direction is irrelevant for processing hits - if(this->active()){ - // cache the processing weights, adding both processing directions - wcache_ += kkdata.wData(); - // add this effect's information - kkdata.append(hitwt_); - } - KKEFF::setState(tdir,KKEFF::processed); + // add this effect's information. direction is irrelevant for processing hits + if(this->active())kkdata.append(hit_->weight()); } - template void Measurement::update(PKTRAJ const& pktraj) { - // reset the processing cache - wcache_ = Weights(); - // update the hit - hit_->update(pktraj); - // get the weight from the hit - hitwt_ = hit_->weight(); - // scale weight for the temp - hitwt_ *= 1.0/vscale_; - // ready for processing! - KKEFF::updateState(); + template void Measurement::updateState(MetaIterConfig const& miconfig,bool first) { + // update the hit's internal state; the actual update depends on the hit + hit_->updateState(miconfig,first); } - template void Measurement::update(PKTRAJ const& pktraj, MetaIterConfig const& miconfig) { - // reset the annealing temp and hit precision - vscale_ = miconfig.varianceScale(); - // update the hit's internal state; the actual update depends on the hit - hit_->update(pktraj,miconfig ); - // update the state of this object - update(pktraj); + template void Measurement::append(PTRAJ& ptraj,TimeDir tdir) { + // measurements do not change the trajectory } - template Chisq Measurement::chisq(Parameters const& pdata) const { - return hit_->chisq(pdata); + template void Measurement::extrapolate(PTRAJ& ptraj,TimeDir tdir) { } - template Chisq Measurement::chisq() const { - if(this->active()) { - Parameters unbiased = unbiasedParameters(); - return chisq(unbiased); - } else - return Chisq(); + template void Measurement::updateReference(PTRAJ const& ptraj) { + hit_->updateReference(ptraj); } - template Parameters Measurement::unbiasedParameters() const { - // this function can't be called on an unprocessed effect - if( !KKEFF::wasProcessed(TimeDir::forwards) || !KKEFF::wasProcessed(TimeDir::backwards)) - throw std::invalid_argument("Can't compute unbiased parameters for unprocessed constraint"); - // Invert the cache to get unbiased parameters at this constraint - return Parameters(wcache_); + template Chisq Measurement::chisq(Parameters const& pdata) const { + return hit_->chisq(pdata); } - template void Measurement::print(std::ostream& ost, int detail) const { + template void Measurement::print(std::ostream& ost, int detail) const { ost << "Measurement " << static_cast const&>(*this) << std::endl; if(detail > 0){ hit_->print(ost,detail); - ost << " Measurement Weight " << hitwt_ << std::endl; } } - template std::ostream& operator <<(std::ostream& ost, Measurement const& kkhit) { - kkhit.print(ost,0); + template std::ostream& operator <<(std::ostream& ost, Measurement const& measurement) { + measurement.print(ost,0); return ost; } + // clone op for reinstantiation + template + Measurement::Measurement(Measurement const& rhs){ + /**/ + } + + template + std::unique_ptr< Effect > Measurement::clone(CloneContext& context) const{ + auto casted = std::make_unique< Measurement >(*this); + HITPTR ptr = context.get(hit_); + casted->setHitPtr(ptr); + //auto rv = std::make_unique< Effect >(casted); + auto rv = std::move(casted); + return rv; + } } #endif diff --git a/Fit/MetaIterConfig.hh b/Fit/MetaIterConfig.hh index 5940ede5..b25547ff 100644 --- a/Fit/MetaIterConfig.hh +++ b/Fit/MetaIterConfig.hh @@ -17,9 +17,8 @@ namespace KinKal { // add updater void addUpdater(std::any const& updater) { updaters_.push_back(updater); } // accessors - double temperature() const { return temp_; } - // interpret temperature as a variance - double varianceScale() const { return (1.0+temp_)*(1.0+temp_); } // variance scale so that temp=0 means no additional variance + double temperature() const { return temp_; } // dimensionless parameter interpreted Additively, scaled by the relevant parameter error + double varianceScale() const { return (1.0+temp_)*(1.0+temp_); } // variance scaling factor size_t nUpdaters() const { return updaters_.size(); } // find a particular updater: note that at most 1 of a type is allowed for a given meta-iteration template const UPDATER* findUpdater() const; diff --git a/Fit/Status.cc b/Fit/Status.cc index ad91502d..dda58614 100644 --- a/Fit/Status.cc +++ b/Fit/Status.cc @@ -9,12 +9,16 @@ namespace KinKal { return "Unconverged "; case Status::converged: return "Converged "; - case Status::diverged: - return "Diverged "; + case Status::chisqdiverged: + return "Chi2Diverged "; + case Status::paramsdiverged: + return "ParamsDiverged "; + case Status::gapdiverged: + return "GapDiverged "; case Status::lowNDOF: return "LowNDOF "; - case Status::paramsdiverged: - return "ParametersDiverged "; + case Status::outsidemap: + return "OutsideBFieldMap "; case Status::failed: return "Failed "; } diff --git a/Fit/Status.hh b/Fit/Status.hh index 0aa4b36f..f94fa954 100644 --- a/Fit/Status.hh +++ b/Fit/Status.hh @@ -1,6 +1,7 @@ #ifndef KinKal_Status_hh #define KinKal_Status_hh #include "KinKal/General/Chisq.hh" +#include "KinKal/General/Vectors.hh" #include #include #include @@ -8,15 +9,16 @@ namespace KinKal { // struct to define fit status struct Status { - enum status {unfit=-1,converged,unconverged,diverged,lowNDOF,paramsdiverged,failed}; // fit status - int miter_; // meta-iteration number; - int iter_; // iteration number; + enum status {unfit=-1,converged,unconverged,lowNDOF,gapdiverged,paramsdiverged,chisqdiverged,outsidemap,failed}; // fit status + unsigned miter_; // meta-iteration number; + unsigned iter_; // iteration number; status status_; // current status Chisq chisq_; // current chisquared - std::string comment_; // further information about the status - bool usable() const { return status_ < diverged; } + std::string comment_; // further information about the status + bool usable() const { return status_ > unfit && status_ < lowNDOF; } + bool unusable() const { return !usable(); } bool needsFit() const { return status_ == unfit || status_ == unconverged; } - Status(unsigned miter) : miter_(miter), iter_(-1), status_(unfit){} + Status(unsigned miter,unsigned iter=0,status stat=unfit,const char* comment="") : miter_(miter), iter_(iter), status_(stat), chisq_(NParams()),comment_(comment){} static std::string statusName(status stat); }; std::ostream& operator <<(std::ostream& os, Status const& fitstatus ); diff --git a/Fit/Track.hh b/Fit/Track.hh index 8f8b49d3..237f2982 100644 --- a/Fit/Track.hh +++ b/Fit/Track.hh @@ -2,7 +2,7 @@ #define KinKal_Track_hh // // Primary class of the Kinematic Kalman fit. This class owns the state describing -// the fit inputs (measurements, material interactions, BField corrections, etc), the result of the fit, +// the fit inputs (measurements, material interactions, BField changes, etc), the result of the fit, // and the methods for computing it. The fit result is expressed as a piecewise kinematic covariant // particle trajectory, providing position, momentum etc information about the particle with covariance // as a function of physical time. @@ -38,17 +38,22 @@ #include "KinKal/Trajectory/ParticleTrajectory.hh" #include "KinKal/Fit/FitState.hh" #include "KinKal/Fit/Effect.hh" -#include "KinKal/Fit/TrackEnd.hh" #include "KinKal/Fit/Measurement.hh" #include "KinKal/Fit/Material.hh" -#include "KinKal/Fit/BField.hh" +#include "KinKal/Fit/DomainWall.hh" #include "KinKal/Fit/Config.hh" #include "KinKal/Fit/Status.hh" +#include "KinKal/Fit/Domain.hh" #include "KinKal/General/BFieldMap.hh" +#include "KinKal/General/CloneContext.hh" +#include "KinKal/General/TimeRange.hh" #include "KinKal/General/TimeDir.hh" #include "TMath.h" #include #include +#include +#include +#include #include #include #include @@ -57,41 +62,60 @@ #include namespace KinKal { + using DOMAINPTR = std::shared_ptr; + bool operator < (DOMAINPTR const& a, DOMAINPTR const& b) { return a->begin() < b->begin(); } + bool operator < (DOMAINPTR const& a, double val) { return a->begin() < val; } + bool operator < (double val, DOMAINPTR const& b) { return val < b->begin(); } + template class Track { public: using KKEFF = Effect; - using KKHIT = Measurement; + struct KKEFFComp { // comparator to sort effects by time + bool operator()(std::unique_ptr const& a, std::unique_ptr const& b) const { + if(a.get() != b.get()) + return a->time() < b->time(); + else + return false; + } + }; + + using KKEFFCOL = std::list>; + using KKEFFFWD = typename KKEFFCOL::iterator; + using KKEFFREV = typename KKEFFCOL::reverse_iterator; + using KKEFFFWDBND = std::array; + using KKEFFREVBND = std::array; + using KKMEAS = Measurement; using KKMAT = Material; - using KKEND = TrackEnd; - using KKBFIELD = BField; + using KKDW = DomainWall; using PKTRAJ = ParticleTrajectory; + using PKTRAJPTR = std::unique_ptr; using HIT = Hit; using HITPTR = std::shared_ptr; using HITCOL = std::vector; using EXING = ElementXing; using EXINGPTR = std::shared_ptr; using EXINGCOL = std::vector; - using DOMAINCOL = std::vector; + using DOMAINCOL = std::set; using CONFIGCOL = std::vector; - struct KKEFFComp { // comparator to sort effects by time - bool operator()(std::unique_ptr const& a, std::unique_ptr const& b) const { - if(a.get() != b.get()) - return a->time() < b->time(); - else - return false; - } - }; - using KKEFFCOL = std::vector> ; // container type for effects - // construct from a set of hits and passive material crossings + using FitStateArray = std::array; + // construct from a set of hits and passive material crossings. This will compute the magnetic domains implicitly from the configuration Track(Config const& config, BFieldMap const& bfield, KTRAJ const& seedtraj, HITCOL& hits, EXINGCOL& exings ); + // reconstitute from a previous track + Track(Config const& config, BFieldMap const& bfield, PKTRAJPTR& fittraj, HITCOL& hits, EXINGCOL& exings, DOMAINCOL& domains); + // copy constructor + Track(const Track& rhs, CloneContext& context); // extend an existing track with either new configuration, new hits, and/or new material xings void extend(Config const& config, HITCOL& hits, EXINGCOL& exings ); + // extrapolate the fit through the magnetic field with the given config until the given predicate is satisfied. This function requires + // the fit be valid, otherwise the return code is false. If successful the status, domains, and trajectory of the fit are updated + // Note that the actual fit itself is unchanged + template bool extrapolate(TimeDir tdir, XTEST const& XTest); + // extrapolate the fit through a material xing. Return value is true if the effect is added and fit updated. + bool extrapolate(EXINGPTR const& exing,TimeDir const& tdir); // accessors std::vector const& history() const { return history_; } Status const& fitStatus() const { return history_.back(); } // most recent status - KTRAJ const& seedTraj() const { return seedtraj_; } - PKTRAJ const& refTraj() const { return reftraj_; } - PKTRAJ const& fitTraj() const { return fittraj_; } + PKTRAJ const& fitTraj() const { return *fittraj_; } KKEFFCOL const& effects() const { return effects_; } Config const& config() const { return config_.back(); } CONFIGCOL const& configs() const { return config_; } @@ -100,165 +124,272 @@ namespace KinKal { EXINGCOL const& exings() const { return exings_; } DOMAINCOL const& domains() const { return domains_; } void print(std::ostream& ost=std::cout,int detail=0) const; + TimeRange activeRange() const; // time range of active hits + void extendTraj(TimeRange const& newrange); + static TimeRange detectorRange(HITCOL& hits, EXINGCOL& exings,bool active=false); protected: - Track(Config const& cfg, BFieldMap const& bfield, KTRAJ const& seedtraj ); - void fit(HITCOL& hits, EXINGCOL& exings ); + Track(Config const& cfg, BFieldMap const& bfield); + void fit(HITCOL& hits, EXINGCOL& exings, KTRAJ const& seedtraj); + void fit(HITCOL& hits, EXINGCOL& exings, DOMAINCOL& domains, PKTRAJPTR& fittraj); private: - // helper functions - TimeRange getRange(HITCOL& hits, EXINGCOL& exings) const; + void createEffects( HITCOL& hits, EXINGCOL& exings, DOMAINCOL const& domains); + void convertSeed(KTRAJ const& seedtraj,TimeRange const& refrange, DOMAINCOL& domains); void fit(); // process the effects and create the trajectory. This executes the current schedule - void update(Status const& fstat, MetaIterConfig const& miconfig); - void fitIteration(Status& status, MetaIterConfig const& miconfig); + bool createDomains(PKTRAJ const& ptraj, TimeRange const& range, DOMAINCOL& domains) const; + bool setBounds(KKEFFFWDBND& fwdbnds,KKEFFREVBND& revbnds); // set the bounds. Returns false if the bounds are empty + bool extendDomains(TimeRange const& fitrange); // extend domains if the fit range changes. Return value says if domains were added + void updateDomains(PKTRAJ const& ptraj); // Update domains between iterations + void iterate(MetaIterConfig const& miconfig); + void setStatus(PKTRAJPTR& ptrajptr); + void initFitState(FitStateArray& states, TimeRange const& fitrange, double dwt=1.0); + PKTRAJPTR initTraj(FitState& state, TimeRange const& fitrange); bool canIterate() const; - void createEffects( HITCOL& hits, EXINGCOL& exings, DOMAINCOL const& domains); - void createRefTraj(KTRAJ const& seedtraj,TimeRange const& refrange, DOMAINCOL const& domains); - void replaceFitTraj(DOMAINCOL const& domains); - void extendRefTraj(DOMAINCOL const& domains); - // divide a kinematic trajectory range into magnetic 'domains' within which the BField inhomogeneity effects are within tolerance - void createDomains(PKTRAJ const& pktraj, TimeRange const& range, std::vector& ranges, TimeDir tdir=TimeDir::forwards) const; + void replaceDomains(DOMAINCOL const& domains); + void extendTraj(DOMAINCOL const& domains); + void processEnds(); + // add a single domain within the tolerance and extend the fit in the specified direction. + void addDomain(Domain const& domain,TimeDir const& tdir,bool exact=false); + auto& status() { return history_.back(); } // most recent status + // divide a trajectory into magnetic 'domains' within which the BField can be considered constant (parameter change is within tolerance) // payload CONFIGCOL config_; // configuration BFieldMap const& bfield_; // magnetic field map std::vector history_; // fit status history; records the current iteration - KTRAJ seedtraj_; // seed for the fit - PKTRAJ reftraj_; // reference against which the derivatives were evaluated and the current fit performed - PKTRAJ fittraj_; // result of the current fit, becomes the reference when the fit is algebraically iterated + PKTRAJPTR fittraj_; // result of the current fit KKEFFCOL effects_; // effects used in this fit, sorted by time HITCOL hits_; // hits used in this fit EXINGCOL exings_; // material xings used in this fit - DOMAINCOL domains_; // BField domains used in this fit + DOMAINCOL domains_; // domains used in this fit, contiguous and sorted by time }; - // sub-class constructor, based just on the seed. It requires added hits to - template Track::Track(Config const& cfg, BFieldMap const& bfield, KTRAJ const& seedtraj ) : - bfield_(bfield), seedtraj_(seedtraj) { - config_.push_back(cfg); + // minimal constructor for subclasses. The resulting object has no fit. + template Track::Track(Config const& cfg, BFieldMap const& bfield) : + config_{cfg}, bfield_(bfield) + { if(config().schedule().size() ==0)throw std::invalid_argument("Invalid configuration: no schedule"); + history_.push_back(Status(0,0,Status::unfit, "Construction")); } - // construct from configuration, reference (seed) fit, hits,and materials specific to this fit. - template Track::Track(Config const& cfg, BFieldMap const& bfield, KTRAJ const& seedtraj, HITCOL& hits, EXINGCOL& exings) : Track(cfg,bfield,seedtraj) { - fit(hits,exings); + // construct from configuration, reference (seed) fit, hits,and materials specific to this fit. This will compute the domains according to the configuration before fitting. + // + template Track::Track(Config const& cfg, BFieldMap const& bfield, KTRAJ const& seedtraj, HITCOL& hits, EXINGCOL& exings) + : Track(cfg,bfield) + { + fit(hits,exings, seedtraj); + } + + template Track::Track(Config const& cfg, BFieldMap const& bfield, PKTRAJPTR& fittraj, HITCOL& hits, EXINGCOL& exings, DOMAINCOL& domains) + : Track(cfg,bfield) + { + fit(hits,exings,domains,fittraj); } - template void Track::fit(HITCOL& hits, EXINGCOL& exings) { - // set the seed time based on the min and max time from the inputs - TimeRange refrange = getRange(hits,exings); - seedtraj_.setRange(refrange); - // if correcting for BField effects, define the domains + + template void Track::fit(HITCOL& hits, EXINGCOL& exings, KTRAJ const& seedtraj) { + auto detrange = detectorRange(hits,exings,true); + // convert the seed traj to a piecewaise traj. This creates the domains DOMAINCOL domains; - PKTRAJ pkseed(seedtraj_); - if(config().bfcorr_ ) createDomains(pkseed, refrange, domains); - // Create the initial reference trajectory - createRefTraj(seedtraj_,refrange,domains); - // create the end effects: these help manage the fit - effects_.reserve(hits.size()+exings.size()+domains.size()+2); - effects_.emplace_back(std::make_unique(config(), bfield_, reftraj_,TimeDir::forwards)); - effects_.emplace_back(std::make_unique(config(), bfield_, reftraj_,TimeDir::backwards)); - // create all the other effects + convertSeed(seedtraj,detrange,domains); + if(status().needsFit()){ + // convert all the primary info to fit effects + createEffects(hits, exings, domains); + // now fit + fit(); + } + } + + template void Track::fit(HITCOL& hits, EXINGCOL& exings, DOMAINCOL& domains, PKTRAJPTR& fittraj) { + fittraj_ = std::move(fittraj); // steal the underlying object + // truncate the domains and fit trajectory to be within the detector range + auto detrange = detectorRange(hits,exings,true); + if(domains.size() > 0){ + auto idom = domains.begin(); + // stop at the 1st domain overlaping the detector range, and erase all elements up to that point + while(idom != domains.end() && !(detrange.overlaps((*idom)->range())))++idom; + if(idom != domains.begin())domains.erase(domains.begin(),--idom);// leave the overlapping piece + auto jdom= domains.rbegin(); + while(jdom != domains.rend() && !(detrange.overlaps((*jdom)->range())))++jdom; + domains.erase(jdom.base(),domains.end()); // base points 1 past the reverse iterator + // trim the trajectory to this range + detrange.combine((*domains.begin())->range()); + detrange.combine((*domains.rbegin())->range()); + } + fittraj_->setRange(detrange,true); createEffects(hits,exings,domains); - // now fit the track fit(); } + // copy constructor + template Track::Track(const Track& rhs, CloneContext& context) : + config_(rhs.configs()), + bfield_(rhs.bfield()), + history_(rhs.history()) + { + fittraj_ = std::make_unique(rhs.fitTraj()); + hits_.reserve(rhs.hits().size()); + for (const auto& ptr: rhs.hits()){ + auto hit = ptr->clone(context); + hits_.push_back(hit); + } + exings_.reserve(rhs.exings().size()); + for (const auto& ptr: rhs.exings()){ + auto xng = ptr->clone(context); + exings_.push_back(xng); + } + for (const auto& ptr: rhs.domains()){ + auto dmn = context.get(ptr); + domains_.insert(dmn); + } + for (const auto& ptr: rhs.effects()){ + auto eff = ptr->clone(context); + effects_.push_back(std::move(eff)); + } + }; + // extend an existing track template void Track::extend(Config const& cfg, HITCOL& hits, EXINGCOL& exings) { - // update the configuration - config_.push_back(cfg); - // configuation check - if(config().schedule().size() ==0)throw std::invalid_argument("Invalid configuration: no schedule"); // require the existing fit to be usable - if(!fitStatus().usable())throw std::invalid_argument("Cannot extend unusable fit"); + if(!fitStatus().usable())return; + // configuation check + if(cfg.schedule().size() ==0)throw std::invalid_argument("Invalid configuration: no schedule"); + // save the new configuration + config_.push_back(cfg); + // remember the previous config + auto oldciter = config_.rbegin(); oldciter++; + auto const& oldconfig = *oldciter; // find the range of the added information, and extend as needed - TimeRange exrange = reftraj_.range(); + TimeRange exrange = fittraj_->range(); if(hits.size() >0 || exings.size() > 0){ - TimeRange newrange = getRange(hits,exings); + TimeRange newrange = detectorRange(hits,exings); exrange.combine(newrange); } DOMAINCOL domains; + bool dok(true); if(config().bfcorr_ ) { // if the previous configuration didn't have domains, then create them for the full reference range - auto oldconfig = config_.end(); --oldconfig; --oldconfig; - if(!oldconfig->bfcorr_){ + if(!oldconfig.bfcorr_ || oldconfig.tol_ != config().tol_){ // create domains for the whole range - createDomains(reftraj_, exrange,domains); - // replace the reftraj with one with BField rotations - replaceFitTraj(domains); + dok &= createDomains(*fittraj_,exrange, domains); + // replace previous domains with these. This replaces the trajectory and bfield-related effects + if(dok)replaceDomains(domains); } else { - // create domains just for the extensions, and extend the reftraj as needed - TimeRange exlow(exrange.begin(),reftraj_.range().begin()); + // create domains just for the extensions + TimeRange exlow(exrange.begin(),fittraj_->range().begin()); if(exlow.range()>0.0) { DOMAINCOL lowdomains; - createDomains(reftraj_, exlow, lowdomains, TimeDir::backwards); - extendRefTraj(domains); - domains.insert(domains.begin(),lowdomains.begin(),lowdomains.end()); + dok &= createDomains(*fittraj_,exlow, lowdomains); + if(dok)domains.insert(lowdomains.begin(),lowdomains.end()); } - TimeRange exhigh(reftraj_.range().end(),exrange.end()); + TimeRange exhigh(fittraj_->range().end(),exrange.end()); if(exhigh.range()>0.0){ DOMAINCOL highdomains; - createDomains(reftraj_, exhigh, highdomains,TimeDir::forwards); - extendRefTraj(highdomains); - domains.insert(domains.end(),highdomains.begin(),highdomains.end()); + dok &= createDomains(*fittraj_,exhigh, highdomains); + if(dok)domains.insert(highdomains.begin(),highdomains.end()); } } } - // create the effects for the new info and the new domains + if(!dok){ + // domain calculation failed: abort the fit + history_.push_back(Status(0)); + status().status_ = Status::outsidemap; + status().comment_ = std::string("Extension error"); + return; + } + // Extend the traj and create the effects for the new info and the new domains + extendTraj(domains); createEffects(hits,exings,domains); // update all the effects for this new configuration - for(auto& ieff : effects_ ) ieff->update(config()); + for(auto& ieff : effects_ ) ieff->updateConfig(config()); // now refit the track fit(); } - // replace the reference traj with one describing the 'same' trajectory in space, but using the local BField as reference - template void Track::replaceFitTraj(DOMAINCOL const& domains) { - // create new traj - PKTRAJ newfit; - // loop over domains - for(auto const& domain : domains) { - double dtime = domain.begin(); - // Set the BField to the start of this domain - auto bf = bfield_.fieldVect(fittraj_.position3(dtime)); - // loop until we're either out of this domain or the piece is out of this domain - while(dtime < domain.end()){ - // find the nearest piece of the current reftraj - auto index = fittraj_.nearestIndex(dtime); - auto const& oldpiece = fittraj_.pieces()[index]; - // create a new piece - KTRAJ newpiece(oldpiece,bf,dtime); - // set the range as needed - double endtime = (index < fittraj_.pieces().size()-1) ? std::min(domain.end(),oldpiece.range().end()) : domain.end(); - newpiece.range() = TimeRange(dtime,endtime); - newfit.append(newpiece); - // update the time - static double epsilon(1e-10); - dtime = newpiece.range().end()+epsilon; // to avoid boundary + // replace domains when DomainWall correction is added or changed. the traj must also be replaced, so that + // the pieces correspond to the new domains. The new traj is geometrically equivalent, but not parametrically equal. + template void Track::replaceDomains(DOMAINCOL const& domains) { + // if domains exist, clear them and remove all DomainWall effects + if(domains_.size() > 0){ + domains_.clear(); + // remove all existing DomainWall effects + auto ieff = effects_.begin(); + while(ieff != effects_.end()){ + const KKDW* kkbf = dynamic_cast(ieff->get()); + if(kkbf != 0){ + ieff = effects_.erase(ieff); + } else { + ++ieff; + } } } - // actually replace the reftraj - fittraj_ = newfit; + auto newtraj = std::make_unique(); + // loop over domains, splitting the overlapping traj pieces at the domain walls, and transforming them to reference the domain's field + // This increases the number of traj pieces. + // extend the existing traj to the domain range + TimeRange drange(domains.begin()->get()->begin(),domains.rbegin()->get()->end()); + fittraj_->setRange(drange); + for(auto const& domain : domains) { + // find the range of existing ptraj pieces that overlaps with this domain's range + using KTRAJPTR = std::shared_ptr; + using DKTRAJ = std::deque; + using DKTRAJCITER = typename DKTRAJ::const_iterator; + DKTRAJCITER first,last; + fittraj_->pieceRange(domain->range(),first,last); + // loop over these pieces; first and last can be the same! + auto olditer = first; + do { + auto const& oldpiece = **olditer; + // copy this piece, translating bnom to this domain's field + KTRAJ newpiece(oldpiece,domain->bnom(),domain->range().mid()); + // set the range for this piece, making sure it is non-zero + double tstart = std::max(domain->begin(), oldpiece.range().begin()); + double tend = std::min(domain->end(),oldpiece.range().end()); + if(tstart < tend){ + newpiece.range() = TimeRange(tstart,tend); + newtraj->append(newpiece); + } + if(olditer != last)++olditer; + } while(olditer != last); + } + // switch over any existing effects to reference this traj (could be none) + for (auto& eff : effects_) { + eff->updateReference(*newtraj); + } + // swap out the fit trajectory; this will be used as reference for the next iterations + fittraj_.swap(newtraj); } - template void Track::extendRefTraj(DOMAINCOL const& domains ) { - // dummy implementation: need to put in parameter rotation at each domain boundary FIXME! + template void Track::extendTraj(DOMAINCOL const& domains ) { if(domains.size() > 0){ - // extend the reftraj range - TimeRange newrange(std::min(reftraj_.range().begin(),domains.front().begin()), - std::max(reftraj_.range().end(),domains.back().end())); - reftraj_.setRange(newrange); + TimeRange newrange(std::min(fittraj_->range().begin(),domains.begin()->get()->begin()), + std::max(fittraj_->range().end(),domains.rbegin()->get()->end())); + fittraj_->setRange(newrange); } } - template void Track::createRefTraj(KTRAJ const& seedtraj , TimeRange const& range, DOMAINCOL const& domains ) { - // if we're making local BField corrections, divide the trajectory into domain pieces. Each will have equivalent parameters, but relative + template void Track::extendTraj(TimeRange const& newrange){ + TimeRange temprange(std::min(fittraj_->range().begin(),newrange.begin()), + std::max(fittraj_->range().end(),newrange.end())); + fittraj_->setRange(temprange); + } + + template void Track::convertSeed(KTRAJ const& seedtraj,TimeRange const& range, DOMAINCOL& domains) { + // if we're making local DomainWall corrections, divide the trajectory into domain pieces. Each will have equivalent parameters, but relative // to the local field if(config().bfcorr_ ) { - if(reftraj_.pieces().size() != 0)throw std::invalid_argument("Initial reference trajectory must be empty"); - if(domains.size() == 0)throw std::invalid_argument("Empty domain"); + // convert the seedtraj to a PKTRAJ + PKTRAJ straj(seedtraj); + bool dok = createDomains(straj, range, domains); + if(!dok){ + // failed initialization + history_.emplace_back(0,0,Status::outsidemap, "Domain initialization error"); + return; + } + // create the initial fittraj from the seed. Each piece will have 'the same' physical trajectory as the seed, but reference the local domain BField + fittraj_ = std::make_unique(); for(auto const& domain : domains) { - // Set the BField to the start of this domain - auto bf = bfield_.fieldVect(seedtraj.position3(domain.begin())); - KTRAJ newpiece(seedtraj,bf,domain.begin()); - newpiece.range() = domain; - reftraj_.append(newpiece); + // Set the DomainWall to the start of this domain + auto bf = bfield_.fieldVect(seedtraj.position3(domain->mid())); + KTRAJ newpiece(seedtraj,bf,domain->mid()); + newpiece.range() = domain->range(); + fittraj_->append(newpiece); } } else { // use the middle of the range as the nominal BField for this fit: @@ -268,138 +399,325 @@ namespace KinKal { KTRAJ firstpiece(seedtraj,bf,tref); firstpiece.range() = range; // create the piecewise trajectory from this - reftraj_ = PKTRAJ(firstpiece); + fittraj_ = std::make_unique(firstpiece); } } template void Track::createEffects( HITCOL& hits, EXINGCOL& exings,DOMAINCOL const& domains ) { - // pre-allocate as needed - effects_.reserve(effects_.size()+hits.size()+exings.size()+domains.size()); - // append the effects. First, loop over the hits + // create and append the effects. First, loop over the hits for(auto& hit : hits ) { - // create the hit effects and insert them in the set - effects_.emplace_back(std::make_unique(hit,reftraj_)); + effects_.emplace_back(std::make_unique(hit,*fittraj_)); } //add material effects for(auto& exing : exings) { - effects_.emplace_back(std::make_unique(exing,reftraj_)); + effects_.emplace_back(std::make_unique(exing,*fittraj_)); } - // add BField effects - for( auto const& domain : domains) { - // create the BField effect for integrated differences over this range - effects_.emplace_back(std::make_unique(config(),bfield_,domain)); + // add DomainWall effects + if(config().bfcorr_ && domains.size() > 1){ + auto nextdom = domains.cbegin(); + auto prevdom = nextdom; + ++nextdom; + while( nextdom != domains.cend() ){ + if(fabs(prevdom->get()->end()-nextdom->get()->begin())>1e-10)throw std::invalid_argument("Invalid domains"); + effects_.emplace_back(std::make_unique(*prevdom,*nextdom ,*fittraj_)); + prevdom = nextdom; + ++nextdom; + } } - // sort - std::sort(effects_.begin(),effects_.end(),KKEFFComp ()); - // store the inputs + // store the inputs; these are just for and may not be in time order hits_.insert(hits_.end(),hits.begin(),hits.end()); exings_.insert(exings_.end(),exings.begin(),exings.end()); - domains_.insert(domains_.end(),domains.begin(),domains.end()); + domains_.insert(domains.cbegin(),domains.cend()); } - // fit iteration management + // fit the track template void Track::fit() { // execute the schedule of meta-iterations for(auto imiconfig=config().schedule().begin(); imiconfig != config().schedule().end(); imiconfig++){ auto miconfig = *imiconfig; - // algebraic convergence iteration - unsigned nmeta = history_.size() == 0? 0 : history_.back().miter_ + 1; - Status fstat(nmeta); - history_.push_back(fstat); - while(canIterate()) { + // keep the meta-iteration count correct even if we extend the fit. + unsigned nmeta = history_.size() == 0? 0 : fitStatus().miter_ + 1; + unsigned niter(0); + do{ + history_.push_back(Status(nmeta,niter++)); // catch exceptions and record them in the status try { - update(fstat,miconfig); - fitIteration(fstat,miconfig); + iterate(miconfig); } catch (std::exception const& error) { - fstat.status_ = Status::failed; - fstat.comment_ = error.what(); + status().status_ = Status::failed; + status().comment_ = error.what(); } - // record this status in the history - history_.push_back(fstat); + } while(canIterate()); + if(!status().usable())break; + } + // if the fit is usable, extend the track as necessary + if(config().ends_ && status().usable()){ + history_.push_back(fitStatus()); + status().comment_ = "EndProcessing"; + try { + processEnds(); + } catch (std::exception const& error) { + status().status_ = Status::failed; + status().comment_ = status().comment_ + error.what(); } - if(!fstat.usable())break; } if(config().plevel_ > Config::none)print(std::cout, config().plevel_); } - // single algebraic iteration - template void Track::fitIteration(Status& fstat, MetaIterConfig const& miconfig) { - if(config().plevel_ >= Config::complete)std::cout << "Processing fit iteration " << fstat.iter_ << std::endl; - // reset counters - fstat.chisq_ = Chisq(0.0, -(int)NParams()); - fstat.iter_++; - // fit in both directions (order doesn't matter) - auto feff = effects_.begin(); - // start with empty fit information; each effect will modify this as necessary, and cache what it needs for later processing - FitState forwardstate; - while(feff != effects_.end()){ - auto ieff = feff->get(); - // update chisquared increment WRT the current state: only needed forwards - Chisq dchisq = ieff->chisq(forwardstate.pData()); - fstat.chisq_ += dchisq; + // single algebraic iteration of the fit + template void Track::iterate(MetaIterConfig const& miconfig) { + if(config().plevel_ >= Config::basic)std::cout << "Processing fit iteration " << fitStatus().iter_ << std::endl; + // update the effects for this configuration; this will sort the effects and find the iteration bounds + bool first = status().iter_ == 0; // 1st iteration of a meta-iteration: update the effect internals + for(auto& ieff : effects_ ) ieff->updateState(miconfig,first); + // sort the sites, and set the iteration bounds + effects_.sort(KKEFFComp()); + KKEFFFWDBND fwdbnds; // bounding sites used for fitting + KKEFFREVBND revbnds; + if(!setBounds(fwdbnds,revbnds)){ + status().status_ = Status::lowNDOF; + return; + } + // update the domains + TimeRange fitrange(fwdbnds[0]->get()->time(),revbnds[0]->get()->time()); + if(fitrange.range() == 0){ + status().status_ = Status::lowNDOF; + return; + } + if(config().bfcorr_){ + // update the limits if new DW effects were added + if(extendDomains(fitrange))setBounds(fwdbnds,revbnds); + } + FitStateArray states; + initFitState(states, fitrange, config().dwt_/miconfig.varianceScale()); + // process the relevant effects forwards in time, adding their info to the fit state. Also compute chisquared + for(auto feff=fwdbnds[0];feff!=fwdbnds[1];++feff){ + auto effptr = feff->get(); + // update chisquared increment WRT the current state: only needed once + Chisq dchisq = effptr->chisq(states[0].pData()); + status().chisq_ += dchisq; // process - ieff->process(forwardstate,TimeDir::forwards); - if(config().plevel_ >= Config::detailed){ - std::cout << "Chisq total " << fstat.chisq_ << " increment " << dchisq << " "; - ieff->print(std::cout,config().plevel_); + effptr->process(states[0],TimeDir::forwards); + if(config().plevel_ >= Config::detailed && dchisq.nDOF() > 0){ + std::cout << "Chisq increment " << dchisq << " "; + effptr->print(std::cout,config().plevel_-Config::detailed); + } + } + if(status().chisq_.nDOF() < (int)config().minndof_) { // I need a better way to define coverage as this test doesn't guarantee all parameters are constrained TODO + status().status_ = Status::lowNDOF; + return; + } + // Now process effects backwards in time + for(auto beff = revbnds[0]; beff!=revbnds[1]; ++beff){ + auto effptr = beff->get(); + effptr->process(states[1],TimeDir::backwards); + } + // convert the innermost state into a new trajectory + auto ptraj = initTraj(states[1],fitrange); + // append forwards in time + for(auto& ieff=fwdbnds[0]; ieff != fwdbnds[1]; ++ieff) { + ieff->get()->append(*ptraj,TimeDir::forwards); + } + setStatus(ptraj); // set the status for this iteration. This compares the trajs, so it must be done before swapping out the old fit + if(status().usable()){ + // prepare for the next iteration. Update the domains + updateDomains(*ptraj); + // update the effects to reference the new trajectory + for(auto& effptr : effects_) effptr->updateReference(*ptraj); + fittraj_.swap(ptraj); + // now all effects reference the new traj: we can swap the fit to that. + if(config().plevel_ >= Config::complete)fittraj_->print(std::cout,1); + } + } + + template std::unique_ptr> Track::initTraj(FitState& state, TimeRange const& fitrange) { + // use the existing fit to define the parameterization + auto front = fittraj_->front(); + // initialize the parameters to the earliest backward processing end + front.params() = state.pData(); + // set bnom for the front piece parameters to the domain used + if(config().bfcorr_){ + // find the relevant domain + double ftime = fitrange.begin(); + for(auto const& domain : domains_) { + if(domain->range().inRange(ftime)){ + front.resetBNom(domain->bnom()); + break; + } + } + } + // set the range + front.setRange(fitrange); + return std::make_unique(front); + } + + // initialize states used before iteration + template void Track::initFitState(FitStateArray& states, TimeRange const& fitrange, double dwt) { + auto fwdtraj = fittraj_->nearestPiece(fitrange.begin()); + auto revtraj = fittraj_->nearestPiece(fitrange.end()); + // dweight the covariance, scaled by the temperature. + fwdtraj.params().covariance() *= dwt; + revtraj.params().covariance() *= dwt; + auto fwdeff = Weights(fwdtraj.params()); + auto reveff = Weights(revtraj.params()); + states[0].append(fwdeff); + states[1].append(reveff); + } + + // finalize after iteration + template void Track::setStatus(PKTRAJPTR& ptraj) { + // compute parameter difference WRT previous iteration. Compare at front and back ends + auto const& ffront = ptraj->front(); + // test covariance + auto fdiag = ffront.params().covariance().Diagonal(); + for(size_t ipar = 0; ipar < NParams(); ++ipar){ + if(fdiag[ipar] < 0.0 || std::isnan(fdiag[ipar])){ + status().status_ = Status::failed; + return; + } + } + auto const& sfront = fittraj_->nearestPiece(ffront.range().mid()); + DVEC dpfront = ffront.params().parameters() - sfront.params().parameters(); + DMAT frontwt = sfront.params().covariance(); + if(! frontwt.Invert())throw std::runtime_error("Reference covariance uninvertible"); + double dpchisqfront = ROOT::Math::Similarity(dpfront,frontwt); + // back + auto const& fback = ptraj->back(); + auto bdiag = fback.params().covariance().Diagonal(); + for(size_t ipar = 0; ipar < NParams(); ++ipar){ + if(bdiag[ipar] < 0.0 || std::isnan(bdiag[ipar])){ + status().status_ = Status::failed; + return; } - feff++; - } - // reset the fit information and process backwards - FitState backwardstate; - auto beff = effects_.rbegin(); - while(beff != effects_.rend()){ - auto ieff = beff->get(); - ieff->process(backwardstate,TimeDir::backwards); - beff++; - } - // convert the fit result into a new trajectory; start with an empty ptraj - fittraj_ = PKTRAJ(); - // process forwards, adding pieces as necessary - for(auto& ieff : effects_) { - ieff->append(fittraj_); - } - // trim the range to the physical elements (past the end sites) - feff = effects_.begin(); feff++; - double fefftime = (*feff)->time() - config().tbuff_; - beff = effects_.rbegin(); beff++; - double befftime = (*beff)->time() + config().tbuff_; - fittraj_.front().range().combine(TimeRange(fefftime,fefftime)); - fittraj_.back().range().combine(TimeRange(befftime,befftime)); - // compute parameter difference WRT reference. Compare in the middle - auto const& mtraj = fittraj_.nearestPiece(fittraj_.range().mid()); - auto const& rtraj = reftraj_.nearestPiece(fittraj_.range().mid()); - DVEC dpar = mtraj.params().parameters() - rtraj.params().parameters(); - DMAT refwt = rtraj.params().covariance(); - if(!refwt.Invert())throw std::runtime_error("Reference covariance uninvertible"); - double delta = ROOT::Math::Similarity(dpar,refwt); - double dchisq = fstat.chisq_.chisqPerNDOF() - fitStatus().chisq_.chisqPerNDOF(); - // update status. Convergence criteria is iteration-dependent. - if (delta > config().pdchi2_) { - fstat.status_ = Status::paramsdiverged; - // skip divergence comparsion in first iteration after a meta-iteration, as that - // is affected by the change in temperature - } else if (fstat.iter_ > 0 && dchisq > config().divdchisq_) { - fstat.status_ = Status::diverged; - } else if (fstat.chisq_.nDOF() < (int)config().minndof_){ - fstat.status_ = Status::lowNDOF; + } + auto const& sback = fittraj_->nearestPiece(fback.range().mid()); + DVEC dpback = fback.params().parameters() - sback.params().parameters(); + DMAT backwt = sback.params().covariance(); + if(! backwt.Invert())throw std::runtime_error("Reference covariance uninvertible"); + double dpchisqback = ROOT::Math::Similarity(dpback,backwt); + // fit chisquared change + double dchisq = config().convdchisq_ + 1e-4; // initialize to insure 0th iteration doesn't converge + if(fitStatus().iter_ > 0){ + auto prevstat = history_.rbegin(); + prevstat++; + dchisq = fitStatus().chisq_.chisqPerNDOF() - prevstat->chisq_.chisqPerNDOF(); + } + // check gap + size_t igap; + double maxgap,avggap; + ptraj-> gaps(maxgap,igap,avggap); + // test and update status + if(avggap > config().divgap_ ) { + status().status_ = Status::gapdiverged; + } else if (dpchisqfront > config().pdchisq_ || dpchisqback > config().pdchisq_) { + status().status_ = Status::paramsdiverged; + } else if (dchisq > config().divdchisq_) { + status().status_ = Status::chisqdiverged; + } else if (status().chisq_.nDOF() < (int)config().minndof_){ + status().status_ = Status::lowNDOF; } else if(fabs(dchisq) < config().convdchisq_) { - fstat.status_ = Status::converged; + status().status_ = Status::converged; } else - fstat.status_ = Status::unconverged; + status().status_ = Status::unconverged; } - // update between iterations - template void Track::update(Status const& fstat, MetaIterConfig const& miconfig) { - if(fittraj_.pieces().size() > 0)reftraj_ = fittraj_; // swap if this isn't the 1st fit - if(fstat.iter_ < 0) { // 1st iteration of a meta-iteration: update the state - for(auto& ieff : effects_ ) ieff->update(reftraj_,miconfig); - } else { - for(auto& ieff : effects_) ieff->update(reftraj_); + template bool Track::setBounds(KKEFFFWDBND& fwdbnds,KKEFFREVBND& revbnds) { + // find the bounding sites for algebraic processing this iteraiton. This excludes effects which can't change the fit + // find first measurement + bool retval(false); + fwdbnds[0] = effects_.end(); + for(auto ieff=effects_.begin();ieff!=effects_.end();++ieff){ + auto const* kkmeas = dynamic_cast(ieff->get()); + if(kkmeas != 0 && kkmeas->active()){ + fwdbnds[0] = ieff; + revbnds[1] = KKEFFREV(ieff); + break; + } + } + if(fwdbnds[0] != effects_.end()){ + retval = true; + // now the last measurement + for(auto ieff=effects_.rbegin();ieff!=effects_.rend();++ieff){ + auto const* kkmeas = dynamic_cast(ieff->get()); + if(kkmeas != 0 && kkmeas->active()){ + revbnds[0] = ieff; + fwdbnds[1] = ieff.base(); + break; + } + } + } + return retval; + } + + template void Track::updateDomains(PKTRAJ const& ptraj) { + for(auto& domain : domains_) { + domain->updateBNom(bfield_.fieldVect(ptraj.position3(domain->mid()))); } - // sort the effects by time - std::sort(effects_.begin(),effects_.end(),KKEFFComp ()); + } + + template bool Track::extendDomains(TimeRange const& fitrange) { + bool retval(false); + // then, check if the range has + TimeRange drange(domains().begin()->get()->begin(),domains().rbegin()->get()->end()); + if(!drange.contains(fitrange)){ + retval = true; + // we need to extend the domains. First backwards + if(drange.begin() > fitrange.begin()){ + double time = drange.begin(); + while(time > fitrange.begin()){ + auto const& ktraj = fittraj_->nearestPiece(time); + double dt = bfield_.rangeInTolerance(ktraj,time,config().tol_); + TimeRange range(time-dt,time); + Domain domain(range,bfield_.fieldVect(ktraj.position3(range.mid()))); + addDomain(domain,TimeDir::backwards); + time = domain.begin(); + } + } + // then forwards + if(drange.end() < fitrange.end()){ + double time = drange.end(); + while(time < fitrange.end()){ + auto const& ktraj = fittraj_->nearestPiece(time); + double dt = bfield_.rangeInTolerance(ktraj,time,config().tol_); + TimeRange range(time,time+dt); + Domain domain(range,bfield_.fieldVect(ktraj.position3(range.mid()))); + addDomain(domain,TimeDir::forwards); + time = domain.end(); + } + } + } + return retval; + } + + template void Track::processEnds() { + // sort effects in case ends have migrated + effects_.sort(KKEFFComp()); + // update domains as needed to cover the end effects + if(config().bfcorr_){ + TimeRange endrange(effects_.front()->time(),effects_.back()->time()); + extendDomains(endrange); + } + KKEFFFWDBND fwdbnds; // bounding sites used for fitting + KKEFFREVBND revbnds; + if(!setBounds(fwdbnds,revbnds)){ + status().status_ = Status::lowNDOF; + return; + } + // initialize the fit state where we left off processing + FitStateArray states; + TimeRange fitrange(fwdbnds[0]->get()->time(),revbnds[0]->get()->time()); + initFitState(states, fitrange, 1.0); // no deweighting + // process forwards and backwards + for(auto feff=fwdbnds[1]; feff != effects_.end(); ++feff) + feff->get()->process(states[1],TimeDir::forwards); + for(auto reff=revbnds[1]; reff != effects_.rend(); ++reff) + reff->get()->process(states[0],TimeDir::backwards); + // finally, append the effects to the trajectory, using these states + // skip any states that migrated to an unprocessed region + for(auto feff=fwdbnds[1]; feff != effects_.end(); ++feff) + if(feff->get()->time() > fittraj_->back().range().begin())feff->get()->append(*fittraj_,TimeDir::forwards); + for(auto reff=revbnds[1]; reff != effects_.rend(); ++reff) + if(reff->get()->time() < fittraj_->front().range().rbegin())reff->get()->append(*fittraj_,TimeDir::backwards); } template bool Track::canIterate() const { @@ -418,41 +736,150 @@ namespace KinKal { fitTraj().print(ost,detail); if(detail > Config::basic) { ost << " Reference "; - refTraj().print(ost,detail-2); + fitTraj().print(ost,detail-2); } if(detail > Config::complete) { ost << " Effects " << endl; for(auto const& eff : effects()) eff.get()->print(ost,detail-3); } } - // divide a trajectory into magnetic 'domains' used to apply the BField corrections - template void Track::createDomains(PKTRAJ const& pktraj, TimeRange const& range, std::vector& ranges, - TimeDir tdir) const { - double tstart; - tstart = range.begin(); - do { - // see how far we can go on the current traj before the BField change causes the momentum estimate to go out of tolerance - auto const& ktraj = pktraj.nearestPiece(tstart); - double tend = bfield_.rangeInTolerance(ktraj,tstart,config().tol_); - ranges.emplace_back(tstart,tend); - // start the next domain and the end of this one - tstart = tend; - } while(tstart < range.end()); + // divide a trajectory into magnetic 'domains' used to apply the DomainWall corrections + template bool Track::createDomains(PKTRAJ const& ptraj, TimeRange const& range, DOMAINCOL& domains) const { + bool retval(true); + if(config().bfcorr_ ) { + auto const& ktraj = ptraj.nearestPiece(range.begin()); + // catch exceptions if the fit extends beyond the range of the field map + try { + double trange = bfield_.rangeInTolerance(ktraj,range.begin(),config().tol_); + // define 1st domain to have the 1st effect in the middle. This avoids effects having exactly the same time + double tstart = range.begin() - 0.5*trange; + do { + // see how far we can go on the current traj before the DomainWall change causes the momentum estimate to go out of tolerance + // note this assumes the trajectory is accurate (geometric extrapolation only) + auto const& ktraj = ptraj.nearestPiece(tstart); + trange = bfield_.rangeInTolerance(ktraj,tstart,config().tol_); + domains.emplace(std::make_shared(tstart,trange,bfield_.fieldVect(ktraj.position3(tstart+0.5*trange)))); + // start the next domain at the end of this one + tstart += trange; + } while(tstart < range.end() + 0.5*trange); // ensure the last domain fully covers the last effect + } catch (std::exception const& error) { + retval = false; + } + } + return retval; } - template TimeRange Track::getRange(HITCOL& hits, EXINGCOL& exings) const { + template TimeRange Track::detectorRange(HITCOL& hits, EXINGCOL& exings,bool active) { double tmin = std::numeric_limits::max(); double tmax = -std::numeric_limits::max(); + // can't assume effects are sorted for(auto const& hit : hits){ - tmin = std::min(tmin,hit->time()); - tmax = std::max(tmax,hit->time()); + if((!active) || hit->active()){ + tmin = std::min(tmin,hit->time()); + tmax = std::max(tmax,hit->time()); + } } for(auto const& exing : exings){ - tmin = std::min(tmin,exing->crossingTime()); - tmax = std::max(tmax,exing->crossingTime()); + if((!active) || exing->active() ){ + tmin = std::min(tmin,exing->time()); + tmax = std::max(tmax,exing->time()); + } } - // add a buffer to the time. This must cover the uncertainty on t0 as the fit iterates - return TimeRange(tmin-config().tbuff_,tmax+config().tbuff_); + return TimeRange(tmin,tmax); + } + + template template bool Track::extrapolate(TimeDir tdir, XTEST const& xtest) { + bool retval = fitStatus().usable(); + if(retval){ + if(config().bfcorr_){ + // test for extrapolation outside the bfield map range + try { + // iterate until the extrapolation condition is met + double time = tdir == TimeDir::forwards ? domains_.crbegin()->get()->end() : domains_.cbegin()->get()->begin(); + double tstart = time; + while(fabs(time-tstart) < xtest.maxDt() && xtest.needsExtrapolation(*fittraj_,tdir) ){ + // create a domain for this extrapolation + auto const& ktraj = fittraj_->nearestPiece(time); + double dt = bfield_.rangeInTolerance(ktraj,time,xtest.dpTolerance()); // always positive + TimeRange range = tdir == TimeDir::forwards ? TimeRange(time,time+dt) : TimeRange(time-dt,time); + Domain domain(range,bfield_.fieldVect(ktraj.position3(range.mid()))); + addDomain(domain,tdir,true); // use exact transport + time = tdir == TimeDir::forwards ? domain.end() : domain.begin(); + } + } catch (std::exception const& error) { + history_.push_back(Status(0)); + status().status_ = Status::outsidemap; + status().comment_ = std::string("Extrapolation error"); + retval = false; + } + retval = true; + } else { + retval = true; + } + } + return retval; + } + + template void Track::addDomain(Domain const& domain,TimeDir const& tdir,bool exact) { + auto dptr = std::make_shared(domain); + if(tdir == TimeDir::forwards){ + auto const& prevdom = *domains_.crbegin(); + auto const& ktraj = fittraj_->nearestPiece(prevdom->end()); + auto& dw = effects_.emplace_back(std::make_unique(prevdom,dptr,ktraj)); + if(exact){ + dw->extrapolate(*fittraj_,tdir); + } else { + FitState fstate(ktraj.params()); + effects_.back()->process(fstate,tdir); + effects_.back()->append(*fittraj_,tdir); + } + } else { + auto const& nextdom = *domains_.cbegin(); + auto const& ktraj = fittraj_->nearestPiece(nextdom->begin()); + auto& dw = effects_.emplace_front(std::make_unique(dptr,nextdom,ktraj)); + if(exact){ + dw->extrapolate(*fittraj_,tdir); + } else { + FitState fstate(ktraj.params()); + effects_.front()->process(fstate,tdir); + effects_.front()->append(*fittraj_,tdir); + } + } + domains_.insert(dptr); + } + + template bool Track::extrapolate(EXINGPTR const& exingptr,TimeDir const& tdir) { + bool retval = fitStatus().usable(); + if(retval){ + if(tdir == TimeDir::forwards){ + // make sure the time is legal + retval = fittraj_->back().range().inRange(exingptr->time()); + if(retval){ + auto& exmat = effects_.emplace_back(std::make_unique(exingptr,*fittraj_)); + exmat->extrapolate(*fittraj_,tdir); + } + } else { + retval = fittraj_->front().range().inRange(exingptr->time()); + if(retval){ + auto& exmat = effects_.emplace_front(std::make_unique(exingptr,*fittraj_)); + exmat->extrapolate(*fittraj_,tdir); + } + } + } + return retval; + } + + + template TimeRange Track::activeRange() const { + double tmin = std::numeric_limits::max(); + double tmax = -std::numeric_limits::max(); + // can't assume effects are sorted + for(auto const& hit : hits_){ + tmin = std::min(tmin,hit->time()); + tmax = std::max(tmax,hit->time()); + } + return TimeRange(tmin,tmax); } } + #endif diff --git a/Fit/TrackEnd.hh b/Fit/TrackEnd.hh deleted file mode 100644 index 02f5d042..00000000 --- a/Fit/TrackEnd.hh +++ /dev/null @@ -1,115 +0,0 @@ -#ifndef KinKal_TrackEnd_hh -#define KinKal_TrackEnd_hh -// -// End of the track effect. This initiates the fit and trajectory building -// -#include "KinKal/Fit/Effect.hh" -#include "KinKal/Fit/Config.hh" -#include "KinKal/General/BFieldMap.hh" -#include -#include -#include - -namespace KinKal { - template class TrackEnd : public Effect { - public: - using KKEFF = Effect; - using PKTRAJ = ParticleTrajectory; - // provide interface - void update(PKTRAJ const& ref) override; - void update(PKTRAJ const& ref, MetaIterConfig const& miconfig) override { - vscale_ = miconfig.varianceScale(); // annealing scale for covariance deweighting, to avoid numerical effects - return update(ref); } - void update(Config const& config) override { - dwt_ = config.dwt_; - bfcorr_ = config.bfcorr_; - }; - double time() const override { return (tdir_ == TimeDir::forwards) ? -std::numeric_limits::max() : std::numeric_limits::max(); } // make sure this is always at the end - bool active() const override { return true; } - void process(FitState& kkdata,TimeDir tdir) override; - void append(PKTRAJ& fit) override; - void print(std::ostream& ost=std::cout,int detail=0) const override; - virtual ~TrackEnd(){} - // accessors - TimeDir const& tDir() const { return tdir_; } - double deWeighting() const { return dwt_; } - KTRAJ const& endTraj() const { return endtraj_; } - Weights const& endEffect() const { return endeff_; } - - // construct from trajectory and direction. Deweighting must be tuned to balance stability vs bias - TrackEnd(Config const& config, BFieldMap const& bfield, PKTRAJ const& pktraj,TimeDir tdir); - // disallow - TrackEnd() = delete; - TrackEnd(TrackEnd const& other) = delete; - TrackEnd& operator =(TrackEnd const& other) = delete; - private: - double dwt_; - bool bfcorr_; - BFieldMap const& bfield_; // BField; needed to define reference - TimeDir tdir_; // direction for this effect; note the early end points forwards, the late backwards - VEC3 bnom_; // nominal BField - double vscale_; // variance scale (from annealing) - Weights endeff_; // wdata representation of this effect's constraint/measurement - KTRAJ endtraj_; // cache of parameters at the end of processing this direction, used in traj creation - }; - - template TrackEnd::TrackEnd(Config const& config, BFieldMap const& bfield, PKTRAJ const& pktraj, TimeDir tdir) : - dwt_(config.dwt_), bfcorr_(config.bfcorr_), bfield_(bfield), tdir_(tdir) , vscale_(1.0), - endtraj_(tdir == TimeDir::forwards ? pktraj.front() : pktraj.back()){ - update(pktraj); - } - - template void TrackEnd::process(FitState& kkdata,TimeDir tdir) { - if(tdir == tdir_) - // start the fit with the de-weighted info cached from the previous iteration or seed - kkdata.append(endeff_); - else - // at the opposite end, cache the final parameters - endtraj_.params() = kkdata.pData(); - KKEFF::setState(tdir,KKEFF::processed); - } - - template void TrackEnd::update(PKTRAJ const& ref) { - auto refend = (tdir_ == TimeDir::forwards) ? ref.front().params() : ref.back().params(); - refend.covariance() *= (dwt_/vscale_); - // convert this to a weight (inversion) - endeff_ = Weights(refend); - // set the range; this should buffer the original traj - endtraj_.setRange(ref.range()); - // update BField reference - double endtime = (tdir_ == TimeDir::forwards) ? ref.range().begin() : ref.range().end(); - bnom_ = bfield_.fieldVect(ref.position3(endtime)); - KKEFF::updateState(); - } - - template void TrackEnd::append(PKTRAJ& fit) { - // Test the fit is empty and we're going in the right direction - if(tdir_ == TimeDir::forwards) { - if(fit.pieces().size() == 0){ - // take the end cache and seed the fit with it - // if we're using local BField, update accordingly - if(bfcorr_ ) endtraj_.setBNom(endtraj_.range().begin(),bnom_); - // append this to the (empty) fit - fit.append(endtraj_); - } else - throw std::invalid_argument("Input ParticleTrajectory isn't empty"); - } - } - - template void TrackEnd::print(std::ostream& ost,int detail) const { - ost << "TrackEnd " << static_castconst&>(*this) << " direction " << tDir() << " deweight " << deWeighting() << std::endl; - ost << "EndTraj "; - endTraj().print(ost,detail); - if(detail > 0){ - ost << "EndWeight " << endeff_ << std::endl; - } - } - - template std::ostream& operator <<(std::ostream& ost, TrackEnd const& kkend) { - kkend.print(ost,0); - return ost; - } - - -} -#endif diff --git a/General/BFieldMap.hh b/General/BFieldMap.hh index 6e2cd7a9..8ed493db 100644 --- a/General/BFieldMap.hh +++ b/General/BFieldMap.hh @@ -56,7 +56,7 @@ namespace KinKal { } // estimate how long the momentum vector from the given trajectory will stay within the given (fractional) tolerance given the field spatial variation - // ie mag(P_true(tstart+dt) - P_traj(tstart+dt)) < tol. This is good to 1st order (ignores trajectory curvature) + // ie mag(P_true(tstart+dt) - P_traj(tstart+dt)) < tol. This is a 2nd order local calculation template double BFieldMap::rangeInTolerance(KTRAJ const& ktraj, double tstart, double tol) const { auto tpos = ktraj.position3(tstart); // starting position double dp = ktraj.momentum(tstart)*tol; // fractional tolerance on momentum @@ -64,9 +64,9 @@ namespace KinKal { auto dBdt = fieldDeriv(tpos,vel); // change in field WRT time along this velocity double d2pdt2 = (dBdt.Cross(vel)).R()*cbar()*fabs(ktraj.charge()); // 2nd derivative of momentum due to B change along the path if(d2pdt2 > 1e-10) - return tstart + sqrt(dp/d2pdt2); + return sqrt(dp/d2pdt2); else - return ktraj.range().end(); + return ktraj.range().range(); } // trivial instance of the above, used for testing diff --git a/General/CMakeLists.txt b/General/CMakeLists.txt index b7bbf71e..556b054b 100644 --- a/General/CMakeLists.txt +++ b/General/CMakeLists.txt @@ -12,7 +12,12 @@ add_library(General SHARED Weights.cc BFieldMap.cc AxialBFieldMap.cc - ) + InterpBilinear.cc + CylBMap.cc + CylBFieldMap.cc + MomBasis.cc + CloneContext.cc +) # create shared library with ROOT dict ROOT_GENERATE_DICTIONARY(GeneralDict @@ -22,12 +27,16 @@ ROOT_GENERATE_DICTIONARY(GeneralDict FitData.hh TimeRange.hh Vectors.hh + PhysicalConstants.h + SystemOfUnits.h LINKDEF LinkDef.h MODULE General ) # set top-level directory as include root -target_include_directories(General PRIVATE ${PROJECT_SOURCE_DIR}/..) +target_include_directories(General PUBLIC + $ + $) # link this library with ROOT libraries target_link_libraries(General ${ROOT_LIBRARIES}) diff --git a/General/Chisq.hh b/General/Chisq.hh index cbcce6eb..82b05d6e 100644 --- a/General/Chisq.hh +++ b/General/Chisq.hh @@ -9,7 +9,7 @@ namespace KinKal { class Chisq { public: // default constructor - Chisq() : chisq_(0.0), ndof_(0) {} + Chisq(int nparams=0) : chisq_(0.0), ndof_(-nparams) {} Chisq(double chisq, unsigned ndof) : chisq_(chisq), ndof_(ndof) {} double chisq() const { return chisq_; } int nDOF() const { return ndof_; } diff --git a/General/CloneContext.cc b/General/CloneContext.cc new file mode 100644 index 00000000..d483b28e --- /dev/null +++ b/General/CloneContext.cc @@ -0,0 +1,10 @@ +// Ed Callaghan +// Memoization context for reinstantiating objects with graph relations +// August 2025 + +#include "KinKal/General/CloneContext.hh" + +void CloneContext::clear(){ + this->map_.clear(); +} + diff --git a/General/CloneContext.hh b/General/CloneContext.hh new file mode 100644 index 00000000..361b2a98 --- /dev/null +++ b/General/CloneContext.hh @@ -0,0 +1,48 @@ +// Ed Callaghan +// Memoization context for reinstantiating objects with graph relations +// August 2025 + +#ifndef KinKal_CloneContext_hh +#define KinKal_CloneContext_hh + +#include +#include + +class CloneContext{ + public: + // compiler defaults propagate to underlying stl-map + CloneContext() = default; + ~CloneContext() = default; + // disallow copy-constructor so as to prevent bugs stemming from + // failure to propagate instantiations across references to context + CloneContext(CloneContext const&) = delete; + + // standard interface: + // - if first call for domain ptr, instantiate a clone + // - return ptr to clone + template + std::shared_ptr get(const std::shared_ptr&); + + void clear(); + + protected: + // map of raw address to stl-pointers + std::unordered_map > map_; + + private: + /**/ +}; + +template +std::shared_ptr CloneContext::get(const std::shared_ptr& ptr){ + void* address = static_cast(ptr.get()); + if (this->map_.count(address) < 1){ + auto cloned = ptr->clone(*this); + this->map_[address] = static_pointer_cast(cloned); + } + auto mapped = this->map_[address]; + auto rv = static_pointer_cast(mapped); + return rv; +} + +#endif diff --git a/General/CylBFieldMap.cc b/General/CylBFieldMap.cc new file mode 100644 index 00000000..281090d5 --- /dev/null +++ b/General/CylBFieldMap.cc @@ -0,0 +1,69 @@ +#include "KinKal/General/CylBFieldMap.hh" + +namespace KinKal{ + using VEC = std::vector; + using MAT = std::vector>; + + CylBFieldMap::CylBFieldMap(std::string const& file) + : B_dat(file), Br_interp(B_dat.r_, B_dat.z_, B_dat.Br_), + Bz_interp(B_dat.r_, B_dat.z_, B_dat.Bz_) {} + + VEC3 CylBFieldMap::fieldVect(VEC3 const& position) const { + // calcualte interpolated field value at position, in cartesian coordinates + double Bz = Bz_interp.interp(position.Rho(), position.Z()); + double Br = Br_interp.interp(position.Rho(), position.Z()); + // handle case where R=0 -- R=X + if (position.Rho() < 1e-6) { + return VEC3(Br, 0., Bz); + } + else { + return VEC3(Br*position.X()/position.Rho(), Br*position.Y()/position.Rho(), Bz); + } + } + + CylBFieldMap::Grad CylBFieldMap::fieldGrad(VEC3 const& position) const { + // calculate gradient matrix at position, in cartesian coordinates + // calculate needed field and gradient values + double Br = Br_interp.interp(position.Rho(), position.Z()); + VEC dBz = Bz_interp.gradient(position.Rho(), position.Z()); + VEC dBr = Br_interp.gradient(position.Rho(), position.Z()); + // vector to fill, will instantiate final Grad + VEC dBi(9); + // handle case where R=0 -- R=X, dR/dX=1, dR/dY=0 + if (position.Rho() < 1e-6) { + dBi = VEC{dBr[0], 0., dBr[1], + 0.,0.,0., + dBz[0], 0., dBz[1]}; + } + else { + // store values that are calculated multiple times + double xr=position.X()/position.Rho(), yr=position.Y()/position.Rho(); + double xryr = xr*yr, xrxr=xr*xr, yryr=yr*yr; + double rinv = 1/position.Rho(); + dBi = VEC{dBr[0]*xrxr + Br * rinv*(1 - xrxr), + dBr[0]*xryr + Br * (- xryr * rinv), + xr * dBr[1], + dBr[0]*xryr + Br * (- xryr * rinv), + dBr[0]*yryr + Br * rinv*(1 - yryr), + yr * dBr[1], + dBz[0]*xr,dBz[0]*yr,dBz[1]}; + } + Grad retval(dBi.begin(), dBi.end()); + return retval; + } + + VEC3 CylBFieldMap::fieldDeriv(VEC3 const& position, VEC3 const& velocity) const { + Grad grad = fieldGrad(position); + VEC3 retval(grad[0][0]*velocity.X()+grad[0][1]*velocity.Y()+grad[0][2]*velocity.Z(), + grad[1][0]*velocity.X()+grad[1][1]*velocity.Y()+grad[1][2]*velocity.Z(), + grad[2][0]*velocity.X()+grad[2][1]*velocity.Y()+grad[2][2]*velocity.Z()); + return retval; + } + + bool CylBFieldMap::inRange(VEC3 const& position) const { + // check if position is in range of the map values + bool rrange = (position.R() >= 0.) && (position.R() <= rMax()); + bool zrange = (position.Z() >= zMin()) && (position.Z() <= zMax()); + return rrange && zrange; + } +} diff --git a/General/CylBFieldMap.hh b/General/CylBFieldMap.hh new file mode 100644 index 00000000..d5a3440b --- /dev/null +++ b/General/CylBFieldMap.hh @@ -0,0 +1,40 @@ +#ifndef KinKal_CylBFieldMap_hh +#define KinKal_CylBFieldMap_hh + +#include "KinKal/General/CylBMap.hh" +#include "KinKal/General/InterpBilinear.hh" +#include "KinKal/General/BFieldMap.hh" + +namespace KinKal { + class CylBFieldMap : public BFieldMap { + public: + using VEC = std::vector; + using MAT = std::vector>; + CylBFieldMap(std::string const& file); // supply data file + VEC3 fieldVect(VEC3 const& position) const override; + Grad fieldGrad(VEC3 const& position) const override; + VEC3 fieldDeriv(VEC3 const& position, VEC3 const& velocity) const override; + bool inRange(VEC3 const& position) const override; + // TO DO! + void print(std::ostream& os=std::cout) const override { + os << "Cylindrically symmetric Bfield with boundaries z=[" << zMin() << ", " << zMax() << "] and r=[" + << rMin() << ", " << rMax() << "] with " << B_dat.ntot_ + << " field values from (lower left, upper left): Br=(" << B_dat.Br_.front().front() << ", " + << B_dat.Br_.back().back() << ") Tesla and Bz=(" << B_dat.Bz_.front().front() << ", " + << B_dat.Bz_.back().back() << ") Tesla" << std::endl; + } + virtual ~CylBFieldMap(){} + // disallow copy and equivalence + CylBFieldMap(CylBFieldMap const& ) = delete; + CylBFieldMap& operator =(CylBFieldMap const& ) = delete; + // getters + double zMax() const { return B_dat.z_.back(); } + double zMin() const { return B_dat.z_.front(); } + double rMax() const { return B_dat.r_.back(); } + double rMin() const { return B_dat.r_.front(); } + private: + const CylBMap B_dat; + const InterpBilinear Br_interp, Bz_interp; + }; +} +#endif diff --git a/General/CylBMap.cc b/General/CylBMap.cc new file mode 100644 index 00000000..243187bc --- /dev/null +++ b/General/CylBMap.cc @@ -0,0 +1,119 @@ +#include "KinKal/General/CylBMap.hh" + +#include +#include +#include + +namespace KinKal{ + + double parse_param_double(const std::string& line, const std::string& substring) { + // split up grid parameter expected to be a double + std::size_t i = line.find(substring)+substring.size(); + std::size_t j = line.find(' ', i); + std::size_t d = j-i; + return std::stod(line.substr(i, d)); + } + + int parse_param_int(const std::string& line, const std::string& substring) { + // split up grid parameter expected to be an int + std::size_t i = line.find(substring)+substring.size(); + std::size_t j = line.find(' ', i); + std::size_t d = j-i; + return std::stoi(line.substr(i, d)); + } + + CylBMap::CylBMap(const std::string& file) { + // read in data from a file + ntot_ = 0; // track number of lines + std::string line; + std::ifstream stream(file); + while (std::getline(stream, line)) + ++ntot_; + stream.close(); + // loop through again, saving data + // first sift through header + stream.open(file); + std::string const data_flag="data"; + std::string const grid_flag="grid"; + double R0=0., Z0=0., dR=0., dZ=0.; + int nR=0, nZ=0; + bool grid_parsed=false; + while (line.substr(0, 4) != data_flag) + { + // get the next line + std::getline(stream, line); + // parse grid, if parameters in line + if (line.substr(0, 4) == grid_flag) + { + if (grid_parsed) throw std::invalid_argument("multiple grid parameter lines found!"); + R0 = parse_param_double(line, "R0="); + Z0 = parse_param_double(line, "Z0="); + nR = parse_param_int(line, "nR="); + nZ = parse_param_int(line, "nZ="); + dR = parse_param_double(line, "dR="); + dZ = parse_param_double(line, "dZ="); + grid_parsed=true; + } + // decrease data count + --ntot_; + } + // throw if no grid parameters found + if (!grid_parsed) throw std::invalid_argument("no grid parameters found in data file!"); + // construct r and z vectors from grid parameters + m_ = nR; + n_ = nZ; + int ntot_exp = m_*n_; + if (ntot_ != ntot_exp) throw std::invalid_argument("number of data does not match expected, based on grid parameters"); + r_.resize(m_); + z_.resize(n_); + // set r and z from grid parameters + for (int i=0; i> temp; + r_temp = temp; + iss >> temp; + z_temp = temp; + // grab field values + iss >> temp; + Br_[i][j] = temp; + iss >> temp; + Bz_[i][j] = temp; + // check that r and z grid values are correct + delta_pos = pow(pow(r_temp-r_[i], 2) + pow(z_temp-z_[j], 2), 0.5); + if(std::abs(delta_pos) > 1e-5) throw std::invalid_argument("field spacing isn't uniform! dist="+ + std::to_string(delta_pos)+" for line i="+ std::to_string(i)+", j="+std::to_string(j)+ + ", step="+std::to_string(step)); + // increment + step++; + } + // Shrink to fit -- fixes issues with passing in values to InterpBilinear + r_.shrink_to_fit(), z_.shrink_to_fit(); + Br_.shrink_to_fit(), Bz_.shrink_to_fit(); + // shrink inner vectors + for (int i=0; i +#include + +namespace KinKal { + class CylBMap { + public: + using VEC = std::vector; + using MAT = std::vector>; + CylBMap(const std::string& file); + private: + int m_, n_, ntot_; + VEC r_, z_; + MAT Br_, Bz_; + friend class CylBFieldMap; + }; +} +#endif diff --git a/General/FitData.hh b/General/FitData.hh index c3816df8..9cd0cd1c 100644 --- a/General/FitData.hh +++ b/General/FitData.hh @@ -19,6 +19,8 @@ namespace KinKal { FitData() {} // copy with optional inversion FitData(FitData const& tdata, bool inv=false) : vec_(tdata.vec_), mat_(tdata.mat_) { if (inv) invert(); } + FitData& operator=(const FitData &) = default; + ~FitData() = default; // accessors DVEC const& vec() const { return vec_; } DMAT const& mat() const { return mat_; } @@ -50,6 +52,12 @@ namespace KinKal { mat_ += other.mat(); return *this; } + bool operator == (FitData const& other) { + return vec_ == other.vec() && mat_ == other.mat(); + } + bool operator != (FitData const& other) { + return vec_ != other.vec() && mat_ != other.mat(); + } private: DVEC vec_; // parameters DMAT mat_; // parameter covariance diff --git a/General/InterpBilinear.cc b/General/InterpBilinear.cc new file mode 100644 index 00000000..8e0964f5 --- /dev/null +++ b/General/InterpBilinear.cc @@ -0,0 +1,48 @@ +#include "KinKal/General/InterpBilinear.hh" + +#include + +namespace KinKal { + using VEC = std::vector; + using MAT = std::vector>; + + double InterpBilinear::interp(double x1p, double x2p) const { + // interpolate values using bilinear interpolation formula at a point x1p, x2p + int i, j; + double yy, t, u; + // find i & j of point at lower left corner of square that encloses x1p,x2p + i = (int)((x1p-x1[0])/dx1_); + j = (int)((x2p-x2[0])/dx2_); + // make sure in available index range + i = std::max(0, std::min(m_-2, i)); + j = std::max(0, std::min(n_-2, j)); + // calculate fractional location of x1p,x2p in square of points + t = (x1p-x1[i])/dx1_; + u = (x2p-x2[j])/dx2_; + // bilinear interpolation + yy = (1.-t)*(1.-u)*y[i][j] + t*(1.-u)*y[i+1][j] + (1.-t)*u*y[i][j+1] + t*u*y[i+1][j+1]; + return yy; + } + + VEC InterpBilinear::gradient(double x1p, double x2p) const { + // calculate derivative of interpolation function at a point x1p, x2p + int i, j; + double t, u, dtx1, dux2; + VEC dyxi(2); + // find i & j of point at lower left corner of square that encloses x1p,x2p + i = (int)((x1p-x1[0])/dx1_); + j = (int)((x2p-x2[0])/dx2_); + // make sure in available index range + i = std::max(0, std::min(m_-2, i)); + j = std::max(0, std::min(n_-2, j)); + // calculate fractional location of x1p,x2p in square of points + t = (x1p-x1[i])/dx1_; + u = (x2p-x2[j])/dx2_; + // calculate numerical derivatives + dtx1 = 1./(x1[i+1]-x1[i]); + dux2 = 1./(x2[j+1]-x2[j]); + dyxi[0] = dtx1 * ((1.-u)*(y[i+1][j]-y[i][j]) + u*(y[i+1][j+1]-y[i][j+1])); + dyxi[1] = dux2 * ((1.-t)*(y[i][j+1]-y[i][j]) + t*(y[i+1][j+1]-y[i+1][j])); + return dyxi; + } +} diff --git a/General/InterpBilinear.hh b/General/InterpBilinear.hh new file mode 100644 index 00000000..32f5d038 --- /dev/null +++ b/General/InterpBilinear.hh @@ -0,0 +1,23 @@ +#ifndef KinKal_InterpBilinear_hh +#define KinKal_InterpBilinear_hh +// class to do a 2D interpolation, which is utilized by the CylBFieldMap interface. +#include + +namespace KinKal { + class InterpBilinear { + public: + using VEC = std::vector; + using MAT = std::vector; + InterpBilinear(const VEC &x1v, const VEC &x2v, const MAT &ym) + : m_(x1v.size()), n_(x2v.size()), dx1_(x1v[1]-x1v[0]), dx2_(x2v[1]-x2v[0]), x1(x1v), x2(x2v), y(ym) {} + double interp(double x1p, double x2p) const; + // the gradient vector of the interpolating function, e.g. dy/dx_i + VEC gradient(double x1p, double x2p) const; + private: + const int m_, n_; + const double dx1_, dx2_; + const VEC &x1, &x2; + const MAT &y; + }; +} +#endif diff --git a/General/MomBasis.cc b/General/MomBasis.cc new file mode 100644 index 00000000..a5db4f83 --- /dev/null +++ b/General/MomBasis.cc @@ -0,0 +1,25 @@ +#include "KinKal/General/MomBasis.hh" +#include +namespace KinKal { + VEC3 MomBasis::direction(Direction tdir, VEC3 momdir) { + VEC3 retval; + if(tdir == momdir_){ + retval = momdir.Unit(); + } else { + // check the direction; the results are undefined for momdir + double perp2 = momdir.Perp2(); + if(perp2 <= 0.0)throw std::invalid_argument("Perpendicular directions when momentum direction is along Z are undefined"); + if( tdir == phidir_){ + VEC3 phidir(momdir.Y(),-momdir.X(),0.0); + retval = phidir.Unit(); + } else if( tdir == perpdir_){ + VEC3 perpdir = momdir; + perpdir.SetZ(-perp2/momdir.Z()); + retval = perpdir.Unit(); + } else { + throw std::invalid_argument("Unknown Direction" ); + } + } + return retval; + } +} diff --git a/General/MomBasis.hh b/General/MomBasis.hh index 733b663d..d63add73 100644 --- a/General/MomBasis.hh +++ b/General/MomBasis.hh @@ -5,6 +5,7 @@ // the momentum and Z, perpdir_ is perpendicular to momdir_ and perpdir_. This basis is used to define directions and // derivatives in the kinematic kalman fit // +#include "KinKal/General/Vectors.hh" #include namespace KinKal { struct MomBasis { @@ -21,6 +22,7 @@ namespace KinKal { return std::string("Unknown"); } } + static VEC3 direction(Direction tdir, VEC3 momdir); }; } #endif diff --git a/General/ParticleState.cc b/General/ParticleState.cc index dfd211bd..043b2e5d 100644 --- a/General/ParticleState.cc +++ b/General/ParticleState.cc @@ -15,3 +15,8 @@ namespace KinKal { string const& ParticleState::stateUnit(size_t index) { return stateUnits_[index];} string const& ParticleState::stateTitle(size_t index) { return stateTitles_[index];} } + +std::ostream& operator <<(std::ostream& ost, KinKal::ParticleState const& pstate){ + ost << "ParticleState position " << pstate.position4() << " momentum " << pstate.momentum4(); + return ost; +} diff --git a/General/ParticleState.hh b/General/ParticleState.hh index 5ccf246b..1f575983 100644 --- a/General/ParticleState.hh +++ b/General/ParticleState.hh @@ -8,6 +8,7 @@ #include "Math/SVector.h" #include #include +#include namespace KinKal { @@ -50,4 +51,5 @@ namespace KinKal { const static std::vector stateUnits_; }; } +std::ostream& operator <<(std::ostream& ost, KinKal::ParticleState const& pstate); #endif diff --git a/General/ParticleStateEstimate.cc b/General/ParticleStateEstimate.cc index 62a330d6..e6019deb 100644 --- a/General/ParticleStateEstimate.cc +++ b/General/ParticleStateEstimate.cc @@ -1,8 +1,29 @@ #include "KinKal/General/ParticleStateEstimate.hh" +#include namespace KinKal { double ParticleStateEstimate::momentumVariance() const { auto momdir = momentum3().Unit(); DVEC dMdm(0.0, 0.0, 0.0, momdir.X(), momdir.Y(), momdir.Z()); return ROOT::Math::Similarity(dMdm,scovar_); } + + double ParticleStateEstimate::positionVariance(MomBasis::Direction dir) const { + if(dir == MomBasis::momdir_)throw std::invalid_argument("Variance along momentum direction is undefined"); + auto momdir = momentum3(); + auto pdir = MomBasis::direction(dir,momdir); + DVEC dPdp(pdir.X(),pdir.Y(),pdir.Z(),0.0, 0.0, 0.0); + return ROOT::Math::Similarity(dPdp,scovar_); + } + + PMAT ParticleStateEstimate::planeCovariance() const { + auto momdir = momentum3(); + auto udir = MomBasis::direction(MomBasis::perpdir_,momdir); + auto vdir = MomBasis::direction(MomBasis::phidir_,momdir); + SVEC6 uvec(udir.X(),udir.Y(),udir.Z(),0.0, 0.0, 0.0); + SVEC6 vvec(vdir.X(),vdir.Y(),vdir.Z(),0.0, 0.0, 0.0); + PPMAT ppmat; + ppmat.Place_in_row(uvec,0,0); + ppmat.Place_in_row(vvec,1,0); + return ROOT::Math::Similarity(ppmat,scovar_); + } } diff --git a/General/ParticleStateEstimate.hh b/General/ParticleStateEstimate.hh index 41e4dad2..159803ac 100644 --- a/General/ParticleStateEstimate.hh +++ b/General/ParticleStateEstimate.hh @@ -4,6 +4,7 @@ #ifndef KinKal_ParticleStateEstimate_hh #define KinKal_ParticleStateEstimate_hh #include "KinKal/General/ParticleState.hh" +#include "KinKal/General/MomBasis.hh" namespace KinKal { class ParticleStateEstimate : public ParticleState { @@ -15,6 +16,10 @@ namespace KinKal { DMAT const& stateCovariance() const { return scovar_; } // project the variance onto the scalar momentum double momentumVariance() const; + // project the position variance in given direction. Note this will throw if given the momentum direction, as that variance is infinite + double positionVariance(MomBasis::Direction dir) const; + // project the position variance onto the Plane defined by the perp direction (u) and the phi direction (v) + PMAT planeCovariance() const; private: DMAT scovar_; // covariance of state vector }; diff --git a/General/TimeDir.cc b/General/TimeDir.cc index db8fdcb0..932d4ee1 100644 --- a/General/TimeDir.cc +++ b/General/TimeDir.cc @@ -14,4 +14,10 @@ namespace KinKal { ost <<"Unknown"; return ost; } + double timeDirSign(TimeDir& tdir) { + if(tdir == TimeDir::forwards) + return 1.0; + else + return -1.0; + } } diff --git a/General/TimeDir.hh b/General/TimeDir.hh index 4a8ddbb6..63005386 100644 --- a/General/TimeDir.hh +++ b/General/TimeDir.hh @@ -4,9 +4,10 @@ // Define directions of time // #include -namespace KinKal { +namespace KinKal { enum struct TimeDir{forwards=0,backwards, end}; // time direction; forwards = increasing time, backwards = decreasing time TimeDir& operator ++ (TimeDir& tdir); std::ostream& operator <<(std::ostream& ost, TimeDir const& tdir); + double timeDirSign(TimeDir& tdir); } #endif diff --git a/General/TimeRange.cc b/General/TimeRange.cc index d8bf0aa4..81fe4b98 100644 --- a/General/TimeRange.cc +++ b/General/TimeRange.cc @@ -1,5 +1,24 @@ #include "KinKal/General/TimeRange.hh" namespace KinKal { + + bool TimeRange::restrict(TimeRange const& other ) { + bool retval = overlaps(other); + if(retval){ + range_[0] = std::max(begin(),other.begin()); + range_[1] = std::min(end(),other.end()); + } + return retval; + } + + void TimeRange::extend(double time, TimeDir tdir) { + // require the resulting range to be >0 + if(tdir == TimeDir::forwards){ + if(time > range_[0])range_[1] = time; + } else { + if(time < range_[1])range_[0] = time; + } + } + std::ostream& operator <<(std::ostream& ost, TimeRange const& trange) { ost << " Range [" << trange.begin() << "," << trange.end() << "]"; return ost; diff --git a/General/TimeRange.hh b/General/TimeRange.hh index be19d949..46875a82 100644 --- a/General/TimeRange.hh +++ b/General/TimeRange.hh @@ -5,6 +5,8 @@ #include #include #include +#include "KinKal/General/TimeDir.hh" + namespace KinKal { class TimeRange { public: @@ -13,12 +15,14 @@ namespace KinKal { if(begin > end)throw std::invalid_argument("Invalid Time Range"); } double begin() const { return range_[0]; } double end() const { return range_[1]; } + double rbegin() const { return range_[1]; } + double rend() const { return range_[0]; } double mid() const { return 0.5*(begin()+end()); } double range() const { return (end()-begin()); } bool inRange(double t) const {return t >= begin() && t < end(); } bool null() const { return end() == begin(); } bool overlaps(TimeRange const& other ) const { - return (end() > other.begin() || begin() < other.end()); } + return inRange(other.begin()) || other.inRange(begin()) || contains(other) || other.contains(*this); } bool contains(TimeRange const& other) const { return (begin() <= other.begin() && end() >= other.end()); } // force time to be in range @@ -28,6 +32,11 @@ namespace KinKal { range_[0] = std::min(begin(),other.begin()); range_[1] = std::max(end(),other.end()); } + // restrict the range to the overlap with another range. If there is no overlap + // leave the object unchanged and return 'false' + bool restrict(TimeRange const& other ); + // extend in a given direction + void extend(double time, TimeDir tdir); private: std::array range_; // range of times }; diff --git a/General/Vectors.hh b/General/Vectors.hh index cb99f3a8..0b03ed76 100644 --- a/General/Vectors.hh +++ b/General/Vectors.hh @@ -10,20 +10,23 @@ #include namespace KinKal { constexpr size_t NParams() { return 6; } // kinematic fit parameter space and phase space dimension - constexpr size_t NDim() { return 3; } // number of spatial dimensions (in our universe) + constexpr size_t NDim() { return 3; } // number of spatial dimensions (in our universe) // Physical vectors (space + spacetime) in GenVector format - using VEC3 = ROOT::Math::XYZVector; // spatial only vector + using VEC3 = ROOT::Math::XYZVector; // cartesian vector using CYL3 = ROOT::Math::Cylindrical3D; // Cylindrical vector. Context-dependent z axis definition using POL3 = ROOT::Math::Polar3D; // 3D polar vector using VEC4 = ROOT::Math::XYZTVector; // double precision spacetime vector, 4th component = time or energy using MOM4 = ROOT::Math::PxPyPzMVector; // 4-momentum with 4th component = mass using POL2 = ROOT::Math::Polar2D; // 2D polar vector. Context-dependent z axis definition + using VEC2 = ROOT::Math::XYVector; // 2D cartesian vector // Algebraic representations of spatial vectors: these are not miscible with the above - using VMAT = ROOT::Math::SMatrix>; // matrix type for spatial vector covariance using DPDV = ROOT::Math::SMatrix>; // parameter derivatives WRT space dimension type using DVDP = ROOT::Math::SMatrix>; // space dimension derivatives WRT parameter type using SMAT = ROOT::Math::SMatrix>; // Spatial covariance matrix - using SVEC3 = ROOT::Math::SVector; + using SSMAT = ROOT::Math::SMatrix>; // Spatial matrix + using SVEC3 = ROOT::Math::SVector; // spatial (algebraic) vector + using PMAT = ROOT::Math::SMatrix>; // Planar covariance matrix + using PPMAT = ROOT::Math::SMatrix>; // Planar projection matrix using SVEC6 = ROOT::Math::SVector; // type for particle state vector payload using PSMAT = ROOT::Math::SMatrix>; // matrix type for translating to/from state and parameters; this is not symmetric using RMAT = ROOT::Math::SMatrix>; // algebraic rotation matrix diff --git a/General/Weights.hh b/General/Weights.hh index 769846c8..b1adad14 100644 --- a/General/Weights.hh +++ b/General/Weights.hh @@ -36,6 +36,11 @@ namespace KinKal { fitdata_.mat() *= scale; return *this; } + Weights scale(double scale) const { + Weights retval = *this; + retval *= scale; + return retval; + } void print(std::ostream& ost=std::cout,int detail=0) const { ost << "Weights wVec " << weightVec() << std::endl; if(detail > 1) diff --git a/Geometry/Annulus.cc b/Geometry/Annulus.cc new file mode 100644 index 00000000..06c72f36 --- /dev/null +++ b/Geometry/Annulus.cc @@ -0,0 +1,15 @@ +#include "KinKal/Geometry/Annulus.hh" +#include "Math/VectorUtil.h" +using namespace ROOT::Math::VectorUtil; +namespace KinKal { + bool Annulus::inBounds(VEC3 const& point, double tol) const { + auto radius = Perp(point - center(),normal()); + return radius > irad_ - tol && radius < orad_ + tol; + } +} + +std::ostream& operator <<(std::ostream& ost, KinKal::Annulus const& ann) { + KinKal::Plane const& plane = static_cast(ann); + ost << "Annulus in " << plane << " Inner, Outer radii " << ann.innerRadius() << " , " << ann.outerRadius(); + return ost; +} diff --git a/Geometry/Annulus.hh b/Geometry/Annulus.hh new file mode 100644 index 00000000..443928b5 --- /dev/null +++ b/Geometry/Annulus.hh @@ -0,0 +1,24 @@ +// +// Description of an annular planar section +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Annulus_hh +#define KinKal_Annulus_hh +#include "KinKal/Geometry/Plane.hh" +namespace KinKal { + class Annulus : public Plane { + public: + ~Annulus() {}; + // construct from necessary parameters + Annulus(VEC3 const& norm, VEC3 const& udir, VEC3 const& center, double innerrad, double outerrad) : Plane(norm,udir,center), irad_(innerrad), orad_(outerrad) {} + // surface interface + bool inBounds(VEC3 const& point, double tol) const override; + // annulus-specific interface + auto innerRadius() const { return irad_; } + auto outerRadius() const { return orad_; } + private: + double irad_, orad_; // inner and outer radii + }; +} +std::ostream& operator <<(std::ostream& ost, KinKal::Annulus const& annulus); +#endif diff --git a/Geometry/CMakeLists.txt b/Geometry/CMakeLists.txt new file mode 100644 index 00000000..2c706a64 --- /dev/null +++ b/Geometry/CMakeLists.txt @@ -0,0 +1,32 @@ +# Explicitly list source files in this shared library +# When a new source file is added, it must be added to this list. + +# Globbing is not recommended, see: https://cmake.org/cmake/help/v3.12/command/file.html#filesystem + +# you can regenerate this list easily by running in this directory: ls -1 *.cc +add_library(Geometry SHARED + Annulus.cc + Cylinder.cc + Disk.cc + Frustrum.cc + IntersectFlag.cc + Intersection.cc + Plane.cc + Ray.cc + LineSegment.cc + Rectangle.cc + ThinSolid.cc +) +# make the library names unique +set(CMAKE_SHARED_LIBRARY_PREFIX "libKinKal_") + +# set top-level directory as include root +target_include_directories(Geometry PUBLIC + $ + $) + +# link this library with ROOT libraries +target_link_libraries(Geometry General ${ROOT_LIBRARIES}) + +# set shared library version equal to project version +set_target_properties(Geometry PROPERTIES VERSION ${PROJECT_VERSION} PREFIX ${CMAKE_SHARED_LIBRARY_PREFIX}) diff --git a/Geometry/Cylinder.cc b/Geometry/Cylinder.cc new file mode 100644 index 00000000..a816a026 --- /dev/null +++ b/Geometry/Cylinder.cc @@ -0,0 +1,136 @@ +#include "KinKal/Geometry/Cylinder.hh" +#include "Math/VectorUtil.h" +#include +using namespace ROOT::Math::VectorUtil; +namespace KinKal { + Cylinder::Cylinder(VEC3 const& axis, VEC3 const& center, double radius, double halflen ) : axis_(axis.Unit()), center_(center), radius_(radius), halflen_(halflen), radius2_(radius*radius) { + if(radius <= 0.0 || halflen <= 0.0 ) throw std::invalid_argument("Invalid Cylinder arguments"); + } + + bool Cylinder::onSurface(VEC3 const& point, double tol) const { + auto rvec = point - center_; + auto pvec = PerpVector(rvec,axis_); + return fabs(pvec.R()-radius_) < tol; + } + + bool Cylinder::inBounds(VEC3 const& point, double tol) const { + auto rvec = point - center_; + return fabs(rvec.Dot(axis_)) < halflen_ + tol; + } + + bool Cylinder::isInside(VEC3 const& point) const { + auto rvec = point - center_; + return Perp2(rvec,axis_) < radius2_; + } + + double Cylinder::distance(VEC3 const& point) const { + auto rvec = point - center_; + return Perp(rvec,axis_) - radius_; + } + + VEC3 Cylinder::normal(VEC3 const& point) const { + // normal is perpendicular part of the difference + auto rvec = point - center_; + auto pvec = PerpVector(rvec,axis_); + return pvec.Unit(); + } + + VEC3 Cylinder::uDirection() const { + // u direction is arbitrary; use 'x' if the axis is mostly along z + static const VEC3 xdir(1.0,0.0,0.0); + static const VEC3 ydir(0.0,1.0,0.0); + static const VEC3 zdir(0.0,0.0,1.0); + double zdot = fabs(axis_.Dot(zdir)); + if(zdot > 0.5){ + return ydir.Cross(axis_).Unit(); + } else if(zdot > 0.01) { + return axis_.Cross(zdir).Unit(); + } else { + double xdot = fabs(axis_.Dot(xdir)); + if(xdot > 0.5) + return ydir.Cross(axis_).Unit(); + else + return xdir.Cross(axis_).Unit(); + } + } + + Disk Cylinder::frontDisk() const { + return Disk(axis_,uDirection(),center_-halflen_*axis_,radius_); + } + + Disk Cylinder::midDisk() const { + return Disk(axis_,uDirection(),center_,radius_); + } + + Disk Cylinder::backDisk() const { + return Disk(axis_,uDirection(),center_+halflen_*axis_,radius_); + } + + Rectangle Cylinder::inscribedRectangle(VEC3 const& norm) const { + // make sure normal is perpendicular to the axis + if(axis_.Dot(norm) > 1e-10) throw std::invalid_argument("normal not perpendicular to axis"); + // U points along the cylinder + return Rectangle(norm,axis_,center_,halflen_,radius_); + } + + Plane Cylinder::tangentPlane(VEC3 const& spoint) const { + // rectangle normal is the local cylinder normal + auto norm = normal(spoint); + // correct for any tolerance if the point isnt exactly on the surface (up to FP accuracy) + double rad = Perp(spoint - center_,axis_); + auto rcent = spoint + norm*(radius_-rad); + return Plane(norm,axis_,rcent); + } + + IntersectFlag Cylinder::intersect(Ray const& ray,double& dist, bool forwards, double tol) const { + IntersectFlag retval; + double ddot = ray.direction().Dot(axis_); + double alpha = (1.0 - ddot*ddot); // always positive + // make sure the ray isn't co-linear on the relevant scale + if(alpha > tol/std::max(radius_,halflen_)){ + auto rvec = ray.start() - center_; + double sdot = rvec.Dot(axis_); + double beta = sdot*ddot - rvec.Dot(ray.direction()); + double gamma = rvec.Mag2() - sdot*sdot - radius2_; + double beta2 = beta*beta; + double ag = alpha*gamma; + // make sure there's a solution + if(beta2 > ag){ + // choose the solution based on the strategy + double delta = sqrt(beta2 - ag); + double d1 = (beta - delta)/alpha; + double d2 = (beta + delta)/alpha; + if(!forwards){ + // closest solution + retval.onsurface_ = true; + dist = fabs(d1) < fabs(d2) ? d1 : d2; + } else { + // closest forwards solution + if(d1 > 0.0 && d2 > 0.0 ){ + retval.onsurface_ = true; + dist = std::min(d1,d2); + } else if(d1 > 0.0) { + retval.onsurface_ = true; + dist = d1; + } else if(d2 > 0.0) { + retval.onsurface_ = true; + dist = d2; + } + } + } + // test that the point is on the cylinder + if(retval.onsurface_){ + auto point = ray.position(dist); + retval.inbounds_ = inBounds(point,tol); + } + } + return retval; + } +} + + + +std::ostream& operator <<(std::ostream& ost, KinKal::Cylinder const& cyl) { + ost << "Cylinder with center = " << cyl.center() << " , axis " << cyl.axis() << " radius " << cyl.radius() << " half-length " << cyl.halfLength(); + return ost; +} diff --git a/Geometry/Cylinder.hh b/Geometry/Cylinder.hh new file mode 100644 index 00000000..b8dfc295 --- /dev/null +++ b/Geometry/Cylinder.hh @@ -0,0 +1,46 @@ +// +// Description of a right circular cylindrical, for use in KinKal intersection +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Cylinder_hh +#define KinKal_Cylinder_hh +#include "KinKal/Geometry/Surface.hh" +#include "KinKal/Geometry/Disk.hh" +#include "KinKal/Geometry/Rectangle.hh" +namespace KinKal { + class Cylinder : public Surface { + public: + virtual ~Cylinder() {} + // construct from necessary parameters + Cylinder(VEC3 const& axis, VEC3 const& center, double radius, double halflen ); + // Surface interface + bool onSurface(VEC3 const& point, double tol) const override; + bool isInside(VEC3 const& point) const override; + double curvature(VEC3 const& point) const override { return 1.0/radius_; } + bool inBounds(VEC3 const& point, double tol) const override; + double distance(VEC3 const& point) const override; + IntersectFlag intersect(Ray const& ray,double& dist, bool forwards, double tol) const override; + VEC3 normal(VEC3 const& point) const override; // radially outward + Plane tangentPlane(VEC3 const& point) const override; + // cylinder-specific interface + auto const& axis() const { return axis_; } + auto const& center() const { return center_; } + double radius() const { return radius_; } + double halfLength() const { return halflen_; } + // front and rear boundaries of this cylinder as disks. U direction is arbitrary + Disk frontDisk() const; + Disk midDisk() const; + Disk backDisk() const; + // inscribed rectangle with given normal direction. U direction is long the axis + Rectangle inscribedRectangle(VEC3 const& norm) const; + private: + VEC3 axis_; // symmetry axis of the cylinder + VEC3 center_; // geometric center + double radius_; // transverse radius + double halflen_; // half length + double radius2_; // squared radius (cache); + VEC3 uDirection() const; // arbitrary direction orthogonal to the axis + }; +} +std::ostream& operator <<(std::ostream& ost, KinKal::Cylinder const& cyl); +#endif diff --git a/Geometry/Disk.cc b/Geometry/Disk.cc new file mode 100644 index 00000000..dc5f111c --- /dev/null +++ b/Geometry/Disk.cc @@ -0,0 +1,7 @@ +#include "KinKal/Geometry/Disk.hh" + +std::ostream& operator <<(std::ostream& ost, KinKal::Disk const& disk) { + KinKal::Plane const& plane = static_cast(disk); + ost << "Disk in " << plane << " radius " << disk.outerRadius(); + return ost; +} diff --git a/Geometry/Disk.hh b/Geometry/Disk.hh new file mode 100644 index 00000000..f6dfd68a --- /dev/null +++ b/Geometry/Disk.hh @@ -0,0 +1,16 @@ +// +// Description of a disk +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Disk_hh +#define KinKal_Disk_hh +#include "KinKal/Geometry/Annulus.hh" +namespace KinKal { + class Disk : public Annulus { + public: + // construct from necessary parameters + Disk(VEC3 const& norm,VEC3 const& udir, VEC3 const& center, double radius) : Annulus(norm,udir,center,0.0,radius) {} + }; +} +std::ostream& operator <<(std::ostream& ost, KinKal::Disk const& disk); +#endif diff --git a/Geometry/Frustrum.cc b/Geometry/Frustrum.cc new file mode 100644 index 00000000..25a921eb --- /dev/null +++ b/Geometry/Frustrum.cc @@ -0,0 +1,157 @@ +#include "KinKal/Geometry/Frustrum.hh" +#include "Math/VectorUtil.h" +#include +using namespace ROOT::Math::VectorUtil; +namespace KinKal { + + Frustrum::Frustrum(VEC3 const& axis, VEC3 const& center, double r0, double r1, double halflen ) : axis_(axis.Unit()), center_(center), r0_(r0), r1_(r1), halflen_(halflen), rmid_(0.5*(r0+r1)), drda_( 0.5*(r1-r0)/halflen) { + if(r0 <= 0.0 || r1 <= 0.0 || halflen <= 0.0 ) throw std::invalid_argument("Invalid Frustrum arguments"); + double dr = r1_ - r0_; + // compute sine and cosine of the cone 1/2 angle + double len = sqrt( dr*dr + 4*halflen_*halflen_); + cost_ = 2*halflen_/len; + sint_ = dr/len; + } + + double Frustrum::radius(VEC3 point) const { + auto rvec = point - center_; + double alen = rvec.Dot(axis_); + return radius(alen); + } + + bool Frustrum::onSurface(VEC3 const& point, double tol) const { + auto rvec = point - center_; + auto pvec = PerpVector(rvec,axis_); + double alen = rvec.Dot(axis_); + return fabs(pvec.R()-radius(alen)) < tol; + } + + bool Frustrum::inBounds(VEC3 const& point, double tol) const { + auto rvec = point - center_; + return fabs(rvec.Dot(axis_)) < halflen_ + tol/cost_; + } + + bool Frustrum::isInside(VEC3 const& point) const { + auto rvec = point - center_; + double alen = rvec.Dot(axis_); + return Perp(rvec,axis_) < radius(alen); + } + + double Frustrum::curvature(VEC3 const& point) const { + auto rvec = point - center_; + double alen = rvec.Dot(axis_); + return 1.0/radius(alen); + } + + double Frustrum::distance(VEC3 const& point) const { + auto rvec = point - center_; + double alen = rvec.Dot(axis_); + return (Perp(rvec,axis_) - radius(alen))*cost_; + } + + VEC3 Frustrum::normal(VEC3 const& point) const { + // normal is perpendicular part of the difference + auto rvec = point - center_; + auto pvec = PerpVector(rvec,axis_); + return pvec.Unit()*cost_ - axis_*sint_; + } + + VEC3 Frustrum::uDirection() const { + // u direction is arbitrary; use 'x' if the axis is mostly along z + static const VEC3 xdir(1.0,0.0,0.0); + static const VEC3 ydir(0.0,1.0,0.0); + static const VEC3 zdir(0.0,0.0,1.0); + double zdot = fabs(axis_.Dot(zdir)); + if(zdot > 0.5){ + return ydir.Cross(axis_).Unit(); + } else if(zdot > 0.01) { + return axis_.Cross(zdir).Unit(); + } else { + double xdot = fabs(axis_.Dot(xdir)); + if(xdot > 0.5) + return ydir.Cross(axis_).Unit(); + else + return xdir.Cross(axis_).Unit(); + } + } + + Disk Frustrum::frontDisk() const { + return Disk(axis_,uDirection(),center_-halflen_*axis_,radius(-halflen_)); + } + + Disk Frustrum::midDisk() const { + return Disk(axis_,uDirection(),center_,radius(0.0)); + } + + Disk Frustrum::backDisk() const { + return Disk(axis_,uDirection(),center_+halflen_*axis_,radius(halflen_)); + } + + Plane Frustrum::tangentPlane(VEC3 const& spoint) const { + auto rvec = spoint - center_; + double alen = rvec.Dot(axis_); + // rectangle normal is the local cone surface normal + auto norm = normal(spoint); + // correct for any tolerance if the point isnt exactly on the surface (up to FP accuracy) + double rad = norm.Dot(spoint - center_); + auto rcent = spoint + norm*(radius(alen)-rad); + // v direction is along the cone + auto udir = axis_.Cross(norm); + return Plane(norm,udir,rcent); + } + + IntersectFlag Frustrum::intersect(Ray const& ray,double& dist, bool forwards, double tol) const { + // exact solution + IntersectFlag retval; + // displace the ray start to be WRT the base of the cone + auto spos = ray.start() - center_ + halflen_*axis_; + double z1 = ray.direction().Dot(axis_); + double z0 = spos.Dot(axis_); + auto pvec = ray.direction() - z1*axis_; + auto qvec = spos - z0*axis_; + double b2 = pvec.Mag2() - z1*z1*drda_*drda_; + double b1 = z1*drda_*(drda_*z0 + r0_) - pvec.Dot(qvec); + double b0 = qvec.Mag2() - pow(drda_*z0 + r0_,2); + // check if ray is parallel to cone surface + if(b2 == 0 && b1 != 0){ + retval.onsurface_ = true; + dist = 0.5*b0/b1; + } else if (b1*b1 > b0*b2){ + double t1 = b1/b2; + double t2 = sqrt(b1*b1 - b0*b2)/b2; + double d1 = t1 + t2; + double d2 = t1 - t2; + if(!forwards){ + retval.onsurface_ = true; + dist = fabs(d1) < fabs(d2) ? d1 : d2; + } else { + // closest forwards solution + if(d1 > 0.0 && d2 > 0.0 ){ + retval.onsurface_ = true; + dist = std::min(d1,d2); + } else if(d1 > 0.0) { + retval.onsurface_ = true; + dist = d1; + } else if(d2 > 0.0) { + retval.onsurface_ = true; + dist = d2; + } + } + } + // test that the point is on the cone + if(retval.onsurface_){ + auto point = ray.position(dist); + retval.inbounds_ = inBounds(point,tol); + } + return retval; + } +} + +std::ostream& operator <<(std::ostream& ost, KinKal::Frustrum const& fru) { + ost << "Frustrum with center = " << fru.center() << " , axis " << fru.axis() + << " low radius " << fru.lowRadius() + << " high radius " << fru.highRadius() + << " half-length " << fru.halfLength() + << " half-angle " << fru.halfAngle(); + return ost; +} diff --git a/Geometry/Frustrum.hh b/Geometry/Frustrum.hh new file mode 100644 index 00000000..f394b054 --- /dev/null +++ b/Geometry/Frustrum.hh @@ -0,0 +1,51 @@ +// +// Description of a right conical frustrum for use in KinKal intersection +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Frustrum_hh +#define KinKal_Frustrum_hh +#include "KinKal/Geometry/Surface.hh" +#include "KinKal/Geometry/Disk.hh" +#include "KinKal/Geometry/Rectangle.hh" +namespace KinKal { + class Frustrum : public Surface { + public: + virtual ~Frustrum() {} + // construct from necessary parameters + Frustrum(VEC3 const& axis, VEC3 const& center, double r0, double r1, double halflen ); + // Surface interface + bool onSurface(VEC3 const& point, double tol) const override; + bool isInside(VEC3 const& point) const override; + double curvature(VEC3 const& point) const override; + bool inBounds(VEC3 const& point, double tol) const override; + double distance(VEC3 const& point) const override; + IntersectFlag intersect(Ray const& ray,double& dist, bool forwards, double tol) const override; + VEC3 normal(VEC3 const& point) const override; // radially outward + Plane tangentPlane(VEC3 const& point) const override; + // frustrum-specific interface + auto const& axis() const { return axis_; } + auto const& center() const { return center_; } + auto lowRadius() const { return r0_; } // radius at the lower end + auto highRadius() const { return r1_; } // radius at the upper end + auto centerRadius() const { return rmid_; } // radius at the center + double radius(VEC3 point) const; + double radius(double alen) const { return rmid_ + alen*drda_; } // radius at a given distance along the axis, WRT the center + auto halfLength() const { return halflen_; } + double halfAngle() const { return atan2(sint_,cost_); } + // front and rear boundaries of this cylinder as disks. U direction is arbitrary + Disk frontDisk() const; + Disk midDisk() const; + Disk backDisk() const; + private: + VEC3 axis_; // symmetry axis of the cylinder + VEC3 center_; // geometric center + double r0_, r1_; // lowest and highest radius + double halflen_; // half length + // caches used to speed calculation + double rmid_,drda_; // mid radius and rate of radial change + double sint_, cost_; // sine and cosine of the cone 1/2 angle + VEC3 uDirection() const; // arbitrary direction orthogonal to the axis + }; +} +std::ostream& operator <<(std::ostream& ost, KinKal::Frustrum const& cyl); +#endif diff --git a/Geometry/Intersect.hh b/Geometry/Intersect.hh new file mode 100644 index 00000000..8de42ee2 --- /dev/null +++ b/Geometry/Intersect.hh @@ -0,0 +1,333 @@ +// +// Calculate the intersection point of a Trajectory with a surf +// This must be specialized for every case (every pair of trajectory and surf) +// original author: David Brown (LBNL) 2023 +// +#ifndef KinKal_Intersect_hh +#define KinKal_Intersect_hh +#include "KinKal/Geometry/Intersection.hh" +#include "KinKal/Geometry/Cylinder.hh" +#include "KinKal/Geometry/Frustrum.hh" +#include "KinKal/Geometry/Plane.hh" +#include "KinKal/Trajectory/LoopHelix.hh" +#include "KinKal/Trajectory/CentralHelix.hh" +#include "KinKal/Trajectory/ParticleTrajectory.hh" +#include "KinKal/Trajectory/KinematicLine.hh" +#include "KinKal/General/TimeDir.hh" +#include "Math/VectorUtil.h" +#include +namespace KinKal { + // + // generic intersection implementation based on stepping across the surface within a given range in the given time direction + // + template Intersection stepIntersect( KTRAJ const& ktraj, SURF const& surf, TimeRange trange, double tol,TimeDir tdir = TimeDir::forwards) { + Intersection retval; + double ttest = tdir == TimeDir::forwards ? trange.begin() : trange.end(); + auto pos = ktraj.position3(ttest); + double rbend = ktraj.bendRadius(); + double tstep = trange.range(); + double speed = ktraj.speed(); + // if there's curvature, take smaller steps + if(rbend > 0.0){ + double tlenmax = 2.0*sqrt(2.0*rbend*tol); // maximum transverse length keeping the sagitta within tolerance + double tspeed = ktraj.transverseSpeed(); + tstep = std::min(tstep,(tlenmax+tol)/tspeed); // trajectory range defines maximum step + } + // sign! + tstep *= timeDirSign(tdir); + bool startinside = surf.isInside(pos); + bool stepinside; + unsigned niter(0); + // step until we cross the surface or the time is out of range + do { + ttest += tstep; + pos = ktraj.position3(ttest); + stepinside = surf.isInside(pos); + } while (startinside == stepinside && trange.inRange(ttest)); + if(startinside != stepinside){ + // we crossed the surface: backup and do a linear search. + ttest -= tstep; + double dist = std::numeric_limits::max(); + IntersectFlag rayinter; + static const unsigned nconv(5); // when to start worrying about non-convergence + double sumdist[2] = {0.0,0.0}; // sum of absolute distance for convergence testing + do { + retval.pos_ = ktraj.position3(ttest); + retval.pdir_ = ktraj.direction(ttest); + auto ray = retval.ray(); + rayinter = surf.intersect(ray,dist,false,tol); + if(rayinter.onsurface_){ + ttest += dist/speed; + } else { + break; + } + ++niter; + unsigned iconv = niter/nconv; // count convergence cycles + if(iconv > 0){ // start testing for non-convergence + unsigned icur = iconv % 2; // current cycle average indicator (0 or 1) + int irem = niter - iconv*nconv; + if( irem == 0){ + // new cycle + if(iconv > 2){ //if we're past the 3rd cycle, test + unsigned iprev = (iconv + 1) % 2; // previous cycle (that we just finished) + if(sumdist[iprev]/sumdist[icur] > 0.5) { + // claim non-convergence if the sum distance hasn't decreased by at least a factor of 2 between cycles + // std::cout << "Non-converged step intersection ray inter dist " << dist << " avg " << sumdist[iprev] << " prev avg " << sumdist[icur] << std::endl; + return retval; // failed + } + } + sumdist[icur] = 0.0; // reset the average counter + } + sumdist[icur] += fabs(dist); // increatement the sum + } + } while (fabs(dist) > tol); + if(rayinter.onsurface_){ + retval.onsurface_ = rayinter.onsurface_; + retval.time_ = ttest; + retval.pos_ = ktraj.position3(retval.time_); + retval.pdir_ = ktraj.direction(retval.time_); + retval.inbounds_ = surf.inBounds(retval.pos_,tol); + retval.norm_ = surf.normal(retval.pos_); + } + } + // check the final time to be in range + retval.inrange_ = trange.inRange(retval.time_); + return retval; + } + // + // specializations for different trajectory and surface types + // Helix and cylinder. This must be explicit to differentiate from the line intersection below. + // All the work is done in common with CentralHelix and LoopHelix + // The actual implementation is generic for any helix + // + // First, a test if the circles can intersect at the specified time + + template < class HELIX> bool canIntersect( HELIX const& helix, KinKal::Cylinder const& cyl, TimeRange const& trange, double tol) { + auto hbeg = helix.center(trange.begin()); + auto hend = helix.center(trange.end()); + // if the circles are fully contained one in the other no intersection is possible + double dbeg = ROOT::Math::VectorUtil::Perp(hbeg - cyl.center(),cyl.axis()); + double dend = ROOT::Math::VectorUtil::Perp(hend - cyl.center(),cyl.axis()); + double drad = fabs(cyl.radius()-helix.bendRadius()); + return dbeg > drad -tol && dend > drad -tol; + } + + template < class HELIX> bool canIntersect( HELIX const& helix, KinKal::Frustrum const& fru, TimeRange const& trange, double tol) { + auto hbeg = helix.center(trange.begin()); + auto hend = helix.center(trange.end()); + // if the circles are fully contained one in the other no intersection is possible + double dbeg = ROOT::Math::VectorUtil::Perp(hbeg - fru.center(),fru.axis()); + double dend = ROOT::Math::VectorUtil::Perp(hend - fru.center(),fru.axis()); + double dradbeg = fru.radius(hbeg)-helix.bendRadius(); + double dradend = fru.radius(hend)-helix.bendRadius(); + return dradbeg*dradend < 0.0 || (dbeg > fabs(dradbeg) -tol && dend > fabs(dradend) -tol); + } + + template < class HELIX> Intersection hcIntersect( HELIX const& helix, KinKal::Cylinder const& cyl, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + Intersection retval; + // test if the range is lnger than the radius. If so, make an axial approximation + double rbend = fabs(helix.bendRadius()); + // compare directions and divide into cases + double ddot = fabs(helix.bnom().Unit().Dot(cyl.axis())); + if(rbend < trange.range()*helix.transverseSpeed() && ddot > 0.9) { // mostly co-linear + // the helix and cylinder are roughly co-linear, and the helix loops within this range + // first try to limit the range using disk-axis intersections + // see if the range is in bounds + auto binb = cyl.inBounds(helix.position3(trange.begin()),tol); + auto einb = cyl.inBounds(helix.position3(trange.end()),tol); + if(binb && einb) { + // both in bounds: use this range + if(canIntersect(helix,cyl,trange,tol)){ + retval = stepIntersect(helix,cyl,trange,tol,tdir); + } + } else { + // create a list of times to select the most restrictive range + std::vectortimes; + if(binb)times.push_back(trange.begin()); + if(einb)times.push_back(trange.end()); + // find the intersections with the front and back disks. Use those to refine the range + // note we don't know the orientation of the trajectory WRT the axis so we have to try both + auto frontdisk = cyl.frontDisk(); + auto backdisk = cyl.backDisk(); + auto frontinter = hpIntersect(helix,frontdisk,trange,tol,tdir); + auto backinter = hpIntersect(helix,backdisk,trange,tol,tdir); + if(trange.inRange(frontinter.time_))times.push_back(frontinter.time_); + if(trange.inRange(backinter.time_))times.push_back(backinter.time_); + if(times.size() >=2){ + TimeRange srange(*std::min_element(times.begin(),times.end()),*std::max_element(times.begin(),times.end())); + // intersection is possible: step within the restricted range + if(canIntersect(helix,cyl,srange,tol)){ + retval = stepIntersect(helix,cyl,srange,tol,tdir); + } + } + } + } else { + // intermediate case: use step intersection + retval = stepIntersect(helix,cyl,trange,tol,tdir); + } + return retval; + } + // + // Helix and planar surfaces + // + template Intersection hpIntersect( HELIX const& helix, KinKal::Plane const& plane, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + Intersection retval; + // test if the range is lnger than the radius. If so, make an axial approximation + double rbend = fabs(helix.bendRadius()); + if(rbend > trange.range()*helix.transverseSpeed()){ + retval = stepIntersect(helix,plane,trange,tol,tdir); + } else { + // the trajectory curves macroscopically over this range: see if the axis is normal + double tstart = tdir == TimeDir::forwards ? trange.begin() : trange.end(); + auto ray = helix.linearize(tstart); + auto velo = helix.velocity(tstart); + if(tdir == TimeDir::backwards) ray.reverse(); // reverse if going backwards in time + double vax = velo.Dot(ray.direction()); // speed along axis + // test for the helix being circular or tangent to the plane + double ddot = fabs(ray.direction().Dot(plane.normal())); + double zrange = fabs(vax*trange.range()); + if(zrange > tol && ddot > tol/zrange ){ + // Find the intersection time of the helix ray (along bnom) with the plane to reduce the range + double dist(0.0); + auto pinter = plane.intersect(ray,dist,true,tol); + if(pinter.onsurface_){ + // translate the ray intersection distance to a time + double tmid = tstart + dist/vax; + // bound the range of intersections by the extrema of the cylinder-plane intersection + double tantheta = sqrt(std::max(1.0 -ddot*ddot,0.0))/ddot; + // distance along axis to the surface to bound the reduced range; add tolerance + double dd = tol+rbend*tantheta; + // intersection should be bounded by the + auto dt = fabs(dd/vax); + TimeRange srange(tmid-dt,tmid+dt); + // if this range overlaps with the original, compute the intersection + if(srange.restrict(trange)){ + // step to the intersection in the restricted range. Use a separate intersection object as the + // range is different + retval = stepIntersect(helix,plane,srange,tol,tdir); + } + } + } else { + // simply step to intersection + retval = stepIntersect(helix,plane,trange,tol,tdir); + } + } + return retval; + } + // + // Helix and frustrum + // + template < class HELIX> Intersection hfIntersect( HELIX const& helix, KinKal::Frustrum const& fru, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + Intersection retval; + double ddot = fabs(helix.bnom().Unit().Dot(fru.axis())); + if (ddot > 0.9) { // I need a more physical co-linear test TODO + // the helix and frustrum are roughly co-linear. + // see if the range is in bounds + auto binb = fru.inBounds(helix.position3(trange.begin()),tol); + auto einb = fru.inBounds(helix.position3(trange.end()),tol); + if(binb && einb) { + // both in bounds: use this range + if(canIntersect(helix,fru,trange,tol)) { + retval = stepIntersect(helix,fru,trange,tol,tdir); + } + } else { + // create a list of times to select the most restrictive range + std::vectortimes; + if(binb)times.push_back(trange.begin()); + if(einb)times.push_back(trange.end()); + // find the intersections with the front and back disks. Use those to refine the range + // note we don't know the orientation of the trajectory WRT the axis so we have to try both + auto frontdisk = fru.frontDisk(); + auto backdisk = fru.backDisk(); + auto frontinter = hpIntersect(helix,frontdisk,trange,tol); + auto backinter = hpIntersect(helix,backdisk,trange,tol); + if(trange.inRange(frontinter.time_))times.push_back(frontinter.time_); + if(trange.inRange(backinter.time_))times.push_back(backinter.time_); + if(times.size() >=2){ + TimeRange srange(*std::min_element(times.begin(),times.end()),*std::max_element(times.begin(),times.end())); + // intersection is possible: step within the restricted range + if(canIntersect(helix,fru,srange,tol) ) { + retval = stepIntersect(helix,fru,srange,tol,tdir); + } + } + } + // } else if (ddot < 0.1) { + // // the helix and frustrum are mostly orthogonal, use POCA to the axis to find an initial estimate, then do a linear search + // // TODO. Construct a KinematicLine object from the helix axis, and a GeometricLine from the frustrum, then invoke POCA. + } else { + // intermediate case: use step intersection + retval = stepIntersect(helix,fru,trange,tol,tdir); + } + return retval; + } + // + // Tie intersect to the explicit helix implementations + // + Intersection intersect( LoopHelix const& lhelix, KinKal::Cylinder const& cyl, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hcIntersect(lhelix,cyl,trange,tol,tdir); + } + Intersection intersect( CentralHelix const& chelix, KinKal::Cylinder const& cyl, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hcIntersect(chelix,cyl,trange,tol,tdir); + } + Intersection intersect( LoopHelix const& lhelix, KinKal::Plane const& plane, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hpIntersect(lhelix,plane,trange,tol,tdir); + } + Intersection intersect( CentralHelix const& chelix, KinKal::Plane const& plane, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hpIntersect(chelix,plane,trange,tol,tdir); + } + Intersection intersect( LoopHelix const& lhelix, KinKal::Frustrum const& fru, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hfIntersect(lhelix,fru,trange,tol,tdir); + } + Intersection intersect( CentralHelix const& chelix, KinKal::Frustrum const& fru, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hfIntersect(chelix,fru,trange,tol,tdir); + } + // + // Line trajectory with generic surfaces; no specialization needed since all surfaces provide a ray intersection implementation + // + Intersection intersect(KinKal::KinematicLine const& kkline, KinKal::Surface const& surf, TimeRange trange,double tol,TimeDir tdir = TimeDir::forwards) { + Intersection retval; + auto tstart = tdir == TimeDir::forwards ? trange.begin() : trange.end(); + auto pos = kkline.position3(tstart); + auto dir = kkline.direction(tstart)*timeDirSign(tdir); + Ray ray(dir,pos); + double dist; + auto rayinter = surf.intersect(ray,dist,true,tol); + if(rayinter.onsurface_){ + retval.onsurface_ = rayinter.onsurface_; + retval.inbounds_ = rayinter.inbounds_; + retval.pos_ = ray.position(dist); + retval.norm_ = surf.normal(retval.pos_); + retval.pdir_ = dir; + // calculate the time + retval.time_ = tstart + dist*timeDirSign(tdir)/kkline.speed(tstart); + } + // check the final time to be in range; + retval.inrange_ = trange.inRange(retval.time_); + return retval; + } + // generic surface intersection cast down till we find something that works. This will only be used for helices, as KinematicLine + // is already complete + template Intersection hsIntersect(KTRAJ const& ktraj, Surface const& surf,TimeRange trange, double tol,TimeDir tdir = TimeDir::forwards) { + // use pointers to cast to avoid avoid a throw + const Surface* surfp = &surf; + // go through the possibilities: I don't know of anything more elegant + auto plane = dynamic_cast(surfp); + if(plane)return intersect(ktraj,*plane,trange,tol,tdir); + auto cyl = dynamic_cast(surfp); + if(cyl)return intersect(ktraj,*cyl,trange,tol,tdir); + auto fru = dynamic_cast(surfp); + if(fru)return intersect(ktraj,*fru,trange,tol,tdir); + // unknown surface subclass; return failure + return Intersection(); + } + // now provide the explicit generic interface + Intersection intersect( LoopHelix const& ploophelix, KinKal::Surface const& surf, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hsIntersect(ploophelix,surf,trange,tol,tdir); + } + Intersection intersect( CentralHelix const& pcentralhelix, KinKal::Surface const& surf, TimeRange trange ,double tol,TimeDir tdir = TimeDir::forwards) { + return hsIntersect(pcentralhelix,surf,trange,tol,tdir); + } + +} + +#endif diff --git a/Geometry/IntersectFlag.cc b/Geometry/IntersectFlag.cc new file mode 100644 index 00000000..1e5dcff7 --- /dev/null +++ b/Geometry/IntersectFlag.cc @@ -0,0 +1,15 @@ +#include "KinKal/Geometry/IntersectFlag.hh" +#include +#include +#include + +std::ostream& operator <<(std::ostream& ost, KinKal::IntersectFlag const& iflag) { + if(iflag.onsurface_){ + ost << "On surface "; + if(iflag.inbounds_)ost << " in bounds "; + if(iflag.inrange_)ost << " in range "; + }else { + ost << "No Intersection"; + } + return ost; +} diff --git a/Geometry/IntersectFlag.hh b/Geometry/IntersectFlag.hh new file mode 100644 index 00000000..2339dd1d --- /dev/null +++ b/Geometry/IntersectFlag.hh @@ -0,0 +1,22 @@ +#ifndef KinKal_IntersectFlag_hh +#define KinKal_IntersectFlag_hh +// +// describe flag bits used for reco intersection +// +// +// Original author David Brown +// +#include +#include +namespace KinKal { + struct IntersectFlag { + bool onsurface_ = false; // intersection is on the surface + bool inbounds_ = false; // intersection is inside the surface boundaries + bool inrange_ = false; // intersection is inside the time range + bool operator ==(IntersectFlag const& other) const { return other.onsurface_ == onsurface_ && other.inbounds_ == inbounds_ && other.inrange_ == inrange_;} + bool operator !=(IntersectFlag const& other) const { return other.onsurface_ != onsurface_ || other.inbounds_ != inbounds_ || other.inrange_ != inrange_;} + bool good() const { return onsurface_ && inbounds_ && inrange_; } + friend std::ostream& operator <<(std::ostream& ost, KinKal::IntersectFlag const& iflag); + }; +} +#endif diff --git a/Geometry/Intersection.cc b/Geometry/Intersection.cc new file mode 100644 index 00000000..af164138 --- /dev/null +++ b/Geometry/Intersection.cc @@ -0,0 +1,19 @@ +#include "KinKal/Geometry/Intersection.hh" +#include +#include +#include +namespace KinKal { + std::ostream& operator <<(std::ostream& ost, KinKal::Intersection const& inter) { + if(inter.onsurface_){ + ost << "Intersection on surface "; + if(inter.inbounds_) ost << " in bounds "; + if(inter.inrange_) ost << " in time range "; + } else { + ost << "No Intersection"; + } + if(inter.good()){ + ost << " at time " << inter.time_ << " position " << inter.pos_ << " surface normal " << inter.norm_; + } + return ost; + } +} diff --git a/Geometry/Intersection.hh b/Geometry/Intersection.hh new file mode 100644 index 00000000..8cf60f47 --- /dev/null +++ b/Geometry/Intersection.hh @@ -0,0 +1,25 @@ +// +// payload for intersection calculation +// original author: David Brown (LBNL) 2023 +// +#ifndef KinKal_Intersection_hh +#define KinKal_Intersection_hh +#include "KinKal/General/Vectors.hh" +#include "KinKal/General/TimeRange.hh" +#include "KinKal/Geometry/IntersectFlag.hh" +#include "KinKal/Geometry/Ray.hh" +#include +#include + +namespace KinKal { + struct Intersection : public IntersectFlag { + VEC3 pos_; // intersection position + VEC3 norm_; // surface normal at intersection + VEC3 pdir_; // particle direction at intersection + double time_ = 0.0; // time at intersection (from particle) + bool gap_ = false; // intersection is in a piecewise-trajectory gap + Ray ray() const { return Ray(pdir_,pos_); } // linear particle trajectory at intersection + friend std::ostream& operator << (std::ostream& ost, KinKal::Intersection const& inter); + }; +} +#endif diff --git a/Geometry/LineSegment.cc b/Geometry/LineSegment.cc new file mode 100644 index 00000000..1098e0cf --- /dev/null +++ b/Geometry/LineSegment.cc @@ -0,0 +1,19 @@ +#include "KinKal/Geometry/LineSegment.hh" +#include +#include +#include +#include + +namespace KinKal { + + void LineSegment::print(std::ostream& ost, int detail) const { + ost << *this << std::endl; + } + + std::ostream& operator <<(std::ostream& ost, LineSegment const& lineseg) { + ost << " LineSegment starting " << lineseg.start() + << " direction " << lineseg.direction() + << " length " << lineseg.length(); + return ost; + } +} diff --git a/Geometry/LineSegment.hh b/Geometry/LineSegment.hh new file mode 100644 index 00000000..682d4062 --- /dev/null +++ b/Geometry/LineSegment.hh @@ -0,0 +1,31 @@ +#ifndef KinKal_LineSegment_hh +#define KinKal_LineSegment_hh +// +// directed line segment +// +#include "KinKal/General/Vectors.hh" +#include "KinKal/Geometry/Ray.hh" + +namespace KinKal { + class LineSegment : public Ray { + public: + // construct from a point and direction + LineSegment(VEC3 const& dir, VEC3 const& start, double length) : Ray(dir,start), length_(length) { + end_ = position(length_); + } + // construct from 2 points + LineSegment(VEC3 const& start, VEC3 const& end) : Ray(end-start,start), length_((end-start).R()), end_(end) {} + // accessors + VEC3 const& end() const { return end_; } + VEC3 middle() const { return 0.5*(start() + end()); } + double length() const { return length_; } + void print(std::ostream& ost, int detail) const; + // position from the end + VEC3 endPosition(double dist) const { return end_ - dist*direction(); } + private: + double length_; // segment length + VEC3 end_; // end position + }; + std::ostream& operator <<(std::ostream& ost, LineSegment const& tLineSegment); +} +#endif diff --git a/Geometry/ParticleTrajectoryIntersect.hh b/Geometry/ParticleTrajectoryIntersect.hh new file mode 100644 index 00000000..b470b849 --- /dev/null +++ b/Geometry/ParticleTrajectoryIntersect.hh @@ -0,0 +1,53 @@ +// +// Calculate the intersection point of a ParticleTrajectory with a surface +// This must be specialized for every case (every pair of trajectory and surf) +// original author: David Brown (LBNL) 2023 +// +#ifndef KinKal_ParticleTrajectoryIntersect_hh +#define KinKal_ParticleTrajectoryIntersect_hh +#include "KinKal/Geometry/Intersection.hh" +#include "KinKal/Geometry/Intersect.hh" +namespace KinKal { + // find intersection by stepping over pieces. This works when the pieces have small curvature + template Intersection intersect(ParticleTrajectory const& ptraj, SURF const& surf, TimeRange trange, double tol,TimeDir tdir = TimeDir::forwards) { + Intersection retval; + double tstart = (tdir == TimeDir::forwards) ? trange.begin() : trange.end(); + double tend = (tdir == TimeDir::forwards) ? trange.end() : trange.begin(); + size_t istart = ptraj.nearestIndex(tstart); + size_t iend = ptraj.nearestIndex(tend); + if(istart == iend){ + retval = intersect(ptraj.piece(istart),surf,trange,tol,tdir); + } else { + int istep = (tdir == TimeDir::forwards) ? 1 : -1; + do { + // if we can approximate this piece as a line, simply test at the endpoints. Time order doesn't matter + bool testinter(true); + auto ttraj = ptraj.indexTraj(istart); + double sag = ttraj->sagitta(ttraj->range().range()); + if(sag < tol){ + auto spos = ttraj->position3(ttraj->range().begin()); + bool sinside = surf.isInside(spos); + auto epos = ttraj->position3(ttraj->range().end()); + bool einside = surf.isInside(epos); + testinter = sinside != einside; + } + if(testinter){ + // try to intersect. use a temporary + auto srange = ttraj->range(); + if(srange.restrict(trange)){ + auto tinter = intersect(*ttraj,surf,srange,tol,tdir); + if(tinter.good()){ + retval = tinter; + break; + } + } + } + // update + istart += istep; + } while( istart != iend+istep); // include the last piece in the test + } + return retval; + } +} +#endif + diff --git a/Geometry/Plane.cc b/Geometry/Plane.cc new file mode 100644 index 00000000..2b6b2b66 --- /dev/null +++ b/Geometry/Plane.cc @@ -0,0 +1,42 @@ +#include "KinKal/Geometry/Plane.hh" +#include +namespace KinKal { + Plane::Plane(VEC3 const& norm, VEC3 const& udir, VEC3 const& center) : norm_(norm.Unit()), udir_(udir.Unit()), center_(center){ + // check that U is perpendicular + if(udir_.Dot(normal()) > 1e-10) throw std::invalid_argument("U direction not perpendicular to normal"); + // V direction is implicit, to create a right-handed coordinate system + vdir_ = normal().Cross(udir_); + } + + bool Plane::onSurface(VEC3 const& point, double tol) const { + return fabs(norm_.Dot(point-center_)) < tol; + } + + bool Plane::isInside(VEC3 const& point) const { + return norm_.Dot(point-center_) < 0.0; + } + + double Plane::distance(VEC3 const& point) const { + return norm_.Dot(point-center_); + } + + IntersectFlag Plane::intersect(Ray const& ray,double& dist, bool forwards, double tol) const { + IntersectFlag retval; + double ddir = norm_.Dot(ray.direction()); + if(fabs(ddir)>0.0) { + double pdist = norm_.Dot(center_ - ray.start()); + dist = pdist/ddir; + if(dist > 0.0 || !forwards){ + retval.onsurface_ = true; + retval.inbounds_ = inBounds(ray.position(dist),tol); + } + } + return retval; + } +} + +std::ostream& operator <<(std::ostream& ost, KinKal::Plane const& plane) { + ost << "Plane with center " << plane.center() << " normal " << plane.normal() + << " U direction " << plane.uDirection() << " V direction " << plane.vDirection(); + return ost; +} diff --git a/Geometry/Plane.hh b/Geometry/Plane.hh new file mode 100644 index 00000000..e113f4bd --- /dev/null +++ b/Geometry/Plane.hh @@ -0,0 +1,39 @@ +// +// Description of an unbounded plane in space. +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Plane_hh +#define KinKal_Plane_hh +#include "KinKal/Geometry/Surface.hh" +namespace KinKal { + class Plane : public Surface { + public: + virtual ~Plane() {}; + // construct from necessary parameters + Plane(VEC3 const& norm, VEC3 const& udir, VEC3 const& center); + // surface interface + bool onSurface(VEC3 const& point, double tol) const override; + // inside is defined as on the opposite side as the normal + // the absolute value should never matter + bool isInside(VEC3 const& point) const override; + bool inBounds(VEC3 const& point, double tol) const override { return true; } + double distance(VEC3 const& point) const override; + double curvature(VEC3 const& point) const override { return 0.0; } + IntersectFlag intersect(Ray const& ray,double& dist, bool forwards, double tol) const override; + VEC3 normal(VEC3 const& point) const override { return norm_; } + auto const& uDirection() const { return udir_; } + auto const& vDirection() const { return vdir_; } + auto const& normal() const { return norm_; } + Plane tangentPlane(VEC3 const& ) const override { return *this; } + // plane interface + auto const& center() const { return center_; } + private: + // note that UVW forms a right-handed orthonormal coordinate system + VEC3 norm_; // normal to the plane (W direction) + VEC3 udir_; // U direction: perpendicular to the normal + VEC3 vdir_; // V direction: perpendicular to the normal and U + VEC3 center_; // point on the plane + }; +} +std::ostream& operator <<(std::ostream& ost, KinKal::Plane const& plane); +#endif diff --git a/Geometry/Ray.cc b/Geometry/Ray.cc new file mode 100644 index 00000000..defa34aa --- /dev/null +++ b/Geometry/Ray.cc @@ -0,0 +1,5 @@ +#include "KinKal/Geometry/Ray.hh" +std::ostream& operator <<(std::ostream& ost, KinKal::Ray const& ray){ + ost << "Ray starting " << ray.start() << " direction " << ray.direction(); + return ost; +} diff --git a/Geometry/Ray.hh b/Geometry/Ray.hh new file mode 100644 index 00000000..5db202c6 --- /dev/null +++ b/Geometry/Ray.hh @@ -0,0 +1,26 @@ +// +// Description a geometric ray as a point and direction +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Ray_hh +#define KinKal_Ray_hh +#include "KinKal/General/Vectors.hh" +#include +namespace KinKal { + class Ray { + public: + // construct, making sure direciton is unit + Ray(VEC3 const& dir, VEC3 const& start) : dir_(dir.Unit()), start_(start){} + VEC3 position(double distance) const { return start_ + distance*dir_; } + VEC3 const& direction() const { return dir_; } + VEC3 const& start() const { return start_; } + // Distance along the the ray to the point of Closest Approach to a point + double distanceTo(VEC3 const& point) const { return (point - start_).Dot(dir_); } + void reverse() { dir_ *= -1.0; } // reverse direction + private: + VEC3 dir_; // direction + VEC3 start_; // starting position + }; +} +std::ostream& operator <<(std::ostream& ost, KinKal::Ray const& ray); +#endif diff --git a/Geometry/Rectangle.cc b/Geometry/Rectangle.cc new file mode 100644 index 00000000..32d3df74 --- /dev/null +++ b/Geometry/Rectangle.cc @@ -0,0 +1,25 @@ +#include "KinKal/Geometry/Rectangle.hh" +#include "Math/VectorUtil.h" +#include +using namespace ROOT::Math::VectorUtil; +namespace KinKal { + + Rectangle::Rectangle(VEC3 const& norm, VEC3 const& udir, VEC3 const& center, double uhalflen, double vhalflen) : + Plane(norm,udir,center), + uhalflen_(uhalflen), vhalflen_(vhalflen) { + if(uhalflen <= 0.0 || vhalflen <= 0.0 ) throw std::invalid_argument("Invalid Rectangle arguments"); + + } + bool Rectangle::inBounds(VEC3 const& point, double tol) const { + auto rvec = point - center(); + double udist = rvec.Dot(uDirection()); + double vdist = rvec.Dot(vDirection()); + return fabs(udist) - uhalflen_ < tol && fabs(vdist) - vhalflen_ < tol; + } +} + +std::ostream& operator <<(std::ostream& ost, KinKal::Rectangle const& rect) { + KinKal::Plane const& plane = static_cast(rect); + ost << "Rectangle in " << plane << " U, V half-lengths " << rect.uHalfLength() << " , " << rect.vHalfLength(); + return ost; +} diff --git a/Geometry/Rectangle.hh b/Geometry/Rectangle.hh new file mode 100644 index 00000000..961227e6 --- /dev/null +++ b/Geometry/Rectangle.hh @@ -0,0 +1,24 @@ +// +// Description of a rectangular planar section +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Rectangle_hh +#define KinKal_Rectangle_hh +#include "KinKal/Geometry/Plane.hh" +namespace KinKal { + class Rectangle : public Plane { + public: + virtual ~Rectangle() {}; + // construct from necessary parameters + Rectangle(VEC3 const& norm, VEC3 const& udir, VEC3 const& center, double uhalflen, double vhalflen); + // surface interface + bool inBounds(VEC3 const& point, double tol) const override; + // rectangle-specific interface + double uHalfLength() const { return uhalflen_; } + double vHalfLength() const { return vhalflen_; } + private: + double uhalflen_, vhalflen_; // u and v half-lengths + }; +} +std::ostream& operator <<(std::ostream& ost, KinKal::Rectangle const& rect); +#endif diff --git a/Geometry/Solid.hh b/Geometry/Solid.hh new file mode 100644 index 00000000..d438d889 --- /dev/null +++ b/Geometry/Solid.hh @@ -0,0 +1,16 @@ +// +// Abstract base class for a solid +// These are used to describe material objects +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Solid_hh +#define KinKal_Solid_hh +namespace KinKal { + class Solid { + public: + virtual ~Solid() {} + // determin if a point is inside the solid or not + virtual bool isInside(VEC3 const& point) const = 0; + }; +} +#endif diff --git a/Geometry/Surface.hh b/Geometry/Surface.hh new file mode 100644 index 00000000..2c9b9f14 --- /dev/null +++ b/Geometry/Surface.hh @@ -0,0 +1,35 @@ +// +// Abstract interface to a 2-dimensional surface in 3-space +// These are used in reconstruction to describe materials and +// common reference locations +// original author: David Brown (LBN) 2023 +// +#ifndef KinKal_Surface_hh +#define KinKal_Surface_hh +#include "KinKal/Geometry/Ray.hh" +#include "KinKal/Geometry/IntersectFlag.hh" +#include "KinKal/General/Vectors.hh" +namespace KinKal { + class Plane; + class Surface { + public: + virtual ~Surface() {} + // determine if the given point is on the surface, within tolerance + virtual bool onSurface(VEC3 const& point, double tol) const = 0; + // determine if a point is 'inside' the surface. For open surfaces this tests one side or the other + virtual bool isInside(VEC3 const& point) const = 0; + // local surface curvature, defined as the maximum 2nd derivative along the surface nearest that point + virtual double curvature(VEC3 const& point) const = 0; + // determine if a point on the surface is in bounds + virtual bool inBounds(VEC3 const& point, double tol) const = 0; + // perpindicular distance to a point from the surface, signed by the normal direction + virtual double distance(VEC3 const& point) const = 0; + // signed distance along a ray where it would intersect this surface; Returned flag describes what happened + virtual IntersectFlag intersect(Ray const& ray,double& dist, bool forwards, double tol) const = 0; + // normal to the surface at the given point. Direction convention is surface-dependent + virtual VEC3 normal(VEC3 const& point) const = 0; + // plane tangent to the surface at the given point. Direction convention is surface-dependent + virtual Plane tangentPlane(VEC3 const& point) const = 0; + }; +} +#endif diff --git a/Geometry/ThinSolid.cc b/Geometry/ThinSolid.cc new file mode 100644 index 00000000..cff8cb62 --- /dev/null +++ b/Geometry/ThinSolid.cc @@ -0,0 +1,6 @@ +#include "KinKal/Geometry/ThinSolid.hh" +namespace KinKal { + bool ThinSolid::isInside(VEC3 const& point) const { + return fabs(surf_->distance(point)) < halfthick_; + } +} diff --git a/Geometry/ThinSolid.hh b/Geometry/ThinSolid.hh new file mode 100644 index 00000000..584029ea --- /dev/null +++ b/Geometry/ThinSolid.hh @@ -0,0 +1,24 @@ +// +// Implementation of a 'thin' solid as a surface with a given thickness +// +#ifndef KinKal_ThinSolid_hh +#define KinKal_ThinSolid_hh +#include "KinKal/Geometry/Surface.hh" +#include "KinKal/Geometry/Solid.hh" +#include +namespace KinKal { + class ThinSolid : public Solid { + public: + virtual ~ThinSolid() {} + // construct from a pointer to a surface. Note this class TAKES OWNERSHIP of that object + ThinSolid(std::unique_ptr surf, double halfthick) : surf_(std::move(surf)), halfthick_(halfthick) {} + // Solid interface + bool isInside(VEC3 const& point) const override; + // thin solid specific interface + auto const& surface() const { return surf_; } + private: + std::unique_ptr surf_; // surface pointer + double halfthick_; // half-thickness, perpendicular to the surface + }; +} +#endif diff --git a/MatEnv/CMakeLists.txt b/MatEnv/CMakeLists.txt index 600aa20f..e3d5b22c 100644 --- a/MatEnv/CMakeLists.txt +++ b/MatEnv/CMakeLists.txt @@ -18,7 +18,9 @@ add_library(MatEnv SHARED ) # set top-level directory as include root -target_include_directories(MatEnv PRIVATE ${PROJECT_SOURCE_DIR}/..) +target_include_directories(MatEnv PUBLIC + $ + $) # set shared library version equal to project version set_target_properties(MatEnv PROPERTIES VERSION ${PROJECT_VERSION} PREFIX ${CMAKE_SHARED_LIBRARY_PREFIX}) diff --git a/MatEnv/DetMaterial.cc b/MatEnv/DetMaterial.cc index d5a03fc2..f11d3a0d 100644 --- a/MatEnv/DetMaterial.cc +++ b/MatEnv/DetMaterial.cc @@ -30,16 +30,11 @@ using std::ostream; // namespace MatEnv { - //double DetMaterial::_msmom = 15.0*MeV; double DetMaterial::_dgev = 0.153536e2; - double DetMaterial::_minkappa(1.0e-3); - //double DetMaterial::_scatterfrac(0.9999); // integrate 99.99% percent of the tail by default, this should be larger // if the materials are very thin. const double bg2lim = 0.0169; const double taulim = 8.4146e-3 ; const double twoln10 = 2.0*log(10.); - const double betapower = 1.667; // most recent PDG gives beta^-5/3 as dE/dx - const int maxnstep = 10; // maximum number of steps through a single material // should be from a physics class const double DetMaterial::_alpha(1.0/137.036); @@ -47,20 +42,15 @@ namespace MatEnv { // Constructor // - double cm(10.0); // temporary hack - DetMaterial::DetMaterial(const char* detMatName, const MtrPropObj* detMtrProp): - _elossmode(mpv), //Energy Loss model: choose 'mpv' for the Most Probable Energy Loss, or 'moyalmean' for the mean calculated via the Moyal Distribution approximation, see end of file for more information, as well as discussion about radiative losses - _msmom(15.0), - _scatterfrac(0.9999), - _cutOffEnergy(1000.), - _elossType(loss), + double cm(10.0); // convert cm to mm + DetMaterial::DetMaterial(const char* detMatName, const MtrPropObj* detMtrProp, DetMaterialConfig const& dmconf): + _elossmode(dmconf.elossmode_), _name(detMatName), _za(detMtrProp->getZ()/detMtrProp->getA()), _zeff(detMtrProp->getZ()), _aeff(detMtrProp->getA()), _radthick(detMtrProp->getRadLength()/cm/cm), _intLength(detMtrProp->getIntLength()/detMtrProp->getDensity()), - _meanion(2.*log(detMtrProp->getMeanExciEnergy()*1.0e6)), _eexc(detMtrProp->getMeanExciEnergy()), _x0(detMtrProp->getX0density()), _x1(detMtrProp->getX1density()), @@ -74,124 +64,70 @@ namespace MatEnv { { _shellCorrectionVector = new std::vector< double >(detMtrProp->getShellCorrectionVector()); - _vecNbOfAtomsPerVolume = - new std::vector< double >(detMtrProp->getVecNbOfAtomsPerVolume()); - _vecTau0 = - new std::vector< double >(detMtrProp->getVecTau0()); - _vecAlow = new std::vector< double >(detMtrProp->getVecAlow()); - _vecBlow = new std::vector< double >(detMtrProp->getVecBlow()); - _vecClow = new std::vector< double >(detMtrProp->getVecClow()); - _vecZ = new std::vector< double >(detMtrProp->getVecZ()); // compute cached values; these are used in detailed scattering models _invx0 = _density/_radthick; - _nbar = _invx0*1.587e7*pow(_zeff,1.0/3.0)/((_zeff+1)*log(287/sqrt(_zeff))); _chic2 = 1.57e1*_zeff*(_zeff+1)/_aeff; _chia2_1 = 2.007e-5*pow(_zeff,2.0/3.0); _chia2_2 = 3.34*pow(_zeff*_alpha,2); - if (detMtrProp->getEnergyTcut()>0.0) { - _cutOffEnergy = detMtrProp->getEnergyTcut(); - _elossType = deposit; - } if (detMtrProp->getState() == "gas" && detMtrProp->getDensity()<0.01) { - _scatterfrac = 0.999999; + _scatterfrac = dmconf.scatterfrac_gas_; + } else { + _scatterfrac = dmconf.scatterfrac_solid_; } + // compute the sum radiated photons*energy for the given cuttoff + double ymax = dmconf.ebrehmsfrac_; + // energy loss through 1 radiation length of this material + _erad = (_density/_radthick)*(4.0*ymax -2.0*pow(ymax,2) + pow(ymax,3)); // energy loss per radiation length per MeV + _eradvar = pow(_density/_radthick,2)*(2.0*pow(ymax,2)/3.0 - 4.0*pow(ymax,3)/9.0 + pow(ymax,4)/4.0); // energy variance per (rad length per MeV)^2 } DetMaterial::~DetMaterial() { delete _shellCorrectionVector; - delete _vecNbOfAtomsPerVolume; - delete _vecTau0; - delete _vecAlow; - delete _vecBlow; - delete _vecClow; - delete _vecZ; } // // Multiple scattering function // - double - DetMaterial::scatterAngleRMS(double mom, double pathlen,double mass) const { - if(mom>0.0){ - double beta = particleBeta(mom,mass); - // pdg formulat - // double radfrac = fabs(pathlen*_invx0); - // double sigpdg = 0.0136*sqrt(radfrac)*(1.0+0.088*log10(radfrac))/(beta*mom); - // old Kalman formula - // double oldsig = 0.011463*sqrt(radfrac)/(mom*particleBeta(mom,mass)); - // DNB 20/1/2011 Updated to use Dahl-Lynch formula from NIMB58 (1991) - double invmom2 = 1.0/pow(mom,2); - double invb2 = 1.0/pow(beta,2); - // convert to path in gm/cm^2!!! - double path = fabs(pathlen)*_density; - double chic2 = _chic2*path*invb2*invmom2; - double chia2 = _chia2_1*(1.0 + _chia2_2*invb2)*invmom2; - double omega = chic2/chia2; - static double vfactor = 0.5/(1-_scatterfrac); - double v = vfactor*omega; - static double sig2factor = 1.0/(1+_scatterfrac*_scatterfrac); - double sig2 = sig2factor*chic2*( (1+v)*log(1+v)/v - 1); - // protect against underflow - double sigdl = sqrt(std::max(0.0,sig2)); - // check - return sigdl; - } else - return 1.0; // 'infinite' scattering - } - - double - DetMaterial::dEdx(double mom,dedxtype type,double mass) const { - if(mom>0.0){ - double Eexc2 = _eexc*_eexc ; - - // New energy loss implementation - double Tmax,gamma2,beta2,bg2,rcut,delta,sh,dedx ; - double beta = particleBeta(mom,mass) ; - double gamma = particleGamma(mom,mass) ; - double tau = gamma-1. ; - - // high energy part , Bethe-Bloch formula - beta2 = beta*beta ; - gamma2 = gamma*gamma ; - bg2 = beta2*gamma2 ; - - double RateMass = e_mass_/ mass; - - Tmax = 2.*e_mass_*bg2 - /(1.+2.*gamma*RateMass+RateMass*RateMass) ; - - dedx = log(2.*e_mass_*bg2*Tmax/Eexc2); - if(type == loss) - dedx -= 2.*beta2; - else { - rcut = ( _cutOffEnergy< Tmax) ? _cutOffEnergy/Tmax : 1; - dedx += log(rcut)-(1.+rcut)*beta2; - } - - //// density correction - delta = densityCorrection(bg2); - - //// shell correction - sh = shellCorrection(bg2, tau); - - dedx -= delta + sh ; - dedx *= -_dgev*_density*_za / beta2 ; + double DetMaterial::scatterAngleVar(double mom, double pathlen,double mass) const { + if(mom>0.0){ + double beta = particleBeta(mom,mass); + // DNB 20/1/2011 Updated to use Dahl-Lynch formula from NIMB58 (1991) + double invmom2 = 1.0/pow(mom,2); + double invb2 = 1.0/pow(beta,2); + // convert to path in gm/cm^2!!! + double path = fabs(pathlen)*_density; + double chic2 = _chic2*path*invb2*invmom2; + double chia2 = _chia2_1*(1.0 + _chia2_2*invb2)*invmom2; + double omega = chic2/chia2; + static double vfactor = 0.5/(1-_scatterfrac); + double v = vfactor*omega; + static double sig2factor = 1.0/(1+_scatterfrac*_scatterfrac); + double sig2 = sig2factor*chic2*( (1+v)*log(1+v)/v - 1); + // protect against underflow + return std::max(0.0,sig2); + } else + return 1.0; // 'infinite' scattering + } - return dedx; + double DetMaterial::energyLoss(double mom,double pathlen,double mass) const { + double retval = ionizationEnergyLoss(mom,pathlen,mass); + retval += radiationEnergyLoss(mom,pathlen,mass); + return retval; + } - } else - return 0.0; - } + double DetMaterial::energyLossVar(double mom,double pathlen,double mass) const { + double retval = ionizationEnergyLossVar(mom,pathlen,mass); + retval += radiationEnergyLossVar(mom,pathlen,mass); + return retval; + } - //Replacement for dEdx-based energy loss function: Most probable energy loss is now used - //from https://pdg.lbl.gov/2019/reviews/rpp2018-rev-passage-particles-matter.pdf - double DetMaterial::energyLoss(double mom, double pathlen, double mass) const { + double DetMaterial::ionizationEnergyLoss(double mom, double pathlen, double mass) const { if(mom>0.0){ double beta = particleBeta(mom,mass) ; double xi = eloss_xi(beta, pathlen); - double deltap = energyLossMPV(mom,pathlen,mass); + double deltap = ionizationEnergyLossMPV(mom,pathlen,mass); //if using mean calculated from the Moyal Dist. Approx: (see end of file for more information) if(_elossmode == moyalmean) { return moyalMean(deltap, xi); @@ -201,7 +137,8 @@ namespace MatEnv { return 0.0; } - double DetMaterial::energyLossMPV(double mom, double pathlen, double mass) const { + //Most probable energy loss from https://pdg.lbl.gov/2019/reviews/rpp2018-rev-passage-particles-matter.pdf + double DetMaterial::ionizationEnergyLossMPV(double mom, double pathlen, double mass) const { if(mom>0.0){ //taking positive lengths pathlen = fabs(pathlen) ; @@ -213,7 +150,6 @@ namespace MatEnv { double j = 0.200 ; double tau = gamma-1; - // most probable energy loss function beta2 = beta*beta ; gamma2 = gamma*gamma ; @@ -237,285 +173,137 @@ namespace MatEnv { return 0.0; } + double DetMaterial::radiationEnergyLoss(double mom,double pathlen, double mass) const { + // truncated average bremhsstrahlung radiation energy loss + // Based on integrating the 'low-y' approximation of the brehms cross-section from 0 to ymax, using + // equation 34.29 of the RPP 'Passage of particles through matter', Phys. Rev. D 110, 030001 (2024) + double retval(0.0); + static const double small(1e-3); + if(fabs(mass-e_mass_) - constexpr static double moyalmeanfactor = 0.57721566490153286 + M_LN2 ; //approximate Euler-Mascheroni (also known as gamma) constant (0.577...), see https://mathworld.wolfram.com/Euler-MascheroniConstant.html, added to log(2). This sum is used for the calculation of the closed-form Moyal mean below - double mmean = energylossmpv + moyalsigma * moyalmeanfactor; //formula from https://reference.wolfram.com/language/ref/MoyalDistribution.html, see end of file for more information + //note: when this is moved to c++20, the eulergamma constant should be replaced by 'egamma_v' in #include + constexpr static double moyalmeanfactor = 0.57721566490153286 + M_LN2 ; //approximate Euler-Mascheroni (also known as gamma) constant (0.577...), see https://mathworld.wolfram.com/Euler-MascheroniConstant.html, added to log(2). This sum is used for the calculation of the closed-form Moyal mean below + double mmean = energylossmpv + moyalsigma * moyalmeanfactor; //formula from https://reference.wolfram.com/language/ref/MoyalDistribution.html, see end of file for more information - return -1.0 * mmean; - } + return -1.0 * mmean; + } ////Calculate density correction for energy loss - double - DetMaterial::densityCorrection(double bg2) const { - // density correction - double x = 0; - double delta = 0; - x = log(bg2)/twoln10 ; - if ( x < _x0 ) { - if(_delta0 > 0) { - delta = _delta0*pow(10.0,2*(x-_x0)); - } - else { - delta = 0.; - } - } else { - delta = twoln10*x - _bigc; - if ( x < _x1 ) - delta += _afactor * pow((_x1 - x), _mpower); - } - return delta; - } - - //// Caluclate shell correction for energy loss - double - DetMaterial::shellCorrection(double bg2, double tau) const { - double sh = 0; - double x = 1; - // shell correction - if ( bg2 > bg2lim ) { - //sh = 0. ; - //x = 1. ; - for (int k=0; k<=2; k++) { - x *= bg2 ; - sh += (*_shellCorrectionVector)[k]/x; - } + double DetMaterial::densityCorrection(double bg2) const { + // density correction + double x = 0; + double delta = 0; + x = log(bg2)/twoln10 ; + if ( x < _x0 ) { + if(_delta0 > 0) { + delta = _delta0*pow(10.0,2*(x-_x0)); } else { - //sh = 0. ; - //x = 1. ; - for (int k=0; k<2; k++) { - x *= bg2lim ; - sh += (*_shellCorrectionVector)[k]/x; - } - sh *= log(tau/_taul)/log(taulim/_taul); + delta = 0.; } - return sh; + } else { + delta = twoln10*x - _bigc; + if ( x < _x1 ) + delta += _afactor * pow((_x1 - x), _mpower); } + return delta; + } - //below, the old BTrk model 'energyLoss' function based on dE/dx has been renamed (G3 for geant3) - //and now 'energyLoss' above refers to the new most probable energy loss method - - double - DetMaterial::energyLossG3(double mom, double pathlen,double mass) const { - // make sure we take positive lengths! - pathlen = fabs(pathlen); - double dedx = dEdx(mom,_elossType,mass); - // see how far I can step, within tolerance, given this energy loss - double maxstep = maxStepdEdx(mom,mass,dedx); - // if this is larger than my path, I'm done - if(maxstep>pathlen){ - return dedx*pathlen; - } else { - // subdivide the material - unsigned nstep = std::min(int(pathlen/maxstep) + 1,maxnstep); - double step = pathlen/nstep; - double energy = particleEnergy(mom,mass); - double deltae = step*dedx; - double newenergy(energy+deltae); - double eloss(deltae); - for(unsigned istep=0;istepmass){ - // compute the new dedx given the new momentum - double newmom = particleMomentum(newenergy,mass); - deltae = step*dEdx(newmom,_elossType,mass); - // compute the loss in this step - eloss += deltae; - newenergy += deltae; - } else { - // lost all kinetic energy; stop - eloss = mass-energy; - break; - } - } - return eloss; - } - } - - - double - DetMaterial::energyGain(double mom, double pathlen, double mass) const { - // make sure we take positive lengths! - pathlen = fabs(pathlen); - double dedx = dEdx(mom,_elossType,mass); - // see how far I can step, within tolerance, given this energy loss - double maxstep = maxStepdEdx(mom,mass,dedx); - // if this is larger than my path, I'm done - if(maxstep>pathlen){ - return -dedx*pathlen; - } else { - // subdivide the material - unsigned nstep = std::min(int(pathlen/maxstep) + 1,maxnstep); - double step = pathlen/nstep; - double energy = particleEnergy(mom,mass); - double deltae = -step*dedx; - // move to the middle of the slice of material - double newenergy(energy+deltae); - double egain(deltae); - for(unsigned istep=0;istep bg2lim ) { + //sh = 0. ; + //x = 1. ; + for (int k=0; k<=2; k++) { + x *= bg2 ; + sh += (*_shellCorrectionVector)[k]/x; } } - // - // calculate the energy deposited in an absorber. That's similiar to - // energyLoss, but the delta electron correction in the Bethe Bloch is - // switched on, and there is no Bremsstrahlung - // - double - DetMaterial::energyDeposit(double mom, double pathlen, double mass) const { - double dedx = dEdx(mom,deposit,mass); - return dedx*fabs(pathlen); - } - - /********************** end of New Routines **************************/ - - - //this 'energyLossRMS' now refers to the closed-form Moyal distribution RMS, see end of file for more information - - double - DetMaterial::energyLossRMS(double mom,double pathlen,double mass) const { - if(mom>0.0){ - //taking positive lengths - pathlen = fabs(pathlen) ; - - double beta = particleBeta(mom, mass) ; - - double moyalsigma = eloss_xi(beta, pathlen); - - - //forming the Moyal RMS - constexpr static double pisqrt2 = M_PI/M_SQRT2 ; //constant that is used to calculate the Moyal closed-form RMS: pi/sqrt(2), approx. - double mrms = pisqrt2 * moyalsigma ; //from https://reference.wolfram.com/language/ref/MoyalDistribution.html - - return mrms; - - } else { - return 0.0 ; + else { + //sh = 0. ; + //x = 1. ; + for (int k=0; k<2; k++) { + x *= bg2lim ; + sh += (*_shellCorrectionVector)[k]/x; } + sh *= log(tau/_taul)/log(taulim/_taul); } + return sh; + } - - //below, the old BTrk model 'energyLossRMS' function, which has been renamed (G3 for geant3) - // - // RMS of energy loss. This is a gaussian approximation, stolen from - // Geant3 (see Phys332) - // - double - DetMaterial::energyLossRMSG3(double mom,double pathlen,double mass) const { - // double beta = particleBeta(mom,mass); - // double emax = eloss_emax(mom,mass); - // double xi = eloss_xi(beta,fabs(pathlen)); - // double kappa = xi/emax; - // double gam = sqrt(1.0-0.5*pow(beta,2)); - // // formula comes from GFLUCT.F in gphys dnb Jun 4 2004 - // // - // // this formula seriously overestimates the rms when kappa<0.001 - // // This only really affects electrons - // // as for heavier particles resolution effects already dominate when we get to - // // this range. I'll truncate - // if(kappa < _minkappa)kappa = _minkappa; - // double elossrms = xi*sqrt(gam/kappa); - // // cout << "beta = " << beta - // // << " emax = " << emax - // // << " xi = " << xi - // // << " kappa = " << kappa - // // << " elossrms = " << elossrms << endl; - // // this value is way too big: scale it down for now but this function needs a rewrite FIXME! - // return elossrms; - return 0.5*energyLossG3(mom,pathlen,mass); - } - - - // - // Functions needed for energy loss calculation, see reference above - // - double - DetMaterial::eloss_emax(double mom,double mass){ - double beta = particleBeta(mom,mass); - double gamma = particleGamma(mom,mass); - double mratio = e_mass_/mass; - double emax = 2*e_mass_*pow(beta*gamma,2)/ - (1+2*gamma*mratio + pow(mratio,2)); - if(mass <= e_mass_) - emax *= 0.5; - return emax; - } - - double - DetMaterial::eloss_xi(double beta,double pathlen) const{ - return _dgev*_za*_density*fabs(pathlen)/pow(beta,2); - } - - void - DetMaterial::print(ostream& os) const { - os << "Material " << _name << endl; + double DetMaterial::ionizationEnergyLossRMS(double mom,double pathlen,double mass) const { + if(mom>0.0){ + //taking positive lengths + pathlen = fabs(pathlen) ; + double beta = particleBeta(mom, mass) ; + double moyalsigma = eloss_xi(beta, pathlen); + //forming the Moyal RMS + constexpr static double pisqrt2 = M_PI/M_SQRT2 ; //constant that is used to calculate the Moyal closed-form RMS: pi/sqrt(2), approx. + double mrms = pisqrt2 * moyalsigma ; //from https://reference.wolfram.com/language/ref/MoyalDistribution.html + return mrms; + } else { + return 0.0 ; } + } - void - DetMaterial::printAll(ostream& os) const { - os << "Material " << _name << " has properties : " << endl - << " Effective Z = " << _zeff << endl - << " Effective A = " << _aeff << endl - << " Density (g/cm^3) = " << _density*cm*cm*cm << endl - << " Radiation Length (g/cm^2) = " << _radthick*cm*cm << endl - << " Interaction Length (g/cm^2) = " << _intLength << endl - // << " Mean Ionization energy (MeV) = " << _meanion << endl - << " Mean Ionization energy (MeV) = " << _eexc << endl; - } - double - DetMaterial::maxStepdEdx(double mom,double mass,double dEdx,double tol) { - // compute betagamma at entrance - double betagamma = particleBetaGamma(mom,mass); - double energy = particleEnergy(mom,mass); - // basic calculation, based on constant dE/dx - if(dEdx<0.0){ - double maxstep = -tol*energy/dEdx; - // Modify for steep rise at low momentum - if(betagamma<2.0) - maxstep *= pow(betagamma,2)/betapower; - return maxstep; - } - else - return 1.0e6; // large step - } + double DetMaterial::eloss_xi(double beta,double pathlen) const{ + return _dgev*_za*_density*fabs(pathlen)/pow(beta,2); + } + void DetMaterial::print(ostream& os) const { + os << "Material " << _name << endl; + } - double - DetMaterial::nSingleScatter(double mom,double pathlen, double mass) const { - double beta = particleBeta(mom,mass); - return pathlen*_nbar/pow(beta,2); - } + void DetMaterial::printAll(ostream& os) const { + os << "Material " << _name << " has properties : " << endl + << " Effective Z = " << _zeff << endl + << " Effective A = " << _aeff << endl + << " Density (g/mm^3) = " << _density*cm*cm*cm << endl + << " Radiation Length (g/mm^2) = " << _radthick*cm*cm << endl + << " Interaction Length (g/mm^2) = " << _intLength << endl + << " Mean Ionization energy (MeV) = " << _eexc << endl; + } // note the scaterring momentum here is hard-coded to the simplified Highland formula, // this should only be used as a reference, not as a real estimate of the scattering sigma!!!!! - double - DetMaterial::highlandSigma(double mom,double pathlen, double mass) const { - if(mom>0.0){ - double radfrac = _invx0*fabs(pathlen); - return _msmom*sqrt(radfrac)/(mom*particleBeta(mom,mass)); - } else - return 1.0; - } + double DetMaterial::highlandSigma(double mom,double pathlen, double mass) const { + if(mom>0.0){ + double radfrac = _invx0*fabs(pathlen); + return 15*sqrt(radfrac)/(mom*particleBeta(mom,mass)); + } else + return 1.0; + } + //Information about the Moyal Distribution Approx.: diff --git a/MatEnv/DetMaterial.hh b/MatEnv/DetMaterial.hh index 172a8d88..33080aaf 100644 --- a/MatEnv/DetMaterial.hh +++ b/MatEnv/DetMaterial.hh @@ -23,15 +23,18 @@ #include #include #include +#include namespace MatEnv { + struct DetMaterialConfig; class DetMaterial{ public: - enum dedxtype {loss=0,deposit}; + //Energy Loss model: choose 'mpv' for the Most Probable Energy Loss, or 'moyalmean' for the mean calculated via the Moyal Distribution approximation, see end of file for more information + enum energylossmode {mpv=0, moyalmean}; // // Constructor // new style - DetMaterial(const char* detName, const MtrPropObj* detMtrProp); + DetMaterial(const char* detName, const MtrPropObj* detMtrProp, DetMaterialConfig const& dmconf); ~DetMaterial(); // @@ -45,43 +48,32 @@ namespace MatEnv { bool operator == (const DetMaterial& other) const { return _name == other._name; } - double dEdx(double mom,dedxtype type,double mass) const; - - enum energylossmode {mpv=0, moyalmean}; - energylossmode _elossmode; - - void setEnergyLossMode(energylossmode elossmode) {_elossmode = elossmode;} - - double energyLossG3(double mom, double pathlen, double mass) const; // this will be the old BTrk model dE/dx-based energy loss function from Geant3 - - double energyLossRMSG3(double mom,double pathlen,double mass) const; //this is the old BTrk model energy loss RMS approx. from Geant3 - - //below, 'energyLoss' and 'energyLossRMS' now refer to the MPV-based energy loss (not dE/dx) and closed-form Moyal calculations, see end of DetMaterial.cc for more information on the Moyal distribution and parameters + // total energy loss (ionization and radiation) and variance double energyLoss(double mom,double pathlen,double mass) const; + double energyLossVar(double mom,double pathlen,double mass) const; + double energyLossRMS(double mom,double pathlen,double mass) const { + return sqrt(energyLossVar(mom,pathlen,mass)); + } + // ionization energy loss functions using closed-form Moyal calculations, see end of DetMaterial.cc for more information + double ionizationEnergyLoss(double mom,double pathlen,double mass) const; // most probable value of energy loss - double energyLossMPV(double mom,double pathlen,double mass) const; - - - double energyLossRMS(double mom,double pathlen,double mass) const; - - double energyLossVar(double mom,double pathlen,double mass) const { - double elrms = energyLossRMS(mom,pathlen,mass); + double ionizationEnergyLossMPV(double mom,double pathlen,double mass) const; + double ionizationEnergyLossRMS(double mom,double pathlen,double mass) const; + double ionizationEnergyLossVar(double mom,double pathlen,double mass) const { + double elrms = ionizationEnergyLossRMS(mom,pathlen,mass); return elrms*elrms; } - double energyDeposit(double mom, double pathlen,double mass) const; - double energyGain(double mom,double pathlen, double mass) const; - double nSingleScatter(double mom,double pathlen, double mass) const; - // terms used in first-principles single scattering model - double aParam(double mom) const { return 2.66e-6*pow(_zeff,0.33333333333333)/mom; } - double bParam(double mom) const { return 0.14/(mom*pow(_aeff,0.33333333333333)); } + // radiation (brehmsstrahlung) energy loss calculation. This is relevant only for electrons // - // Single Gaussian approximation, used in Kalman filtering - double scatterAngleRMS(double mom,double pathlen,double mass) const; - double scatterAngleVar(double mom,double pathlen,double mass) const { - double sarms = scatterAngleRMS(mom,pathlen,mass); - return sarms*sarms; + double radiationEnergyLoss(double mom,double pathlen, double mass) const; + double radiationEnergyLossVar(double mom,double pathlen, double mass) const; + // Single Gaussian approximation + double scatterAngleVar(double mom,double pathlen,double mass) const; + double scatterAngleRMS(double mom,double pathlen,double mass) const { + return sqrt(scatterAngleVar(mom,pathlen,mass)); } + double highlandSigma(double mom,double pathlen, double mass) const; static double particleEnergy(double mom,double mass) { @@ -108,31 +100,20 @@ namespace MatEnv { // // functions used to compute energy loss // - static double eloss_emax(double mom,double mass) ; double eloss_xi(double beta,double pathlen) const; double densityCorrection(double bg2) const; double shellCorrection(double bg2, double tau) const; double moyalMean(double deltap, double xi) const; - double kappa(double mom,double pathlen,double mass) const { - return eloss_xi(particleBeta(mom,mass),pathlen)/eloss_emax(mom,mass);} - // - // return the maximum step one can make through this material - // for a given momentum and particle type without dE/dx changing - // by more than the given tolerance (fraction). This is an _approximate_ - // function, based on a crude model of dE/dx. - static double maxStepdEdx(double mom,double mass, double dEdx,double tol=0.05); protected: // // Constants used in material calculations // - double _msmom; // constant in Highland scattering formula - static double _minkappa; // ionization randomization parameter static double _dgev; // energy characterizing energy loss static const double _alpha; // fine structure constant + energylossmode _elossmode; double _scatterfrac; // fraction of scattering distribution to include in RMS - double _cutOffEnergy; // cut on max energy loss - dedxtype _elossType; - + double _erad; // bremsstrahlong fractional energy loss scale + double _eradvar; // bremsstrahlong fractional energy loss variance scale // // Specific data for this material // @@ -142,7 +123,6 @@ namespace MatEnv { double _aeff; // effective Z of our material double _radthick; // radiation thickness in g/cm**2 double _intLength; // ineraction length from MatMtrObj in g/cm**2 - double _meanion; // mean ionization energy loss double _eexc; // mean ionization energy loss for new e_loss routine double _x0; /* The following specify parameters for energy loss. see Sternheimer etal,'Atomic Data and @@ -156,17 +136,10 @@ namespace MatEnv { int _noem; std::vector< double >* _shellCorrectionVector; - std::vector< double >* _vecNbOfAtomsPerVolume; - std::vector< double >* _vecTau0; - std::vector< double >* _vecAlow; - std::vector< double >* _vecBlow; - std::vector< double >* _vecClow; - std::vector< double >* _vecZ; double _taul; // cached values to speed calculations double _invx0; - double _nbar; double _chic2; double _chia2_1; double _chia2_2; @@ -178,7 +151,6 @@ namespace MatEnv { double aeff() const { return _aeff;} double radiationLength()const {return _radthick;} double intLength()const {return _intLength;} - double meanIon()const {return _meanion;} double eexc() const { return _eexc; } double X0()const {return _x0;} double X1()const {return _x1;} @@ -195,20 +167,19 @@ namespace MatEnv { void print(std::ostream& os) const; void printAll(std::ostream& os ) const; - // parameters used in ionization energy loss - static double energyLossScale() { return _dgev; } - static void setEnergyLossScale(double dgev) { _dgev = dgev; } // parameters used in ionization energy loss randomization - static double minKappa() { return _minkappa; } - static void setMinimumKappa(double minkappa) { _minkappa = minkappa; } // scattering parameter double scatterFraction() const { return _scatterfrac;} - void setScatterFraction(double scatterfrac) {_scatterfrac = scatterfrac;} - double cutOffEnergy() const { return _cutOffEnergy;} - void setCutOffEnergy(double cutOffEnergy) {_cutOffEnergy = cutOffEnergy; _elossType = deposit; } - void setDEDXtype(dedxtype elossType) { _elossType = elossType;} static constexpr double e_mass_ = 5.10998910E-01; // electron mass in MeVC^2 }; + + struct DetMaterialConfig { + DetMaterial::energylossmode elossmode_ = DetMaterial::moyalmean; // ionization energy loss mode + double scatterfrac_solid_ = 0.995; // scattering angle fraction cutoff for computing RMS in solids + double scatterfrac_gas_ = 0.999; // scattering angle fraction cutoff for computing RMS in gases + double ebrehmsfrac_ = 0.04; // electron Brehmsstrahlung energy fraction cutoff + }; + } #endif diff --git a/MatEnv/ErrLog.hh b/MatEnv/ErrLog.hh index c553f46f..f6f33f97 100644 --- a/MatEnv/ErrLog.hh +++ b/MatEnv/ErrLog.hh @@ -22,7 +22,7 @@ namespace MatEnv { public: explicit ErrMsg(Severity severity) - // : _severity(severity) + // : _severity(severity) { } @@ -33,25 +33,25 @@ namespace MatEnv { operator std::ostream&() { return std::cout; } template< class T > - ErrMsg & operator<< ( T const & t ) - { - std::cout << t; - return *this; - } + ErrMsg & operator<< ( T const & t ) + { + std::cout << t; + return *this; + } ErrMsg & operator<< ( std::ostream & f(std::ostream &) ) { - std::cout << f; - return *this; + std::cout << f; + return *this; } ErrMsg & operator<< ( std::ios_base & f(std::ios_base &) ) { - std::cout << f; - return *this; + std::cout << f; + return *this; } void operator<< ( void f(ErrMsg &) ) { - f(*this); + f(*this); } static Severity ErrLoggingLevel; @@ -60,11 +60,11 @@ namespace MatEnv { }; // Implementation of the global operator<< that allows "endmsg" to be used - // with a plain ostream as a manipulator. + // with a plain ostream as a manipulator. inline std::ostream& operator<<( std::ostream& os, void (* fp)(ErrMsg&) ) { os< namespace MatEnv { - MatDBInfo::MatDBInfo(FileFinderInterface const& interface, DetMaterial::energylossmode elossmode ) : - _genMatFactory(RecoMatFactory::getInstance(interface)), _elossmode(elossmode) + MatDBInfo::MatDBInfo(FileFinderInterface const& interface, DetMaterialConfig const& dmconf ) : + _genMatFactory(RecoMatFactory::getInstance(interface)), _dmconf(dmconf) {;} - MatDBInfo::~MatDBInfo() { - // delete the materials - std::map< std::string*, DetMaterial*, PtrLess >::iterator - iter = _matList.begin(); - for (; iter != _matList.end(); ++iter) { - delete iter->first; - delete iter->second; - } - _matList.clear(); - } + MatDBInfo::MatDBInfo(FileFinderInterface const& interface) : MatDBInfo(interface, DetMaterialConfig()){} + + + MatDBInfo::~MatDBInfo() {;} void MatDBInfo::declareMaterial( const std::string& db_name, @@ -44,11 +38,11 @@ namespace MatEnv { return; } - const DetMaterial* MatDBInfo::findDetMaterial( const std::string& matName ) const + const std::shared_ptr MatDBInfo::findDetMaterial( const std::string& matName ) const { - DetMaterial* theMat; - std::map< std::string*, DetMaterial*, PtrLess >::const_iterator pos; - if ((pos = _matList.find((std::string*)&matName)) != _matList.end()) { + std::shared_ptr theMat; + std::map< std::string, std::shared_ptr >::const_iterator pos; + if ((pos = _matList.find(matName)) != _matList.end()) { theMat = pos->second; } else { // first, look for aliases @@ -71,20 +65,19 @@ namespace MatEnv { return theMat; } - DetMaterial* MatDBInfo::createDetMaterial( const std::string& db_name, + std::shared_ptr MatDBInfo::createDetMaterial( const std::string& db_name, const std::string& detMatName ) const { MtrPropObj* genMtrProp; - DetMaterial* theMat; + std::shared_ptr theMat; genMtrProp = _genMatFactory->GetMtrProperties(db_name); if(genMtrProp != 0){ - theMat = new DetMaterial( detMatName.c_str(), genMtrProp ) ; - theMat->setEnergyLossMode(_elossmode); - that()->_matList[new std::string( detMatName )] = theMat; + theMat = std::make_shared ( detMatName.c_str(), genMtrProp, _dmconf ) ; + that()->_matList[ detMatName ] = theMat; return theMat; } else { - return 0; + return nullptr; } } } diff --git a/MatEnv/MatDBInfo.hh b/MatEnv/MatDBInfo.hh index dc56abc5..5aa7720f 100644 --- a/MatEnv/MatDBInfo.hh +++ b/MatEnv/MatDBInfo.hh @@ -28,6 +28,7 @@ #include "KinKal/MatEnv/FileFinderInterface.hh" #include #include +#include namespace MatEnv { @@ -36,27 +37,28 @@ namespace MatEnv { class MatDBInfo : public MaterialInfo { public: - MatDBInfo(FileFinderInterface const& interface, DetMaterial::energylossmode elossmode); + MatDBInfo(FileFinderInterface const& interface); + MatDBInfo(FileFinderInterface const& interface, DetMaterialConfig const& dmconf); virtual ~MatDBInfo(); // Find the material, given the name - const DetMaterial* findDetMaterial( const std::string& matName ) const override; + const std::shared_ptr findDetMaterial( const std::string& matName ) const override; // utility functions private: - DetMaterial* createDetMaterial( const std::string& dbName, + std::shared_ptr createDetMaterial( const std::string& dbName, const std::string& detMatName ) const; void declareMaterial( const std::string& dbName, const std::string& detMatName ); // Cache of RecoMatFactory pointer RecoMatFactory* _genMatFactory; // Cache of list of materials for DetectorModel - std::map< std::string*, DetMaterial*, PtrLess > _matList; + std::map< std::string, std::shared_ptr > _matList; // Map for reco- and DB material names std::map< std::string, std::string > _matNameMap; // function to cast-off const MatDBInfo* that() const { return const_cast(this); } - DetMaterial::energylossmode _elossmode; + DetMaterialConfig _dmconf; }; } diff --git a/MatEnv/MaterialInfo.hh b/MatEnv/MaterialInfo.hh index 186607df..c81cf9fe 100644 --- a/MatEnv/MaterialInfo.hh +++ b/MatEnv/MaterialInfo.hh @@ -21,6 +21,7 @@ #include #include +#include namespace MatEnv { class DetMaterial; @@ -29,7 +30,7 @@ namespace MatEnv { MaterialInfo(){;} virtual ~MaterialInfo(){;} // Find the material, given the name - virtual const DetMaterial* findDetMaterial( const std::string& matName ) const = 0; + virtual const std::shared_ptr findDetMaterial( const std::string& matName ) const = 0; const std::vector< std::string >& materialNames() const { return _matNameList; } std::vector< std::string >& materialNames() { diff --git a/MatEnv/MtrPropObj.cc b/MatEnv/MtrPropObj.cc index ce751e36..8919ed07 100644 --- a/MatEnv/MtrPropObj.cc +++ b/MatEnv/MtrPropObj.cc @@ -3,9 +3,9 @@ // $Id: MtrPropObj.cc 516 2010-01-15 08:22:00Z stroili $ // // Description: -// Class MtrPropObj +// Class MtrPropObj // Source file (see.hh file for more details) -// +// // Environment: // Software developed for the BaBar Detector at the SLAC B-Factory. // @@ -42,7 +42,7 @@ namespace MatEnv { static const double GasThreshold = 10.*mg/cm3; - // Constructor to create Material from scratch + // Constructor to create Material from scratch MtrPropObj::MtrPropObj() : _matDensity(0), @@ -117,17 +117,17 @@ namespace MatEnv { _shellCorrectionVector = new std::vector< double >( numShellV ); _state = new std::string( theMaterial->getState() ); - if (_matDensity*g/cm3 < universe_mean_density) { + if (_matDensity*g/cm3 < universe_mean_density) { ErrMsg(error) - << " Warning: define a material with density=0 is not allowed. \n" - << " The material " << *_matName << " will be constructed with the" - << " default minimal density: " << universe_mean_density/(g/cm3) - << "g/cm3" << endmsg; + << " Warning: define a material with density=0 is not allowed. \n" + << " The material " << *_matName << " will be constructed with the" + << " default minimal density: " << universe_mean_density/(g/cm3) + << "g/cm3" << endmsg; _matDensity = universe_mean_density/(g/cm3); - } + } int ncomp = theMaterial->getNbrComp(); - if (ncomp == 0) + if (ncomp == 0) { // Initialize theElementVector allocating one // element corresponding to this material @@ -153,7 +153,7 @@ namespace MatEnv { } - MtrPropObj::~MtrPropObj() + MtrPropObj::~MtrPropObj() { delete _matName; delete _state; @@ -221,30 +221,30 @@ namespace MatEnv { MtrPropObj::operator==(const MtrPropObj& other) const { bool equal = true; - if ( *_matName != other.getName() || - _matDensity != other.getDensity() || - _cdensity != other.getCdensity() || - _mdensity != other.getMdensity() || - _adensity != other.getAdensity() || - _x0density != other.getX0density() || - _x1density != other.getX1density() || - _taul != other.getTaul() || - _dEdxFactor != other.getDEdxFactor() || - _meanExciEnergy != other.getMeanExciEnergy() || - _radLength != other.getRadLength() || - _intLength != other.getIntLength() - ) equal=false; + if ( *_matName != other.getName() || + _matDensity != other.getDensity() || + _cdensity != other.getCdensity() || + _mdensity != other.getMdensity() || + _adensity != other.getAdensity() || + _x0density != other.getX0density() || + _x1density != other.getX1density() || + _taul != other.getTaul() || + _dEdxFactor != other.getDEdxFactor() || + _meanExciEnergy != other.getMeanExciEnergy() || + _radLength != other.getRadLength() || + _intLength != other.getIntLength() + ) equal=false; const std::vector< double >& myV = *_shellCorrectionVector; const std::vector< double >& otherV = other.getShellCorrectionVector(); size_t i=0, length = myV.size(); if ( length != otherV.size() ) { - equal = false; + equal = false; } else { - for ( i=0; i( _maxNbComponents ); - _massFractionVector = new std::vector< double >( _maxNbComponents ); + _atomsVector = new std::vector< int >( _maxNbComponents ); + _massFractionVector = new std::vector< double >( _maxNbComponents ); } // filling ... if ( _numberOfElements < _maxNbComponents ) { - (*_theElementVector)[_numberOfElements] = element; - (*_atomsVector) [_numberOfElements] = nAtoms; - _numberOfComponents = ++_numberOfElements; + (*_theElementVector)[_numberOfElements] = element; + (*_atomsVector) [_numberOfElements] = nAtoms; + _numberOfComponents = ++_numberOfElements; } else - ErrMsg(error) - << "Attempt to add more than the declared number of elements." - << endmsg; + ErrMsg(error) + << "Attempt to add more than the declared number of elements." + << endmsg; // filled. - if ( _numberOfElements == _maxNbComponents ) { - // compute proportion by mass - size_t i=0; - double Zmol(0.), Amol(0.); - for (i=0;i<_numberOfElements;i++) { - Zmol += (*_atomsVector)[i]*(*_theElementVector)[i]->getZ(); - Amol += (*_atomsVector)[i]*(*_theElementVector)[i]->getA(); - } - for (i=0;i<_numberOfElements;i++) { - (*_massFractionVector)[i] = (*_atomsVector)[i]*(*_theElementVector)[i]->getA()/Amol; - } - ComputeDerivedQuantities(); + if ( _numberOfElements == _maxNbComponents ) { + // compute proportion by mass + size_t i=0; + double Amol(0.); + for (i=0;i<_numberOfElements;i++) { + Amol += (*_atomsVector)[i]*(*_theElementVector)[i]->getA(); + } + for (i=0;i<_numberOfElements;i++) { + (*_massFractionVector)[i] = (*_atomsVector)[i]*(*_theElementVector)[i]->getA()/Amol; + } + ComputeDerivedQuantities(); } } @@ -361,53 +360,53 @@ namespace MatEnv { { // if atomsVector is non-NULL, complain. Apples and oranges. $$$ if ( _atomsVector != 0 ) { - ErrMsg(error) - << "This material is already being defined via elements by" - << "atoms." << endmsg; + ErrMsg(error) + << "This material is already being defined via elements by" + << "atoms." << endmsg; } // initialization if ( _numberOfComponents == 0) { - _massFractionVector = new std::vector< double >( 100 ); - // zero all the elements before doing += - for ( size_t zeroer = 0; zeroer<100; zeroer++ ) - (*_massFractionVector)[zeroer]=0; + _massFractionVector = new std::vector< double >( 100 ); + // zero all the elements before doing += + for ( size_t zeroer = 0; zeroer<100; zeroer++ ) + (*_massFractionVector)[zeroer]=0; } // filling ... if (_numberOfComponents < _maxNbComponents) { - size_t el = 0; - while ((el<_numberOfElements)&&(element!=(*_theElementVector)[el])) el++; - if (el<_numberOfElements) (*_massFractionVector)[el] += fraction; - else { - if(el>=_theElementVector->size()) _theElementVector->resize(el+1); - (*_theElementVector)[el] = element; - (*_massFractionVector)[el] = fraction; - _numberOfElements ++; - } - _numberOfComponents++; + size_t el = 0; + while ((el<_numberOfElements)&&(element!=(*_theElementVector)[el])) el++; + if (el<_numberOfElements) (*_massFractionVector)[el] += fraction; + else { + if(el>=_theElementVector->size()) _theElementVector->resize(el+1); + (*_theElementVector)[el] = element; + (*_massFractionVector)[el] = fraction; + _numberOfElements ++; + } + _numberOfComponents++; } else - ErrMsg(error) - << "Attempt to add more than the declared number of components." - << endmsg; + ErrMsg(error) + << "Attempt to add more than the declared number of components." + << endmsg; // filled. if (_numberOfComponents == _maxNbComponents) { - // check sum of weights -- OK? - size_t i; - double wtSum(0.0); - for (i=0;i<_numberOfElements;i++) { wtSum += (*_massFractionVector)[i]; } - if (fabs(1.-wtSum) > 0.001) { - ErrMsg(error) - << "WARNING !! - Fractional masses do not sum to 1: Delta is > 0.001" - << "( the weights are NOT renormalized; the results may be wrong)" - << endmsg; - } - - ComputeDerivedQuantities(); + // check sum of weights -- OK? + size_t i; + double wtSum(0.0); + for (i=0;i<_numberOfElements;i++) { wtSum += (*_massFractionVector)[i]; } + if (fabs(1.-wtSum) > 0.001) { + ErrMsg(error) + << "WARNING !! - Fractional masses do not sum to 1: Delta is > 0.001" + << "( the weights are NOT renormalized; the results may be wrong)" + << endmsg; + } + + ComputeDerivedQuantities(); } } @@ -419,55 +418,55 @@ namespace MatEnv { { // if atomsVector is non-NULL, complain. Apples and oranges. $$$ if ( _atomsVector != 0 ) { - ErrMsg(error) - << "This material is already being defined via elements by" - << "atoms." << endmsg; + ErrMsg(error) + << "This material is already being defined via elements by" + << "atoms." << endmsg; } // initialization if (_numberOfComponents == 0) { - _massFractionVector = new std::vector< double >( 100 ); - // zero all the elements before doing += - for ( size_t zeroer = 0; zeroer<100; zeroer++ ) - (*_massFractionVector)[zeroer]=0; + _massFractionVector = new std::vector< double >( 100 ); + // zero all the elements before doing += + for ( size_t zeroer = 0; zeroer<100; zeroer++ ) + (*_massFractionVector)[zeroer]=0; } // filling ... if (_numberOfComponents < _maxNbComponents) { - for (size_t elm=0; elm < material->getNumberOfElements(); elm++) - { - ElmPropObj* element = (*(material->getElementVector()))[elm]; - size_t el = 0; - while ((el<_numberOfElements)&&(element!=(*_theElementVector)[el])) el++; - if (el<_numberOfElements) (*_massFractionVector)[el] += fraction - *(material->getFractionVector())[elm]; - else { - if(el>=_theElementVector->size()) _theElementVector->resize(el+1); - (*_theElementVector)[el] = element; - (*_massFractionVector)[el] = fraction*(material->getFractionVector())[elm]; - _numberOfElements ++; - } - } - _numberOfComponents++; + for (size_t elm=0; elm < material->getNumberOfElements(); elm++) + { + ElmPropObj* element = (*(material->getElementVector()))[elm]; + size_t el = 0; + while ((el<_numberOfElements)&&(element!=(*_theElementVector)[el])) el++; + if (el<_numberOfElements) (*_massFractionVector)[el] += fraction + *(material->getFractionVector())[elm]; + else { + if(el>=_theElementVector->size()) _theElementVector->resize(el+1); + (*_theElementVector)[el] = element; + (*_massFractionVector)[el] = fraction*(material->getFractionVector())[elm]; + _numberOfElements ++; + } + } + _numberOfComponents++; } else - ErrMsg(error) - << "Attempt to add more than the declared number of components." - << endmsg; + ErrMsg(error) + << "Attempt to add more than the declared number of components." + << endmsg; // filled. if (_numberOfComponents == _maxNbComponents) { - // check sum of weights -- OK? - size_t i; - double wtSum(0.0); - for (i=0;i<_numberOfElements;i++) { wtSum += (*_massFractionVector)[i]; } - if (fabs(1.-wtSum) > 0.001) { - ErrMsg(error) - << "WARNING !! - Fractional masses do not sum to 1: Delta is > 0.001" - << "( the weights are NOT renormalized; the results may be wrong)" - << endmsg; - } - - ComputeDerivedQuantities(); + // check sum of weights -- OK? + size_t i; + double wtSum(0.0); + for (i=0;i<_numberOfElements;i++) { wtSum += (*_massFractionVector)[i]; } + if (fabs(1.-wtSum) > 0.001) { + ErrMsg(error) + << "WARNING !! - Fractional masses do not sum to 1: Delta is > 0.001" + << "( the weights are NOT renormalized; the results may be wrong)" + << endmsg; + } + + ComputeDerivedQuantities(); } } @@ -486,22 +485,22 @@ namespace MatEnv { _totNbOfAtomsPerVolume = 0.; _vecNbOfAtomsPerVolume = new std::vector< double >( _numberOfElements ); _totNbOfElectPerVolume = 0.; - for (size_t i=0;i<_numberOfElements;i++) + for (size_t i=0;i<_numberOfElements;i++) { - Zi = (*_theElementVector)[i]->getZ(); - Ai = (*_theElementVector)[i]->getA(); - Ai *= g/mole; + Zi = (*_theElementVector)[i]->getZ(); + Ai = (*_theElementVector)[i]->getA(); + Ai *= g/mole; - ElmPropObj* element = (*_theElementVector)[i]; - (*_theTau0Vector)[i] = element->getTau0(); - (*_theAlowVector)[i] = element->getAlow(); - (*_theBlowVector)[i] = element->getBlow(); - (*_theClowVector)[i] = element->getClow(); - (*_theZVector)[i] = element->getZ(); + ElmPropObj* element = (*_theElementVector)[i]; + (*_theTau0Vector)[i] = element->getTau0(); + (*_theAlowVector)[i] = element->getAlow(); + (*_theBlowVector)[i] = element->getBlow(); + (*_theClowVector)[i] = element->getClow(); + (*_theZVector)[i] = element->getZ(); - (*_vecNbOfAtomsPerVolume)[i] = Avogadro*density*(*_massFractionVector)[i]/Ai; - _totNbOfAtomsPerVolume += (*_vecNbOfAtomsPerVolume)[i]; - _totNbOfElectPerVolume += (*_vecNbOfAtomsPerVolume)[i]*Zi; + (*_vecNbOfAtomsPerVolume)[i] = Avogadro*density*(*_massFractionVector)[i]/Ai; + _totNbOfAtomsPerVolume += (*_vecNbOfAtomsPerVolume)[i]; + _totNbOfElectPerVolume += (*_vecNbOfAtomsPerVolume)[i]*Zi; } @@ -515,7 +514,7 @@ namespace MatEnv { { double radinv = 0.0 ; for (size_t i=0;i<_numberOfElements;i++) { - radinv += (*_vecNbOfAtomsPerVolume)[i]*((*_theElementVector)[i]->getRadTsai()); + radinv += (*_vecNbOfAtomsPerVolume)[i]*((*_theElementVector)[i]->getRadTsai()); } _radLength = (radinv <= 0.0 ? DBL_MAX : 1./radinv); @@ -541,8 +540,8 @@ namespace MatEnv { MtrPropObj::ComputeOtherParams() { - // _dEdxFactor (in Mev/(g/cm2)) is exactly equal to 0.153536*Z/A - // (see R.M. Sternheimer et al. in Atomic and Nuclear Data Tables + // _dEdxFactor (in Mev/(g/cm2)) is exactly equal to 0.153536*Z/A + // (see R.M. Sternheimer et al. in Atomic and Nuclear Data Tables // vol. 30, N02, March 1984, page 267.) _dEdxFactor = twopi_mc2_rcl2*_totNbOfElectPerVolume; @@ -565,23 +564,23 @@ namespace MatEnv { for (size_t i=0; i<_numberOfElements; i++) { - logMeanExciEnergy += (*_vecNbOfAtomsPerVolume)[i] - *((*_theElementVector)[i]->getZ()) - *log((*_theElementVector)[i]->getMeanExciEnergy()); + logMeanExciEnergy += (*_vecNbOfAtomsPerVolume)[i] + *((*_theElementVector)[i]->getZ()) + *log((*_theElementVector)[i]->getMeanExciEnergy()); } logMeanExciEnergy /= _totNbOfElectPerVolume; _meanExciEnergy = exp(logMeanExciEnergy); for (int j=0; j<=2; j++) { - (*_shellCorrectionVector)[j] = 0.; - for (size_t k=0; k<_numberOfElements; k++) - { - (*_shellCorrectionVector)[j] += (*_vecNbOfAtomsPerVolume)[k] - *((*_theElementVector)[k]->getShellCorrectionVector()[j]); - } - (*_shellCorrectionVector)[j] /= _totNbOfElectPerVolume; - } + (*_shellCorrectionVector)[j] = 0.; + for (size_t k=0; k<_numberOfElements; k++) + { + (*_shellCorrectionVector)[j] += (*_vecNbOfAtomsPerVolume)[k] + *((*_theElementVector)[k]->getShellCorrectionVector()[j]); + } + (*_shellCorrectionVector)[j] /= _totNbOfElectPerVolume; + } // Compute parameters for the density effect correction in DE/Dx formula. // The parametrization is from R.M. Sternheimer, Phys. Rev.B,3:3681 (1971) @@ -592,64 +591,64 @@ namespace MatEnv { _cdensity = 1. + log(_meanExciEnergy*_meanExciEnergy/(Cd2*_totNbOfElectPerVolume)); // - // condensed materials + // condensed materials // if ((*_state == "solid")||(*_state == "liquid")) { - const double E100keV = 100.*keV; - const double ClimiS[] = {3.681 , 5.215 }; - const double X0valS[] = {1.0 , 1.5 }; - const double X1valS[] = {2.0 , 3.0 }; + const double E100keV = 100.*keV; + const double ClimiS[] = {3.681 , 5.215 }; + const double X0valS[] = {1.0 , 1.5 }; + const double X1valS[] = {2.0 , 3.0 }; - if(_meanExciEnergy < E100keV) icase = 0 ; - else icase = 1 ; + if(_meanExciEnergy < E100keV) icase = 0 ; + else icase = 1 ; - if(_cdensity < ClimiS[icase]) _x0density = 0.2; - else _x0density = 0.326*_cdensity-X0valS[icase]; + if(_cdensity < ClimiS[icase]) _x0density = 0.2; + else _x0density = 0.326*_cdensity-X0valS[icase]; - _x1density = X1valS[icase] ; - _mdensity = 3.0; + _x1density = X1valS[icase] ; + _mdensity = 3.0; - //special: Hydrogen - if ((_numberOfElements==1)&&(getZ()==1)) { - _x0density = 0.425; _x1density = 2.0; _mdensity = 5.949; - } + //special: Hydrogen + if ((_numberOfElements==1)&&(getZ()==1)) { + _x0density = 0.425; _x1density = 2.0; _mdensity = 5.949; + } } // // gases // - if (*_state == "gas") { + if (*_state == "gas") { - const double ClimiG[] = { 10. , 10.5 , 11. , 11.5 , 12.25 , 13.804}; - const double X0valG[] = { 1.6 , 1.7 , 1.8 , 1.9 , 2.0 , 2.0 }; - const double X1valG[] = { 4.0 , 4.0 , 4.0 , 4.0 , 4.0 , 5.0 }; + const double ClimiG[] = { 10. , 10.5 , 11. , 11.5 , 12.25 , 13.804}; + const double X0valG[] = { 1.6 , 1.7 , 1.8 , 1.9 , 2.0 , 2.0 }; + const double X1valG[] = { 4.0 , 4.0 , 4.0 , 4.0 , 4.0 , 5.0 }; - icase = 5; - _x0density = 0.326*_cdensity-2.5 ; _x1density = 5.0 ; _mdensity = 3. ; - while((icase > 0)&&(_cdensity < ClimiG[icase])) icase-- ; - _x0density = X0valG[icase] ; _x1density = X1valG[icase] ; + icase = 5; + _x0density = 0.326*_cdensity-2.5 ; _x1density = 5.0 ; _mdensity = 3. ; + while((icase > 0)&&(_cdensity < ClimiG[icase])) icase-- ; + _x0density = X0valG[icase] ; _x1density = X1valG[icase] ; - //special: Hydrogen - if ((_numberOfElements==1)&&(getZ()==1.)) { - _x0density = 1.837; _x1density = 3.0; _mdensity = 4.754; - } + //special: Hydrogen + if ((_numberOfElements==1)&&(getZ()==1.)) { + _x0density = 1.837; _x1density = 3.0; _mdensity = 4.754; + } - //special: Helium - if ((_numberOfElements==1)&&(getZ()==2.)) { - _x0density = 2.191; _x1density = 3.0; _mdensity = 3.297; - } + //special: Helium + if ((_numberOfElements==1)&&(getZ()==2.)) { + _x0density = 2.191; _x1density = 3.0; _mdensity = 3.297; + } - // change parameters if the gas is not in STP. - // For the correction the density(STP) is needed. - // Density(STP) is calculated here : + // change parameters if the gas is not in STP. + // For the correction the density(STP) is needed. + // Density(STP) is calculated here : - double DensitySTP = _matDensity*STP_Pressure*_temp/(_pressure*STP_Temperature); + double DensitySTP = _matDensity*STP_Pressure*_temp/(_pressure*STP_Temperature); - double ParCorr = log(_matDensity/DensitySTP) ; + double ParCorr = log(_matDensity/DensitySTP) ; - _cdensity -= ParCorr; - _x0density -= ParCorr/twoln10 ; - _x1density -= ParCorr/twoln10 ; + _cdensity -= ParCorr; + _x0density -= ParCorr/twoln10 ; + _x1density -= ParCorr/twoln10 ; } double Xa = _cdensity/twoln10 ; @@ -660,50 +659,50 @@ namespace MatEnv { MtrPropObj::print() { ErrMsg( routine ) << " Name: " << getName() - << " Density: " << getDensity() << endl - << " _cdensity: " << getCdensity() - << " _mdensity: " << getMdensity() - << " _adensity: " << getAdensity() - << " _x0density: " << getX0density() - << " _x1density: " << getX1density() << endl - << " _taul: " << getTaul() - << " _meanExciEnergy: " << getMeanExciEnergy() - << " _radLength: " << getRadLength() - << " _intLength: " << getIntLength() - << endmsg; + << " Density: " << getDensity() << endl + << " _cdensity: " << getCdensity() + << " _mdensity: " << getMdensity() + << " _adensity: " << getAdensity() + << " _x0density: " << getX0density() + << " _x1density: " << getX1density() << endl + << " _taul: " << getTaul() + << " _meanExciEnergy: " << getMeanExciEnergy() + << " _radLength: " << getRadLength() + << " _intLength: " << getIntLength() + << endmsg; } double MtrPropObj::getZ() const - { + { if (_numberOfElements > 1) { - // ErrMsg(error) - // << "WARNING in getZ. The material: " << *_matName << " is a mixture." - // <<" the Atomic number is not well defined." << endmsg; - double Zsum = 0.0; - for (size_t i = 0; i<_numberOfElements; i++) - Zsum += (*_vecNbOfAtomsPerVolume)[i]/_totNbOfAtomsPerVolume * - ((*_theElementVector)[i]->getZ()); - return Zsum; - - } - return (*_theElementVector)[0]->getZ(); + // ErrMsg(error) + // << "WARNING in getZ. The material: " << *_matName << " is a mixture." + // <<" the Atomic number is not well defined." << endmsg; + double Zsum = 0.0; + for (size_t i = 0; i<_numberOfElements; i++) + Zsum += (*_vecNbOfAtomsPerVolume)[i]/_totNbOfAtomsPerVolume * + ((*_theElementVector)[i]->getZ()); + return Zsum; + + } + return (*_theElementVector)[0]->getZ(); } double MtrPropObj::getA() const - { - if (_numberOfElements > 1) { - // ErrMsg(error) - // << "WARNING in getA. The material: " << *_matName << " is a mixture." - // <<" the Atomic mass is not well defined." << endmsg; - double Asum = 0.0; - for (size_t i = 0; i<_numberOfElements; i++) - Asum += (*_vecNbOfAtomsPerVolume)[i]/_totNbOfAtomsPerVolume * - ((*_theElementVector)[i]->getA()); - return Asum; - } - return (*_theElementVector)[0]->getA(); + { + if (_numberOfElements > 1) { + // ErrMsg(error) + // << "WARNING in getA. The material: " << *_matName << " is a mixture." + // <<" the Atomic mass is not well defined." << endmsg; + double Asum = 0.0; + for (size_t i = 0; i<_numberOfElements; i++) + Asum += (*_vecNbOfAtomsPerVolume)[i]/_totNbOfAtomsPerVolume * + ((*_theElementVector)[i]->getA()); + return Asum; + } + return (*_theElementVector)[0]->getA(); } } diff --git a/MatEnv/MtrPropObj.hh b/MatEnv/MtrPropObj.hh index a78bbb73..d0bbdc87 100644 --- a/MatEnv/MtrPropObj.hh +++ b/MatEnv/MtrPropObj.hh @@ -3,16 +3,16 @@ // $Id: MtrPropObj.hh 516 2010-01-15 08:22:00Z stroili $ // // Description: -// Class MtrPropObj -// this class computes the materials properties (radiation length -// and ionisation parameters) from the basic elements data stored -// in the condition database. -// The functions AddElement(), AddMaterial(), -// ComputeDerivedQuantities(), ComputeRadiationLength(), -// ComputeIonisationParam() are taken from Geant4 -// (the 4 first are from G4Material and the last one is from -// G4IonisParamMat). -// +// Class MtrPropObj +// this class computes the materials properties (radiation length +// and ionisation parameters) from the basic elements data stored +// in the condition database. +// The functions AddElement(), AddMaterial(), +// ComputeDerivedQuantities(), ComputeRadiationLength(), +// ComputeIonisationParam() are taken from Geant4 +// (the 4 first are from G4Material and the last one is from +// G4IonisParamMat). +// // Environment: // Software developed for the BaBar Detector at the SLAC B-Factory. // @@ -28,11 +28,11 @@ //------------- // C Headers -- //------------- -#include //--------------- // C++ Headers -- //--------------- +//#include //---------------------- // Base Class Headers -- @@ -49,15 +49,15 @@ namespace MatEnv { class MtrPropObj { - public: + public: enum { numShellV = 3 }; - // Default constructor - MtrPropObj(); + // Default constructor + MtrPropObj(); // Constructor to create Material from a combination of Elements or Materials - MtrPropObj( MatMaterialObj* theMaterial ); + MtrPropObj( MatMaterialObj* theMaterial ); // Copy constructor MtrPropObj(const MtrPropObj&); @@ -66,7 +66,7 @@ namespace MatEnv { ~MtrPropObj(); // Operators (Assignment op) - MtrPropObj& operator=( const MtrPropObj& ); + MtrPropObj& operator=( const MtrPropObj& ); bool operator==( const MtrPropObj& ) const; // Add and element, giving number of atoms @@ -107,7 +107,7 @@ namespace MatEnv { const std::vector< double >& getVecClow() const; const std::vector< double >& getVecZ() const; - double getZ() const; + double getZ() const; double getA() const; const std::string& getState() const; @@ -125,15 +125,15 @@ namespace MatEnv { // Basic data members (define a Material) - std::string* _matName; - double _matDensity; + std::string* _matName; + double _matDensity; double _cdensity; double _mdensity; double _adensity; double _x0density; double _x1density; double _taul; - double _radLength; + double _radLength; double _intLength; double _dEdxFactor; @@ -163,8 +163,8 @@ namespace MatEnv { std::vector< double >* _vecNbOfAtomsPerVolume; // vector of nb of atoms per volume - double _totNbOfAtomsPerVolume; // total nb of atoms per volume - double _totNbOfElectPerVolume; // total nb of electrons per volume + double _totNbOfAtomsPerVolume; // total nb of atoms per volume + double _totNbOfElectPerVolume; // total nb of electrons per volume }; } diff --git a/README.md b/README.md index 95bfc645..b27dd134 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ # Kinematic Kalman filter track fit code package - KinKal implements a kinematic Kalman filter track fit (future ref to CTD/pub). + KinKal implements a kinematic Kalman filter track fit, as described in https://www.epj-conferences.org/articles/epjconf/abs/2024/05/epjconf_chep2024_06001/epjconf_chep2024_06001.html. The primary class of KinKal is Track, which shares the state describing the fit inputs (hits, material interactions, BField corrections, etc), and owns the result of the fit, and the methods for computing it. The fit result is expressed as a piecewise kinematic covariant @@ -42,7 +42,9 @@ uncertainties, convergence critera, and flags to specify which physical effects (like material interactions) should be updated based on the current complete kinematic trajectory estimate. KinKal iterates each meta-iteration to algebraic convergence, by re-evaluating the extended Kalman filter derivatives, holding the physical paramters of the fit fixed. - The fit is performed on construction. + + The fit is performed on construction. After a fit, the track may be extrapolated by dead-reckoning through a magnetic field map and additional materials to make + predictions. KinKal uses the root SVector and SMatrix classes for algebraic manipulation, and GenVector classes to represent geometric and kinematic vectors, both part of the root Math package. These are described on the [root website](https://root.cern.ch/root/html608/namespaceROOT_1_1Math.html) diff --git a/Tests/BFieldMapTest.hh b/Tests/BFieldMapTest.hh index 4bba870e..530a811b 100644 --- a/Tests/BFieldMapTest.hh +++ b/Tests/BFieldMapTest.hh @@ -2,7 +2,6 @@ // test basic functions of BFieldMap class // #include "KinKal/Trajectory/ParticleTrajectory.hh" -#include "KinKal/Trajectory/Line.hh" #include "KinKal/Trajectory/ClosestApproach.hh" #include "KinKal/General/BFieldMap.hh" #include "KinKal/General/Vectors.hh" @@ -10,13 +9,12 @@ #include "KinKal/Tests/ToyMC.hh" #include -#include +#include #include #include #include "TFile.h" #include "TH1F.h" -#include "TSystem.h" #include "THelix.h" #include "TPolyLine3D.h" #include "TAxis3D.h" @@ -40,7 +38,7 @@ void print_usage() { template int BFieldMapTest(int argc, char **argv) { - using PKTRAJ = ParticleTrajectory; + using PTRAJ = ParticleTrajectory; using HIT = Hit; using HITPTR = std::shared_ptr; using EXING = ElementXing; @@ -53,7 +51,7 @@ int BFieldMapTest(int argc, char **argv) { double pmass(0.511); BFieldMap *BF(0); double Bgrad(0.0), dBx(0.0), dBy(0.0), dBz(0.0); - double tol(0.1); + double tol(1e-4), simtol(1e-5); double zrange(3000.0); // tracker dimension static struct option long_options[] = { @@ -64,6 +62,7 @@ int BFieldMapTest(int argc, char **argv) { {"dBz", required_argument, 0, 'Z' }, {"Bgrad", required_argument, 0, 'g' }, {"Tol", required_argument, 0, 't' }, + {"SimTol", required_argument, 0, 's' }, {NULL, 0,0,0} }; int opt; @@ -83,6 +82,8 @@ int BFieldMapTest(int argc, char **argv) { break; case 't' : tol = atof(optarg); break; + case 's' : simtol = atof(optarg); + break; case 'q' : icharge = atoi(optarg); break; default: print_usage(); @@ -101,22 +102,27 @@ int BFieldMapTest(int argc, char **argv) { } // first, create a traj based on the actual field at this point KKTest::ToyMC toy(*BF, mom, icharge, zrange, iseed, 0, false, false, -1.0, pmass ); - toy.setTolerance(tol); - PKTRAJ tptraj; - HITCOL thits; - EXINGCOL dxings; - toy.simulateParticle(tptraj, thits, dxings); + toy.setTolerance(simtol); + PTRAJ tptraj; +// HITCOL thits; +// EXINGCOL dxings; +// toy.simulateParticle(tptraj, thits, dxings); + toy.createTraj(tptraj); + double tbeg = tptraj.range().begin(); + auto vel = tptraj.velocity(tbeg); + double tend = tbeg + zrange/vel.Z(); + toy.extendTraj(tptraj,tend); // then, create a piecetraj around the nominal field with corrections, auto pos = tptraj.position4(tptraj.range().begin()); auto momv = tptraj.momentum4(pos.T()); KTRAJ start(pos,momv,icharge,bnom,tptraj.range()); - PKTRAJ xptraj(start); - PKTRAJ lptraj(start); + PTRAJ xptraj(start); + PTRAJ lptraj(start); // see how far I can go within tolerance TimeRange prange = start.range(); do { auto const& piece = xptraj.back(); - prange = TimeRange(prange.begin(),BF->rangeInTolerance(piece,prange.begin(), tol)); + prange = TimeRange(prange.begin(),prange.begin() + BF->rangeInTolerance(piece,prange.begin(), tol)); // integrate the momentum change over this range VEC3 dp = BF->integrate(piece,prange); // approximate change in position @@ -155,9 +161,9 @@ int BFieldMapTest(int argc, char **argv) { xdp = BF->integrate( xptraj, xptraj.range()); ldp = BF->integrate( lptraj, lptraj.range()); ndp = BF->integrate( start, start.range()); - cout << "TTraj " << tptraj << " integral " << tdp << endl; - cout << "XTraj " << xptraj << " integral " << xdp << endl; - cout << "LTraj " << lptraj << " integral " << ldp << endl; + cout << "Simulated Traj " << tptraj << " integral " << tdp << endl; + cout << "Extract Traj " << xptraj << " integral " << xdp << endl; + cout << "Linear Traj " << lptraj << " integral " << ldp << endl; cout << "Nominal " << start << " integral " << ndp << endl; // setup histograms @@ -305,10 +311,10 @@ int BFieldMapTest(int argc, char **argv) { // particle path, it just tests mechanics // loop over the time ranges given by the 'sim' trajectory KTRAJ const& sktraj = tptraj.front(); - PKTRAJ rsktraj(sktraj); + PTRAJ rsktraj(sktraj); for (auto const& piece : tptraj.pieces()) { // rotate the parameters at the end of this piece to form the next. Sample B in the middle of that range - KTRAJ newpiece(piece,BF->fieldVect(sktraj.position3(piece.range().mid())),piece.range().begin()); + KTRAJ newpiece(*piece,BF->fieldVect(sktraj.position3(piece->range().mid())),piece->range().begin()); rsktraj.append(newpiece); } // draw the trajs diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index fc1173c8..0aab2c23 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -1,21 +1,21 @@ # List of unit test sources set( TEST_SOURCE_FILES + CaloDistanceToTime_unit.cc CentralHelixClosestApproach_unit.cc CentralHelixBField_unit.cc CentralHelixDerivs_unit.cc CentralHelixFit_unit.cc CentralHelixHit_unit.cc CentralHelixPKTraj_unit.cc - CentralHelixTPoca_unit.cc CentralHelix_unit.cc + ConstantDistanceToTime_unit.cc KinematicLineClosestApproach_unit.cc KinematicLineBField_unit.cc KinematicLineDerivs_unit.cc KinematicLineFit_unit.cc KinematicLineHit_unit.cc KinematicLinePKTraj_unit.cc - KinematicLineTPoca_unit.cc KinematicLine_unit.cc LoopHelixClosestApproach_unit.cc LoopHelixBField_unit.cc @@ -23,8 +23,9 @@ set( TEST_SOURCE_FILES LoopHelixFit_unit.cc LoopHelixHit_unit.cc LoopHelixPKTraj_unit.cc - LoopHelixTPoca_unit.cc LoopHelix_unit.cc + Geometry_unit.cc + Intersection_unit.cc MatEnv_unit.cc ) @@ -42,7 +43,7 @@ foreach( testsourcefile ${TEST_SOURCE_FILES} ) # add the project root as an include directory # link all unit tests to KinKal, MatEnv, and ROOT libraries. target_include_directories(Test_${testname} PRIVATE ${PROJECT_SOURCE_DIR}/..) - target_link_libraries( Test_${testname} General Trajectory Detector Fit MatEnv Examples ${ROOT_LIBRARIES} ) + target_link_libraries( Test_${testname} General Trajectory Geometry Detector Fit MatEnv Examples ${ROOT_LIBRARIES} ) # ensure the unit test executable filename is just its test name set_target_properties( Test_${testname} PROPERTIES OUTPUT_NAME ${testname}) diff --git a/Tests/CaloDistanceToTime_unit.cc b/Tests/CaloDistanceToTime_unit.cc new file mode 100644 index 00000000..b905cb35 --- /dev/null +++ b/Tests/CaloDistanceToTime_unit.cc @@ -0,0 +1,82 @@ +#include "KinKal/Examples/CaloDistanceToTime.cc" +#include "KinKal/Trajectory/DistanceToTime.hh" +#include "KinKal/Trajectory/ConstantDistanceToTime.hh" +#include +#include + +#include "TH1F.h" +#include "THelix.h" +#include "TFile.h" +#include "TPolyLine3D.h" +#include "TAxis3D.h" +#include "TCanvas.h" +#include "TStyle.h" +#include "TVector3.h" +#include "TPolyLine3D.h" +#include "TPolyMarker3D.h" +#include "TLegend.h" +#include "TGraph.h" +#include "TRandom3.h" +#include "TH2F.h" +#include "TDirectory.h" +#include "TProfile.h" +#include "TProfile2D.h" + +using namespace std; + +TGraph* graph(size_t numIter, double start, double stepSize, DistanceToTime* d, function fn) { + std::vector x(numIter,0); + std::vector y(numIter,0); + for (size_t i = 0; i < numIter; i++) { + x[i] = i * stepSize + start; + y[i] = fn(x[i], d); + } + return new TGraph(numIter, x.data(), y.data()); +} + +double timeWrapper(double x, DistanceToTime* d) { + return d->time(x); +} + +double distanceWrapper(double x, DistanceToTime* d) { + return d->distance(x); +} + +double inverseSpeedWrapper(double x, DistanceToTime* d) { + return d->inverseSpeed(x); +} + +double speedWrapper(double x, DistanceToTime* d) { + return d->speed(x); +} + +int main(int argc, char **argv) { + // cout << "Hello World" << endl; + double static const calorimeterLength = 200; + CaloDistanceToTime* d = new CaloDistanceToTime(85.76, calorimeterLength-27.47, 0.003); + //ConstantDistanceToTime* linear = new ConstantDistanceToTime(85.76); + TGraph* g1 = graph(220, -10, 1, d, &timeWrapper); + g1->SetTitle("Distance->deltaT;Distance (mm);deltaT (ns)"); + TGraph* g2 = graph(220, -10, 1, d, &inverseSpeedWrapper); + g2->SetTitle("Inverse Speed (ns/mm);Distance (mm); dt/dx (ns/mm)"); + TGraph* g3 = graph(220, -10, 1, d, &speedWrapper); + g3->SetTitle("Speed (mm/ns); Distance (mm); dx/dt (mm/ns)"); + TGraph* g4 = graph(361, -0.5, 0.00623, d, &distanceWrapper); + g4->SetTitle("DeltaT->Distance; deltaT (ns); Distance (mm)"); + + TFile mefile("CaloDistanceToTime.root","RECREATE"); + + TCanvas* can = new TCanvas("CaloDistanceToTime", "CaloDistanceToTime", 1000, 1000); + can->Divide(2, 2); + can->cd(1); + g1->Draw("AC*"); + can->cd(2); + g2->Draw("AC*"); + can->cd(3); + g3->Draw("AC*"); + can->cd(4); + g4->Draw("AC*"); + can->Write(); + mefile.Write(); + mefile.Close(); +} diff --git a/Tests/CentralHelixClosestApproach_unit.cc b/Tests/CentralHelixClosestApproach_unit.cc index f348ffec..262f620a 100644 --- a/Tests/CentralHelixClosestApproach_unit.cc +++ b/Tests/CentralHelixClosestApproach_unit.cc @@ -1,5 +1,6 @@ #include "KinKal/Trajectory/CentralHelix.hh" #include "KinKal/Tests/ClosestApproachTest.hh" int main(int argc, char **argv) { - return ClosestApproachTest(argc,argv); + KinKal::DVEC pchange(0.5, 0.001, 0.0001, 0.5, 0.001, 0.5); // range for parameter variation + return ClosestApproachTest(argc,argv,pchange); } diff --git a/Tests/CentralHelixFit_unit.cc b/Tests/CentralHelixFit_unit.cc index 65feecdb..f1e451e3 100644 --- a/Tests/CentralHelixFit_unit.cc +++ b/Tests/CentralHelixFit_unit.cc @@ -6,8 +6,15 @@ int main(int argc, char **argv) { cout << "Testing gradient field, correction 2" << endl; std::vector arguments; arguments.push_back(argv[0]); - arguments.push_back("--Bgrad"); - arguments.push_back("-0.036"); // mu2e-like field gradient +// arguments.push_back("--Bgrad"); +// arguments.push_back("-0.036"); // mu2e-like field gradient + arguments.push_back("--Schedule"); + arguments.push_back("MCAmbigFixedField.txt"); +// arguments.push_back("seedfit.txt"); +// arguments.push_back("--extend"); +// arguments.push_back("driftextend.txt"); +// arguments.push_back("--ttree"); +// arguments.push_back("1"); std::vector myargv; for (const auto& arg : arguments) myargv.push_back((char*)arg.data()); diff --git a/Tests/CentralHelixTPoca_unit.cc b/Tests/CentralHelixTPoca_unit.cc deleted file mode 100644 index f348ffec..00000000 --- a/Tests/CentralHelixTPoca_unit.cc +++ /dev/null @@ -1,5 +0,0 @@ -#include "KinKal/Trajectory/CentralHelix.hh" -#include "KinKal/Tests/ClosestApproachTest.hh" -int main(int argc, char **argv) { - return ClosestApproachTest(argc,argv); -} diff --git a/Tests/CentralHelix_unit.cc b/Tests/CentralHelix_unit.cc index 3e522405..49f78e64 100644 --- a/Tests/CentralHelix_unit.cc +++ b/Tests/CentralHelix_unit.cc @@ -1,5 +1,6 @@ #include "KinKal/Trajectory/CentralHelix.hh" #include "KinKal/Tests/Trajectory.hh" -int main(int argc, char **argv) { - return test(argc,argv); +int main(int argc, char **argv){ + KinKal::DVEC sigmas(0.5, 0.003, 0.00001, 3.0 , 0.004, 0.1); // expected parameter sigmas + return TrajectoryTest(argc,argv,sigmas); } diff --git a/Tests/ClosestApproachTest.hh b/Tests/ClosestApproachTest.hh index 203fb5e5..99e796b4 100644 --- a/Tests/ClosestApproachTest.hh +++ b/Tests/ClosestApproachTest.hh @@ -1,7 +1,7 @@ // // test basic functions of ClosestApproach using KTraj and Line // -#include "KinKal/Trajectory/Line.hh" +#include "KinKal/Trajectory/SensorLine.hh" #include "KinKal/Trajectory/ClosestApproach.hh" #include "KinKal/Trajectory/PointClosestApproach.hh" #include "KinKal/Trajectory/PiecewiseClosestApproach.hh" @@ -10,12 +10,12 @@ #include "KinKal/General/PhysicalConstants.h" #include -#include +#include #include #include +#include #include "TH1F.h" -#include "TSystem.h" #include "THelix.h" #include "TFile.h" #include "TPolyLine3D.h" @@ -32,63 +32,61 @@ #include "TDirectory.h" #include "TProfile.h" #include "TProfile2D.h" +#include "TRandom3.h" +#include "TROOT.h" +#include "TStyle.h" +#include "TF1.h" +#include "TFitResult.h" using namespace KinKal; using namespace std; -// avoid confusion with root -using KinKal::Line; void print_usage() { - printf("Usage: ClosestApproachTest --charge i--gap f --tmin f --tmax f --phi f --vprop f --costheta f --eta f\n"); + printf("Usage: ClosestApproachTest --charge i--gap f --tmin f --tmax f --vprop f --delta f \n"); } template -int ClosestApproachTest(int argc, char **argv) { - using TCA = ClosestApproach; +int ClosestApproachTest(int argc, char **argv, KinKal::DVEC pchange ){ + gROOT->SetBatch(kTRUE); + gStyle->SetOptFit(1); + using TCA = ClosestApproach; using TCAP = PointClosestApproach; - using PTCA = PiecewiseClosestApproach; - using PKTRAJ = ParticleTrajectory; + using PCA = PiecewiseClosestApproach; + using PTRAJ = ParticleTrajectory; int opt; - double mom(105.0), cost(0.7), phi(0.5); + int status(0); + double mom(105.0), mincost(0.1), maxcost(0.7); int icharge(-1); double pmass(0.511), oz(0.0), ot(0.0); double tmin(-10.0), tmax(10.0); double wlen(1000.0); //length of the wire - double gap(2.0); // distance between Line and KTRAJ + double maxgap(2.5); // distance between Line and KTRAJ double vprop(0.7); - double eta(0.0); - unsigned nstep(50),ntstep(10); + double delta(5e-2); + unsigned nstep(50),ntstep(100), ntrks(10); + TRandom3 tr_; // random number generator static struct option long_options[] = { {"charge", required_argument, 0, 'q' }, - {"costheta", required_argument, 0, 'c' }, - {"gap", required_argument, 0, 'g' }, {"tmin", required_argument, 0, 't' }, - {"tmax", required_argument, 0, 'd' }, - {"phi", required_argument, 0, 'f' }, + {"tmax", required_argument, 0, 'T' }, {"vprop", required_argument, 0, 'v' }, - {"eta", required_argument, 0, 'e' }, + {"delta", required_argument, 0, 'd' } }; int long_index =0; while ((opt = getopt_long_only(argc, argv,"", long_options, &long_index )) != -1) { switch (opt) { - case 'c' : cost = atof(optarg); - break; case 'q' : icharge = atoi(optarg); break; - case 'g' : gap = atof(optarg); - break; case 't' : tmin = atof(optarg); break; - case 'd' : tmax = atof(optarg); - break; - case 'f' : phi = atof(optarg); + case 'T' : tmax = atof(optarg); break; case 'v' : vprop = atof(optarg); break; - case 'e' : eta = atof(optarg); + case 'd' : delta = atof(optarg); break; default: print_usage(); exit(EXIT_FAILURE); @@ -98,109 +96,165 @@ int ClosestApproachTest(int argc, char **argv) { VEC3 bnom(0.0,0.0,1.0); UniformBFieldMap BF(bnom); // 1 Tesla VEC4 origin(0.0,0.0,oz,ot); - double sint = sqrt(1.0-cost*cost); - MOM4 momv(mom*sint*cos(phi),mom*sint*sin(phi),mom*cost,pmass); - KTRAJ ktraj(origin,momv,icharge,bnom); TFile tpfile((KTRAJ::trajName()+"ClosestApproach.root").c_str(),"RECREATE"); TCanvas* ttpcan = new TCanvas("ttpcan","DToca",1200,800); ttpcan->Divide(3,2); TCanvas* dtpcan = new TCanvas("dtpcan","DDoca",1200,800); dtpcan->Divide(3,2); std::vector dtpoca, ttpoca; - std::vector pchange = {10,0.1,0.0001,10,0.01,0.1}; for(size_t ipar=0;ipar(ipar); - dtpoca.push_back(new TGraph(nstep*ntstep)); + dtpoca.push_back(new TGraph(nstep*ntstep*ntrks)); string ts = KTRAJ::paramTitle(parindex)+string(" DOCA Change;#Delta DOCA (exact);#Delta DOCA (derivative)"); dtpoca.back()->SetTitle(ts.c_str()); - ttpoca.push_back(new TGraph(nstep*ntstep)); + ttpoca.push_back(new TGraph(nstep*ntstep*ntrks)); ts = KTRAJ::paramTitle(parindex)+string(" TOCA Change;#Delta TOCA (exact);#Delta TOCA (derivative)"); ttpoca.back()->SetTitle(ts.c_str()); } - for(unsigned itime=0;itime < ntstep;itime++){ - double time = tmin + itime*(tmax-tmin)/(ntstep-1); - // create tline perp to trajectory at the specified time, separated by the specified gap - VEC3 pos, dir; - pos = ktraj.position3(time); - dir = ktraj.direction(time); - VEC3 perp1 = ktraj.direction(time,MomBasis::perpdir_); - VEC3 perp2 = ktraj.direction(time,MomBasis::phidir_); - // choose a specific direction for DOCA - // the line traj must be perp. to this and perp to the track - VEC3 docadir = cos(eta)*perp1 + sin(eta)*perp2; - VEC3 pdir = sin(eta)*perp1 - cos(eta)*perp2; - double pspeed = CLHEP::c_light*vprop; // vprop is relative to c - VEC3 pvel = pdir*pspeed; - // shift the position - VEC3 ppos = pos + gap*docadir; - // create the Line - Line tline(ppos, time, pvel, wlen); - // create ClosestApproach from these - CAHint tphint(time,time); - TCA tp(ktraj,tline,tphint,1e-8); - // test: delta vector should be perpendicular to both trajs - VEC3 del = tp.delta().Vect(); - auto pd = tp.particleDirection(); - auto sd = tp.sensorDirection(); - double dp = del.Dot(pd); - if(dp>1e-9) cout << "CA delta not perpendicular to particle direction" << endl; - double ds = del.Dot(sd); - if(ds>1e-9) cout << "CA delta not perpendicular to sensor direction" << endl; - // test PointClosestApproach - VEC4 pt(ppos.X(),ppos.Y(),ppos.Z(),time-1.0); - TCAP tpp(ktraj,pt,1e-8); - if(fabs(fabs(tpp.doca()) - gap) > 1e-8) cout << "Point DOCA not correct " << endl; - // test against a piece-traj - PKTRAJ pktraj(ktraj); - PTCA ptp(pktraj,tline,tphint,1e-8); - if(tp.status() != ClosestApproachData::converged)cout << "ClosestApproach status " << tp.statusName() << " doca " << tp.doca() << " dt " << tp.deltaT() << endl; - if(tpp.status() != ClosestApproachData::converged)cout << "PointClosestApproach status " << tpp.statusName() << " doca " << tpp.doca() << " dt " << tpp.deltaT() << endl; - if(ptp.status() != ClosestApproachData::converged)cout << "PiecewiseClosestApproach status " << ptp.statusName() << " doca " << ptp.doca() << " dt " << ptp.deltaT() << endl; - VEC3 thpos, tlpos; - thpos = tp.particlePoca().Vect(); - tlpos = tp.sensorPoca().Vect(); - double refd = tp.doca(); - double reft = tp.deltaT(); - // cout << " Helix Pos " << pos << " ClosestApproach KTRAJ pos " << thpos << " ClosestApproach Line pos " << tlpos << endl; - // cout << " ClosestApproach particlePoca " << tp.particlePoca() << " ClosestApproach sensorPoca " << tp.sensorPoca() << " DOCA " << refd << endl; - // cout << "ClosestApproach dDdP " << tp.dDdP() << " dTdP " << tp.dTdP() << endl; - // test against numerical derivatives - // range to change specific parameters; most are a few mm - for(size_t ipar=0;iparSetPoint(istep,xd-refd,dd); - ttpoca[ipar]->SetPoint(istep,xt-reft,dt); + for(unsigned itrk=0;itrk1e-9){ + cout << "CA delta not perpendicular to particle direction" << endl; + status = 2; + } + double ds = del.Dot(sd); + if(ds>1e-9){ + cout << "CA delta not perpendicular to sensor direction" << endl; + status = 2; + } + // test PointClosestApproach + VEC4 pt(spos.X(),spos.Y(),spos.Z(),time-1.0); + TCAP tpp(ktraj,pt,1e-8); + if(fabs(fabs(tpp.doca()) - gap) > 1e-8){ + cout << "Point DOCA not correct, doca = " << tpp.doca() << " gap " << gap << endl; + status = 3; + } + + // test against a piece-traj + PTRAJ ptraj(ktraj); + PCA pca(ptraj,tline,tphint,1e-8); + if(tp.status() != ClosestApproachData::converged)cout << "ClosestApproach status " << tp.statusName() << " doca " << tp.doca() << " dt " << tp.deltaT() << endl; + if(tpp.status() != ClosestApproachData::converged)cout << "PointClosestApproach status " << tpp.statusName() << " doca " << tpp.doca() << " dt " << tpp.deltaT() << endl; + if(pca.status() != ClosestApproachData::converged)cout << "PiecewiseClosestApproach status " << pca.statusName() << " doca " << pca.doca() << " dt " << pca.deltaT() << endl; + VEC3 thpos, tlpos; + thpos = tp.particlePoca().Vect(); + tlpos = tp.sensorPoca().Vect(); + double refd = tp.doca(); + double reft = tp.deltaT(); // what matters physically is deltaT + // cout << " Helix Pos " << pos << " ClosestApproach KTRAJ pos " << thpos << " ClosestApproach Line pos " << tlpos << endl; + // cout << " ClosestApproach particlePoca " << tp.particlePoca() << " ClosestApproach sensorPoca " << tp.sensorPoca() << " DOCA " << refd << endl; + // cout << "ClosestApproach dDdP " << tp.dDdP() << " dTdP " << tp.dTdP() << endl; + // test against numerical derivatives + // range to change specific parameters; most are a few mm + for(size_t ipar=0;iparSetPoint(ientry,xd-refd,dd); + ttpoca[ipar]->SetPoint(ientry,xt-reft,dt); + } } } } + TF1* pline = new TF1("pline","[0]+[1]*x"); for(size_t ipar=0;iparSetStats(1); dtpcan->cd(ipar+1); - dtpoca[ipar]->Draw("A*"); + // test linearity + pline->SetParameters(0.0,1.0); + // ignore parameters that don't have appreciable range + double xmax = -std::numeric_limits::max(); + double xmin = std::numeric_limits::max(); + unsigned npt = (unsigned)dtpoca[ipar]->GetN(); + for(unsigned ipt=0; ipt < npt;++ipt){ + xmax = std::max(dtpoca[ipar]->GetPointX(ipt),xmax); + xmin = std::min(dtpoca[ipar]->GetPointX(ipt),xmin); + } + if(xmax-xmin > 1e-6) { + TFitResultPtr pfitr = dtpoca[ipar]->Fit(pline,"SQ","AC*"); + if(fabs(pfitr->Parameter(0))> 10*delta || fabs(pfitr->Parameter(1)-1.0) > delta){ + cout << "DOCA derivative for parameter " + << KTRAJ::paramName(typename KTRAJ::ParamIndex(ipar)) + << " Out of tolerance : Offset " << pfitr->Parameter(0) << " Slope " << pfitr->Parameter(1) << endl; + status = 1; + } + } + dtpoca[ipar]->Draw("AF*"); } for(size_t ipar=0;iparcd(ipar+1); - ttpoca[ipar]->Draw("A*"); + double xmax = -std::numeric_limits::max(); + double xmin = std::numeric_limits::max(); + unsigned npt = (unsigned)dtpoca[ipar]->GetN(); + for(unsigned ipt=0; ipt < npt;++ipt){ + xmax = std::max(ttpoca[ipar]->GetPointX(ipt),xmax); + xmin = std::min(ttpoca[ipar]->GetPointX(ipt),xmin); + } + if(xmax-xmin > 1e-6) { + TFitResultPtr pfitr = ttpoca[ipar]->Fit(pline,"SQ","AC*"); + if(fabs(pfitr->Parameter(0))> 10*delta || fabs(pfitr->Parameter(1)-1.0) > delta){ + cout << "DeltaT derivative for parameter " + << KTRAJ::paramName(typename KTRAJ::ParamIndex(ipar)) + << " Out of tolerance : Offset " << pfitr->Parameter(0) << " Slope " << pfitr->Parameter(1) << endl; + status = 1; + } + } + ttpoca[ipar]->Draw("AF*"); } dtpcan->Write(); ttpcan->Write(); tpfile.Write(); tpfile.Close(); - return 0; + return status; } diff --git a/Tests/ConstantDistanceToTime_unit.cc b/Tests/ConstantDistanceToTime_unit.cc new file mode 100644 index 00000000..c99a30a5 --- /dev/null +++ b/Tests/ConstantDistanceToTime_unit.cc @@ -0,0 +1,81 @@ +#include "KinKal/Trajectory/ConstantDistanceToTime.cc" +#include "KinKal/Trajectory/DistanceToTime.hh" +#include +#include + +#include "TH1F.h" +#include "THelix.h" +#include "TFile.h" +#include "TPolyLine3D.h" +#include "TAxis3D.h" +#include "TCanvas.h" +#include "TStyle.h" +#include "TVector3.h" +#include "TPolyLine3D.h" +#include "TPolyMarker3D.h" +#include "TLegend.h" +#include "TGraph.h" +#include "TRandom3.h" +#include "TH2F.h" +#include "TDirectory.h" +#include "TProfile.h" +#include "TProfile2D.h" + +using namespace std; + +TGraph* graph(size_t numIter, double start, double stepSize, DistanceToTime* d, function fn) { + std::vector x(numIter,0); + std::vector y(numIter,0); + + for (size_t i = 0; i < numIter; i++) { + x[i] = i * stepSize + start; + y[i] = fn(x[i], d); + } + return new TGraph(numIter, x.data(), y.data()); +} + +double timeWrapper(double x, DistanceToTime* d) { + return d->time(x); +} + +double distanceWrapper(double x, DistanceToTime* d) { + return d->distance(x); +} + +double inverseSpeedWrapper(double x, DistanceToTime* d) { + return d->inverseSpeed(x); +} + +double speedWrapper(double x, DistanceToTime* d) { + return d->speed(x); +} + +int main(int argc, char **argv) { + // cout << "Hello World" << endl; + ConstantDistanceToTime* d = new ConstantDistanceToTime(-136); + + TGraph* g1 = graph(200, 0, 1, d, &timeWrapper); + g1->SetTitle("Distance->deltaT;Distance (mm);deltaT (ns)"); + TGraph* g2 = graph(200, 0, 1, d, &inverseSpeedWrapper); + g2->SetTitle("Inverse Speed (mm/ns);Distance (mm);deltaT (ns)"); + TGraph* g3 = graph(200, 0, 1, d, &speedWrapper); + g3->SetTitle("Speed (ns/mm); Distance (mm); deltaT (ns)"); + TGraph* g4 = graph(200, 0, 0.0099, d, &distanceWrapper); + g4->SetTitle("DeltaT->Distance; deltaT (ns); Distance (mm)"); + + TFile mefile("ConstantDistanceToTime.root","RECREATE"); + + TCanvas* can = new TCanvas("ConstantDistanceToTime", "ConstantDistanceToTime", 1000, 1000); + can->Divide(2, 2); + can->cd(1); + g1->Draw("AC*"); + can->cd(2); + g2->Draw("AC*"); + can->cd(3); + g3->Draw("AC*"); + can->cd(4); + g4->Draw("AC*"); + can->Write(); + mefile.Write(); + mefile.Close(); +} diff --git a/Tests/Derivatives.hh b/Tests/Derivatives.hh index 590deea8..d1197276 100644 --- a/Tests/Derivatives.hh +++ b/Tests/Derivatives.hh @@ -4,7 +4,7 @@ #include "KinKal/Trajectory/ClosestApproach.hh" #include -#include +#include #include #include #include @@ -12,7 +12,6 @@ #include "TROOT.h" #include "TH1F.h" #include "TFile.h" -#include "TSystem.h" #include "THelix.h" #include "TPolyLine3D.h" #include "TArrow.h" @@ -432,16 +431,17 @@ int test(int argc, char **argv) { << dPdM << endl; } - // test changes due to BFieldMap + // test changing bnom TCanvas* dbcan[3]; // 3 directions std::vector bgraphs[3]; std::array basis; basis[0] = reftraj.bnom().Unit(); basis[1] = reftraj.direction(MomBasis::phidir_); basis[2] = VEC3(basis[1].Y(),-basis[1].X(), 0.0); //perp - std::array bnames{"BDirection", "PhiDirection", "MomPerpendicular"}; + std::array bnames{"BDir", "PhiDir", "PerpDir"}; // gaps TGraph* bgapgraph[3]; + auto state = reftraj.stateEstimate(ttest); for(size_t idir =0; idir<3;idir++){ bgraphs[idir] = std::vector(NParams(),0); for(size_t ipar = 0; ipar < NParams(); ipar++){ @@ -458,24 +458,33 @@ int test(int argc, char **argv) { for(int id=0;id1.0e-6) cout << "State vector " << ipar << " doesn't match: original " - << state.state()[ipar] << " rotated " << newstate.state()[ipar] << endl; - } - dpx = newbfhel.params().parameters() - reftraj.params().parameters(); - // 1st order change trajectory - KTRAJ dbtraj(reftraj,bf,ttest); + KTRAJ xdbtraj(state,bf); + dpx = xdbtraj.params().parameters() - reftraj.params().parameters(); + // test 1st order change + auto dbtraj = reftraj; + dbtraj.setBNom(ttest, bf); dpdb = dbtraj.params().parameters() - reftraj.params().parameters(); for(size_t ipar = 0; ipar < NParams(); ipar++){ bgraphs[idir][ipar]->SetPoint(id,dpx[ipar], dpdb[ipar]); } - bgapgraph[idir]->SetPoint(id,dval,(dbtraj.position3(ttest)-newbfhel.position3(ttest)).R()); - // BField derivatives + bgapgraph[idir]->SetPoint(id,dval,(dbtraj.position3(ttest)-reftraj.position3(ttest)).R()); + // test state + if(id==0){ + // exact state test + auto xdbstate = xdbtraj.stateEstimate(ttest); + for(size_t ipar=0;ipar < ParticleState::dimension(); ipar++){ + if(fabs(state.state()[ipar] - xdbstate.state()[ipar])>1.0e-6) cout << "Exact State vector " << ipar << " doesn't match: dir " + << idir << " original " << state.state()[ipar] << " changed " << xdbstate.state()[ipar] << endl; + } + // 1st order state test + auto dbstate = dbtraj.stateEstimate(ttest); + for(size_t ipar=0;ipar < ParticleState::dimension(); ipar++){ + if(fabs(state.state()[ipar] - dbstate.state()[ipar])>1.0e-6) cout << "1st order State vector " << ipar << " doesn't match: dir " + << idir << " original " << state.state()[ipar] << " changed " << dbstate.state()[ipar] << endl; + } + } } char gtitle[80]; @@ -505,6 +514,33 @@ int test(int argc, char **argv) { dbcan[idir]->Draw(); dbcan[idir]->Write(); } + +// test covariance rotation +// double db = 0.001; +// // PSMAT dpdbdp = traj.dPardBdPar(traj.t0()); +// KTRAJ dbtraj(traj); +//// DVEC dpdb = traj.dPardB(traj.t0()); +// auto bnomp = (1.0+db)*traj.bnom(); +// dbtraj.setBNom(traj.t0(),bnomp); +//// dbtraj.resetBNom(bnomp); +//// dbtraj.params().parameters() += dpdb*db/(1.0+db); +// auto dbpstate = dbtraj.stateEstimate(traj.t0()); +// for(size_t ipar=0; ipar < NParams(); ipar++){ +// auto dp = fabs(pstate.state()[ipar]-dbpstate.state()[ipar])/db; +// if(dp > 1.0e-8){ +// std::cout << "State mismatch, par " << ipar << " diff " << dp << " pars " << pstate.state()[ipar] << " " << dbpstate.state()[ipar] << std::endl; +// retval = -1; +// } +// for(size_t jpar=0; jpar < NParams(); jpar++){ +// auto dc = fabs(pstate.stateCovariance()[ipar][jpar]-dbpstate.stateCovariance()[ipar][jpar])/sqrt(pstate.stateCovariance()[ipar][ipar]*pstate.stateCovariance()[jpar][jpar]); +// if( dc > 1.0e2) { // temporarily disable FIXME! +// std::cout << "State Covariance mismatch par " << ipar << " , " << jpar << " diff " << dc << " covs " << pstate.stateCovariance()[ipar][jpar] << " " << dbpstate.stateCovariance()[ipar][jpar] << std::endl; +// retval = -1; +// } +// } +// } +// } + TCanvas* dbgcan = new TCanvas("dbgcan","DB Gap",800,800); dbgcan->Divide(2,2); for(int idir=0;idir<3;++idir){ diff --git a/Tests/FitTest.hh b/Tests/FitTest.hh index 10c7d546..f2762718 100644 --- a/Tests/FitTest.hh +++ b/Tests/FitTest.hh @@ -3,23 +3,24 @@ // #include "KinKal/General/Vectors.hh" #include "KinKal/Trajectory/ParticleTrajectory.hh" -#include "KinKal/Trajectory/Line.hh" #include "KinKal/Examples/SimpleWireHit.hh" -#include "KinKal/Detector/StrawMaterial.hh" -#include "KinKal/Detector/ParameterHit.hh" #include "KinKal/Examples/ScintHit.hh" +#include "KinKal/Examples/StrawXing.hh" +#include "KinKal/Examples/StrawMaterial.hh" +#include "KinKal/Detector/ParameterHit.hh" #include "KinKal/Detector/ElementXing.hh" #include "KinKal/General/BFieldMap.hh" #include "KinKal/Fit/Config.hh" #include "KinKal/Fit/Measurement.hh" #include "KinKal/Fit/Material.hh" -#include "KinKal/Fit/BField.hh" +#include "KinKal/Fit/DomainWall.hh" #include "KinKal/Fit/Track.hh" #include "KinKal/Tests/ToyMC.hh" #include "KinKal/Examples/HitInfo.hh" #include "KinKal/Examples/MaterialInfo.hh" -#include "KinKal/Examples/BFieldInfo.hh" +#include "KinKal/Examples/DomainWallInfo.hh" #include "KinKal/Examples/ParticleTrajectoryInfo.hh" +#include "KinKal/Examples/DOCAWireHitUpdater.hh" #include "KinKal/General/PhysicalConstants.h" #include @@ -36,10 +37,10 @@ #include #include +#include "TF1.h" #include "TH1F.h" #include "TTree.h" #include "TBranch.h" -#include "TSystem.h" #include "THelix.h" #include "TPolyLine3D.h" #include "TAxis3D.h" @@ -64,9 +65,8 @@ using namespace MatEnv; using namespace KinKal; using namespace std; // avoid confusion with root -using KinKal::Line; void print_usage() { - printf("Usage: FitTest --momentum f --simparticle i --fitparticle i--charge i --nhits i --hres f --seed i -ambigdoca f --nevents i --simmat i--fitmat i --ttree i --Bz f --dBx f --dBy f --dBz f--Bgrad f --tolerance f --TFilesuffix c --PrintBad i --PrintDetail i --ScintHit i --invert i --Schedule a --ssmear i --constrainpar i --inefficiency f --extend s --lighthit i --TimeBuffer f\n"); + printf("Usage: FitTest --momentum f --simparticle i --fitparticle i--charge i --nhits i --hres f --seed i -ambigdoca f --nevents i --simmat i--fitmat i --ttree i --Bz f --dBx f --dBy f --dBz f--Bgrad f --tolerance f --TFilesuffix c --PrintBad i --PrintDetail i --ScintHit i --invert i --Schedule a --ssmear i --constrainpar i --inefficiency f --extend s --TimeBuffer f --matvarscale i\n"); } // utility function to compute transverse distance between 2 similar trajectories. Also @@ -93,7 +93,61 @@ double dTraj(KTRAJ const& kt1, KTRAJ const& kt2, double t1, double& t2) { return ((pos2-pos1).Cross(dir1)).R(); } -int makeConfig(string const& cfile, KinKal::Config& config) { +template +int testState(KinKal::Track const& kktrk,DVEC sigmas) { + // test parameterstate + int retval(0); + auto const& traj = kktrk.fitTraj().nearestPiece(kktrk.fitTraj().range().mid()); + auto pstate = traj.stateEstimate(traj.t0()); + auto pos1 = traj.position3(traj.t0()); + auto pos2 = pstate.position3(); + if(fabs(pos1.Dot(pos2) - pos1.Mag2()) >1e-10){ + std::cout << "Position difference " << pos1 << " " << pos2 << std::endl; + retval = -1; + } + auto mom1 = traj.momentum3(traj.t0()); + auto mom2 = pstate.momentum3(); + if(fabs(mom1.Dot(mom2)-mom1.Mag2())>1e-10){ + std::cout << "Momentum difference " << mom1 << " " << mom2 << std::endl; + retval = -1; + } + double momvar1 = traj.momentumVariance(traj.t0()); + double momvar2 = pstate.momentumVariance(); + if(fabs(momvar1-momvar2)>1e-10){ + std::cout << "Momentum variance differencer " << momvar1 << " " << momvar2 << std::endl; + retval = -1; + } + // test reversibility + KTRAJ testtraj(pstate,traj.bnom(),traj.range()); + testtraj.syncPhi0(traj); + + for(size_t ipar=0; ipar < NParams(); ipar++){ + auto dp = fabs(traj.paramVal(ipar)-testtraj.paramVal(ipar))/sigmas(ipar); + if( dp > 1.0e-10){ + if(ipar == KTRAJ::phi0Index()){ + if(fabs(traj.paramVal(ipar)-testtraj.paramVal(ipar))-2*M_PI > 1.0e-8){ + std::cout << "Parameter mismatch, par " << ipar << " diff " << traj.paramVal(ipar) << " " << testtraj.paramVal(ipar) << std::endl; + retval = -1; + } + // allow phi0 to be off by 2pi + } else { + std::cout << "Parameter mismatch, par " << ipar << " diff " << traj.paramVal(ipar) << " " << testtraj.paramVal(ipar) << std::endl; + retval = -1; + } + } + for(size_t jpar=0; jpar < NParams(); jpar++){ + auto dc = fabs(traj.params().covariance()(ipar,jpar)-testtraj.params().covariance()(ipar,jpar))/sigmas(ipar)*sigmas(jpar); + if(dc > 1.0e-8){ + std::cout << "Covariance mismatch par " << ipar << " , " << jpar << " diff " << traj.params().covariance()(ipar,jpar) << " " << testtraj.params().covariance()(ipar,jpar) << std::endl; + retval = -1; + } + } + } + + return retval; +} + +int makeConfig(string const& cfile, KinKal::Config& config,bool mvarscale=true) { string fullfile; if(strncmp(cfile.c_str(),"/",1) == 0) { fullfile = string(cfile); @@ -112,26 +166,35 @@ int makeConfig(string const& cfile, KinKal::Config& config) { } string line; int plevel(-1); - unsigned nmiter(0); + unsigned nmiter(0); while (getline(ifs,line)){ if(strncmp(line.c_str(),"#",1)!=0){ istringstream ss(line); if(plevel < 0) { ss >> config.maxniter_ >> config.dwt_ >> config.convdchisq_ >> config.divdchisq_ >> - config.pdchi2_ >> config.tbuff_ >> config.tol_ >> config.minndof_ >> config.bfcorr_ >> - plevel; + config.pdchisq_ >> config.divgap_ >> config.tol_ >> config.minndof_ >> config.bfcorr_ >> + config.ends_ >> plevel; config.plevel_ = Config::printLevel(plevel); } else { - double temp, mindoca(-1.0),maxdoca(-1.0), minprob(-1.0); - ss >> temp >> mindoca >> maxdoca >> minprob; - MetaIterConfig mconfig(temp); - if(mindoca >0.0 || maxdoca > 0.0){ - // setup and insert the updater - cout << "SimpleWireHitUpdater for iteration " << nmiter << " with mindoca " << mindoca << " maxdoca " << maxdoca << " minprob " << minprob << endl; - SimpleWireHitUpdater updater(mindoca,maxdoca,minprob); - mconfig.addUpdater(std::any(updater)); + int utype(-1); + double temp, mindoca(-1.0),maxdoca(-1.0); + ss >> temp >> utype; + MetaIterConfig miconfig(temp); + miconfig.addUpdater(StrawXingConfig(0.3,5.0,10.0,mvarscale)); // hardcoded values, should come from outside, FIXME + if(utype == 0 ){ + cout << "NullWireHitUpdater for iteration " << nmiter << endl; + miconfig.addUpdater(std::any(NullWireHitUpdater())); + } else if(utype == 1) { + ss >> mindoca >> maxdoca; + cout << "DOCAWireHitUpdater for iteration " << nmiter << " with mindoca " << mindoca << " maxdoca " << maxdoca << endl; + DOCAWireHitUpdater updater(mindoca,maxdoca); + miconfig.addUpdater(std::any(updater)); + } else if(utype > 0){ + cout << "Unknown updater " << utype << endl; + return -20; } - config.schedule_.push_back(mconfig); + config.schedule_.push_back(miconfig); + ++nmiter; } } } @@ -154,21 +217,21 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { }; using KKEFF = Effect; - using KKHIT = Measurement; + using KKMEAS = Measurement; using KKMAT = Material; - using KKBFIELD = BField; - using KKEND = TrackEnd; - using PKTRAJ = ParticleTrajectory; + using KKDW = DomainWall; + using PTRAJ = ParticleTrajectory; using MEAS = Hit; using MEASPTR = std::shared_ptr; using MEASCOL = std::vector; using EXING = ElementXing; using EXINGPTR = std::shared_ptr; using EXINGCOL = std::vector; + using STRAWXING = StrawXing; using KKTRK = KinKal::Track; using KKCONFIGPTR = std::shared_ptr; - using STRAWHIT = WireHit; - using STRAWHITPTR = std::shared_ptr; + using WIREHIT = SimpleWireHit; + using WIREHITPTR = std::shared_ptr; using SCINTHIT = ScintHit; using SCINTHITPTR = std::shared_ptr; using PARHIT = ParameterHit; @@ -186,13 +249,13 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { double simmass, fitmass; unsigned nevents(1000); bool ttree(false), printbad(false); - string tfname(""), sfile("Schedule.txt"); + string tfname(""), sfile("driftfit.txt"); int detail(Config::none), invert(0); double ambigdoca(0.25);// minimum doca to set ambiguity, default sets for all hits bool fitmat(true); bool extend(false); + bool mvarscale(true); string exfile; - double extendfrac(0.0); BFieldMap *BF(0); double Bgrad(0.0), dBx(0.0), dBy(0.0), dBz(0.0), Bz(1.0); double zrange(3000); @@ -202,9 +265,9 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { unsigned nhits(40); unsigned nsteps(200); // steps for traj comparison double seedsmear(10.0); - double momsigma(0.2); + double momsigma(0.3); double ineff(0.05); - bool simmat(true), lighthit(true); + bool simmat(true), scinthit(true); int retval(EXIT_SUCCESS); TRandom3 tr_; // random number generator @@ -236,9 +299,9 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { {"constrainpar", required_argument, 0, 'c' }, {"inefficiency", required_argument, 0, 'E' }, {"iprint", required_argument, 0, 'p' }, - {"extendconfig", required_argument, 0, 'X' }, - {"lighthit", required_argument, 0, 'L' }, + {"extend", required_argument, 0, 'X' }, {"TimeBuffer", required_argument, 0, 'W' }, + {"MatVarScale", required_argument, 0, 'v' }, {NULL, 0,0,0} }; @@ -264,7 +327,7 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { break; case 'f' : fitmat = atoi(optarg); break; - case 'L' : lighthit = atoi(optarg); + case 'L' : scinthit = atoi(optarg); break; case 'r' : ttree = atoi(optarg); break; @@ -300,6 +363,8 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { break; case 'u' : sfile = optarg; break; + case 'v' : mvarscale = atoi(optarg); + break; case 'X' : exfile = optarg; extend = true; break; @@ -322,21 +387,23 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { // create ToyMC simmass = masses[isimmass]; fitmass = masses[ifitmass]; - KKTest::ToyMC toy(*BF, mom, icharge, zrange, iseed, nhits, simmat, lighthit, ambigdoca, simmass ); + KKTest::ToyMC toy(*BF, mom, icharge, zrange, iseed, nhits, simmat, scinthit, ambigdoca, simmass ); toy.setInefficiency(ineff); toy.setTolerance(tol/10.0); // finer precision on sim // setup fit configuration Config config; - makeConfig(sfile,config); - cout << "Main fit config " << config << endl; + makeConfig(sfile,config,mvarscale); + cout << "Main fit " << config << endl; // read the schedule from the file Config exconfig; - if(extend) makeConfig(exfile,exconfig); - cout << "Extension config " << exconfig << endl; + if(extend){ + makeConfig(exfile,exconfig,mvarscale); + cout << "Extension " << exconfig << endl; + } // generate hits MEASCOL thits, exthits; // this program shares hit ownership with Track EXINGCOL dxings, exdxings; // this program shares det xing ownership with Track - PKTRAJ tptraj; + PTRAJ tptraj; toy.simulateParticle(tptraj, thits, dxings,fitmat); if(nevents == 0)cout << "True initial " << tptraj.front() << endl; // cout << "vector of hit points " << thits.size() << endl; @@ -356,13 +423,14 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { auto bmid = BF->fieldVect(seedpos.Vect()); seedmom.SetM(fitmass); // buffer the seed range - TimeRange seedrange(tptraj.range().begin()-config.tbuff_,tptraj.range().end()+config.tbuff_); - KTRAJ seedtraj(seedpos,seedmom,midhel.charge(),bmid,seedrange); - if(invert) seedtraj.invertCT(); // for testing wrong propagation direction - toy.createSeed(seedtraj,sigmas,seedsmear); - if(nevents == 0)cout << "Seed Traj " << seedtraj << endl; + TimeRange seedrange(thits.front()->time(),thits.back()->time()); + KTRAJ straj(seedpos,seedmom,midhel.charge(),bmid,seedrange); + if(invert) straj.invertCT(); // for testing wrong propagation direction + toy.createSeed(straj,sigmas,seedsmear); + if(nevents == 0)cout << "Seed Traj " << straj << endl; // Create the Track from these hits - // + PTRAJ seedtraj(straj); + // // if requested, constrain a parameter PMASK mask = {false}; if(conspar >= 0 && conspar < (int)NParams()){ @@ -376,19 +444,19 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { cparams.covariance()[ipar][ipar] = perr*perr; cparams.parameters()[ipar] += tr_.Gaus(0.0,perr); } - thits.push_back(std::make_shared(front.range().mid(),cparams,mask)); + thits.push_back(std::make_shared(front.range().mid(),seedtraj,cparams,mask)); } // if extending, take a random set of hits and materials out, to be replaced later if(extend){ for(auto ihit = thits.begin(); ihit != thits.end();){ - if(tr_.Uniform(0.0,1.0) < extendfrac){ + if(tr_.Uniform(0.0,1.0) < ineff){ exthits.push_back(*ihit); ihit = thits.erase(ihit); } else ++ihit; } for(auto ixing = dxings.begin(); ixing != dxings.end();){ - if(tr_.Uniform(0.0,1.0) < extendfrac){ + if(tr_.Uniform(0.0,1.0) < ineff){ exdxings.push_back(*ixing); ixing = dxings.erase(ixing); } else @@ -396,7 +464,7 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { } } // create and fit the track - KKTRK kktrk(config,*BF,seedtraj,thits,dxings); + KKTRK kktrk(config,*BF,straj,thits,dxings); if(extend && kktrk.fitStatus().usable())kktrk.extend(exconfig,exthits, exdxings); if(!printbad)kktrk.print(cout,detail); TFile fitfile((KTRAJ::trajName() + string("FitTest") + tfname + string(".root")).c_str(),"RECREATE"); @@ -404,34 +472,14 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { KTRAJPars ftpars_, mtpars_, btpars_, spars_, ffitpars_, ffiterrs_, mfitpars_, mfiterrs_, bfitpars_, bfiterrs_; float chisq_, btmom_, mtmom_, ftmom_, ffmom_, mfmom_, bfmom_, ffmomerr_, mfmomerr_, bfmomerr_, chiprob_; float fft_,mft_, bft_; - int ndof_, niter_, status_, igap_, nmeta_, nkkbf_, nkkhit_, nkkmat_; + int ndof_, niter_, status_, igap_, nmeta_, nkkdw_, nkkhit_, nkkmat_; + int nactivehit_, nstrawhit_, nscinthit_, nnull_; float sbeg_, send_, fbeg_, fend_; float maxgap_, avgap_; + int ts = testState(kktrk,sigmas); +// if(ts != 0)return ts; + if(ts != 0)std::cout << "state test failed" << std::endl; - // test parameterstate - auto const& traj = kktrk.fitTraj().front(); - auto pstate = traj.stateEstimate(traj.t0()); - double momvar1 = traj.momentumVariance(traj.t0()); - double momvar2 = pstate.momentumVariance(); - if(fabs(momvar1-momvar2)>1e-10){ - std::cout << "Momentum variance error " << momvar1 << " " << momvar2 << std::endl; - return -3; - } - // full reversibility - KTRAJ testtraj(pstate,traj.bnom(),traj.range()); - for(size_t ipar=0; ipar < NParams(); ipar++){ - if(fabs(traj.paramVal(ipar)-testtraj.paramVal(ipar)) > 1.0e-10){ - std::cout << "Parameter error " << traj.paramVal(ipar) << " " << testtraj.paramVal(ipar) << std::endl; - return -3; - } - for(size_t jpar=0; jpar < NParams(); jpar++){ - if(fabs(traj.params().covariance()(ipar,jpar)-testtraj.params().covariance()(ipar,jpar)) > 1.0e-6){ - std::cout << "Covariance error " << traj.paramVal(ipar) << " " << testtraj.paramVal(ipar) << std::endl; - return -3; - } - } - } - std::cout << "Passed ParameterState tests" << std::endl; if(nevents ==0 ){ // draw the fit result TCanvas* pttcan = new TCanvas("pttcan","PieceKTRAJ",1000,1000); @@ -466,12 +514,12 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { TPolyMarker3D* hpos = new TPolyMarker3D(1,21); TPolyMarker3D* tpos = new TPolyMarker3D(1,22); VEC3 plow, phigh; - STRAWHITPTR shptr = std::dynamic_pointer_cast (thit); + WIREHITPTR shptr = std::dynamic_pointer_cast (thit); SCINTHITPTR lhptr = std::dynamic_pointer_cast (thit); if(shptr.use_count() > 0){ auto const& tline = shptr->wire(); - plow = tline.position3(tline.range().begin()); - phigh = tline.position3(tline.range().end()); + plow = tline.start(); + phigh = tline.end(); line->SetLineColor(kRed); auto hitpos = tline.position3(shptr->closestApproach().sensorToca()); auto trkpos = fptraj.position3(shptr->closestApproach().particleToca()); @@ -481,8 +529,8 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { tpos->SetMarkerColor(kGreen); } else if (lhptr.use_count() > 0){ auto const& tline = lhptr->sensorAxis(); - plow = tline.position3(tline.range().begin()); - phigh = tline.position3(tline.range().end()); + plow = tline.start(); + phigh = tline.end(); line->SetLineColor(kCyan); auto hitpos = tline.position3(lhptr->closestApproach().sensorToca()); auto trkpos = fptraj.position3(lhptr->closestApproach().particleToca()); @@ -530,9 +578,13 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { ftree->Branch("bferrs.", &bfiterrs_,KTRAJPars::leafnames().c_str()); ftree->Branch("chisq", &chisq_,"chisq/F"); ftree->Branch("ndof", &ndof_,"ndof/I"); - ftree->Branch("nkkbf", &nkkbf_,"nkkbf/I"); + ftree->Branch("nkkdw", &nkkdw_,"nkkdw/I"); ftree->Branch("nkkmat", &nkkmat_,"nkkmat/I"); ftree->Branch("nkkhit", &nkkhit_,"nkkhit/I"); + ftree->Branch("nactivehit", &nactivehit_,"nactivehit/I"); + ftree->Branch("nstrawhit", &nstrawhit_,"nstrawhit/I"); + ftree->Branch("nnull", &nnull_,"nnull/I"); + ftree->Branch("nscinthit", &nscinthit_,"nscinthit/I"); ftree->Branch("chiprob", &chiprob_,"chiprob/F"); ftree->Branch("niter", &niter_,"niter/I"); ftree->Branch("nmeta", &nmeta_,"nmeta/I"); @@ -586,7 +638,6 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { TAxis* xax = corravg->GetXaxis(); TAxis* yax = corravg->GetYaxis(); double nsig(10.0); - // double pscale = nsig/sqrt(nhits); double pscale = nsig; for(size_t ipar=0;ipar< NParams(); ipar++){ auto tpar = static_cast(ipar); @@ -623,12 +674,12 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { TH1F* mmompull = new TH1F("mmompull","Mid Momentum Pull;#Delta P/#sigma _{p}",100,-nsig,nsig); TH1F* bmompull = new TH1F("bmompull","Back Momentum Pull;#Delta P/#sigma _{p}",100,-nsig,nsig); double duration (0.0); - unsigned nfail(0), ndiv(0), npdiv(0), nlow(0), nconv(0), nuconv(0); + unsigned nfail(0), ndiv(0), ndgap(0), npdiv(0), nlow(0), nconv(0), nuconv(0); for(unsigned ievent=0;ieventfieldVect(seedpos.Vect()); KTRAJ seedtraj(seedpos,seedmom,midhel.charge(),bmid,seedrange); @@ -654,18 +705,18 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { cparams.covariance()[ipar][ipar] = perr*perr; cparams.parameters()[ipar] += tr_.Gaus(0.0,perr); } - thits.push_back(std::make_shared(front.range().mid(),cparams,mask)); + thits.push_back(std::make_shared(front.range().mid(),seedtraj,cparams,mask)); } if(extend){ for(auto ihit = thits.begin(); ihit != thits.end();){ - if(tr_.Uniform(0.0,1.0) < extendfrac){ + if(tr_.Uniform(0.0,1.0) < ineff){ exthits.push_back(*ihit); ihit = thits.erase(ihit); } else ++ihit; } for(auto ixing = dxings.begin(); ixing != dxings.end();){ - if(tr_.Uniform(0.0,1.0) < extendfrac){ + if(tr_.Uniform(0.0,1.0) < ineff){ exdxings.push_back(*ixing); ixing = dxings.erase(ixing); } else @@ -682,8 +733,9 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { if(fstat.status_ == Status::converged)nconv++; if(fstat.status_ == Status::unconverged)nuconv++; if(fstat.status_ == Status::lowNDOF)nlow++; - if(fstat.status_ == Status::diverged)ndiv++; + if(fstat.status_ == Status::chisqdiverged)ndiv++; if(fstat.status_ == Status::paramsdiverged)npdiv++; + if(fstat.status_ == Status::gapdiverged)ndgap++; niter_ = 0; for(auto const& fstat: kktrk.history()){ if(fstat.status_ != Status::unfit)niter_++; @@ -693,11 +745,10 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { maxgap_ = avgap_ = -1; igap_ = -1; // fill effect information - nkkbf_ = 0; nkkhit_ = 0; nkkmat_ = 0; + nkkdw_ = 0; nkkhit_ = 0; nkkmat_ = 0; // accumulate chisquared info chisq_ = fstat.chisq_.chisq(); ndof_ = fstat.chisq_.nDOF(); - niter_ = fstat.iter_; nmeta_ = fstat.miter_; status_ = fstat.status_; chiprob_ = fstat.chisq_.probability(); @@ -706,130 +757,49 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { minfovec.clear(); tinfovec.clear(); statush->Fill(fstat.status_); - if(fstat.status_ != KinKal::Status::failed){ - // basic info - auto const& fptraj = kktrk.fitTraj(); - sbeg_ = seedtraj.range().begin(); - send_ = seedtraj.range().end(); - fbeg_ = fptraj.range().begin(); - fend_ = fptraj.range().end(); - - for(auto const& eff: kktrk.effects()) { - const KKHIT* kkhit = dynamic_cast(eff.get()); - const KKBFIELD* kkbf = dynamic_cast(eff.get()); - const KKMAT* kkmat = dynamic_cast(eff.get()); - if(kkhit != 0){ - nkkhit_++; - HitInfo hinfo; - hinfo.active_ = kkhit->active(); - hinfo.time_ = kkhit->time(); - hinfo.chisq_ = kkhit->chisq().chisq(); - hinfo.ndof_ = kkhit->chisq().nDOF(); - hinfo.state_ = WireHitState::inactive; - hinfo.pos_ = fptraj.position3(kkhit->hit()->time()); - hinfo.t0_ = 0.0; - const STRAWHIT* strawhit = dynamic_cast(kkhit->hit().get()); - const SCINTHIT* scinthit = dynamic_cast(kkhit->hit().get()); - const PARHIT* constraint = dynamic_cast(kkhit->hit().get()); - if(strawhit != 0){ - hinfo.type_ = HitInfo::straw; - hinfo.state_ = strawhit->hitState().state_; - hinfo.t0_ = strawhit->closestApproach().particleToca(); - hinfo.doca_ = strawhit->closestApproach().doca(); - hinfo.deltat_ = strawhit->closestApproach().deltaT(); - hinfo.docavar_ = strawhit->closestApproach().docaVar(); - hinfo.tocavar_ = strawhit->closestApproach().tocaVar(); - // straw hits can have multiple residuals - if(strawhit->activeRes(STRAWHIT::tresid)){ - hinfo.tresid_ = strawhit->residual(STRAWHIT::tresid).value(); - hinfo.tresidvar_ = strawhit->residual(STRAWHIT::tresid).variance(); - } - // - if(strawhit->activeRes(STRAWHIT::dresid)){ - hinfo.dresid_ = strawhit->residual(STRAWHIT::dresid).value(); - hinfo.dresidvar_ = strawhit->residual(STRAWHIT::dresid).variance(); - } - hinfovec.push_back(hinfo); - } else if(scinthit != 0){ - hinfo.type_ = HitInfo::scint; - hinfo.tresid_ = scinthit->residual().value(); - hinfo.tresidvar_ = scinthit->residual().variance(); - hinfo.t0_ = scinthit->closestApproach().particleToca(); - hinfo.doca_ = scinthit->closestApproach().doca(); - hinfo.deltat_ = scinthit->closestApproach().deltaT(); - hinfo.docavar_ = scinthit->closestApproach().docaVar(); - hinfo.tocavar_ = scinthit->closestApproach().tocaVar(); - hinfovec.push_back(hinfo); - } else if(constraint != 0){ - hinfo.type_ = HitInfo::constraint; - hinfo.dresid_ = sqrt(constraint->chisq().chisq()); - hinfo.dresidvar_ = 1.0; - hinfovec.push_back(hinfo); - } else { - hinfo.type_ = HitInfo::unknown; - } - } - if(kkmat != 0){ - nkkmat_++; - KinKal::MaterialInfo minfo; - minfo.time_ = kkmat->time(); - minfo.active_ = kkmat->active(); - minfo.nxing_ = kkmat->detXing().matXings().size(); - std::array dmom = {0.0,0.0,0.0}, momvar = {0.0,0.0,0.0}; - kkmat->detXing().materialEffects(kkmat->refKTraj(),TimeDir::forwards, dmom, momvar); - minfo.dmomf_ = dmom[MomBasis::momdir_]; - minfo.momvar_ = momvar[MomBasis::momdir_]; - minfo.perpvar_ = momvar[MomBasis::perpdir_]; - minfovec.push_back(minfo); - } - if(kkbf != 0){ - nkkbf_++; - BFieldInfo bfinfo; - bfinfo.active_ = kkbf->active(); - bfinfo.time_ = kkbf->time(); - bfinfo.range_ = kkbf->range().range(); - bfinfovec.push_back(bfinfo); - } - } + // truth parameters, front and back + double ttlow = thits.front()->time(); + double ttmid = tptraj.range().mid(); + double tthigh = thits.back()->time(); + KTRAJ const& fttraj = tptraj.nearestPiece(ttlow); + KTRAJ const& mttraj = tptraj.nearestPiece(ttmid); + KTRAJ const& bttraj = tptraj.nearestPiece(tthigh); + Parameters ftpars, mtpars, btpars; + ftpars = fttraj.params(); + mtpars = mttraj.params(); + btpars = bttraj.params(); + ftmom_ = tptraj.momentum(ttlow); + mtmom_ = tptraj.momentum(ttmid); + btmom_ = tptraj.momentum(tthigh); + // seed + sbeg_ = seedtraj.range().begin(); + send_ = seedtraj.range().end(); + for(size_t ipar=0;ipar<6;ipar++){ + spars_.pars_[ipar] = seedtraj.params().parameters()[ipar]; + ftpars_.pars_[ipar] = ftpars.parameters()[ipar]; + mtpars_.pars_[ipar] = mtpars.parameters()[ipar]; + btpars_.pars_[ipar] = btpars.parameters()[ipar]; } - if(fstat.usable()){ - // truth parameters, front and back - double ttlow = tptraj.range().begin(); - double ttmid = tptraj.range().mid(); - double tthigh = tptraj.range().end(); - KTRAJ const& fttraj = tptraj.nearestPiece(ttlow); - KTRAJ const& mttraj = tptraj.nearestPiece(ttmid); - KTRAJ const& bttraj = tptraj.nearestPiece(tthigh); - for(size_t ipar=0;ipar<6;ipar++){ - spars_.pars_[ipar] = seedtraj.params().parameters()[ipar]; - ftpars_.pars_[ipar] = fttraj.params().parameters()[ipar]; - mtpars_.pars_[ipar] = mttraj.params().parameters()[ipar]; - btpars_.pars_[ipar] = bttraj.params().parameters()[ipar]; - } - ftmom_ = tptraj.momentum(ttlow); - mtmom_ = tptraj.momentum(ttmid); - btmom_ = tptraj.momentum(tthigh); - ndof->Fill(ndof_); - chisq->Fill(chisq_); - chisqndof->Fill(fstat.chisq_.chisqPerNDOF()); - chisqprob->Fill(chiprob_); - if(chiprob_ > 0.0) logchisqprob->Fill(log10(chiprob_)); - hniter->Fill(niter_); - hnmeta->Fill(nmeta_); + // fit: initialize to 0 + ffmom_ = -1.0; + mfmom_ = -1.0; + bfmom_ = -1.0; + ffmomerr_ = -1.0; + mfmomerr_ = -1.0; + bfmomerr_ = -1.0; + // gaps + double maxgap, avgap; + size_t igap; + kktrk.fitTraj().gaps(maxgap, igap, avgap); + maxgap_ = maxgap; + avgap_ = avgap; + igap_ = igap; - // step through the fit traj and compare to the truth + if(fstat.usable()){ + int ts = testState(kktrk,sigmas); + if(ts != 0)std::cout << "state test failed" << std::endl; + // basic info auto const& fptraj = kktrk.fitTraj(); - double dt = fptraj.range().range()/nsteps; - for(unsigned istep=0;istep < nsteps;istep++){ - double tstep = fptraj.range().begin()+dt*istep; - double ttrue; - double dperp = dTraj(fptraj,tptraj,tstep,ttrue); - ParticleTrajectoryInfo ktinfo; - ktinfo.time_ = tstep; - ktinfo.dperp_ = dperp; - ktinfo.dt_= tstep-ttrue; - tinfovec.push_back(ktinfo); - } // compare parameters at the first traj of both true and fit // correct the true parameters in case the BFieldMap isn't nominal // correct the sampling time for the t0 difference @@ -844,20 +814,17 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { auto const& ffpars = fftraj.params(); auto const& mfpars = mftraj.params(); auto const& bfpars = bftraj.params(); - double maxgap, avgap; - size_t igap; - fptraj.gaps(maxgap, igap, avgap); - maxgap_ = maxgap; - avgap_ = avgap; - igap_ = igap; - - Parameters ftpars, mtpars, btpars; - ftpars = fttraj.params(); - mtpars = mttraj.params(); - btpars = bttraj.params(); + ndof->Fill(ndof_); + chisq->Fill(chisq_); + chisqndof->Fill(fstat.chisq_.chisqPerNDOF()); + chisqprob->Fill(chiprob_); + if(chiprob_ > 0.0) logchisqprob->Fill(log10(chiprob_)); + hniter->Fill(niter_); + hnmeta->Fill(nmeta_); // accumulate parameter difference and pull vector fcerr(6,0.0), mcerr(6,0.0), bcerr(6,0.0); + for(size_t ipar=0;ipar< NParams(); ipar++){ fcerr[ipar] = sqrt(ffpars.covariance()[ipar][ipar]); mcerr[ipar] = sqrt(mfpars.covariance()[ipar][ipar]); @@ -880,37 +847,155 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { corravg->Fill(ipar,jpar,fabs(corr)); } } - // extract fit parameters and errors - for(size_t ipar=0;ipar<6;ipar++){ - ffitpars_.pars_[ipar] = fftraj.params().parameters()[ipar]; - mfitpars_.pars_[ipar] = mftraj.params().parameters()[ipar]; - bfitpars_.pars_[ipar] = bftraj.params().parameters()[ipar]; - ffiterrs_.pars_[ipar] = sqrt(fftraj.params().covariance()(ipar,ipar)); - mfiterrs_.pars_[ipar] = sqrt(mftraj.params().covariance()(ipar,ipar)); - bfiterrs_.pars_[ipar] = sqrt(bftraj.params().covariance()(ipar,ipar)); - } ffmom_ = fptraj.momentum(ftlow); mfmom_ = fptraj.momentum(ftmid); bfmom_ = fptraj.momentum(fthigh); ffmomerr_ = sqrt(fptraj.momentumVariance(ftlow)); mfmomerr_ = sqrt(fptraj.momentumVariance(ftmid)); bfmomerr_ = sqrt(fptraj.momentumVariance(fthigh)); - fft_ = fptraj.range().begin(); - mft_ = fptraj.range().mid(); - bft_ = fptraj.range().end(); fmomres->Fill((ffmom_-ftmom_)); mmomres->Fill((mfmom_-mtmom_)); bmomres->Fill((bfmom_-btmom_)); fmompull->Fill((ffmom_-ftmom_)/ffmomerr_); mmompull->Fill((mfmom_-mtmom_)/mfmomerr_); bmompull->Fill((bfmom_-btmom_)/bfmomerr_); - // state space parameter difference and errors - // ParticleStateEstimate tslow = tptraj.state(tlow); - // ParticleStateEstimate tshigh = tptraj.state(thigh); - // ParticleStateEstimate slow = fptraj.stateEstimate(tlow); - // ParticleStateEstimate shigh = fptraj.stateEstimate(thigh); + // + if(ttree && fstat.usable()){ + fbeg_ = fptraj.range().begin(); + fend_ = fptraj.range().end(); + + nactivehit_ = nstrawhit_ = nnull_ = nscinthit_ = 0; + for(auto const& eff: kktrk.effects()) { + const KKMEAS* kkhit = dynamic_cast(eff.get()); + const KKDW* kkdw = dynamic_cast(eff.get()); + const KKMAT* kkmat = dynamic_cast(eff.get()); + if(kkhit != 0){ + nkkhit_++; + const WIREHIT* strawhit = dynamic_cast(kkhit->hit().get()); + const SCINTHIT* scinthit = dynamic_cast(kkhit->hit().get()); + const PARHIT* parhit = dynamic_cast(kkhit->hit().get()); + if(kkhit->active())nactivehit_++; + HitInfo hinfo; + hinfo.active_ = kkhit->active(); + hinfo.time_ = kkhit->time(); + auto chisq = kkhit->hit()->chisquared(); + hinfo.chisq_ = chisq.chisq(); + hinfo.prob_ = chisq.probability(); + hinfo.ndof_ = chisq.nDOF(); + hinfo.state_ = -10; + hinfo.pos_ = fptraj.position3(kkhit->hit()->time()); + hinfo.t0_ = 0.0; + hinfo.dresid_ = -1000.0; + hinfo.dresidvar_ = -1.0; + hinfo.dresidpull_ = -1000.0; + hinfo.tresid_ = -1000.0; + hinfo.tresidvar_ = -1.0; + hinfo.tresidpull_ = -1000.0; + if(strawhit != 0){ + if(strawhit->active()){ + nstrawhit_++; + if(!strawhit->hitState().useDrift())nnull_++; + } + hinfo.type_ = HitInfo::straw; + hinfo.state_ = strawhit->hitState().state_; + hinfo.t0_ = strawhit->closestApproach().particleToca(); + hinfo.id_ = strawhit->id(); + hinfo.doca_ = strawhit->closestApproach().doca(); + hinfo.deltat_ = strawhit->closestApproach().deltaT(); + hinfo.docavar_ = strawhit->closestApproach().docaVar(); + hinfo.tocavar_ = strawhit->closestApproach().tocaVar(); + hinfo.dirdot_ = strawhit->closestApproach().dirDot(); + // straw hits can have multiple residuals + if(strawhit->refResidual(WIREHIT::tresid).active()){ + auto resid = strawhit->residual(WIREHIT::tresid); + hinfo.tresid_ = resid.value(); + hinfo.tresidvar_ = resid.variance(); + hinfo.tresidpull_ = resid.pull(); + } + // + if(strawhit->refResidual(WIREHIT::dresid).active()){ + auto resid = strawhit->residual(WIREHIT::dresid); + hinfo.dresid_ = resid.value(); + hinfo.dresidvar_ = resid.variance(); + hinfo.dresidpull_ = resid.pull(); + } + hinfovec.push_back(hinfo); + } else if(scinthit != 0){ + if(scinthit->active())nscinthit_++; + hinfo.type_ = HitInfo::scint; + auto resid = scinthit->residual(0); + hinfo.tresid_ = resid.value(); + hinfo.tresidvar_ = resid.variance(); + hinfo.tresidpull_ = resid.pull(); + hinfo.t0_ = scinthit->closestApproach().particleToca(); + hinfo.doca_ = scinthit->closestApproach().doca(); + hinfo.deltat_ = scinthit->closestApproach().deltaT(); + hinfo.docavar_ = scinthit->closestApproach().docaVar(); + hinfo.tocavar_ = scinthit->closestApproach().tocaVar(); + hinfo.dirdot_ = scinthit->closestApproach().dirDot(); + hinfovec.push_back(hinfo); + } else if(parhit != 0){ + hinfo.type_ = HitInfo::parcon; + hinfo.dresid_ = sqrt(parhit->chisquared().chisq()); + hinfo.dresidvar_ = 1.0; + hinfovec.push_back(hinfo); + } else { + hinfo.type_ = HitInfo::unknown; + } + } + if(kkmat != 0){ + nkkmat_++; + KinKal::MaterialInfo minfo; + minfo.time_ = kkmat->time(); + minfo.active_ = kkmat->active(); + minfo.nxing_ = kkmat->elementXing().matXings().size(); + double dmomf, momvar, perpvar; + kkmat->elementXing().materialEffects(dmomf, momvar, perpvar); + minfo.dmomf_ = dmomf; minfo.momvar_ = momvar; minfo.perpvar_ = perpvar; + STRAWXING* sxing = dynamic_cast(kkmat->elementXingPtr().get()); + if(sxing != 0){ + minfo.doca_ = sxing->closestApproach().doca(); + minfo.docavar_ = sxing->closestApproach().docaVar(); + minfo.dirdot_ = sxing->closestApproach().dirDot(); + } + minfovec.push_back(minfo); + } + if(kkdw != 0){ + nkkdw_++; + DomainWallInfo bfinfo; + bfinfo.active_ = kkdw->active(); + bfinfo.time_ = kkdw->time(); + bfinfo.range_ = kkdw->nextDomain().mid()-kkdw->prevDomain().mid(); + bfinfovec.push_back(bfinfo); + } + } + fft_ = fptraj.range().begin(); + mft_ = fptraj.range().mid(); + bft_ = fptraj.range().end(); + // extract fit parameters and errors + for(size_t ipar=0;ipar<6;ipar++){ + ffitpars_.pars_[ipar] = fftraj.params().parameters()[ipar]; + mfitpars_.pars_[ipar] = mftraj.params().parameters()[ipar]; + bfitpars_.pars_[ipar] = bftraj.params().parameters()[ipar]; + ffiterrs_.pars_[ipar] = sqrt(fftraj.params().covariance()(ipar,ipar)); + mfiterrs_.pars_[ipar] = sqrt(mftraj.params().covariance()(ipar,ipar)); + bfiterrs_.pars_[ipar] = sqrt(bftraj.params().covariance()(ipar,ipar)); + } - // test + // step through the fit traj and compare to the truth + auto const& fptraj = kktrk.fitTraj(); + double dt = fptraj.range().range()/nsteps; + for(unsigned istep=0;istep < nsteps;istep++){ + double tstep = fptraj.range().begin()+dt*istep; + double ttrue; + double dperp = dTraj(fptraj,tptraj,tstep,ttrue); + ParticleTrajectoryInfo ktinfo; + ktinfo.time_ = tstep; + ktinfo.dperp_ = dperp; + ktinfo.dt_= tstep-ttrue; + tinfovec.push_back(ktinfo); + } + } } else if(printbad){ cout << "Bad Fit event " << ievent << " status " << kktrk.fitStatus() << endl; cout << "True Traj " << tptraj << endl; @@ -925,7 +1010,8 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { << nuconv << " Unconverged fits " << nfail << " Failed fits " << nlow << " low NDOF fits " - << ndiv << " Diverged fits " + << ndiv << " ChisqDiverged fits " + << ndgap << " GapDiverged fits " << npdiv << " ParameterDiverged fits " << endl; hnfail->Fill(nfail); hndiv->Fill(ndiv); @@ -934,7 +1020,7 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { } cout <<"Time/fit = " << duration/double(nevents) << " Nanoseconds " << endl; // fill canvases - TCanvas* fdpcan = new TCanvas("fdpcan","fdpcan",800,600); + TCanvas* fdpcan = new TCanvas("fdpcan","fdpcan",1200,800); fdpcan->Divide(3,3); for(size_t ipar=0;iparcd(ipar+1); @@ -943,14 +1029,19 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { fdpcan->cd(NParams()+1); // Test momentum resolution TFitResultPtr ffitr = fmomres->Fit("gaus","qS"); - if(fabs(ffitr->Parameter(1))/ffitr->Error(1) > 10.0 || ffitr->Parameter(2) > 2.0*momsigma ){ - cout << "Front momentum resolution out of tolerance " - << ffitr->Parameter(1) << " +- " << ffitr->Error(1) << " sigma " << ffitr->Parameter(2) << endl; - retval=-3; + TF1* gfit = fmomres->GetFunction("gaus"); + if(gfit != 0){ + if(fabs(gfit->GetParameter(1)) > momsigma || gfit->GetParError(1) > momsigma || gfit->GetParameter(2) > momsigma ){ + cout << "Front momentum resolution out of tolerance " + << gfit->GetParameter(1) << " +- " << gfit->GetParError(1) << " sigma " << gfit->GetParameter(2) << endl; + retval=-3; + } + } else { + retval = -5; } fdpcan->Write(); - TCanvas* mdpcan = new TCanvas("mdpcan","mdpcan",800,600); + TCanvas* mdpcan = new TCanvas("mdpcan","mdpcan",1200,800); mdpcan->Divide(3,3); for(size_t ipar=0;iparcd(ipar+1); @@ -959,13 +1050,18 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { mdpcan->cd(NParams()+1); // Test momentum resolution TFitResultPtr mfitr = mmomres->Fit("gaus","qS"); - if(fabs(mfitr->Parameter(1))/mfitr->Error(1) > 10.0 || mfitr->Parameter(2) > 2.0*momsigma ){ - cout << "Mid momentum resolution out of tolerance " - << mfitr->Parameter(1) << " +- " << mfitr->Error(1) << " sigma " << mfitr->Parameter(2) << endl; - retval=-3; + gfit = mmomres->GetFunction("gaus"); + if(gfit != 0){ + if(fabs(gfit->GetParameter(1)) > momsigma || gfit->GetParError(1) > momsigma || gfit->GetParameter(2) > momsigma ){ + cout << "Middle momentum resolution out of tolerance " + << gfit->GetParameter(1) << " +- " << gfit->GetParError(1) << " sigma " << gfit->GetParameter(2) << endl; + retval=-3; + } + } else { + retval = -5; } mdpcan->Write(); - TCanvas* bdpcan = new TCanvas("bdpcan","bdpcan",800,600); + TCanvas* bdpcan = new TCanvas("bdpcan","bdpcan",1200,800); bdpcan->Divide(3,3); for(size_t ipar=0;iparcd(ipar+1); @@ -973,36 +1069,46 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { } bdpcan->cd(NParams()+1); TFitResultPtr bfitr = bmomres->Fit("gaus","qS"); - if(fabs(bfitr->Parameter(1))/bfitr->Error(1) > 10.0 || bfitr->Parameter(2) > 2.0*momsigma ){ - cout << "Back momentum resolution out of tolerance " - << bfitr->Parameter(1) << " +- " << bfitr->Error(1) << " sigma " << bfitr->Parameter(2) << endl; - retval=-3; + gfit = bmomres->GetFunction("gaus"); + if(gfit != 0){ + if(fabs(gfit->GetParameter(1)) > momsigma || gfit->GetParError(1) > momsigma || gfit->GetParameter(2) > momsigma ){ + cout << "Back momentum resolution out of tolerance " + << gfit->GetParameter(1) << " +- " << gfit->GetParError(1) << " sigma " << gfit->GetParameter(2) << endl; + retval=-3; + } + } else { + retval = -5; } bdpcan->Write(); - TCanvas* fpullcan = new TCanvas("fpullcan","fpullcan",800,600); + TCanvas* fpullcan = new TCanvas("fpullcan","fpullcan",1200,800); fpullcan->Divide(3,3); for(size_t ipar=0;iparcd(ipar+1); TFitResultPtr fpfitr = fpull[ipar]->Fit("gaus","qS"); - if(fpull[ipar]->GetEntries() > 1000 && (fabs(fpfitr->Parameter(1)) > 0.1 || (fpfitr->Parameter(2)-1.0) > 0.2) ){ - cout << "front pull " << fpull[ipar]->GetName() << " out of tolerance " - << fpfitr->Parameter(1) << " +- " << fpfitr->Error(1) << " sigma " << fpfitr->Parameter(2) << endl; - retval=-3; + gfit = fpull[ipar]->GetFunction("gaus"); + if(gfit != 0){ + if(fpull[ipar]->GetEntries() > 1000 && (fabs(gfit->GetParameter(1)) > 0.1 || (gfit->GetParameter(2)-1.0) > 0.2) ){ + cout << "front pull " << fpull[ipar]->GetName() << " out of tolerance " + << gfit->GetParameter(1) << " +- " << gfit->GetParError(1) << " sigma " << gfit->GetParameter(2) << endl; + retval=-3; + } + } else { + retval = -5; } } fpullcan->cd(NParams()+1); fmompull->Fit("gaus","q"); fpullcan->Write(); - TCanvas* mpullcan = new TCanvas("mpullcan","mpullcan",800,600); + TCanvas* mpullcan = new TCanvas("mpullcan","mpullcan",1200,800); mpullcan->Divide(3,3); for(size_t ipar=0;iparcd(ipar+1); mpull[ipar]->Fit("gaus","q"); } mpullcan->cd(NParams()+1); - fmompull->Fit("gaus","q"); + mmompull->Fit("gaus","q"); mpullcan->Write(); - TCanvas* bpullcan = new TCanvas("bpullcan","bpullcan",800,600); + TCanvas* bpullcan = new TCanvas("bpullcan","bpullcan",1200,800); bpullcan->Divide(3,3); for(size_t ipar=0;iparcd(ipar+1); @@ -1011,7 +1117,7 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { bpullcan->cd(NParams()+1); bmompull->Fit("gaus","q"); bpullcan->Write(); - TCanvas* perrcan = new TCanvas("perrcan","perrcan",800,600); + TCanvas* perrcan = new TCanvas("perrcan","perrcan",1200,800); perrcan->Divide(3,2); for(size_t ipar=0;iparcd(ipar+1); @@ -1027,7 +1133,7 @@ int FitTest(int argc, char *argv[],KinKal::DVEC const& sigmas) { corravg->Draw("colorztext0"); corrcan->Write(); - TCanvas* statuscan = new TCanvas("statuscan","statuscan",800,600); + TCanvas* statuscan = new TCanvas("statuscan","statuscan",1200,800); statuscan->Divide(3,2); statuscan->cd(1); statush->Draw(); diff --git a/Tests/Geometry_unit.cc b/Tests/Geometry_unit.cc new file mode 100644 index 00000000..cef99045 --- /dev/null +++ b/Tests/Geometry_unit.cc @@ -0,0 +1,143 @@ +#include "KinKal/Geometry/Cylinder.hh" +#include "KinKal/Geometry/Annulus.hh" +#include "KinKal/Geometry/Rectangle.hh" +#include "KinKal/Geometry/Frustrum.hh" +#include +#include +#include +#include + +using VEC3 = ROOT::Math::XYZVectorD; +using KinKal::Ray; +using KinKal::Cylinder; +using KinKal::Frustrum; +using KinKal::Annulus; +using KinKal::Rectangle; +using KinKal::IntersectFlag; +static struct option long_options[] = { + {"x", required_argument, 0, 'x' }, + {"y", required_argument, 0, 'y' }, + {"z", required_argument, 0, 'z' }, + {"costheta", required_argument, 0, 't' }, + {"phi", required_argument, 0, 'p' }, + {"tol", required_argument, 0, 'T' }, + {"forwards", required_argument, 0, 'f' }, + {NULL, 0,0,0} +}; + +void print_usage() { + printf("Usage: GeometryTest --x --y --z (point) --costheta, --phi (direction angles) --tol (tolerance) --forwards \n "); +} + +int main(int argc, char** argv) { + + int opt; + int long_index =0; + VEC3 point(0.0,0.0,-1.0); + double cost(0.5), phi(0.0), tol(1e-8); + bool forwards(true); + while ((opt = getopt_long_only(argc, argv,"", + long_options, &long_index )) != -1) { + switch (opt) { + case 'x' : point.SetX(atof(optarg)); + break; + case 'y' : point.SetY(atof(optarg)); + break; + case 'z' : point.SetZ(atof(optarg)); + break; + case 't' : cost = atof(optarg); + break; + case 'p' : phi = atof(optarg); + break; + case 'f' : forwards = atoi(optarg); + break; + case 'T' : tol = atof(optarg); + break; + default: print_usage(); + exit(EXIT_FAILURE); + } + } + + double sint = sqrt(1.0-cost*cost); + VEC3 rdir(sint*cos(phi), sint*sin(phi), cost); + Ray ray(rdir,point); + + VEC3 zdir(0.0,0.0,1.0); + VEC3 origin(0.0,0.0,0.0); + VEC3 udir(1.0,0.0,0.0); + Annulus ann(zdir,udir,origin,1.0,2.0); + Rectangle rect(zdir,udir,origin,1.0,2.0); + Cylinder cyl(zdir,origin,2.0,10.0); + Frustrum fru(zdir,origin,3,1,10); + + std::cout << "Test " << ray << std::endl; + std::cout << "Test " << ann << std::endl; + std::cout << "Test " << rect << std::endl; + std::cout << "Test " << fru << std::endl; + std::cout << "Test " << cyl << std::endl; + + if(ann.onSurface(point,tol)) + std::cout << "On Annulus "<< std::endl; + else + std::cout << "Not On Annulus "<< std::endl; + if(rect.onSurface(point,tol)) + std::cout << "On Rectangle "<< std::endl; + else + std::cout << "Not On Rectangle "<< std::endl; + if(cyl.onSurface(point,tol)) + std::cout << "On Cylinder "<< std::endl; + else + std::cout << "Not On Cylinder "<< std::endl; + if(fru.onSurface(point,tol)) + std::cout << "On Frustrum "<< std::endl; + else + std::cout << "Not On Frustrum "<< std::endl; + + double dist; + auto iflag = ann.intersect(ray,dist,forwards,tol); + if(iflag.onsurface_){ + std::cout << "Annulus intersect " << iflag.inbounds_ << " at distance " << dist << " point " << ray.position(dist) << std::endl; + if(!ann.onSurface(ray.position(dist),tol)){ + std::cout << "Intersection point not on surface " << std::endl; + return -2; + } + } else + std::cout << "No Annulus intersection" << std::endl; + + iflag = rect.intersect(ray,dist,forwards,tol); + if(iflag.onsurface_) + std::cout << "Rectangle intersect " << iflag.inbounds_ << " at distance " << dist << " point " << ray.position(dist) << std::endl; + else + std::cout << "No Rectangle intersection" << std::endl; + + iflag = cyl.intersect(ray,dist,forwards,tol); + if(iflag.onsurface_){ + std::cout << "Cylinder intersect " << iflag.inbounds_ << " at distance " << dist << " point " << ray.position(dist) << std::endl; + if(!cyl.onSurface(ray.position(dist),tol)){ + std::cout << "Intersection point not on surface " << std::endl; + return -2; + } + auto tplane = cyl.tangentPlane(ray.position(dist)); + std::cout << "tangent " << tplane << std::endl; + } else + std::cout << "No Cylinder intersection" << std::endl; + + iflag = fru.intersect(ray,dist,forwards,tol); + if(iflag.onsurface_){ + std::cout << "Frustrum intersect " << iflag.inbounds_ << " at distance " << dist << " point " << ray.position(dist) << std::endl; + if(!fru.onSurface(ray.position(dist),tol)){ + std::cout << "Intersection point not on surface " << std::endl; + return -2; + } + auto tplane = fru.tangentPlane(ray.position(dist)); + std::cout << "tangent " << tplane << std::endl; + if(fabs(tplane.vDirection().Dot(fru.axis()) -cos(fru.halfAngle())) > tol/fru.halfLength()){ + std::cout << "Tangent angle doesn't match" << std::endl; + return -3; + } + + } else + std::cout << "No Frustrum intersection" << std::endl; + + return 0; +} diff --git a/Tests/HitTest.hh b/Tests/HitTest.hh index 493cf518..c5b6d32d 100644 --- a/Tests/HitTest.hh +++ b/Tests/HitTest.hh @@ -3,25 +3,22 @@ // #include "KinKal/Trajectory/ParticleTrajectory.hh" #include "KinKal/Trajectory/LoopHelix.hh" -#include "KinKal/Trajectory/Line.hh" #include "KinKal/Trajectory/ClosestApproach.hh" #include "KinKal/Examples/SimpleWireHit.hh" #include "KinKal/Examples/ScintHit.hh" -#include "KinKal/Detector/StrawMaterial.hh" +#include "KinKal/Examples/StrawMaterial.hh" #include "KinKal/Detector/Residual.hh" #include "KinKal/General/BFieldMap.hh" #include "KinKal/General/Vectors.hh" -#include "KinKal/Fit/Measurement.hh" #include "KinKal/General/PhysicalConstants.h" #include "KinKal/Tests/ToyMC.hh" #include -#include +#include #include #include #include "TH1F.h" -#include "TSystem.h" #include "THelix.h" #include "TPolyLine3D.h" #include "TFile.h" @@ -43,25 +40,22 @@ using namespace MatEnv; using namespace KinKal; using namespace std; -// avoid confusion with root -using KinKal::Line; void print_usage() { - printf("Usage: HitTest --momentum f --particle i --charge i --strawhit i --scinthit i --zrange f --nhits i --hres f --seed i --ambigdoca f --By f --Bgrad f --simmat_ i --prec f\n"); + printf("Usage: HitTest --momentum f --particle i --charge i --strawhit i --scinthit i --zrange f --nhits i --hres f --seed i --ambigdoca f --By f --Bgrad f --simmat_ i --prec f --maxdr f \n"); } template int HitTest(int argc, char **argv, const vector& delpars) { - using PKTRAJ = ParticleTrajectory; - using KKHIT = Measurement; + using PTRAJ = ParticleTrajectory; using HIT = Hit; using HITPTR = std::shared_ptr; using HITCOL = vector; using EXING = ElementXing; using EXINGPTR = std::shared_ptr; using EXINGCOL = std::vector; - using STRAWHIT = WireHit; - using STRAWHITPTR = std::shared_ptr; + using WIREHIT = SimpleWireHit; + using WIREHITPTR = std::shared_ptr; using SCINTHIT = ScintHit; using SCINTHITPTR = std::shared_ptr; using STRAWXING = StrawXing; @@ -79,8 +73,8 @@ int HitTest(int argc, char **argv, const vector& delpars) { int iseed(124223); double Bgrad(0.0), By(0.0); bool simmat_(true), scinthit_(true), strawhit_(true); - double precision(1e-8); double zrange(3000.0); // tracker dimension + double maxdr(1.0); static struct option long_options[] = { {"momentum", required_argument, 0, 'm' }, @@ -97,6 +91,7 @@ int HitTest(int argc, char **argv, const vector& delpars) { {"By", required_argument, 0, 'y' }, {"Bgrad", required_argument, 0, 'g' }, {"prec", required_argument, 0, 'P' }, + {"maxdr", required_argument, 0, 'M' }, {NULL, 0,0,0} }; @@ -106,10 +101,10 @@ int HitTest(int argc, char **argv, const vector& delpars) { switch (opt) { case 'm' : mom = atof(optarg); break; - case 'P' : precision = atof(optarg); - break; case 'p' : imass = atoi(optarg); break; + case 'M' : maxdr = atof(optarg); + break; case 'q' : icharge = atoi(optarg); break; case 'z' : zrange = atof(optarg); @@ -148,7 +143,7 @@ int HitTest(int argc, char **argv, const vector& delpars) { } KKTest::ToyMC toy(*BF, mom, icharge, zrange, iseed, nhits, simmat_, scinthit_,ambigdoca, pmass ); toy.setInefficiency(0.0); - PKTRAJ tptraj; + PTRAJ tptraj; // cout << "True " << tptraj << endl; StrawMaterial const& smat = toy.strawMaterial(); TGraph* ggplen = new TGraph(nhits); ggplen->SetTitle("Gas Pathlength;Doca (mm);Pathlength (mm)"); ggplen->SetMinimum(0.0); @@ -176,35 +171,33 @@ int HitTest(int argc, char **argv, const vector& delpars) { hel->SetLineColor(kBlue); hel->Draw(); unsigned ihit(0); - StrawXingConfig sxconfig(toy.strawMaterial().strawRadius()*0.05,1.0); + StrawXingConfig sxconfig(0.3,5.0,10.0,false); for(auto& thit : thits) { Residual res; ClosestApproachData tpdata; - STRAWHIT* strawhit = dynamic_cast(thit.get()); + WIREHIT* strawhit = dynamic_cast(thit.get()); SCINTHIT* scinthit = dynamic_cast(thit.get()); if(strawhit && strawhit_){ - strawhit->update(tptraj); - res = strawhit->residual(0); - tpdata = strawhit->closestApproach(); + res = strawhit->refResidual(0); + tpdata = strawhit->closestApproach().tpData(); } else if(scinthit && scinthit_){ - scinthit->update(tptraj); - res = scinthit->residual(0); - tpdata = scinthit->closestApproach(); + res = scinthit->refResidual(0); + tpdata = scinthit->closestApproach().tpData(); } else continue; TPolyLine3D* line = new TPolyLine3D(2); VEC3 plow, phigh; - STRAWHITPTR shptr = std::dynamic_pointer_cast (thit); + WIREHITPTR shptr = std::dynamic_pointer_cast (thit); SCINTHITPTR lhptr = std::dynamic_pointer_cast (thit); if((bool)shptr){ auto const& tline = shptr->wire(); - plow = tline.position3(tline.range().begin()); - phigh = tline.position3(tline.range().end()); + plow = tline.start(); + phigh = tline.end(); line->SetLineColor(kRed); } else if ((bool)lhptr){ auto const& tline = lhptr->sensorAxis(); - plow = tline.position3(tline.range().begin()); - phigh = tline.position3(tline.range().end()); + plow = tline.start(); + phigh = tline.end(); line->SetLineColor(kCyan); } line->SetPoint(0,plow.X(),plow.Y(), plow.Z()); @@ -251,18 +244,19 @@ int HitTest(int argc, char **argv, const vector& delpars) { } unsigned ipt(0); // cout << tptraj << endl; + MetaIterConfig miconfig; for(auto& thit : thits) { - KKHIT kkhit(thit,tptraj,precision); + thit->updateState(miconfig,false); Residual ores; ClosestApproachData tpdata; - STRAWHIT* strawhit = dynamic_cast(thit.get()); + WIREHIT* strawhit = dynamic_cast(thit.get()); SCINTHIT* scinthit = dynamic_cast(thit.get()); if(strawhit && strawhit_){ - ores = strawhit->residual(0); - tpdata = strawhit->closestApproach(); + ores = strawhit->refResidual(0); + tpdata = strawhit->closestApproach().tpData(); } else if(scinthit && scinthit_){ - ores = scinthit->residual(0); - tpdata = scinthit->closestApproach(); + ores = scinthit->refResidual(0); + tpdata = scinthit->closestApproach().tpData(); } else continue; auto pder = ores.dRdP(); @@ -273,17 +267,18 @@ int HitTest(int argc, char **argv, const vector& delpars) { for(size_t istep=0;isteptime()); modktraj.params().parameters()[ipar] += dpar; - PKTRAJ modtptraj(modktraj); + PTRAJ modtptraj(modktraj); KinKal::DVEC dpvec; dpvec[ipar] = dpar; - kkhit.update(modtptraj);// refer to moded helix + thit->updateReference(modtptraj);// refer to moded helix + thit->updateState(miconfig,false); Residual mres; if(strawhit){ - mres = strawhit->residual(0); + mres = strawhit->refResidual(WIREHIT::dresid); } else if(scinthit) { - mres = scinthit->residual(0); + mres = scinthit->refResidual(0); } double dr = ores.value()-mres.value(); // this sign is confusing. I think // it means the fit needs to know how much to change the ref parameters, which is @@ -291,7 +286,7 @@ int HitTest(int argc, char **argv, const vector& delpars) { // compare the change with the expected from the derivatives double ddr = ROOT::Math::Dot(pder,dpvec); hderivg[ipar]->SetPoint(ipt++,dr,ddr); - if(fabs(dr - ddr) > 1.0 ){ + if(fabs(dr - ddr) > maxdr ){ cout << "Large ddiff " << KTRAJ::paramName(tpar) << " " << *thit << " delta " << dpar << " doca " << tpdata.doca() << " DirDot " << tpdata.dirDot() <<" Exact change " << dr << " deriv " << ddr << endl; status = 2; @@ -306,18 +301,20 @@ int HitTest(int argc, char **argv, const vector& delpars) { TCanvas* hderivgc = new TCanvas("hderiv","hderiv",800,600); hderivgc->Divide(3,2); for(size_t ipar=0;ipar<6;++ipar){ - if(fabs(hderivg[ipar]->GetRMS(1)) > 1e-10 && fabs(hderivg[ipar]->GetRMS(2)) > 1e-10){ + if(fabs(hderivg[ipar]->GetRMS(1)) > 1e-5 && fabs(hderivg[ipar]->GetRMS(2)) > 1e-5){ pline->SetParameters(0.0,1.0); - hderivgc->cd(ipar+1); TFitResultPtr pfitr = hderivg[ipar]->Fit(pline,"SQ","AC*"); - hderivg[ipar]->Draw("AC*"); if(fabs(pfitr->Parameter(0))> 100*delpars[ipar] || fabs(pfitr->Parameter(1)-1.0) > 1e-2){ cout << "Parameter " << KTRAJ::paramName(typename KTRAJ::ParamIndex(ipar)) << " Residual derivative Out of tolerance : Offset " << pfitr->Parameter(0) << " Slope " << pfitr->Parameter(1) << endl; status = 1; } + } else { + cout << "Zero derivatives for parameter " << ipar << endl; } + hderivgc->cd(ipar+1); + hderivg[ipar]->Draw("AC*"); } hderivgc->Write(); diff --git a/Tests/Intersection_unit.cc b/Tests/Intersection_unit.cc new file mode 100644 index 00000000..887ef459 --- /dev/null +++ b/Tests/Intersection_unit.cc @@ -0,0 +1,189 @@ +// +// Test intersections with KinKal objects +// +#include "KinKal/General/ParticleState.hh" +#include "KinKal/General/TimeRange.hh" +#include "KinKal/Geometry/Intersection.hh" +#include "KinKal/Geometry/Intersect.hh" +#include "KinKal/Geometry/Cylinder.hh" +#include "KinKal/Geometry/Disk.hh" +#include "KinKal/Geometry/Annulus.hh" +#include "KinKal/Geometry/Rectangle.hh" +#include "KinKal/Geometry/Frustrum.hh" +#include +#include +#include +#include + +using VEC3 = ROOT::Math::XYZVectorD; +using KinKal::Ray; +using KinKal::Surface; +using KinKal::Cylinder; +using KinKal::Annulus; +using KinKal::Frustrum; +using KinKal::Rectangle; +using KinKal::Disk; +using KinKal::IntersectFlag; +using KinKal::ParticleState; +using KinKal::TimeRange; +using KinKal::TimeDir; +using KinKal::LoopHelix; +using KinKal::CentralHelix; +using KinKal::KinematicLine; +static struct option long_options[] = { + {"scost", required_argument, 0, 'c' }, + {"sphi", required_argument, 0, 'p' }, + {"slen1", required_argument, 0, 'r' }, + {"slen2", required_argument, 0, 'l' }, + {"slen3", required_argument, 0, 'L' }, + {"pcost", required_argument, 0, 'C' }, + {"pphi", required_argument, 0, 'P' }, + {"pmom", required_argument, 0, 'M' }, + {"zpos", required_argument, 0, 'z' }, + {"ypos", required_argument, 0, 'y' }, + {"xpos", required_argument, 0, 'x' }, + {"tdir", required_argument, 0, 't' }, + {NULL, 0,0,0} +}; + +void print_usage() { + printf("Usage: IntersectionTest --slen1 --slen2 -(cylinder radius, half length, disk u, v 1/2 size, or Annulus inner/outer radius), -scost --sphi (surface axis direction) --pmom, --pcost, --pphi --zpos (particle momentum in MeV/c, direction angles, z position) --tdir (0=forwards, 1=backwards)"); +} + +int main(int argc, char** argv) { + + int opt; + int long_index =0; + VEC3 point(0.0,0.0,0.0); + double scost(1.0), sphi(0.0), slen1(400), slen2(1000), slen3(500); + double pcost(0.5), pphi(1.0), pmom(100); + VEC3 ppos(0.0,0.0,-1.0); + TimeDir tdir(TimeDir::forwards); + while ((opt = getopt_long_only(argc, argv,"", + long_options, &long_index )) != -1) { + switch (opt) { + case 'c' : scost = atof(optarg); + break; + case 'p' : sphi = atof(optarg); + break; + case 'r' : slen1 = atof(optarg); + break; + case 'l' : slen2 = atof(optarg); + break; + case 'L' : slen3 = atof(optarg); + break; + case 'C' : pcost = atof(optarg); + break; + case 'P' : pphi = atof(optarg); + break; + case 'M' : pmom = atof(optarg); + break; + case 'x' : ppos.SetX(atof(optarg)); + break; + case 'y' : ppos.SetY(atof(optarg)); + break; + case 'z' : ppos.SetZ(atof(optarg)); + break; + case 't' : tdir = TimeDir(atoi(optarg)); + break; + default: print_usage(); + exit(EXIT_FAILURE); + } + } + VEC3 origin(0.0,0.0,0.0); + + double ssint = sqrt(1.0-scost*scost); + VEC3 axis(ssint*cos(sphi), ssint*sin(sphi), scost); + VEC3 udir(scost*cos(sphi), scost*sin(sphi), -ssint); + + double psint = sqrt(1.0-pcost*pcost); + VEC3 momvec(psint*cos(pphi), psint*sin(pphi), pcost); + momvec *= pmom; + ParticleState pstate(ppos,momvec,0.0,0.5,-1); +// std::cout << "Test " << pstate << std::endl; + std::cout << "Test particle position " << ppos << " momentum " << pstate.momentum3() << std::endl; + double speed = pstate.velocity().Dot(axis); + double tmax = 10*sqrt(slen1*slen1 + slen2*slen2)/speed; + + double tol(1.0e-8); + VEC3 bnom(0.0,0.0,1.0); + TimeRange trange; + if(tdir == TimeDir::forwards){ + trange = TimeRange(0.0,tmax); + std::cout << "Forwards Time Intersection Test, range " << trange << std::endl; + } else{ + trange = TimeRange(-tmax, 0.0); + std::cout << "Backwards Time Intersection Test, range " << trange << std::endl; + } + KinematicLine ktraj(pstate,bnom,trange); + // intersect with various surfaces + Cylinder cyl(axis,origin,slen1,slen2); + std::cout << "Test " << cyl << std::endl; + auto kc_inter = intersect(ktraj,cyl, trange, tol, tdir); + std::cout << "KinematicLine Cylinder Intersection status " << kc_inter << std::endl; + if(kc_inter.good()){ + auto iplane = cyl.tangentPlane(kc_inter.pos_); + auto dist = cyl.distance(kc_inter.pos_); + std::cout << "distance " << dist << " tangent plane at intersection " << iplane << std::endl; + if(fabs(dist) > tol) return -1; + } + + Frustrum fru(axis,origin,slen1,slen3,slen2); + std::cout << "Test " << fru << std::endl; + auto kf_inter = intersect(ktraj,fru, trange, tol, tdir); + std::cout << "KinematicLine Frustrum Intersection status " << kf_inter << std::endl; + if(kf_inter.good()){ + auto iplane = fru.tangentPlane(kf_inter.pos_); + auto dist = fru.distance(kf_inter.pos_); + std::cout << "distance " << dist << " tangent plane at intersection " << iplane << std::endl; + if(fabs(dist) > tol) return -1; + } + + Disk disk(axis,udir,origin,slen1); + std::cout << "Test " << disk << std::endl; + + auto kd_inter = intersect(ktraj,disk, trange, tol, tdir); + std::cout << "KinematicLine Disk Intersection status " << kd_inter << std::endl; + + Annulus ann(axis,udir,origin,slen1, slen2); + std::cout << "Test " << ann << std::endl; + + auto ka_inter = intersect(ktraj,ann, trange, tol, tdir); + std::cout << "KinematicLine Annulus Intersection status " << ka_inter << std::endl; + + Rectangle rect(axis, udir, origin, slen1, slen2); + std::cout << "Test " << rect << std::endl; + + auto kr_inter = intersect(ktraj,rect, trange, tol, tdir); + std::cout << "KinematicLine Rectangle Intersection status " << kr_inter << std::endl; + + // now try with helices + LoopHelix lhelix(pstate,bnom,trange); + auto ld_inter = intersect(lhelix,disk, trange, tol, tdir); + std::cout << "LoopHelix Disk Intersection status " << ld_inter << std::endl; + + auto lc_inter = intersect(lhelix,cyl, trange, tol, tdir); + std::cout << "loophelix cylinder intersection status " << lc_inter << std::endl; + + auto lf_inter = intersect(lhelix,fru, trange, tol, tdir); + std::cout << "loophelix frustrum intersection status " << lf_inter << std::endl; + + // test generic surface intersection + Surface const& psurf = static_cast(disk); + auto ls_inter = intersect(lhelix,psurf,trange,tol, tdir); + std::cout << "loophelix surface (plane) intersection status " << ls_inter << std::endl; + if(ls_inter.onsurface_ != ld_inter.onsurface_ || (ls_inter.pos_-ld_inter.pos_).R() > tol){ + std::cout << "Generic plane intersection failed" << std::endl; + return -1; + } + // test generic surface intersection + Surface const& csurf = static_cast(cyl); + auto ls2_inter = intersect(lhelix,csurf,trange,tol, tdir); + std::cout << "loophelix surface (cylinder) intersection status " << ls2_inter << std::endl; + if(ls2_inter.onsurface_ != lc_inter.onsurface_ || (ls2_inter.pos_-lc_inter.pos_).R() > tol){ + std::cout << "Generic cylinder intersection failed" << std::endl; + return -1; + } + + return 0; +} diff --git a/Tests/KinematicLineClosestApproach_unit.cc b/Tests/KinematicLineClosestApproach_unit.cc index a6fd700a..a7551440 100644 --- a/Tests/KinematicLineClosestApproach_unit.cc +++ b/Tests/KinematicLineClosestApproach_unit.cc @@ -1,5 +1,6 @@ #include "KinKal/Trajectory/KinematicLine.hh" #include "KinKal/Tests/ClosestApproachTest.hh" int main(int argc, char **argv) { - return ClosestApproachTest(argc,argv); + KinKal::DVEC pchange(0.5, 0.001, 0.5, 0.001, 0.5, 0.5); // range for parameter variation + return ClosestApproachTest(argc,argv,pchange); } diff --git a/Tests/KinematicLineFit_unit.cc b/Tests/KinematicLineFit_unit.cc index a0173ef1..279892e7 100644 --- a/Tests/KinematicLineFit_unit.cc +++ b/Tests/KinematicLineFit_unit.cc @@ -13,12 +13,14 @@ int main(int argc, char *argv[]){ arguments.push_back("5"); arguments.push_back("--Bz"); arguments.push_back("0.0"); + arguments.push_back("--Schedule"); + arguments.push_back("driftfit.txt"); std::vector myargv; for (const auto& arg : arguments) myargv.push_back((char*)arg.data()); myargv.push_back(nullptr); return FitTest(myargv.size()-1,myargv.data(),sigmas); - } else + } else return FitTest(argc,argv,sigmas); } diff --git a/Tests/KinematicLineTPoca_unit.cc b/Tests/KinematicLineTPoca_unit.cc deleted file mode 100644 index e9cc083f..00000000 --- a/Tests/KinematicLineTPoca_unit.cc +++ /dev/null @@ -1,9 +0,0 @@ -/* - Original Author: S Middleton 2020 -*/ -#include "KinKal/Trajectory/KinematicLine.hh" -#include "KinKal/Tests/ClosestApproachTest.hh" -int main(int argc, char **argv){ - return ClosestApproachTest(argc,argv); - -} diff --git a/Tests/KinematicLine_unit.cc b/Tests/KinematicLine_unit.cc index 3dcab1f1..9f73a3f5 100644 --- a/Tests/KinematicLine_unit.cc +++ b/Tests/KinematicLine_unit.cc @@ -4,5 +4,6 @@ #include "KinKal/Trajectory/KinematicLine.hh" #include "KinKal/Tests/Trajectory.hh" int main(int argc, char **argv) { - return test(argc,argv); + KinKal::DVEC sigmas(0.5, 0.004, 0.5, 0.002, 0.4, 0.05); // expected parameter sigmas + return TrajectoryTest(argc,argv,sigmas); } diff --git a/Tests/LoopHelixBField_unit.cc b/Tests/LoopHelixBField_unit.cc index dde979d5..b0d5d387 100644 --- a/Tests/LoopHelixBField_unit.cc +++ b/Tests/LoopHelixBField_unit.cc @@ -1,5 +1,20 @@ #include "KinKal/Trajectory/LoopHelix.hh" #include "KinKal/Tests/BFieldMapTest.hh" int main(int argc, char **argv) { - return BFieldMapTest(argc,argv); + if(argc == 1){ + cout << "Testing gradient field, correction 2" << endl; + std::vector arguments; + arguments.push_back(argv[0]); + arguments.push_back("--Bgrad"); + arguments.push_back("-0.036"); // mu2e-like field gradient + arguments.push_back("--Tol"); + arguments.push_back("1.0e-4"); + arguments.push_back("1"); + std::vector myargv; + for (const auto& arg : arguments) + myargv.push_back((char*)arg.data()); + myargv.push_back(nullptr); + return BFieldMapTest(myargv.size()-1,myargv.data()); + } else + return BFieldMapTest(argc,argv); } diff --git a/Tests/LoopHelixClosestApproach_unit.cc b/Tests/LoopHelixClosestApproach_unit.cc index 970abbec..16569ed4 100644 --- a/Tests/LoopHelixClosestApproach_unit.cc +++ b/Tests/LoopHelixClosestApproach_unit.cc @@ -1,5 +1,6 @@ #include "KinKal/Trajectory/LoopHelix.hh" #include "KinKal/Tests/ClosestApproachTest.hh" int main(int argc, char **argv) { - return ClosestApproachTest(argc,argv); + KinKal::DVEC pchange(0.5, 0.5, 0.5, 0.5, 0.001, 0.5); // range for parameter variation + return ClosestApproachTest(argc,argv,pchange); } diff --git a/Tests/LoopHelixFit_unit.cc b/Tests/LoopHelixFit_unit.cc index 8aa104db..a63acd24 100644 --- a/Tests/LoopHelixFit_unit.cc +++ b/Tests/LoopHelixFit_unit.cc @@ -1,18 +1,25 @@ #include "KinKal/Trajectory/LoopHelix.hh" #include "KinKal/Tests/FitTest.hh" int main(int argc, char **argv) { - KinKal::DVEC sigmas(0.5, 0.5, 0.5, 0.5, 0.02, 0.5); // expected parameter sigmas + KinKal::DVEC sigmas(0.5, 0.5, 0.5, 0.5, 0.005, 0.5); // expected parameter sigmas if(argc == 1){ cout << "Testing gradient field, correction 2" << endl; std::vector arguments; arguments.push_back(argv[0]); - arguments.push_back("--Bgrad"); - arguments.push_back("-0.036"); // mu2e-like field gradient +// arguments.push_back("--Bgrad"); +// arguments.push_back("-0.036"); // mu2e-like field gradient + arguments.push_back("--Schedule"); + arguments.push_back("MCAmbigFixedField.txt"); +// arguments.push_back("seedfit.txt"); +// arguments.push_back("--extend"); +// arguments.push_back("driftextend.txt"); +// arguments.push_back("--ttree"); +// arguments.push_back("1"); std::vector myargv; for (const auto& arg : arguments) myargv.push_back((char*)arg.data()); myargv.push_back(nullptr); return FitTest(myargv.size()-1,myargv.data(),sigmas); } else - return FitTest(argc,argv,sigmas); + return FitTest(argc,argv,sigmas); } diff --git a/Tests/LoopHelixTPoca_unit.cc b/Tests/LoopHelixTPoca_unit.cc deleted file mode 100644 index 970abbec..00000000 --- a/Tests/LoopHelixTPoca_unit.cc +++ /dev/null @@ -1,5 +0,0 @@ -#include "KinKal/Trajectory/LoopHelix.hh" -#include "KinKal/Tests/ClosestApproachTest.hh" -int main(int argc, char **argv) { - return ClosestApproachTest(argc,argv); -} diff --git a/Tests/LoopHelix_unit.cc b/Tests/LoopHelix_unit.cc index 8ae5db23..536509c8 100644 --- a/Tests/LoopHelix_unit.cc +++ b/Tests/LoopHelix_unit.cc @@ -1,5 +1,7 @@ #include "KinKal/Trajectory/LoopHelix.hh" #include "KinKal/Tests/Trajectory.hh" + int main(int argc, char **argv) { - return test(argc,argv); + KinKal::DVEC sigmas(0.5, 0.5, 0.5, 0.5, 0.005, 0.5); // expected parameter sigmas + return TrajectoryTest(argc,argv,sigmas); } diff --git a/Tests/MCAmbig.txt b/Tests/MCAmbig.txt new file mode 100644 index 00000000..e64917c5 --- /dev/null +++ b/Tests/MCAmbig.txt @@ -0,0 +1,8 @@ +# +# Test Configuration file for iteration schedule +# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge gapdiv tol minndof bfcor ends plevel +10 1.0e6 1.0 50.0 1.0e6 100.0 1.0e-4 5 1 0 0 +# Then, meta-iteration specific parameters: +2.0 +1.0 +0.0 diff --git a/Tests/MCAmbigFixedField.txt b/Tests/MCAmbigFixedField.txt new file mode 100644 index 00000000..5211fa32 --- /dev/null +++ b/Tests/MCAmbigFixedField.txt @@ -0,0 +1,8 @@ +# +# Test Configuration file for iteration schedule +# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge gapdiv tbuff tol minndof bfcor plevel +10 1.0e6 1.0 1.0e8 1.0e8 100.0 1.0e-4 5 0 0 0 +# Then, meta-iteration specific parameters: +2.0 +1.0 +0.0 diff --git a/Tests/MatEnv_unit.cc b/Tests/MatEnv_unit.cc index 203a842a..30e66d75 100644 --- a/Tests/MatEnv_unit.cc +++ b/Tests/MatEnv_unit.cc @@ -6,9 +6,10 @@ #include "KinKal/MatEnv/SimpleFileFinder.hh" #include -#include +#include #include #include +#include #include "TH1F.h" #include "TSystem.h" @@ -84,8 +85,14 @@ int main(int argc, char **argv) { cout << "Test for particle " << pname << " mass " << pmass << endl; cout << "Searching for material " << matname << endl; MatEnv::SimpleFileFinder sfinder; - MatDBInfo matdbinfo(sfinder,MatEnv::DetMaterial::moyalmean); - const DetMaterial* dmat = matdbinfo.findDetMaterial(matname); + DetMaterialConfig dmconf; + dmconf.elossmode_ = MatEnv::DetMaterial::moyalmean; + dmconf.scatterfrac_solid_ = 0.995; + dmconf.scatterfrac_gas_ = 0.999; + dmconf.ebrehmsfrac_ = 0.04; + + MatDBInfo matdbinfo(sfinder,dmconf); + const std::shared_ptr dmat = matdbinfo.findDetMaterial(matname); if(dmat != 0){ cout << "Found DetMaterial " << dmat->name() << endl; unsigned nstep(100); diff --git a/Tests/ParticleTrajectoryTest.hh b/Tests/ParticleTrajectoryTest.hh index 2e7f88ba..202e3370 100644 --- a/Tests/ParticleTrajectoryTest.hh +++ b/Tests/ParticleTrajectoryTest.hh @@ -2,13 +2,16 @@ // test basic functions of the ParticleTrajectory, using the LoopHelix class // #include "KinKal/Trajectory/ParticleTrajectory.hh" -#include "KinKal/Trajectory/Line.hh" +#include "KinKal/Trajectory/SensorLine.hh" #include "KinKal/Trajectory/PiecewiseClosestApproach.hh" #include "KinKal/General/BFieldMap.hh" #include "KinKal/General/PhysicalConstants.h" +#include "KinKal/Geometry/Cylinder.hh" +#include "KinKal/Geometry/Disk.hh" +#include "KinKal/Geometry/ParticleTrajectoryIntersect.hh" #include -#include +#include #include #include @@ -34,7 +37,7 @@ using namespace KinKal; using namespace std; // avoid confusion with root -using KinKal::Line; +using KinKal::SensorLine; void print_usage() { printf("Usage: ParticleTrajectoryTest --changedir i --delta f --tstep f --nsteps i \n"); @@ -42,8 +45,8 @@ void print_usage() { template int ParticleTrajectoryTest(int argc, char **argv) { - using PKTRAJ = ParticleTrajectory; - using PTCA = PiecewiseClosestApproach; + using PTRAJ = ParticleTrajectory; + using PCA = PiecewiseClosestApproach; double mom(105.0), cost(0.7), phi(0.5); unsigned npts(50); int icharge(-1); @@ -99,11 +102,11 @@ int ParticleTrajectoryTest(int argc, char **argv) { TimeRange range(tstart,tend); KTRAJ lhel(origin,momv,icharge,bnom,range); // create initial piecewise helix from this - PKTRAJ ptraj(lhel); + PTRAJ ptraj(lhel); // append pieces for(int istep=0;istep < nsteps; istep++){ // use derivatives of last piece to define new piece - KTRAJ const& back = ptraj.pieces().back(); + KTRAJ const& back = *ptraj.pieces().back(); double tcomp = back.range().end(); DVEC pder = back.momDeriv(tcomp,tdir); // create modified helix @@ -131,7 +134,7 @@ int ParticleTrajectoryTest(int argc, char **argv) { } // prepend pieces for(int istep=0;istep < nsteps; istep++){ - KTRAJ const& front = ptraj.pieces().front(); + KTRAJ const& front = *ptraj.pieces().front(); double tcomp = front.range().begin(); DVEC pder = front.momDeriv(tcomp,tdir); // create modified helix @@ -175,12 +178,12 @@ int ParticleTrajectoryTest(int argc, char **argv) { icolor = kRed; else if(icolor == kRed) icolor = kBlue; - double tstart = piece.range().begin(); - double ts = (piece.range().end()-piece.range().begin())/(npts-1); + double tstart = piece->range().begin(); + double ts = (piece->range().end()-piece->range().begin())/(npts-1); VEC3 ppos; for(unsigned ipt=0;iptposition3(t); plhel.back()->SetPoint(ipt,ppos.X(),ppos.Y(),ppos.Z()); } plhel.back()->Draw(); @@ -222,40 +225,52 @@ int ParticleTrajectoryTest(int argc, char **argv) { // shift the position VEC3 lpos = midpos + gap*rdir; double wlen(1000.0); - Line tline(lpos, ptraj.range().mid(), pvel, wlen); + SensorLine tline(lpos, ptraj.range().mid(), pvel, wlen); // create ClosestApproach from these CAHint tphint(ptraj.range().mid(),0.0); - PTCA tp(ptraj,tline, tphint,1e-8); - cout << "ClosestApproach status " << tp.statusName() << " doca " << tp.doca() << " dt " << tp.deltaT() << endl; + PCA pca(ptraj,tline, tphint,1e-8); + cout << "ClosestApproach status " << pca.statusName() << " doca " << pca.doca() << " dt " << pca.deltaT() << endl; VEC3 thpos, tlpos; - thpos = tp.particlePoca().Vect(); - tlpos = tp.sensorPoca().Vect(); - double refd = tp.doca(); + thpos = pca.particlePoca().Vect(); + tlpos = pca.sensorPoca().Vect(); + double refd = pca.doca(); cout << " Helix Pos " << midpos << " ClosestApproach KTRAJ pos " << thpos << " ClosestApproach Line pos " << tlpos << endl; - cout << " ClosestApproach particlePoca " << tp.particlePoca() << " ClosestApproach sensorPoca " << tp.sensorPoca() << " DOCA " << refd << endl; - if(tp.status() == ClosestApproachData::converged) { + cout << " ClosestApproach particlePoca " << pca.particlePoca() << " ClosestApproach sensorPoca " << pca.sensorPoca() << " DOCA " << refd << endl; + if(pca.status() == ClosestApproachData::converged) { // draw the line and ClosestApproach TPolyLine3D* line = new TPolyLine3D(2); VEC3 plow, phigh; - plow = tline.position3(tline.range().begin()); - phigh = tline.position3(tline.range().end()); + plow = tline.start(); + phigh = tline.end(); line->SetPoint(0,plow.X(),plow.Y(), plow.Z()); line->SetPoint(1,phigh.X(),phigh.Y(), phigh.Z()); line->SetLineColor(kOrange); line->Draw(); TPolyLine3D* poca = new TPolyLine3D(2); - poca->SetPoint(0,tp.particlePoca().X() ,tp.particlePoca().Y() ,tp.particlePoca().Z()); - poca->SetPoint(1,tp.sensorPoca().X() ,tp.sensorPoca().Y() ,tp.sensorPoca().Z()); + poca->SetPoint(0,pca.particlePoca().X() ,pca.particlePoca().Y() ,pca.particlePoca().Z()); + poca->SetPoint(1,pca.sensorPoca().X() ,pca.sensorPoca().Y() ,pca.sensorPoca().Z()); poca->SetLineColor(kBlack); poca->Draw(); } - cout << "ClosestApproach dDdP" << tp.dDdP() << " dTdP " << tp.dTdP() << endl; + cout << "ClosestApproach dDdP" << pca.dDdP() << " dTdP " << pca.dTdP() << endl; pttcan->Write(); pkfile.Write(); pkfile.Close(); + + // test intersection + + Cylinder cyl(bnom,origin.Vect(),hlen,wlen); + std::cout << "Test " << cyl << std::endl; + auto kc_inter = intersect(ptraj,cyl, ptraj.range(), 1.0e-8); + std::cout << "KinematicLine Cylinder " << kc_inter << std::endl; + + KinKal::Surface const& csurf = static_cast(cyl); + auto kc2_inter = intersect(ptraj,csurf, ptraj.range(), 1.0e-8); + std::cout << "KinematicLine Surface (Cylinder) " << kc2_inter << std::endl; + // return 0; } diff --git a/Tests/Schedule.txt b/Tests/Schedule.txt deleted file mode 100644 index d9eb0d8c..00000000 --- a/Tests/Schedule.txt +++ /dev/null @@ -1,8 +0,0 @@ -# -# Test Configuration file for iteration schedule -# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge tbuff tol minndof bfcor plevel -10 1.0e6 1.0 50.0 1.0e6 0.0 1e-4 5 1 0 -# Then, meta-iteration specific parameters: temperature (mindoca maxdoca minprob) -2.0 -1.0 -0.0 diff --git a/Tests/Schedule_driftextend.txt b/Tests/Schedule_driftextend.txt deleted file mode 100644 index 5f5f9583..00000000 --- a/Tests/Schedule_driftextend.txt +++ /dev/null @@ -1,10 +0,0 @@ -# -# Test Configuration iteration schedule for a drift extension -# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge tbuff tol minndof bfcor plevel -10 1.0e6 1.0 50.0 1.0e6 0.0 1e-4 5 1 0 -# Order: -# temperature (mindoca maxdoca minprob) -2.0 1.5 5 1e-8 -1.0 0.5 3.5 1e-8 -0.5 0.5 2.8 1e-8 -0.0 0.5 2.8 1e-6 diff --git a/Tests/Schedule_driftfit.txt b/Tests/Schedule_driftfit.txt deleted file mode 100644 index 001452df..00000000 --- a/Tests/Schedule_driftfit.txt +++ /dev/null @@ -1,13 +0,0 @@ -# -# Test Configuration iteration schedule for a drift fit -# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge tbuff tol minndof bfcor plevel -10 1.0e6 1.0 50.0 1.0e6 0.0 1e-4 5 1 0 -# Order: -# temperature dchisquared_converge dchisquared_diverge (mindoca maxdoca) -2.0 20 20 1e-8 -1.0 10 10 1e-8 -0.5 5 5 1e-8 -2.0 1.5 5 1e-8 -1.0 1.0 3.5 1e-8 -0.5 0.5 2.8 1e-8 -0.0 0.5 2.8 1e-6 diff --git a/Tests/Schedule_seedfit.txt b/Tests/Schedule_seedfit.txt deleted file mode 100644 index 3b3b8697..00000000 --- a/Tests/Schedule_seedfit.txt +++ /dev/null @@ -1,7 +0,0 @@ -# Test Configuration iteration schedule for a null (no drift) fit -# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge tbuff tol minndof bfcor plevel -10 1.0e6 1.0 50.0 1.0e6 0.0 1e-4 5 1 0 -# Then, meta-iteration specific parameters: temperature (mindoca maxdoca minprob) -2.0 20 20 1.0e-8 -1.0 10 10 1.0e-8 -0.0 5 5 1.0e-8 diff --git a/Tests/ToyMC.hh b/Tests/ToyMC.hh index 6075aebb..d0891748 100644 --- a/Tests/ToyMC.hh +++ b/Tests/ToyMC.hh @@ -3,26 +3,29 @@ // // Toy MC for fit and hit testing // +#include #include "TRandom3.h" #include "KinKal/MatEnv/MatDBInfo.hh" #include "KinKal/MatEnv/DetMaterial.hh" #include "KinKal/MatEnv/SimpleFileFinder.hh" -#include "KinKal/Trajectory/Line.hh" +#include "KinKal/Examples/CaloDistanceToTime.hh" +#include "KinKal/Trajectory/SensorLine.hh" #include "KinKal/Trajectory/ParticleTrajectory.hh" #include "KinKal/Trajectory/PiecewiseClosestApproach.hh" #include "KinKal/Examples/SimpleWireHit.hh" -#include "KinKal/Detector/StrawXing.hh" -#include "KinKal/Detector/StrawMaterial.hh" +#include "KinKal/Examples/StrawXing.hh" +#include "KinKal/Examples/StrawMaterial.hh" #include "KinKal/Examples/ScintHit.hh" #include "KinKal/General/BFieldMap.hh" #include "KinKal/General/Vectors.hh" #include "KinKal/General/PhysicalConstants.h" +#include "KinKal/Geometry/Cylinder.hh" namespace KKTest { using namespace KinKal; template class ToyMC { public: - using PKTRAJ = ParticleTrajectory; + using PTRAJ = ParticleTrajectory; using HIT = Hit; using HITPTR = std::shared_ptr; using HITCOL = std::vector; @@ -35,27 +38,31 @@ namespace KKTest { using SCINTHITPTR = std::shared_ptr; using STRAWXING = StrawXing; using STRAWXINGPTR = std::shared_ptr; - using PTCA = PiecewiseClosestApproach; + using PCA = PiecewiseClosestApproach; // create from aseed - ToyMC(BFieldMap const& bfield, double mom, int icharge, double zrange, int iseed, unsigned nhits, bool simmat, bool lighthit, double ambigdoca ,double simmass) : - bfield_(bfield), matdb_(sfinder_,MatEnv::DetMaterial::moyalmean), // use the moyal based eloss model + ToyMC(BFieldMap const& bfield, double mom, int icharge, double zrange, int iseed, unsigned nhits, bool simmat, bool scinthit, double ambigdoca ,double simmass) : + bfield_(bfield), matdb_(sfinder_), mom_(mom), icharge_(icharge), - tr_(iseed), nhits_(nhits), simmat_(simmat), lighthit_(lighthit), ambigdoca_(ambigdoca), simmass_(simmass), + tr_(iseed), nhits_(nhits), simmat_(simmat), scinthit_(scinthit), ambigdoca_(ambigdoca), simmass_(simmass), sprop_(0.8*CLHEP::c_light), sdrift_(0.065), - zrange_(zrange), rstraw_(2.5), rwire_(0.025), wthick_(0.015), wlen_(1000.0), sigt_(3.0), ineff_(0.05), - scitsig_(0.1), shPosSig_(10.0), shmax_(80.0), coff_(50.0), clen_(200.0), cprop_(0.8*CLHEP::c_light), + zrange_(zrange), rstraw_(2.5), rwire_(0.025), wthick_(0.015), wlen_(1000.0), sigt_(3.0), sigtot_(7.0), ineff_(0.05), + scitsig_(0.5), shPosSig_(10.0), shmax_(40.0), cz_(zrange_+50), clen_(200.0), cprop_(0.8*CLHEP::c_light), osig_(10.0), ctmin_(0.5), ctmax_(0.8), tol_(1e-5), tprec_(1e-8), t0off_(700.0), - smat_(matdb_,rstraw_, wthick_,rwire_) {} + smat_(matdb_,rstraw_, wthick_, 3*wthick_, rwire_), + ipacyl_(VEC3(0.0,0.0,1.0), VEC3(0.0,0.0,-0.75*zrange),400.0, zrange/4.0), ipathick_(0.5), ipamat_(matdb_.findDetMaterial("HDPE")), + miconfig_(0.0) { + miconfig_.addUpdater(std::any(StrawXingConfig(1.0e6,1.0e6,1.0e6,false))); // updater to force exact straw xing material calculation + } // generate a straw at the given time. direction and drift distance are random - Line generateStraw(PKTRAJ const& traj, double htime); + SensorLine generateStraw(PTRAJ const& traj, double htime); // create a seed by randomizing the parameters void createSeed(KTRAJ& seed,DVEC const& sigmas, double seedsmear); - void extendTraj(PKTRAJ& pktraj,double htime); - void createTraj(PKTRAJ& pktraj); - void createScintHit(PKTRAJ& pktraj, HITCOL& thits); - void simulateParticle(PKTRAJ& pktraj,HITCOL& thits, EXINGCOL& dxings, bool addmat=true); - double createStrawMaterial(PKTRAJ& pktraj, const EXING* sxing); + void extendTraj(PTRAJ& ptraj,double htime); + void createTraj(PTRAJ& ptraj); + void createScintHit(PTRAJ& ptraj, HITCOL& thits); + void simulateParticle(PTRAJ& ptraj,HITCOL& thits, EXINGCOL& dxings, bool addmat=true); + double createStrawMaterial(PTRAJ& ptraj, EXING* sxing); // set functions, for special purposes void setInefficiency(double ineff) { ineff_ = ineff; } void setTolerance(double tol) { tol_ = tol; } @@ -66,6 +73,9 @@ namespace KKTest { double zRange() const { return zrange_; } double strawRadius() const { return rstraw_; } StrawMaterial const& strawMaterial() const { return smat_; } + auto const& IPACyl() { return ipacyl_; } + auto const& IPAMat() { return ipamat_; } + auto const& IPAThick() { return ipathick_; } private: BFieldMap const& bfield_; @@ -75,37 +85,41 @@ namespace KKTest { int icharge_; TRandom3 tr_; // random number generator unsigned nhits_; // number of hits to simulate - bool simmat_, lighthit_; + bool simmat_, scinthit_; double ambigdoca_, simmass_; double sprop_; // propagation speed along straw double sdrift_; // drift speed inside straw double zrange_, rstraw_; // tracker dimension double rwire_, wthick_, wlen_; // wire radius, thickness, length double sigt_; // drift time resolution in ns + double sigtot_; // TOT drift time resolution (ns) double ineff_; // hit inefficiency - // time hit parameters - double scitsig_, shPosSig_, shmax_, coff_, clen_, cprop_; + // time hit parameters + double scitsig_, shPosSig_, shmax_, cz_, clen_, cprop_; double osig_, ctmin_, ctmax_; double tol_; // tolerance on momentum accuracy due to BField effects double tprec_; // time precision on TCA double t0off_; // t0 offset StrawMaterial smat_; // straw material - + Cylinder ipacyl_; + double ipathick_; + const std::shared_ptr ipamat_; + MetaIterConfig miconfig_; // configuration used when calculating initial effect }; - template Line ToyMC::generateStraw(PKTRAJ const& traj, double htime) { + template SensorLine ToyMC::generateStraw(PTRAJ const& traj, double htime) { // start with the true helix position at this time auto hpos = traj.position4(htime); - auto hdir = traj.direction(htime); - // generate a random direction for the straw - double eta = tr_.Uniform(-M_PI,M_PI); - VEC3 sdir(cos(eta),sin(eta),0.0); + auto tdir = traj.direction(htime); + // generate a random azimuth direction for the straw + double azimuth = tr_.Uniform(-M_PI,M_PI); + VEC3 sdir(cos(azimuth),sin(azimuth),0.0); // generate a random drift perp to this and the trajectory double rdrift = tr_.Uniform(-rstraw_,rstraw_); - VEC3 drift = (sdir.Cross(hdir)).Unit(); - VEC3 dpos = hpos.Vect() + rdrift*drift; + VEC3 driftdir = (sdir.Cross(tdir)).Unit(); + VEC3 dpos = hpos.Vect() + rdrift*driftdir; // cout << "Generating hit at position " << dpos << endl; - double dprop = tr_.Uniform(0.0,0.5*wlen_); + double dprop = tr_.Uniform(0.0,wlen_); VEC3 mpos = dpos + sdir*dprop; VEC3 vprop = sdir*sprop_; // measured time is after propagation and drift @@ -113,110 +127,108 @@ namespace KKTest { // smear measurement time tmeas = tr_.Gaus(tmeas,sigt_); // construct the trajectory for this hit - return Line(mpos,tmeas,vprop,wlen_); + return SensorLine(mpos,tmeas,vprop,wlen_); } - template void ToyMC::simulateParticle(PKTRAJ& pktraj,HITCOL& thits, EXINGCOL& dxings, bool addmat) { + template void ToyMC::simulateParticle(PTRAJ& ptraj,HITCOL& thits, EXINGCOL& dxings, bool addmat) { // create the seed first - createTraj(pktraj); + createTraj(ptraj); + double mom = ptraj.momentum(0.0); // divide time range double dt(0.0); - if(nhits_ > 0) dt = (pktraj.range().range())/(nhits_); + if(nhits_ > 0) dt = (ptraj.range().range())/(nhits_); VEC3 bsim; // create the hits (and associated materials) for(size_t ihit=0; ihit ineff_){ WireHitState::State ambig(WireHitState::null); if(fabs(tp.doca())> ambigdoca_) ambig = tp.doca() < 0 ? WireHitState::left : WireHitState::right; WireHitState whstate(ambig); double mindoca = std::min(ambigdoca_,rstraw_); - thits.push_back(std::make_shared(bfield_, tp, whstate, mindoca, sdrift_, sigt_*sigt_, rstraw_)); + // true TOT is the drift time + double tot = tr_.Gaus(tp.deltaT(),sigtot_); + thits.push_back(std::make_shared(bfield_, tp, whstate, mindoca, sdrift_, sigt_*sigt_, tot, sigtot_*sigtot_, rstraw_, ihit)); } // compute material effects and change trajectory accordingly auto xing = std::make_shared(tp,smat_); if(addmat) dxings.push_back(xing); if(simmat_){ - double defrac = createStrawMaterial(pktraj, xing.get()); + double de = createStrawMaterial(ptraj, xing.get()); // terminate if there is catastrophic energy loss - if(fabs(defrac) > 0.1)break; + if(fabs(de/mom) > 0.1)break; } } - if(lighthit_ && tr_.Uniform(0.0,1.0) > ineff_){ - createScintHit(pktraj,thits); + if(scinthit_ && tr_.Uniform(0.0,1.0) > ineff_){ + createScintHit(ptraj,thits); } // extend if there are no hits - if(thits.size() == 0) extendTraj(pktraj,pktraj.range().end()); + if(thits.size() == 0) extendTraj(ptraj,ptraj.range().end()); } - template double ToyMC::createStrawMaterial(PKTRAJ& pktraj, const EXING* sxing) { - double desum = 0.0; - double tstraw = sxing->crossingTime(); - auto const& endpiece = pktraj.nearestPiece(tstraw); - double mom = endpiece.momentum(tstraw); + template double ToyMC::createStrawMaterial(PTRAJ& ptraj, EXING* sxing) { + double tstraw = sxing->time(); + auto const& endtraj = ptraj.nearestTraj(tstraw); + auto const& endpiece = *endtraj; + sxing->updateReference(ptraj); + sxing->updateState(miconfig_,true); auto endmom = endpiece.momentum4(tstraw); auto endpos = endpiece.position4(tstraw); - std::array dmom {0.0,0.0,0.0}, momvar {0.0,0.0,0.0}; - sxing->materialEffects(pktraj,TimeDir::forwards, dmom, momvar); + double dm, paramomvar, perpmomvar; + sxing->materialEffects(dm, paramomvar,perpmomvar); + // compute the momentum change; start with energy loss + VEC3 dmvec = dm*endmom.Vect().Unit(); + // randomize momentum change with Gaussian + SVEC3 momsig(sqrt(paramomvar),sqrt(perpmomvar), sqrt(perpmomvar)); for(int idir=0;idir<=MomBasis::phidir_; idir++) { auto mdir = static_cast(idir); - double momsig = sqrt(momvar[idir]); - double dm; - // generate a random effect given this variance and mean. Note momEffect is scaled to momentum - switch( mdir ) { - case KinKal::MomBasis::perpdir_: case KinKal::MomBasis::phidir_ : - dm = tr_.Gaus(dmom[idir],momsig); - break; - case KinKal::MomBasis::momdir_ : - dm = std::min(0.0,tr_.Gaus(dmom[idir],momsig)); - desum += dm*mom; - break; - default: - throw std::invalid_argument("Invalid direction"); - } - auto dmvec = endpiece.direction(tstraw,mdir); - dmvec *= dm*mom; - endmom.SetCoordinates(endmom.Px()+dmvec.X(), endmom.Py()+dmvec.Y(), endmom.Pz()+dmvec.Z(),endmom.M()); + dmvec += tr_.Gaus(0.0,momsig[idir])*endpiece.direction(tstraw,mdir); } - // generate a new piece and append + endmom.SetCoordinates(endmom.Px()+dmvec.X(), endmom.Py()+dmvec.Y(), endmom.Pz()+dmvec.Z(),endmom.M()); + // generate a new piece with this momentumand append VEC3 bnom = bfield_.fieldVect(endpos.Vect()); - KTRAJ newend(endpos,endmom,endpiece.charge(),bnom,TimeRange(tstraw,pktraj.range().end())); + KTRAJ newend(endpos,endmom,endpiece.charge(),bnom,TimeRange(tstraw,ptraj.range().end())); // newend.print(cout,1); - pktraj.append(newend); - return desum/mom; + ptraj.append(newend); + return dm; } - template void ToyMC::createScintHit(PKTRAJ& pktraj, HITCOL& thits) { + template void ToyMC::createScintHit(PTRAJ& ptraj, HITCOL& thits) { // create a ScintHit at the end, axis parallel to z - // first, find the position at showermax_. - VEC3 shmaxTrue,shmaxMeas; + // first, find the position at showermax. + VEC3 shmax,shmeas; double tend = thits.back()->time(); - VEC3 pvel = pktraj.velocity(tend); - double shstart = tend + coff_/pvel.Z(); + // extend to the calorimeter z + VEC3 pvel = ptraj.velocity(tend); + VEC3 ppos = ptraj.position3(tend); + double shstart = tend + (cz_-ppos.Z())/pvel.Z(); + // extend the trajectory to here + extendTraj(ptraj,shstart); + pvel = ptraj.velocity(shstart); + // compute time at showermax double shmaxtime = shstart + shmax_/pvel.R(); - auto endpos = pktraj.position4(shstart); - shmaxTrue = pktraj.position3(shmaxtime); // true position at shower-max - // smear the x-y position by the transverse variance. - shmaxMeas.SetX(tr_.Gaus(shmaxTrue.X(),shPosSig_)); - shmaxMeas.SetY(tr_.Gaus(shmaxTrue.Y(),shPosSig_)); - // set the z position to the sensor plane (end of the crystal) - shmaxMeas.SetZ(endpos.Z()+clen_); - // set the measurement time to correspond to the light propagation from showermax_, smeared by the resolution - double tmeas = tr_.Gaus(shmaxtime+(shmaxMeas.Z()-shmaxTrue.Z())/cprop_,scitsig_); + shmax = ptraj.position3(shmaxtime); // true position at shower-max + // Compute measurement position: smear the x-y position by the transverse variance. + shmeas.SetX(tr_.Gaus(shmax.X(),shPosSig_)); + shmeas.SetY(tr_.Gaus(shmax.Y(),shPosSig_)); + // set the z position to the sensor plane (forward end of the crystal) + shmeas.SetZ(cz_+clen_); + // set the measurement time to correspond to the light propagation from showermax_, smeared by the time resolution + double tmeas = tr_.Gaus(shmaxtime+(shmeas.Z() - shmax.Z())/cprop_,scitsig_); // create the ttraj for the light propagation VEC3 lvel(0.0,0.0,cprop_); - Line lline(shmaxMeas,tmeas,lvel,clen_); + SensorLine lline(shmeas, tmeas, lvel, clen_); // then create the hit and add it; the hit has no material CAHint tphint(tmeas,tmeas); - PTCA tp(pktraj,lline,tphint,tprec_); - thits.push_back(std::make_shared(tp, scitsig_*scitsig_, shPosSig_*shPosSig_)); + PCA pca(ptraj,lline,tphint,tprec_); + thits.push_back(std::make_shared(pca, scitsig_*scitsig_, shPosSig_*shPosSig_)); } template void ToyMC::createSeed(KTRAJ& seed,DVEC const& sigmas,double seedsmear){ @@ -229,40 +241,47 @@ namespace KKTest { } } - template void ToyMC::extendTraj(PKTRAJ& pktraj,double htime) { - ROOT::Math::SMatrix bgrad; - VEC3 pos,vel, dBdt; - pos = pktraj.position3(htime); - vel = pktraj.velocity(htime); - dBdt = bfield_.fieldDeriv(pos,vel); - // std::cout << "end time " << pktraj.back().range().begin() << " hit time " << htime << std::endl; - if(dBdt.R() != 0.0){ - double tbeg = bfield_.rangeInTolerance(pktraj.back(),pktraj.back().range().begin(),tol_); - double tend = pktraj.back().range().end(); - while(tbeg < htime) { - auto pos = pktraj.position4(tbeg); - auto mom = pktraj.momentum4(tbeg); - double tnew = bfield_.rangeInTolerance(pktraj.back(),tbeg,tol_); - VEC3 bf = bfield_.fieldVect(pktraj.position3(0.5*(tbeg+tnew))); - TimeRange prange(tbeg,tend); - KTRAJ newend(pos,mom,pktraj.charge(),bf,prange); - pktraj.append(newend); - tbeg = tnew; + template void ToyMC::extendTraj(PTRAJ& ptraj,double htime) { + if(htime > ptraj.range().end()){ + VEC3 pos,vel, dBdt; + pos = ptraj.position3(htime); + vel = ptraj.velocity(htime); + dBdt = bfield_.fieldDeriv(pos,vel); + // std::cout << "end time " << ptraj.back().range().begin() << " hit time " << htime << std::endl; + if(dBdt.R() != 0.0){ + double tbeg = ptraj.range().end(); + while(tbeg < htime) { + double tend = std::min(tbeg + bfield_.rangeInTolerance(ptraj.back(),tbeg,tol_),htime); + double tmid = 0.5*(tbeg+tend); + auto bf = bfield_.fieldVect(ptraj.position3(tmid)); + auto pos = ptraj.position4(tend); + auto mom = ptraj.momentum4(tend); + TimeRange prange(tbeg,tend); + KTRAJ newend(pos,mom,ptraj.charge(),bf,prange); + // make sure phi0 stays continuous + newend.syncPhi0(ptraj.back()); + ptraj.append(newend); + tbeg = tend; + } } } } - template void ToyMC::createTraj(PKTRAJ& pktraj) { + template void ToyMC::createTraj(PTRAJ& ptraj) { // randomize the position and momentum double tphi = tr_.Uniform(-M_PI,M_PI); double tcost = tr_.Uniform(ctmin_,ctmax_); double tsint = sqrt(1.0-tcost*tcost); MOM4 tmomv(mom_*tsint*cos(tphi),mom_*tsint*sin(tphi),mom_*tcost,simmass_); double tmax = fabs(zrange_/(CLHEP::c_light*tcost)); - VEC4 torigin(tr_.Gaus(0.0,osig_), tr_.Gaus(0.0,osig_), tr_.Gaus(-0.5*zrange_,osig_),tr_.Uniform(t0off_-tmax,t0off_+tmax)); + double tbeg = tr_.Uniform(t0off_-tmax,t0off_+tmax); + VEC4 torigin(tr_.Gaus(0.0,osig_), tr_.Gaus(0.0,osig_), tr_.Gaus(-zrange_,osig_),tbeg); VEC3 bsim = bfield_.fieldVect(torigin.Vect()); - KTRAJ ktraj(torigin,tmomv,icharge_,bsim,TimeRange(torigin.T(),torigin.T()+tmax)); - pktraj = PKTRAJ(ktraj); + KTRAJ ktraj(torigin,tmomv,icharge_,bsim,TimeRange(tbeg,tbeg+tmax)); + // set initial range + double tend = tbeg + bfield_.rangeInTolerance(ktraj,tbeg,tol_); + ktraj.setRange(TimeRange(tbeg,tend)); + ptraj.append(ktraj); } } #endif diff --git a/Tests/Trajectory.hh b/Tests/Trajectory.hh index cdce196b..93077a67 100644 --- a/Tests/Trajectory.hh +++ b/Tests/Trajectory.hh @@ -1,18 +1,17 @@ // // test basic functions of kinematic trajectory class // -#include "KinKal/Trajectory/Line.hh" +#include "KinKal/Trajectory/SensorLine.hh" #include "KinKal/Trajectory/ClosestApproach.hh" #include "KinKal/General/ParticleStateEstimate.hh" #include "KinKal/General/PhysicalConstants.h" #include -#include +#include #include #include #include "TH1F.h" -#include "TSystem.h" #include "THelix.h" #include "TPolyLine3D.h" #include "TAxis3D.h" @@ -31,8 +30,6 @@ using namespace KinKal; using namespace std; -// avoid confusion with root -using KinKal::Line; void print_usage() { printf("Usage: Trajectory --momentum f --costheta f --azimuth f --particle i --charge i --xorigin f -- yorigin f --zorigin f --torigin --tmin f--tmax f --ltime f --By f --invert i\n"); @@ -59,7 +56,7 @@ void drawMom(VEC3 const& start, VEC3 const& momvec,int momcolor,MomVec& mom) { } template -int test(int argc, char **argv) { +int TrajectoryTest(int argc, char **argv,KinKal::DVEC sigmas) { int opt; double mom(105.0), cost(0.7), phi(0.5); double masses[5]={0.511,105.66,139.57, 493.68, 938.0}; @@ -134,34 +131,45 @@ int test(int argc, char **argv) { VEC4 origin(ox,oy,oz,ot); double sint = sqrt(1.0-cost*cost); MOM4 momv(mom*sint*cos(phi),mom*sint*sin(phi),mom*cost,pmass); - KTRAJ lhel(origin,momv,icharge,bnom,TimeRange(-10,10)); - if(invert)lhel.invertCT(); - auto testmom = lhel.momentum4(ot); - // cout << "KTRAJ with momentum " << momv.Vect() << " position " << origin << " has parameters: " << lhel << endl; - // cout << "origin time position = " << lhel.position3(ot) << " momentum " << lhel.momentum3(ot) << " mag " << lhel.momentum(ot) << endl; + KTRAJ ktraj(origin,momv,icharge,bnom,TimeRange(-10,10)); + +// test state + auto state = ktraj.state(3.0); + KTRAJ straj(state,bnom,ktraj.range()); + for(size_t ipar=0; ipar < NParams(); ipar++){ + if(fabs(ktraj.paramVal(ipar)-straj.paramVal(ipar)) > 1.0e-10){ + std::cout << "Parameter mismatch, par " << ipar << " diff " << ktraj.paramVal(ipar) << " " << straj.paramVal(ipar) << std::endl; + return -1; + } + } + + if(invert)ktraj.invertCT(); + auto testmom = ktraj.momentum4(ot); + // cout << "KTRAJ with momentum " << momv.Vect() << " position " << origin << " has parameters: " << ktraj << endl; + // cout << "origin time position = " << ktraj.position3(ot) << " momentum " << ktraj.momentum3(ot) << " mag " << ktraj.momentum(ot) << endl; VEC3 tvel, tdir; double ttime; - double tstp = lhel.range().range()/9; + double tstp = ktraj.range().range()/9; for(int istep=0;istep<10;istep++){ - ttime = lhel.range().begin() + istep*tstp; - tvel = lhel.velocity(ttime); - tdir = lhel.direction(ttime); - testmom = lhel.momentum4(ttime); + ttime = ktraj.range().begin() + istep*tstp; + tvel = ktraj.velocity(ttime); + tdir = ktraj.direction(ttime); + testmom = ktraj.momentum4(ttime); // cout << "velocity " << tvel << " direction " << tdir << " momentum " << testmom.R() << endl; - // cout << "momentum beta =" << testmom.Beta() << " KTRAJ beta = " << lhel.beta() << " momentum gamma = " << testmom.Gamma() << - // " KTRAJ gamma = " << lhel.gamma() << " scalar mom " << lhel.momentum(ot) << endl; + // cout << "momentum beta =" << testmom.Beta() << " KTRAJ beta = " << ktraj.beta() << " momentum gamma = " << testmom.Gamma() << + // " KTRAJ gamma = " << ktraj.gamma() << " scalar mom " << ktraj.momentum(ot) << endl; } - VEC3 mdir = lhel.direction(ot); + VEC3 mdir = ktraj.direction(ot); // create the helix at tmin and tmax - auto tmom = lhel.momentum4(tmax); - auto tpos = lhel.position4(tmax); - KTRAJ lhelmax(tpos,tmom,icharge,bnom); - tmom = lhel.momentum4(tmin); - tpos = lhel.position4(tmin); - KTRAJ lhelmin(tpos,tmom,icharge,bnom); + auto tmom = ktraj.momentum4(tmax); + auto tpos = ktraj.position4(tmax); + KTRAJ ktrajmax(tpos,tmom,icharge,bnom); + tmom = ktraj.momentum4(tmin); + tpos = ktraj.position4(tmin); + KTRAJ ktrajmin(tpos,tmom,icharge,bnom); - // cout << "KTRAJ at tmax has parameters : " << lhelmax << endl; - // cout << "KTRAJ at tmin has parameters : " << lhelmin << endl; + // cout << "KTRAJ at tmax has parameters : " << ktrajmax << endl; + // cout << "KTRAJ at tmin has parameters : " << ktrajmin << endl; // now graph this as a polyline over the specified time range. double tstep = 0.1; // nanoseconds @@ -173,7 +181,7 @@ int test(int argc, char **argv) { TPolyLine3D* hel = new TPolyLine3D(nsteps+1); for(int istep=0;istepSetPoint(istep, hpos.X(), hpos.Y(), hpos.Z()); } @@ -185,11 +193,11 @@ int test(int argc, char **argv) { hel->Draw(); // inversion test TPolyLine3D* ihel = new TPolyLine3D(nsteps+1); - auto ilhel = lhel; - ilhel.invertCT(); + auto iktraj = ktraj; + iktraj.invertCT(); for(int istep=0;istepSetPoint(istep, hpos.X(), hpos.Y(), hpos.Z()); } @@ -199,18 +207,18 @@ int test(int argc, char **argv) { ihel->Draw(); // now draw momentum vectors at reference, start and end MomVec imstart,imend,imref; - auto imompos = ilhel.position3(ot); - mdir =ilhel.direction(ot); + auto imompos = iktraj.position3(ot); + mdir =iktraj.direction(ot); VEC3 imomvec =mom*mdir; drawMom(imompos,imomvec,kRed,imref); // - imompos = lhel.position3(tmin); - mdir = lhel.direction(tmin); + imompos = ktraj.position3(tmin); + mdir = ktraj.direction(tmin); imomvec =mom*mdir; drawMom(imompos,imomvec,kBlue,imstart); // - imompos = lhel.position3(tmax); - mdir = lhel.direction(tmax); + imompos = ktraj.position3(tmax); + mdir = ktraj.direction(tmax); imomvec =mom*mdir; drawMom(imompos,imomvec,kGreen,imend); // @@ -227,18 +235,18 @@ int test(int argc, char **argv) { // now draw momentum vectors at reference, start and end MomVec mstart,mend,mref; - VEC3 mompos = lhel.position3(ot); - mdir = lhel.direction(ot); + VEC3 mompos = ktraj.position3(ot); + mdir = ktraj.direction(ot); VEC3 momvec =mom*mdir; drawMom(mompos,momvec,kBlack,mref); // - mompos = lhel.position3(tmin); - mdir = lhel.direction(tmin); + mompos = ktraj.position3(tmin); + mdir = ktraj.direction(tmin); momvec =mom*mdir; drawMom(mompos,momvec,kBlue,mstart); // - mompos = lhel.position3(tmax); - mdir = lhel.direction(tmax); + mompos = ktraj.position3(tmax); + mdir = ktraj.direction(tmax); momvec =mom*mdir; drawMom(mompos,momvec,kGreen,mend); // @@ -257,8 +265,8 @@ int test(int argc, char **argv) { leg->Draw(); // create a Line near this helix, and draw it and the ClosestApproach vector - VEC3 pos = lhel.position3(ltime); - VEC3 dir = lhel.direction(ltime); + VEC3 pos = ktraj.position3(ltime); + VEC3 dir = ktraj.direction(ltime); // rotate the direction double lhphi = atan2(dir.Y(),dir.X()); double pphi = lhphi + M_PI/2.0; @@ -268,16 +276,16 @@ int test(int argc, char **argv) { // shift the position VEC3 perpdir(-sin(phi),cos(phi),0.0); VEC3 ppos = pos + gap*perpdir; - Line tline(ppos, ltime, pvel, wlen); + SensorLine tline(ppos, ltime, pvel, wlen); // find ClosestApproach CAHint hint(ltime,ltime); - ClosestApproach tp(lhel,tline,hint, 1e-6); + ClosestApproach tp(ktraj,tline,hint, 1e-6); // cout << "ClosestApproach status " << tp.statusName() << " doca " << tp.doca() << " dt " << tp.deltaT() << endl; if(tp.status() == ClosestApproachData::converged) { // draw the line and ClosestApproach TPolyLine3D* line = new TPolyLine3D(2); - auto plow = tline.position3(tline.range().begin()); - auto phigh = tline.position3(tline.range().end()); + auto plow = tline.start(); + auto phigh = tline.end(); line->SetPoint(0,plow.X(),plow.Y(), plow.Z()); line->SetPoint(1,phigh.X(),phigh.Y(), phigh.Z()); line->SetLineColor(kOrange); @@ -292,13 +300,59 @@ int test(int argc, char **argv) { return -1; } // test particle state back-and-forth - ParticleStateEstimate pmeas = lhel.stateEstimate(ltime); - KTRAJ newhel(pmeas,lhel.bnom()); + // set the covariance first + for(size_t ipar=0; ipar < NParams(); ipar++){ + ktraj.params().covariance()[ipar][ipar] = sigmas[ipar]*sigmas[ipar]; + } + ParticleStateEstimate pmeas = ktraj.stateEstimate(ltime); + KTRAJ newktraj(pmeas,ktraj.bnom()); for(size_t ipar=0;ipar < NParams();ipar++){ - if(fabs(lhel.paramVal(ipar)-newhel.paramVal(ipar)) > 1e-9){ + if(fabs(ktraj.paramVal(ipar)-newktraj.paramVal(ipar)) > 1e-9){ cout << "Parameter check failed par " << ipar << endl; return -2; } + if(fabs(ktraj.paramVar(ipar)-newktraj.paramVar(ipar)) > 1e-9){ + cout << "Parameter covariance check failed par " << ipar << endl; + return -2; + } + } + // test position and momentum variance + double pmvar = pmeas.momentumVariance(); + double pphivar = pmeas.positionVariance(MomBasis::phidir_); + double pperpvar = pmeas.positionVariance(MomBasis::perpdir_); + + double tmvar = ktraj.momentumVariance(ltime); + double tphivar = ktraj.positionVariance(ltime,MomBasis::phidir_); + double tperpvar = ktraj.positionVariance(ltime,MomBasis::perpdir_); + + auto momdir = ktraj.direction(ltime); + auto udir = ktraj.direction(ltime,MomBasis::perpdir_); + Plane plane(momdir,udir,ppos); + auto pcov = ktraj.planeCovariance(ltime,plane); +// cout << "Plane covariance " << pcov << endl; + + if(fabs(pcov(0,0) - pperpvar) > 1e-9 || + fabs(pcov(1,1) - pphivar) > 1e-9) { + cout << "Position covariance check failed" << endl; + return -2; + } + + // test acceleration + auto acc = ktraj.acceleration(ltime); + auto vel = ktraj.velocity(ltime); + if(acc.Dot(vel) > 1e-9 || acc.Dot(ktraj.bnom()) > 1e-9){ + cout << "Acceleration check failed " << endl; + return -2; + } + + if(fabs(pmvar-tmvar)>1e-9 ) { + cout << "Momentum covariance check failed " << endl; + return -2; + } + + if(fabs(pphivar-tphivar) > 1e-9 || fabs(pperpvar-tperpvar) > 1e-9){ + cout << "Position covariance check failed " << endl; + return -2; } std::string tfname = KTRAJ::trajName() + ".root"; diff --git a/Tests/debug.txt b/Tests/debug.txt new file mode 100644 index 00000000..529abef1 --- /dev/null +++ b/Tests/debug.txt @@ -0,0 +1,5 @@ +# Test Configuration iteration schedule for a null (no drift) fit +# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge gapdiv tol minndof bfcor ends plevel +10 1.0e6 1.0 50.0 1.0e6 100.0 1e-2 5 1 0 0 +# Then, meta-iteration specific parameters: temperature and update type (null hit) +2.0 0 diff --git a/Tests/driftextend.txt b/Tests/driftextend.txt new file mode 100644 index 00000000..cd39dbb5 --- /dev/null +++ b/Tests/driftextend.txt @@ -0,0 +1,10 @@ +# +# Test Configuration iteration schedule for a drift extension +# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge gapdiv tol minndof bfcor ends plevel +10 1.0e6 1.0 50.0 1.0e6 100.0 1e-4 5 1 1 0 +# Order: +# temperature updater mindoca maxdoca +2.0 1 1.5 5 +1.0 1 0.5 3.5 +0.5 1 0.5 2.8 +0.0 1 0.5 2.8 diff --git a/Tests/driftfit.txt b/Tests/driftfit.txt new file mode 100644 index 00000000..a6af9ff5 --- /dev/null +++ b/Tests/driftfit.txt @@ -0,0 +1,14 @@ +# +# Test Configuration iteration schedule for a drift fit +# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge divgap tol minndof bfcor ends plevel +10 1.0e7 1.0 50.0 1.0e6 100.0 1e-4 5 1 0 0 +# Order: +# temperature updater (mindoca maxdoca) +2.0 0 +1.0 0 +0.0 0 +2.0 1 1.5 5 +1.0 1 1.0 3.5 +0.5 1 0.5 2.8 +0.2 1 0.5 2.8 +0.0 1 0.5 2.8 diff --git a/Tests/fixedBfit.txt b/Tests/fixedBfit.txt new file mode 100644 index 00000000..6864cd33 --- /dev/null +++ b/Tests/fixedBfit.txt @@ -0,0 +1,15 @@ + +# +# Test Configuration iteration schedule for a drift fit +# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge divgap tol minndof bfcor ends plevel +10 1.0e7 1.0 50.0 1.0e6 100.0 1e-4 5 0 0 0 +# Order: +# temperature updater (mindoca maxdoca) +2.0 0 +1.0 0 +0.5 0 +2.0 1 1.5 5 +1.0 1 1.0 3.5 +0.5 1 0.5 2.8 +0.2 1 0.5 2.8 +0.0 1 0.5 2.8 diff --git a/Tests/seedfit.txt b/Tests/seedfit.txt new file mode 100644 index 00000000..8795db05 --- /dev/null +++ b/Tests/seedfit.txt @@ -0,0 +1,7 @@ +# Test Configuration iteration schedule for a null (no drift) fit +# first global parameters: maxniter dewight dchisquared_converge dchisquared_diverge dchisq_paramdiverge gapdiv tol minndof bfcor ends plevel +10 1.0e6 1.0 50.0 1.0e6 100.0 1e-2 5 1 0 0 +# Then, meta-iteration specific parameters: temperature and update type (null hit) +2.0 0 +1.0 0 +0.0 0 diff --git a/Trajectory/CMakeLists.txt b/Trajectory/CMakeLists.txt index 29fad367..1042195c 100644 --- a/Trajectory/CMakeLists.txt +++ b/Trajectory/CMakeLists.txt @@ -1,4 +1,3 @@ - # Explicitly list source files in this shared library # When a new source file is added, it must be added to this list. @@ -8,10 +7,12 @@ add_library(Trajectory SHARED CentralHelix.cc ClosestApproachData.cc + SensorLine.cc KinematicLine.cc - Line.cc LoopHelix.cc POCAUtil.cc + ConstantDistanceToTime.cc + ParticleTrajectory.cc ) # make the library names unique set(CMAKE_SHARED_LIBRARY_PREFIX "libKinKal_") @@ -23,10 +24,12 @@ ROOT_GENERATE_DICTIONARY(TrajectoryDict ) # set top-level directory as include root -target_include_directories(Trajectory PRIVATE ${PROJECT_SOURCE_DIR}/..) +target_include_directories(Trajectory PUBLIC + $ + $) # link this library with ROOT libraries -target_link_libraries(Trajectory General ${ROOT_LIBRARIES}) +target_link_libraries(Trajectory Geometry General ${ROOT_LIBRARIES}) # set shared library version equal to project version set_target_properties(Trajectory PROPERTIES VERSION ${PROJECT_VERSION} PREFIX ${CMAKE_SHARED_LIBRARY_PREFIX}) diff --git a/Trajectory/CentralHelix.cc b/Trajectory/CentralHelix.cc index cd3ff0e4..3bf09854 100644 --- a/Trajectory/CentralHelix.cc +++ b/Trajectory/CentralHelix.cc @@ -1,6 +1,7 @@ #include "KinKal/Trajectory/CentralHelix.hh" #include "Math/AxisAngle.h" #include +#include #include using namespace std; @@ -29,25 +30,23 @@ namespace KinKal { CentralHelix::CentralHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, double bnom, TimeRange const& range) : CentralHelix(pos0,mom0,charge,VEC3(0.0,0.0,bnom),range) {} CentralHelix::CentralHelix(VEC4 const &pos0, MOM4 const &mom0, int charge, VEC3 const &bnom, - TimeRange const &trange) : trange_(trange), mass_(mom0.M()), charge_(charge), bnom_(bnom) + TimeRange const &trange) : trange_(trange), mass_(mom0.M()), bnom_(bnom) { // Transform into the system where Z is along the Bfield. This is a pure rotation about the origin VEC4 pos(pos0); MOM4 mom(mom0); - g2l_ = Rotation3D(AxisAngle(VEC3(sin(bnom_.Phi()),-cos(bnom_.Phi()),0.0),bnom_.Theta())); - if(fabs(g2l_(bnom_).Theta()) > 1.0e-6)throw invalid_argument("Rotation Error"); + setTransforms(); pos = g2l_(pos); mom = g2l_(mom); - // create inverse rotation; this moves back into the original coordinate system - l2g_ = g2l_.Inverse(); // kinematic to geometric conversion - double radToMom = BFieldMap::cbar()*charge_*bnom_.R(); + double radToMom = BFieldMap::cbar()*charge*bnom_.R(); double momToRad = 1.0/radToMom; - mbar_ = -mass_ * momToRad; + abscharge_ = abs(charge); + double mbar = -mass_ * momToRad; // caches double pt = sqrt(mom.perp2()); double radius = fabs(pt*momToRad); - double amsign = sign(); + double amsign = copysign(1.0,mbar); param(omega_) = amsign/radius; param(tanDip_) = mom.Z()/pt; // vector pointing to the circle center from the measurement point; this is perp to the transverse momentum @@ -75,49 +74,28 @@ namespace KinKal { param(z0_) = z0 - nwind*deltaz; // t0, also correcting for winding param(t0_) = pos.T() -(dphi + 2*M_PI*nwind)/Omega(); - // test - auto testpos = position3(pos0.T()); - auto testmom = momentum3(pos0.T()); - auto dp = testpos - pos0.Vect(); - auto dm = testmom - mom0.Vect(); - if(dp.R() > 1.0e-5 || dm.R() > 1.0e-5)throw invalid_argument("Rotation Error"); - // check - auto lmom = localMomentum(pos0.T()); - auto tcent = center(); - if(fabs(lcent.phi()-tcent.phi())>1e-5 || fabs(lcent.perp2()-tcent.perp2()) > 1e-5){ - cout << "center " << lcent << " test center " << tcent << endl; - } - if(fabs(tan(phi0()) +1.0/tan(lcent.phi())) > 1e-5){ - cout << "phi0 " << phi0() << " test phi0 " << -1.0/tan(lcent.phi()) << endl; - } - double d0t = sign()*sqrt(lcent.perp2())-sqrt(lmom.perp2())/Q(); - if(fabs(d0t - d0()) > 1e-5){ - cout << " d0 " << d0() << " d0 test " << d0t << endl; - } } void CentralHelix::setBNom(double time, VEC3 const& bnom) { - // adjust the parameters for the change in bnom - mbar_ *= bnom_.R()/bnom.R(); - pars_.parameters() += dPardB(time,bnom); + // adjust the parameters for the change in bnom holding the state constant + VEC3 db = bnom-bnom_; + pars_.parameters() += dPardB(time,db); + resetBNom(bnom); + // rotate covariance TODO + } + + void CentralHelix::resetBNom(VEC3 const& bnom) { bnom_ = bnom; - // adjust rotations to global space - g2l_ = Rotation3D(AxisAngle(VEC3(sin(bnom_.Phi()),-cos(bnom_.Phi()),0.0),bnom_.Theta())); - l2g_ = g2l_.Inverse(); + setTransforms(); } CentralHelix::CentralHelix(CentralHelix const& other, VEC3 const& bnom, double trot) : CentralHelix(other) { - mbar_ *= bnom_.R()/bnom.R(); - bnom_ = bnom; - pars_.parameters() += other.dPardB(trot,bnom); - g2l_ = Rotation3D(AxisAngle(VEC3(sin(bnom_.Phi()),-cos(bnom_.Phi()),0.0),bnom_.Theta())); - l2g_ = g2l_.Inverse(); + setBNom(trot,bnom); } - CentralHelix::CentralHelix(Parameters const &pdata, double mass, int charge, double bnom, TimeRange const& range) : trange_(range), pars_(pdata), mass_(mass), charge_(charge), bnom_(VEC3(0.0,0.0,bnom)){ - // compute kinematic cache - double momToRad = 1.0/(BFieldMap::cbar()*charge_*bnom); - mbar_ = -mass_ * momToRad; + CentralHelix::CentralHelix(Parameters const &pdata, double mass, int charge, double bnom, TimeRange const& range) : trange_(range), pars_(pdata), mass_(mass), bnom_(VEC3(0.0,0.0,bnom)){ + //FIXME for now just ignore sign + abscharge_ = abs(charge); } CentralHelix::CentralHelix(Parameters const &pdata, CentralHelix const& other) : CentralHelix(other) { @@ -135,14 +113,49 @@ namespace KinKal { pars_.covariance() = ROOT::Math::Similarity(dpds,pstate.stateCovariance()); } + void CentralHelix::syncPhi0(CentralHelix const& other) { + // adjust the phi0 of this traj to agree with the reference, keeping its value (mod 2pi) the same. + static double twopi = 2*M_PI; + int nloop = static_cast(std::round( (other.phi0() - phi0())/twopi)); + if(nloop != 0) pars_.parameters()[phi0_] += nloop*twopi; + } + + void CentralHelix::setTransforms() { + // adjust rotations to global space + g2l_ = Rotation3D(AxisAngle(VEC3(sin(bnom_.Phi()),-cos(bnom_.Phi()),0.0),bnom_.Theta())); + l2g_ = g2l_.Inverse(); + } + double CentralHelix::momentumVariance(double time) const { DVEC dMomdP(0.0, 0.0, -1.0/omega() , 0.0 , sinDip()*cosDip() , 0.0); dMomdP *= momentum(time); return ROOT::Math::Similarity(dMomdP,params().covariance()); } - VEC4 CentralHelix::position4(double time) const - { + double CentralHelix::positionVariance(double time, MomBasis::Direction mdir) const { + auto dxdpvec = dXdPar(time); + auto momdir = direction(time); + auto posdir = MomBasis::direction(mdir, momdir); + SVEC3 sdir(posdir.X(),posdir.Y(),posdir.Z()); + DVEC dxdp = sdir*dxdpvec; + return ROOT::Math::Similarity(dxdp,params().covariance()); + } + + PMAT CentralHelix::planeCovariance(double time,Plane const& plane) const { + // project covariance onto the U, V direction of the given plane + // particle direction cannot be orthogonal to the plane normal + auto momdir = direction(time); + if(fabs(plane.normal().Dot(momdir)) < 1.0e-10)throw invalid_argument("Momentum direction lies in the plane"); + auto dxdpvec = dXdPar(time); + SVEC3 uvec(plane.uDirection().X(),plane.uDirection().Y(),plane.uDirection().Z()); + SVEC3 vvec(plane.vDirection().X(),plane.vDirection().Y(),plane.vDirection().Z()); + PPMAT dPlanedPar; + dPlanedPar.Place_in_row(uvec*dxdpvec,0,0); + dPlanedPar.Place_in_row(vvec*dxdpvec,1,0); + return ROOT::Math::Similarity(dPlanedPar,params().covariance()); + } + + VEC4 CentralHelix::position4(double time) const { VEC3 pos3 = position3(time); return VEC4( pos3.X(), pos3.Y(), pos3.Z(), time); } @@ -188,6 +201,12 @@ namespace KinKal { return l2g_(localDirection(time,mdir)); } + VEC3 CentralHelix::acceleration(double time) const { + auto phival = phi(time); + auto locacc = acceleration()*VEC3(-sin(phival),cos(phival),0.0); + return l2g_(locacc); + } + VEC3 CentralHelix::localDirection(double time,MomBasis::Direction mdir) const { double cosdip = cosDip(); @@ -404,14 +423,21 @@ namespace KinKal { retval[phi0_] = -sign()*rad*cent.Dot(mperp)/(rcval*rcval); retval[z0_] = lpos.Z()-z0() + tanDip()*retval[phi0_]/omega(); retval[t0_] = time-t0() + retval[phi0_]/Omega(); - return (1.0/bnom_.R())*retval; + return retval; } - DVEC CentralHelix::dPardB(double time, VEC3 const& BPrime) const { + PSMAT CentralHelix::dPardPardB(double time,VEC3 const& db) const { + PSMAT dpdpdb = ROOT::Math::SMatrixIdentity(); + return dpdpdb; + } + + + DVEC CentralHelix::dPardB(double time, VEC3 const& dB) const { // rotate Bfield difference into local coordinate system - VEC3 dB = g2l_(BPrime-bnom_); + VEC3 dBloc = g2l_(dB); // find the parameter change due to BField magnitude change using component parallel to the local nominal Bfield (always along z) - DVEC retval = dPardB(time)*dB.Z(); + double bfrac = dBloc.Z()/bnom_.R(); + DVEC retval = dPardB(time)*bfrac/(1.0 + bfrac); // find the change in (local) position and momentum due to the rotation implied by the B direction change // work in local coordinate system to avoid additional matrix mulitplications auto xvec = localPosition(time); @@ -426,6 +452,29 @@ namespace KinKal { return retval; } + VEC3 CentralHelix::center(double time) const { + // local transverse position is at the center. Use the time to define the Z position + VEC3 cpos = center(); + cpos.SetZ(z0()+tanDip()*dphi(time)/omega()); + // transform to global coordinates + auto gcpos = l2g_(cpos); + return gcpos; + } + + Ray CentralHelix::tangent(double time) const { + return Ray(direction(time),position3(time)); + } + + double CentralHelix::sagitta(double trange) const { + double tlen = trange*transverseSpeed(); + double brad = bendRadius(); + if(tlen < M_PI*brad){ + double drunit = (1.0-cos(0.5*tlen/brad)); // unit circle + return 0.125*brad*drunit*drunit; + } + return brad; + } + void CentralHelix::print(std::ostream& ost, int detail) const { ost << " CentralHelix parameters: "; for(size_t ipar=0;ipar < CentralHelix::npars_;ipar++){ diff --git a/Trajectory/CentralHelix.hh b/Trajectory/CentralHelix.hh index 2669e186..a63e6cf3 100644 --- a/Trajectory/CentralHelix.hh +++ b/Trajectory/CentralHelix.hh @@ -14,6 +14,8 @@ #include "KinKal/General/MomBasis.hh" #include "KinKal/General/BFieldMap.hh" #include "KinKal/General/PhysicalConstants.h" +#include "KinKal/Geometry/Ray.hh" +#include "KinKal/Geometry/Plane.hh" #include "Math/Rotation3D.h" #include #include @@ -27,6 +29,7 @@ namespace KinKal { // classes implementing the Kalman fit // define the indices and names of the parameters enum ParamIndex {d0_=0,phi0_=1,omega_=2,z0_=3,tanDip_=4,t0_=5,npars_=6}; + constexpr static ParamIndex phi0Index() { return phi0_; } constexpr static ParamIndex t0Index() { return t0_; } static std::vector const ¶mNames(); @@ -43,7 +46,7 @@ namespace KinKal { CentralHelix(VEC4 const& pos, MOM4 const& mom, int charge, VEC3 const& bnom, TimeRange const& range=TimeRange()); CentralHelix(VEC4 const& pos, MOM4 const& mom, int charge, double bnom, TimeRange const& range=TimeRange()); // construct from explicit parametric and kinematic info - CentralHelix(Parameters const &pdata, double mass, int charge, double bnom, TimeRange const& range); + CentralHelix(Parameters const &pdata, double mass, int abscharge, double bnom, TimeRange const& range); // copy payload and adjust for a different BFieldMap and range CentralHelix(CentralHelix const& other, VEC3 const& bnom, double trot); // copy and override parameters @@ -52,34 +55,41 @@ namespace KinKal { explicit CentralHelix(ParticleState const& pstate, VEC3 const& bnom, TimeRange const& range=TimeRange()); // same, including covariance information explicit CentralHelix(ParticleStateEstimate const& pstate, VEC3 const& bnom, TimeRange const& range=TimeRange()); + void syncPhi0(CentralHelix const& other); // particle position and momentum as a function of time VEC4 position4(double time) const; VEC3 position3(double time) const; MOM4 momentum4(double time) const; VEC3 momentum3(double time) const; VEC3 velocity(double time) const; + double speed(double time=0) const { return CLHEP::c_light * beta(); } + double transverseSpeed() const { return CLHEP::c_light * beta()*cosDip(); } + double acceleration() const { return CLHEP::c_light*CLHEP::c_light/(omega()*ebar2()); } + VEC3 acceleration(double time) const; VEC3 direction(double time, MomBasis::Direction mdir= MomBasis::momdir_) const; // scalar momentum and energy in MeV/c units - double momentum(double time=0) const { return fabs(mass_ * pbar() / mbar_); } + double momentum(double time=0) const { return fabs(mass_ * pbar() / mbar()); } double momentumVariance(double time=0) const; - double energy(double time=0) const { return fabs(mass_ * ebar() / mbar_); } - // speed in mm/ns - double speed(double time=0) const { return CLHEP::c_light * beta(); } + double positionVariance(double time,MomBasis::Direction dir) const; + PMAT planeCovariance(double time,Plane const& plane) const; + double energy(double time=0) const { return fabs(mass_ * ebar() / mbar()); } // local momentum direction basis void print(std::ostream& ost, int detail) const; TimeRange const& range() const { return trange_; } TimeRange& range() { return trange_; } void setRange(TimeRange const& trange) { trange_ = trange; } void setBNom(double time, VEC3 const& bnom); + // change the BField. This also resets the transforms + void resetBNom(VEC3 const& bnom); bool inRange(double time) const { return trange_.inRange(time); } // momentum change derivatives; this is required to instantiate a KalTrk using this KTraj DVEC momDeriv(double time, MomBasis::Direction mdir) const; double mass() const { return mass_;} // mass - int charge() const { return charge_;} // charge in proton charge units - + int charge() const { return (-1*omega() < 0) ? -1*abscharge_ : abscharge_;} // named parameter accessors double paramVal(size_t index) const { return pars_.parameters()[index]; } + double paramVar(size_t index) const { return pars_.covariance()(index,index); } Parameters const ¶ms() const { return pars_; } Parameters ¶ms() { return pars_; } double d0() const { return paramVal(d0_); } @@ -93,23 +103,25 @@ namespace KinKal { ParticleStateEstimate stateEstimate(double time) const; // simple functions - double sign() const { return copysign(1.0,mbar_); } // combined bending sign including Bz and charge + double sign() const { return copysign(1.0,omega()); } // combined bending sign including Bz and charge + double parameterSign() const { return copysign(1.0,omega()); } + // helicity is defined as the sign of the projection of the angular momentum vector onto the linear momentum vector + double helicity() const { return copysign(1.0,tanDip()); } // needs to be checked TODO double pbar() const { return 1./ (omega() * cosDip() ); } // momentum in mm - double ebar() const { return sqrt(pbar()*pbar() + mbar_ * mbar_); } // energy in mm + double ebar2() const { return pbar()*pbar() + mbar() * mbar(); } + double ebar() const { return sqrt(ebar2()); } // energy in mm + double mbar() const { return fabs(mass_/Q()); } // mass in mm + double Q() const { return -BFieldMap::cbar()*charge()*bnom_.R(); } // reduced charge double cosDip() const { return 1./sqrt(1.+ tanDip() * tanDip() ); } double sinDip() const { return tanDip()*cosDip(); } - double mbar() const { return mbar_; } // mass in mm; includes charge information! - double Q() const { return mass_/mbar_; } // reduced charge + double omegaZ() const { return omega()/(CLHEP::c_light*beta()*tanDip()); } // dPhi/dz double beta() const { return fabs(pbar()/ebar()); } // relativistic beta - double gamma() const { return fabs(ebar()/mbar_); } // relativistic gamma - double betaGamma() const { return fabs(pbar()/mbar_); } // relativistic betagamma + double gamma() const { return fabs(ebar()/mbar()); } // relativistic gamma + double betaGamma() const { return fabs(pbar()/mbar()); } // relativistic betagamma double Omega() const { return Q()*CLHEP::c_light/energy(); } // true angular velocity double dphi(double t) const { return Omega()*(t - t0()); } // rotation WRT 0 at a given time double phi(double t) const { return dphi(t) + phi0(); } // absolute azimuth at a given time - double rc() const { return -1.0/omega() - d0(); } - double bendRadius() const { return fabs(1.0/omega()); } - VEC3 center() const { return VEC3(rc()*sin(phi0()), -rc()*cos(phi0()), 0.0); } // circle center (2d) - VEC3 const &bnom(double time=0.0) const { return bnom_; } + VEC3 const& bnom(double time=0.0) const { return bnom_; } double bnomR() const { return bnom_.R(); } DPDV dPardX(double time) const; DPDV dPardM(double time) const; @@ -121,18 +133,23 @@ namespace KinKal { // Parameter derivatives given a change in BFieldMap DVEC dPardB(double time) const; DVEC dPardB(double time, VEC3 const& BPrime) const; + PSMAT dPardPardB(double time,VEC3 const& db) const; // Parameter covariance rotation for a change in BField // flip the helix in time and charge; it remains unchanged geometrically void invertCT() { - mbar_ *= -1.0; - charge_ *= -1; pars_.parameters()[omega_] *= -1.0; pars_.parameters()[tanDip_] *= -1.0; pars_.parameters()[d0_] *= -1.0; pars_.parameters()[phi0_] += M_PI; pars_.parameters()[t0_] *= -1.0; } - // + // helix interface + VEC3 center(double time) const; // helix center in global coordinates + Ray tangent(double time) const; // tangent to the helix at a given time + // linear approximation + Ray linearize(double time) const { return tangent(time); } + double bendRadius() const { return fabs(1.0/omega()); } + double sagitta(double range) const; // compute maximum sagitta over a time range private : VEC3 localDirection(double time, MomBasis::Direction mdir= MomBasis::momdir_) const; VEC3 localMomentum(double time) const; @@ -140,11 +157,13 @@ namespace KinKal { DPDV dPardMLoc(double time) const; // return the derivative of the parameters WRT the local (unrotated) momentum vector DPDV dPardXLoc(double time) const; PSMAT dPardStateLoc(double time) const; // derivative of parameters WRT local state + double rc() const { return -1.0/omega() - d0(); } + VEC3 center() const { return VEC3(rc()*sin(phi0()), -rc()*cos(phi0()), 0.0); } // local circle center + void setTransforms(); // define global to local and local to global given BNom TimeRange trange_; Parameters pars_; // parameters double mass_; // in units of MeV/c^2 - int charge_; // charge in units of proton charge - double mbar_; // reduced mass in units of mm, computed from the mass and nominal field + int abscharge_; // absolute value of charge in units of proton charge; we need to take the sign of the charge from omega VEC3 bnom_; // nominal BField vector, from the map ROOT::Math::Rotation3D l2g_, g2l_; // rotations between local and global coordinates const static std::vector paramTitles_; diff --git a/Trajectory/ClosestApproach.hh b/Trajectory/ClosestApproach.hh index 9d24ce0c..0c95ef22 100644 --- a/Trajectory/ClosestApproach.hh +++ b/Trajectory/ClosestApproach.hh @@ -7,8 +7,10 @@ // Used as part of the kinematic Kalman fit // #include "KinKal/Trajectory/ClosestApproachData.hh" +#include #include #include +#include namespace KinKal { // Hint class for TCA calculation. TCA search will start at these TOCA values. This allows to @@ -22,14 +24,24 @@ namespace KinKal { // Templated on the types of trajectories. The actual implementations must be specializations for particular trajectory classes. template class ClosestApproach { public: + using KTRAJPTR = std::shared_ptr; // construct from the particle and sensor trajectories; TCA is computed on construction, given a hint as to where // to start looking, which disambiguates functions with multiple solutions ClosestApproach(KTRAJ const& ktraj, STRAJ const& straj, CAHint const& hint, double precision); + // same, using Ptrs + ClosestApproach(KTRAJPTR const& ktrajptr, STRAJ const& straj, CAHint const& hint, double precision); // construct without a hint: TCA isn't calculated, state is invalid - ClosestApproach(KTRAJ const& ptraj, STRAJ const& straj, double precision); + ClosestApproach(KTRAJ const& ktraj, STRAJ const& straj, double precision); + ClosestApproach(KTRAJPTR const& ktrajptr, STRAJ const& straj, double precision); + // explicitly construct from all content (no calculation) + ClosestApproach(KTRAJPTR const& ktrajptr, STRAJ const& straj, double precision, + ClosestApproachData const& tpdata, DVEC const& dDdP, DVEC const& dTdP); + // copy constructor + ClosestApproach(ClosestApproach const&) = default; // accessors ClosestApproachData const& tpData() const { return tpdata_; } - KTRAJ const& particleTraj() const { return ktraj_; } + KTRAJ const& particleTraj() const { return *ktrajptr_; } + KTRAJPTR const& particleTrajPtr() const { return ktrajptr_; } STRAJ const& sensorTraj() const { return straj_; } // derviatives of TOCA and DOCA WRT particle trajectory parameters DVEC const& dDdP() const { return dDdP_; } @@ -54,94 +66,138 @@ namespace KinKal { VEC4 delta() const { return tpdata_.delta(); } VEC3 const& particleDirection() const { return tpdata_.particleDirection(); } VEC3 const& sensorDirection() const { return tpdata_.sensorDirection(); } - // calculate CA given the hint, and fill the state - void findTCA(CAHint const& hint); + // return the hint from the current state + CAHint hint() const { return CAHint(particleToca(),sensorToca()); } + // equivalence + ClosestApproach& operator = (ClosestApproach const& other); + // other accessors + void setTrajectory(KTRAJPTR ktrajptr){ ktrajptr_ = ktrajptr; } private: double precision_; // precision used to define convergence + KTRAJPTR ktrajptr_; // kinematic particle trajectory + STRAJ const& straj_; // sensor trajectory This should be a shared ptr TODO + protected: + // calculate CA given the hint, and fill the state + void findTCA(CAHint const& hint); ClosestApproachData tpdata_; // data payload of CA calculation - KTRAJ const& ktraj_; // kinematic particle trajectory - STRAJ const& straj_; // sensor trajectory - // consider moving the followinginto ClosestApproachData TODO DVEC dDdP_; // derivative of DOCA WRT Parameters DVEC dTdP_; // derivative of TOCA WRT Parameters + void setParticleTOCA( double ptoca); + void setSensorTOCA( double stoca); }; template ClosestApproach::ClosestApproach(KTRAJ const& ktraj, STRAJ const& straj, double prec) : - precision_(prec),ktraj_(ktraj), straj_(straj) {} + precision_(prec),ktrajptr_(new KTRAJ(ktraj)), straj_(straj) {} + + template ClosestApproach::ClosestApproach(KTRAJPTR const& ktrajptr, STRAJ const& straj, double prec) : + precision_(prec),ktrajptr_(ktrajptr), straj_(straj) {} + + template ClosestApproach::ClosestApproach(KTRAJPTR const& ktrajptr, STRAJ const& straj, double prec, + ClosestApproachData const& tpdata, DVEC const& dDdP, DVEC const& dTdP) : + precision_(prec),ktrajptr_(ktrajptr), straj_(straj), tpdata_(tpdata),dDdP_(dDdP), dTdP_(dTdP) {} template ClosestApproach::ClosestApproach(KTRAJ const& ktraj, STRAJ const& straj, CAHint const& hint, double prec) : ClosestApproach(ktraj,straj,prec) { findTCA(hint); } + template ClosestApproach::ClosestApproach(KTRAJPTR const& ktrajptr, STRAJ const& straj, CAHint const& hint, + double prec) : ClosestApproach(ktrajptr,straj,prec) { + findTCA(hint); + } + + template ClosestApproach& ClosestApproach::operator = (ClosestApproach const& other) + { + tpdata_ = other. tpData(); + dDdP_ = other.dDdP(); + dTdP_ = other.dTdP(); + ktrajptr_ = other.ktrajptr_; + // make sure the sensor traj is the same + if(&straj_ != &other.sensorTraj()) throw std::invalid_argument("Inconsistent ClosestApproach SensorTraj"); + return *this; + } + template void ClosestApproach::findTCA(CAHint const& hint) { // reset status tpdata_.reset(); - // initialize TOCA using hints - tpdata_.partCA_.SetE(hint.particleToca_); - tpdata_.sensCA_.SetE(hint.sensorToca_); - static const unsigned maxiter=100; // don't allow infinite iteration. This should be a parameter FIXME! + // initialize using hints + setParticleTOCA(hint.particleToca_); + setSensorTOCA(hint.sensorToca_); + static const unsigned maxiter=100; // don't allow infinite iteration. This should be a parameter TODO unsigned niter(0); // speed doesn't change - double pspeed = ktraj_.speed(particleToca()); + double pspeed = ktrajptr_->speed(particleToca()); double sspeed = straj_.speed(sensorToca()); // iterate until change in TOCA is less than precision double dptoca(std::numeric_limits::max()), dstoca(std::numeric_limits::max()); while(tpdata_.usable() && (fabs(dptoca) > precision() || fabs(dstoca) > precision()) && niter++ < maxiter) { // find positions and directions at the current TOCA estimate - tpdata_.partCA_ = ktraj_.position4(tpdata_.particleToca()); - tpdata_.sensCA_ = straj_.position4(tpdata_.sensorToca()); - tpdata_.pdir_ = ktraj_.direction(particleToca()); - tpdata_.sdir_ = straj_.direction(sensorToca()); - VEC3 dpos = sensorPoca().Vect()-particlePoca().Vect(); - // dot products + VEC3 delta = sensorPoca().Vect()-particlePoca().Vect(); double ddot = sensorDirection().Dot(particleDirection()); double denom = 1.0 - ddot*ddot; - // check for parallel) + // check for error conditions if(denom<1.0e-5){ - tpdata_.status_ = ClosestApproachData::pocafailed; + tpdata_.status_ = ClosestApproachData::parallel; break; } - double hdd = dpos.Dot(particleDirection()); - double ldd = dpos.Dot(sensorDirection()); - // compute the change in times - dptoca = (hdd - ldd*ddot)/(denom*pspeed); - dstoca = (hdd*ddot - ldd)/(denom*sspeed); + if(std::isnan(denom)){ + tpdata_.status_ = ClosestApproachData::failed; + break; + } + double pdd = delta.Dot(particleDirection()); + double sdd = delta.Dot(sensorDirection()); + // compute the change in TOCA + dptoca = (pdd - sdd*ddot)/(denom*pspeed); + dstoca = (pdd*ddot - sdd)/(denom*sspeed); // update the TOCA estimates - tpdata_.partCA_.SetE(particleToca()+dptoca); - tpdata_.sensCA_.SetE(sensorToca()+dstoca); - } - if(tpdata_.status_ != ClosestApproachData::pocafailed){ - if(niter < maxiter) - tpdata_.status_ = ClosestApproachData::converged; - else - tpdata_.status_ = ClosestApproachData::unconverged; - // need to add divergence and oscillation tests FIXME! + setParticleTOCA(particleToca()+dptoca); + setSensorTOCA(sensorToca()+dstoca); + // need to add divergence and oscillation tests TODO } - // final update - tpdata_.partCA_ = ktraj_.position4(tpdata_.particleToca()); - tpdata_.sensCA_ = straj_.position4(tpdata_.sensorToca()); - tpdata_.pdir_ = ktraj_.direction(particleToca()); - tpdata_.sdir_ = straj_.direction(sensorToca()); // fill the rest of the state if(usable()){ - // sign doca by angular momentum projected onto difference vector + if(fabs(dptoca) < precision() && fabs(dstoca) < precision() )tpdata_.status_ = ClosestApproachData::converged; + // sign doca by angular momentum projected onto difference vector. This is just a convention VEC3 dvec = delta().Vect(); - tpdata_.lsign_ = copysign(1.0,sensorDirection().Cross(particleDirection()).Dot(dvec)); + VEC3 pdir = particleDirection(); + tpdata_.lsign_ = copysign(1.0,sensorDirection().Cross(pdir).Dot(dvec)); tpdata_.doca_ = dvec.R()*tpdata_.lsign_; - VEC3 dvechat = dvec.Unit(); // now variances due to the particle trajectory parameter covariance // for DOCA, project the spatial position derivative along the delta-CA direction - DVDP dxdp = ktraj_.dXdPar(particleToca()); - SVEC3 dv(dvechat.X(),dvechat.Y(),dvechat.Z()); - dDdP_ = -dv*dxdp; - dTdP_[KTRAJ::t0Index()] = -1.0; // TOCA is 100% anti-correlated with the (mandatory) t0 component. - // project the parameter covariance onto DOCA and TOCA - tpdata_.docavar_ = ROOT::Math::Similarity(dDdP(),ktraj_.params().covariance()); - tpdata_.tocavar_ = ROOT::Math::Similarity(dTdP(),ktraj_.params().covariance()); + DVDP dxdp = ktrajptr_->dXdPar(particleToca()); // position change WRT parameters + // change in DOCA WRT parameters; this is straight forwards + VEC3 dvechat = dvec.Unit(); + SVEC3 dvh(dvechat.X(),dvechat.Y(),dvechat.Z()); + dDdP_ = tpdata_.lsign_*dvh*dxdp; + // change in particle DeltaT WRT parameters; both PTOCA and STOCA terms are important. + // The dependendence on the momentum direction change is an order of magnitude smaller but + // might be included someday TODO + + double dd = sensorDirection().Dot(particleDirection()); + double denom = (1.0-dd*dd); + double pfactor = 1.0/(pspeed*denom); + VEC3 beta = pfactor*(sensorDirection()*dd - particleDirection()); + SVEC3 sbeta(beta.X(),beta.Y(),beta.Z()); + double sfactor = 1.0/(sspeed*denom); + VEC3 gamma = sfactor*(sensorDirection() - dd*particleDirection()); + SVEC3 sgamma(gamma.X(),gamma.Y(),gamma.Z()); + dTdP_ = (sbeta - sgamma)*dxdp; + tpdata_.docavar_ = ROOT::Math::Similarity(dDdP(),ktrajptr_->params().covariance()); + tpdata_.tocavar_ = ROOT::Math::Similarity(dTdP(),ktrajptr_->params().covariance()); } } + template void ClosestApproach::setParticleTOCA(double ptoca) { + tpdata_.partCA_ = ktrajptr_->position4(ptoca); + tpdata_.pdir_ = ktrajptr_->direction(ptoca); + } + + template void ClosestApproach::setSensorTOCA(double stoca) { + tpdata_.sensCA_ = straj_.position4(stoca); + tpdata_.sdir_ = straj_.direction(stoca); + } + + template void ClosestApproach::print(std::ostream& ost,int detail) const { ost << "ClosestApproach status " << statusName() << " Doca " << doca() << " +- " << sqrt(docaVar()) << " dToca " << deltaT() << " +- " << sqrt(tocaVar()) << " cos(theta) " << dirDot() << " Precision " << precision() << std::endl; diff --git a/Trajectory/ClosestApproachData.cc b/Trajectory/ClosestApproachData.cc index 0a8a8f72..f4db21ed 100644 --- a/Trajectory/ClosestApproachData.cc +++ b/Trajectory/ClosestApproachData.cc @@ -1,6 +1,6 @@ #include "KinKal/Trajectory/ClosestApproachData.hh" namespace KinKal { - const std::vector ClosestApproachData::statusNames_ = { "converged", "unconverged", "oscillating", "diverged","pocafailed", "invalid"}; + const std::vector ClosestApproachData::statusNames_ = { "converged", "unconverged", "oscillating", "diverged","parallel","failed", "invalid"}; std::string const& ClosestApproachData::statusName(TPStat status) { return statusNames_[static_cast(status)];} std::ostream& operator << (std::ostream& ost, ClosestApproachData const& cadata) { diff --git a/Trajectory/ClosestApproachData.hh b/Trajectory/ClosestApproachData.hh index 03f2f78a..d271541e 100644 --- a/Trajectory/ClosestApproachData.hh +++ b/Trajectory/ClosestApproachData.hh @@ -10,7 +10,7 @@ namespace KinKal { struct ClosestApproachData { - enum TPStat{converged=0,unconverged,oscillating,diverged,pocafailed,invalid}; + enum TPStat{converged=0,unconverged,oscillating,diverged,parallel,failed,invalid}; static std::string const& statusName(TPStat status); //accessors VEC4 const& particlePoca() const { return partCA_; } @@ -30,7 +30,7 @@ namespace KinKal { VEC4 delta() const { return sensCA_-partCA_; } // measurement - prediction convention double deltaT() const { return sensCA_.T() - partCA_.T(); } bool usable() const { return status_ < diverged; } - ClosestApproachData() : status_(invalid), doca_(-1.0), docavar_(-1.0), tocavar_(-1.0) {} + ClosestApproachData() : status_(invalid), doca_(-1.0), docavar_(-1.0), tocavar_(-1.0), lsign_(0.0) {} TPStat status_; // status of computation double doca_, docavar_, tocavar_, lsign_; VEC3 pdir_, sdir_; // particle and sensor directions at CA, signed by time propagation diff --git a/Trajectory/ConstantDistanceToTime.cc b/Trajectory/ConstantDistanceToTime.cc new file mode 100644 index 00000000..64d0a694 --- /dev/null +++ b/Trajectory/ConstantDistanceToTime.cc @@ -0,0 +1,25 @@ +#include "KinKal/Trajectory/ConstantDistanceToTime.hh" +#include + +ConstantDistanceToTime::ConstantDistanceToTime(double constantSpeed) : + constantSpeed_(constantSpeed) {} + + double ConstantDistanceToTime::distance(double deltaT) { + return deltaT * constantSpeed_; + } + +double ConstantDistanceToTime::time(double distance) { + return distance * inverseSpeed(distance); +} + +double ConstantDistanceToTime::speed(double distance) { + return constantSpeed_; +} + +double ConstantDistanceToTime::inverseSpeed(double distance) { + double static const speedOfLight = 299792458.0; + if (abs(constantSpeed_) < 1/speedOfLight) { + return speedOfLight; + } + return 1/constantSpeed_; +} diff --git a/Trajectory/ConstantDistanceToTime.hh b/Trajectory/ConstantDistanceToTime.hh new file mode 100644 index 00000000..45f222cb --- /dev/null +++ b/Trajectory/ConstantDistanceToTime.hh @@ -0,0 +1,19 @@ +// +// Trivial implementation of Distance-to-time using a constant speed +// +#include "KinKal/Trajectory/DistanceToTime.hh" +#ifndef KinKal_ConstantDistanceToTime_hh +#define KinKal_ConstantDistanceToTime_hh + +class ConstantDistanceToTime : public DistanceToTime { + public: + ConstantDistanceToTime(double constantSpeed); + virtual ~ConstantDistanceToTime(){} + double distance(double deltaT) override; + double time(double distance) override; + double speed(double distance) override; + double inverseSpeed(double distance) override; + private: + double constantSpeed_; +}; +#endif diff --git a/Trajectory/DistanceToTime.hh b/Trajectory/DistanceToTime.hh new file mode 100644 index 00000000..61c3055c --- /dev/null +++ b/Trajectory/DistanceToTime.hh @@ -0,0 +1,16 @@ +// +// Define generic distance-to-time interface used to describe signal propagatation along a sensor trajectory +// Original author: Ryan Cheng +// +#ifndef KinKal_DistanceToTime_hh +#define KinKal_DistanceToTime_hh + +class DistanceToTime { + public: + virtual ~DistanceToTime(){} + virtual double distance(double deltaT) = 0; + virtual double time(double distance) = 0; + virtual double speed(double distance) = 0; + virtual double inverseSpeed(double distance) = 0; +}; +#endif diff --git a/Trajectory/KinematicLine.cc b/Trajectory/KinematicLine.cc index 0da8729e..57c5b51d 100644 --- a/Trajectory/KinematicLine.cc +++ b/Trajectory/KinematicLine.cc @@ -8,6 +8,7 @@ #include "Math/AxisAngle.h" #include "Math/VectorUtil.h" #include +#include #include using namespace std; @@ -17,12 +18,12 @@ namespace KinKal { typedef ROOT::Math::SVector SVEC3; const vector KinematicLine::paramTitles_ = { "Transverse DOCA to Z Axis (d_{0})", "Azimuth of POCA (#phi_{0})", - "Z at POCA (z_{0})", "Cos #theta", "Time at POCA (t_{0})", "Momentum"}; + "Z at POCA (z_{0})", "#theta", "Time at POCA (t_{0})", "Momentum"}; const vector KinematicLine::paramNames_ = {"d_{0}", "#phi_{0}", "z_{0}", - "cos(#theta)", "t_{0}", "mom"}; + "#theta", "t_{0}", "mom"}; - const vector KinematicLine::paramUnits_ = {"mm", "radians", "mm", "", "ns","MeV/c"}; + const vector KinematicLine::paramUnits_ = {"mm", "radians", "mm", "radians", "ns","MeV/c"}; std::vector const &KinematicLine::paramUnits() { return paramUnits_; } std::vector const &KinematicLine::paramNames() { return paramNames_; } @@ -62,7 +63,7 @@ namespace KinKal { param(d0_) = amsign*poca.dca(); // dca2d and dca are the same for POCA to the Z axis param(phi0_) = dir.Phi(); // same as at POCA param(z0_) = poca.point1().Z(); - param(cost_) = dir.Z(); + param(theta_) = acos(dir.Z()); param(t0_) = pos0.T() - flen/speed; param(mom_) = mommag; } @@ -96,6 +97,13 @@ namespace KinKal { KinematicLine::KinematicLine(KinematicLine const& other, VEC3 const& bnom, double trot) : KinematicLine(other) { } + void KinematicLine::syncPhi0(KinematicLine const& other) { +// adjust the phi0 of this traj to agree with the reference, keeping its value (mod 2pi) the same. + static double twopi = 2*M_PI; + int nloop = static_cast(std::round( (other.phi0() - phi0())/twopi)); + if(nloop != 0) pars_.parameters()[phi0_] += nloop*twopi; + } + VEC3 KinematicLine::position3(double time) const { return (pos0() + flightLength(time) * direction()); } @@ -114,6 +122,29 @@ namespace KinKal { return MOM4(mom3.X(),mom3.Y(),mom3.Z(),mass_); } + double KinematicLine::positionVariance(double time, MomBasis::Direction mdir) const { + auto dxdpvec = dXdPar(time); + auto momdir = direction(time); + auto posdir = MomBasis::direction(mdir, momdir); + SVEC3 sdir(posdir.X(),posdir.Y(),posdir.Z()); + DVEC dxdp = sdir*dxdpvec; + return ROOT::Math::Similarity(dxdp,params().covariance()); + } + + PMAT KinematicLine::planeCovariance(double time,Plane const& plane) const { + // project covariance onto the U, V direction of the given plane + // particle direction cannot be orthogonal to the plane normal + auto momdir = direction(time); + if(fabs(plane.normal().Dot(momdir)) < 1.0e-10)throw invalid_argument("Momentum direction lies in the plane"); + auto dxdpvec = dXdPar(time); + SVEC3 uvec(plane.uDirection().X(),plane.uDirection().Y(),plane.uDirection().Z()); + SVEC3 vvec(plane.vDirection().X(),plane.vDirection().Y(),plane.vDirection().Z()); + PPMAT dPlanedPar; + dPlanedPar.Place_in_row(uvec*dxdpvec,0,0); + dPlanedPar.Place_in_row(vvec*dxdpvec,1,0); + return ROOT::Math::Similarity(dPlanedPar,params().covariance()); + } + ParticleState KinematicLine::state(double time) const { return ParticleState(position4(time),momentum4(time), charge()); } @@ -137,10 +168,10 @@ namespace KinKal { switch (mdir) { case MomBasis::perpdir_: // purely polar change theta 1 = theta - return VEC3(cosTheta() * cosPhi0(), cosTheta() * sinPhi0(), -1 * sinTheta()); + return VEC3(cos(theta()) * cos(phi0()), cos(theta()) * sin(phi0()), -1 * sin(theta())); break; case MomBasis::phidir_: // purely transverse theta2 = -phi()*sin(theta) - return VEC3(-sinPhi0(), cosPhi0(), 0.0); + return VEC3(-sin(phi0()), cos(phi0()), 0.0); break; case MomBasis::momdir_: // along momentum return direction(); @@ -172,9 +203,8 @@ namespace KinKal { DVDP KinematicLine::dXdPar(double time) const { double deltat = time-t0(); - double sinT = sinTheta(); - double cotT = 1.0/tanTheta(); - double cosT = cosTheta(); + double sinT = sin(theta()); + double cosT = cos(theta()); double sinF = sin(phi0()); double cosF = cos(phi0()); double spd = speed(); @@ -182,7 +212,7 @@ namespace KinKal { SVEC3 dX_dd0(-sinF, cosF, 0.0); SVEC3 dX_dphi0 = (sinT*spd*deltat)*dX_dd0 - d0()*SVEC3(cosF,sinF,0.0); SVEC3 dX_dz0 (0.0,0.0,1.0); - SVEC3 dX_dcost = (spd*deltat)*SVEC3(-cotT*cosF,-cotT*sinF,1.0); + SVEC3 dX_dtheta = (spd*deltat)*SVEC3(cosT*cosF,cosT*sinF,-sinT); SVEC3 dX_dt0 = -spd*SVEC3(sinT*cosF,sinT*sinF,cosT); SVEC3 dX_dmom = -(deltat/(gam*gam*mom()))*dX_dt0; @@ -190,33 +220,32 @@ namespace KinKal { dXdP.Place_in_col(dX_dd0,0,d0_); dXdP.Place_in_col(dX_dphi0,0,phi0_); dXdP.Place_in_col(dX_dz0,0,z0_); - dXdP.Place_in_col(dX_dcost,0,cost_); + dXdP.Place_in_col(dX_dtheta,0,theta_); dXdP.Place_in_col(dX_dt0,0,t0_); dXdP.Place_in_col(dX_dmom,0,mom_); return dXdP; } DVDP KinematicLine::dMdPar(double time) const { - double sinT = sinTheta(); - double cotT = 1.0/tanTheta(); - double cosT = cosTheta(); + double sinT = sin(theta()); + double cosT = cos(theta()); double sinF = sin(phi0()); double cosF = cos(phi0()); // note: dMdd0 = dMdz0 = dMdt0 = 0 SVEC3 dM_dphi0 = (mom()*sinT)*SVEC3(-sinF,cosF,0); - SVEC3 dM_dcost = mom()*SVEC3(-cotT*cosF,-cotT*sinF,1.0); + SVEC3 dM_dtheta = mom()*SVEC3(cosT*cosF,cosT*sinF,-sinT); SVEC3 dM_dmom = SVEC3(sinT*cosF,sinT*sinF,cosT); DVDP dMdP; dMdP.Place_in_col(dM_dphi0,0,phi0_); - dMdP.Place_in_col(dM_dcost,0,cost_); + dMdP.Place_in_col(dM_dtheta,0,theta_); dMdP.Place_in_col(dM_dmom,0,mom_); return dMdP; } DPDV KinematicLine::dPardX(double time) const { - double sinT = sinTheta(); - double cotT = 1.0/tanTheta(); + double sinT = sin(theta()); + double cotT = 1/tan(theta()); double sinF = sin(phi0()); double cosF = cos(phi0()); double E = energy(); @@ -233,9 +262,9 @@ namespace KinKal { } DPDV KinematicLine::dPardM(double time) const { - double sinT = sinTheta(); - double cosT = cosTheta(); - double cotT = 1.0/tanTheta(); + double sinT = sin(theta()); + double cosT = cos(theta()); + double cotT = 1/tan(theta()); double sinF = sin(phi0()); double cosF = cos(phi0()); double cos2F = cosF*cosF-sinF*sinF; @@ -248,7 +277,7 @@ namespace KinKal { double xmt = pos.Dot(momt); double E = energy(); SVEC3 dmom_dM(sinT*cosF, sinT*sinF, cosT); - SVEC3 dcost_dM = (1.0/mom())*(SVEC3(0.0,0.0,1.0) - cosT*dmom_dM); + SVEC3 dtheta_dM = -1.0/sqrt(1-cosT*cosT)*((1.0/mom())*(SVEC3(0.0,0.0,1.0) - cosT*dmom_dM)); SVEC3 dphi0_dM = (1.0/(mom()*sinT))*SVEC3(-sinF,cosF,0.0); SVEC3 dt0_dM = (1.0/(momt2*CLHEP::c_light))*( xmt*( (2.0*E/momt2)*SVEC3(momv.X(),momv.Y(),0.0) @@ -258,7 +287,7 @@ namespace KinKal { SVEC3 dd0_dM = ( xmt/momt2 )* SVEC3(sinF, -cosF, 0.0); DPDV dPdM; dPdM.Place_in_row(dmom_dM,mom_,0); - dPdM.Place_in_row(dcost_dM,cost_,0); + dPdM.Place_in_row(dtheta_dM,theta_,0); dPdM.Place_in_row(dphi0_dM,phi0_,0); dPdM.Place_in_row(dz0_dM,z0_,0); dPdM.Place_in_row(dt0_dM,t0_,0); @@ -274,6 +303,13 @@ namespace KinKal { return mommag*(dPdM*SVEC3(dir.X(), dir.Y(), dir.Z())); } + Ray KinematicLine::linearize(double time) const { + VEC3 lpos = position3(time); + // direction is along the trajectory + VEC3 adir = direction(); + return Ray(adir,lpos); + } + void KinematicLine::print(ostream &ost, int detail) const { auto perr = params().covariance().Diagonal(); ost << " KinematicLine " << range() << " parameters: "; diff --git a/Trajectory/KinematicLine.hh b/Trajectory/KinematicLine.hh index fa6096f7..acb019ca 100644 --- a/Trajectory/KinematicLine.hh +++ b/Trajectory/KinematicLine.hh @@ -11,6 +11,8 @@ #include "KinKal/General/Parameters.hh" #include "KinKal/General/TimeRange.hh" #include "KinKal/General/Vectors.hh" +#include "KinKal/Geometry/Ray.hh" +#include "KinKal/Geometry/Plane.hh" #include "Math/Rotation3D.h" #include #include @@ -22,7 +24,7 @@ class KinematicLine { d0_ = 0, phi0_ = 1, z0_ = 2, - cost_ = 3, + theta_ = 3, t0_ = 4, mom_ = 5, npars_ = 6 @@ -37,6 +39,7 @@ class KinematicLine { static std::string const ¶mUnit(ParamIndex index); constexpr static ParamIndex t0Index() { return t0_; } + constexpr static ParamIndex phi0Index() { return phi0_; } static std::string const &trajName(); // This also requires the nominal BField, which can be a vector (3d) or a @@ -52,22 +55,25 @@ class KinematicLine { KinematicLine(Parameters const& pars, double mass, int charge, VEC3 const& bnom, TimeRange const& trange=TimeRange() ); KinematicLine(Parameters const& pars); + void syncPhi0(KinematicLine const& other); explicit KinematicLine(ParticleState const& pstate, VEC3 const& bnom, TimeRange const& range=TimeRange()); // same, including covariance information explicit KinematicLine(ParticleStateEstimate const& pstate, VEC3 const& bnom, TimeRange const& range=TimeRange()); - virtual ~KinematicLine() {} + virtual ~KinematicLine() = default; // particle momentum as a function of time MOM4 momentum4(double time) const; VEC3 momentum3(double time) const; // scalar momentum and energy in MeV/c units --> Needed for KKTrk: - double momentum(double time) const { return mom(); } + double momentum(double time=0.0) const { return mom(); } double momentumVariance(double time) const { return pars_.covariance()(mom_,mom_); } + double positionVariance(double time,MomBasis::Direction dir) const; + PMAT planeCovariance(double time,Plane const& plane) const; double energy() const { double momval = mom(); return sqrt(momval*momval+mass_*mass_); } double energy(double time) const { return energy(); } @@ -79,7 +85,7 @@ class KinematicLine { double d0() const { return paramVal(d0_); } double phi0() const { return paramVal(phi0_); } double z0() const { return paramVal(z0_); } - double cost() const { return paramVal(cost_); } + double theta() const { return paramVal(theta_); } double t0() const { return paramVal(t0_); } double mom() const { return paramVal(mom_); } @@ -87,31 +93,30 @@ class KinematicLine { ParticleState state(double time) const; ParticleStateEstimate stateEstimate(double time) const; - double translen(const double &f) const { return sinTheta() * f; } - // simple functions - double cosTheta() const { return cost(); } - double sinTheta() const { return sqrt(1.0 - cost() * cost()); } - double cosPhi0() const { return cos(phi0()); } - double sinPhi0() const { return sin(phi0()); } - double theta() const { return acos(cost()); } - double tanTheta() const { return sqrt(1.0 - cost() * cost()) / cost(); } - VEC3 pos0() const { return VEC3(-d0()*sinPhi0(),d0()*cosPhi0(),z0()); } + double translen(const double &f) const { return sin(theta()) * f; } + + VEC3 pos0() const { return VEC3(-d0()*sin(phi0()),d0()*cos(phi0()),z0()); } double flightLength(double t)const { return (t-t0())*speed(); } - VEC3 direction() const { double st = sinTheta(); return VEC3(st*cosPhi0(),st*sinPhi0(),cosTheta()); } + VEC3 direction() const { double st = sin(theta()); return VEC3(st*cos(phi0()),st*sin(phi0()),cos(theta())); } TimeRange const &range() const { return trange_; } TimeRange &range() {return trange_; } virtual void setRange(TimeRange const &trange) { trange_ = trange; } - void setBNom(double time, VEC3 const& bnom) { bnom_ = bnom; } + void setBNom(double time, VEC3 const& bnom) { resetBNom(bnom); } + void resetBNom(VEC3 const& bnom) { bnom_ = bnom; } bool inRange(double time) const { return trange_.inRange(time); } double speed() const { return ( mom()/ energy()) * CLHEP::c_light; } double speed(double t) const { return speed(); } + double transverseSpeed() const { return 0.0; } VEC3 position3(double time) const; VEC4 position4(double time) const; VEC3 velocity(double time) const { return direction() * speed(); } + double acceleration() const { return 0; } + VEC3 acceleration(double time) const { return VEC3(0.0,0.0,0.0); } + void print(std::ostream &ost, int detail) const; // local momentum direction basis @@ -138,6 +143,12 @@ class KinematicLine { // Parameter derivatives given a change in BField. These return null for KinematicLine DVEC dPardB(double time) const { return DVEC(); } DVEC dPardB(double time, VEC3 const& BPrime) const { return DVEC(); } + PSMAT dPardPardB(double time,VEC3 const& db) const { return ROOT::Math::SMatrixIdentity(); } + // linear approximation + Ray linearize(double time) const; + + double bendRadius() const { return 0.0; } + double sagitta(double) const { return 0.0; } void invertCT() { charge_ *= -1; diff --git a/Trajectory/Line.cc b/Trajectory/Line.cc deleted file mode 100644 index 143ae06b..00000000 --- a/Trajectory/Line.cc +++ /dev/null @@ -1,46 +0,0 @@ -#include "KinKal/Trajectory/Line.hh" -#include -#include -#include -#include - -using namespace std; -using namespace ROOT::Math; - -namespace KinKal { - Line::Line(VEC4 const& pos0, VEC3 const& svel, double length ) : Line(pos0.Vect(), pos0.T(), svel, length) {} - Line::Line(VEC3 const& pos0, double tmeas , VEC3 const& svel, double length ) : - pos0_(pos0), dir_(svel.Unit()), t0_(tmeas), speed_(sqrt(svel.Mag2())), length_(length) {} - Line::Line(VEC3 const& p0, VEC3 const& p1, double t0, double speed ) : pos0_(p0), dir_((p0-p1).Unit()), t0_(t0), speed_(speed), length_((p1-p0).R()) {} - - VEC3 Line::position3(double time) const { - return pos0_ + ((time-t0_)*speed_)*dir_; - } - - VEC4 Line::position4(double time) const { - VEC3 pos3 = position3(time); - return VEC4(pos3.X(),pos3.Y(),pos3.Z(),time); - } - - VEC3 Line::velocity(double time) const { - return dir_*speed_; - } - - double Line::TOCA(VEC3 const& point) const { - double s = (point - pos0_).Dot(dir_); - return s/speed_ - t0_; - } - - void Line::print(std::ostream& ost, int detail) const { - ost << " Line, intial position " << pos0_ - << " t0 " << t0_ - << " direction " << dir_ - << " speed " << speed_ << endl; - } - - std::ostream& operator <<(std::ostream& ost, Line const& tline) { - tline.print(ost,0); - return ost; - } - -} diff --git a/Trajectory/Line.hh b/Trajectory/Line.hh deleted file mode 100644 index d518b015..00000000 --- a/Trajectory/Line.hh +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef KinKal_Line_hh -#define KinKal_Line_hh -// -// Linear time-based trajectory with a constant velocity. -// Used as part of the kinematic Kalman fit -// -#include "KinKal/General/Vectors.hh" -#include "KinKal/General/TimeRange.hh" -namespace KinKal { - class Line { - public: - // construct from a spacetime point (typically the measurement position and time) and propagation velocity (mm/ns). - Line(VEC4 const& p0, VEC3 const& svel, double length); - Line(VEC3 const& p0, double t0, VEC3 const& svel, double length); - // construct from 2 points plus timing information. P0 is the measurement (near) end, p1 the far end. Signals propagate from far to near - Line(VEC3 const& p0, VEC3 const& p1, double t0, double speed ); - // accessors - double t0() const { return t0_; } - double& t0() { return t0_; } // detector updates need to refine t0 - VEC3 const& startPosition() const { return pos0_; } - VEC3 endPosition() const { return pos0_ + length_*dir_; } - double speed() const { return speed_; } - double speed(double time) const { return speed_; } - double length() const { return length_; } - VEC3 const& direction() const { return dir_; } - // TOCA to a point - double TOCA(VEC3 const& point) const; - // geometric accessors - VEC3 position3(double time) const; - VEC4 position4(double time) const; - VEC3 velocity(double time) const; - VEC3 const& direction(double time) const { return dir_; } - void print(std::ostream& ost, int detail) const; - TimeRange range() const { return TimeRange(t0_,t0_ +length_/speed_); } - - private: - VEC3 pos0_, dir_; // position and direction - double t0_; // intial time (at pos0) - double speed_; // signed linear velocity, translates time to distance along the trajectory (mm/nsec) - double length_; // line length - }; - std::ostream& operator <<(std::ostream& ost, Line const& tline); -} -#endif diff --git a/Trajectory/LoopHelix.cc b/Trajectory/LoopHelix.cc index 31d12056..8152ec43 100644 --- a/Trajectory/LoopHelix.cc +++ b/Trajectory/LoopHelix.cc @@ -27,58 +27,65 @@ namespace KinKal { string const& LoopHelix::paramTitle(ParamIndex index) { return paramTitles_[static_cast(index)];} string const& LoopHelix::trajName() { return trajName_; } -LoopHelix::LoopHelix() : mass_(0.0), charge_(0) {} -LoopHelix::LoopHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, double bnom, TimeRange const& range) : LoopHelix(pos0,mom0,charge,VEC3(0.0,0.0,bnom),range) {} -LoopHelix::LoopHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, VEC3 const& bnom, TimeRange const& trange) : trange_(trange), mass_(mom0.M()), charge_(charge), bnom_(bnom) { - static double twopi = 2*M_PI; - // Transform into the system where Z is along the Bfield, which is the implicit coordinate system of the parameterization. - // The transform is a pure rotation about the origin - VEC4 pos(pos0); - MOM4 mom(mom0); - g2l_ = Rotation3D(AxisAngle(VEC3(sin(bnom_.Phi()),-cos(bnom_.Phi()),0.0),bnom_.Theta())); - if(fabs(g2l_(bnom_).Theta()) > 1.0e-6)throw invalid_argument("Rotation Error"); - // to convert global vectors into parameters they must first be rotated into the local system. - pos = g2l_(pos); - mom = g2l_(mom); - // create inverse rotation; this moves back into the global coordinate system - l2g_ = g2l_.Inverse(); + LoopHelix::LoopHelix() : mass_(0.0), charge_(0) {} + LoopHelix::LoopHelix( VEC4 const& gpos, MOM4 const& gmom, int charge, double bnom, TimeRange const& range) : LoopHelix(gpos,gmom,charge,VEC3(0.0,0.0,bnom),range) {} + LoopHelix::LoopHelix( VEC4 const& gpos, MOM4 const& gmom, int charge, VEC3 const& bnom, TimeRange const& trange) : trange_(trange), mass_(gmom.M()), charge_(charge), bnom_(bnom) { + static double twopi = 2*M_PI; + // Transform position and momentum into the system where Z is along the Bfield, which is the implicit coordinate system of the parameterization. + // This is a pure rotation about the origin + setTransforms(); + auto lpos = g2l_(gpos); + auto lmom = g2l_(gmom); // compute some simple useful parameters - double pt = mom.Pt(); - double phibar = mom.Phi(); + double pt = lmom.Pt(); + double lmomphi = lmom.Phi(); // translation factor from MeV/c to curvature radius in mm, B in Tesla; signed by the charge!!! - double momToRad = 1.0/Q(); + double invq = 1.0/Q(); // transverse radius of the helix - param(rad_) = pt*momToRad; + param(rad_) = pt*invq; // longitudinal wavelength - param(lam_) = mom.Z()*momToRad; - // time at z=0 - double om = omega(); - param(t0_) = pos.T() - pos.Z()/(om*lam()); + param(lam_) = lmom.Z()*invq; + // time when particle reaches local z=0 + double zbar = lpos.Z()/lam(); + param(t0_) = lpos.T() - zbar/omega(); // compute winding that puts phi0 in the range -pi,pi - double nwind = rint((pos.Z()/lam() - phibar)/twopi); - // cout << "winding number = " << nwind << endl; - // azimuth at z=0 - param(phi0_) = phibar - om*(pos.T()-t0()) + twopi*nwind; + double nwind = round((zbar - lmomphi)/twopi); + // particle momentum azimuth at z=0 + param(phi0_) = lmomphi - zbar + twopi*nwind; // circle center - param(cx_) = pos.X() - mom.Y()*momToRad; - param(cy_) = pos.Y() + mom.X()*momToRad; - // test position and momentum function - // auto testpos = position3(pos0.T()); - // auto testmom = momentum3(pos0.T()); - // auto dp = testpos - pos0.Vect(); - // auto dm = testmom - mom0.Vect(); - // if(dp.R() > 1.0e-5 || dm.R() > 1.0e-5)throw invalid_argument("Rotation Error"); - } - - void LoopHelix::setBNom(double time, VEC3 const& bnom) { - // adjust the parameters for the change in bnom - pars_.parameters() += dPardB(time,bnom); - bnom_ = bnom; + param(cx_) = lpos.X() - lmom.Y()*invq; + param(cy_) = lpos.Y() + lmom.X()*invq; + } + + void LoopHelix::syncPhi0(LoopHelix const& other) { +// adjust the phi0 of this traj to agree with the reference, keeping its value (mod 2pi) the same. + static double twopi = 2*M_PI; + int nloop = static_cast(round( (other.phi0() - phi0())/twopi)); + if(nloop != 0) pars_.parameters()[phi0_] += nloop*twopi; + } + + void LoopHelix::setTransforms() { // adjust rotations to global space g2l_ = Rotation3D(AxisAngle(VEC3(sin(bnom_.Phi()),-cos(bnom_.Phi()),0.0),bnom_.Theta())); l2g_ = g2l_.Inverse(); } + void LoopHelix::setBNom(double time, VEC3 const& newbnom) { + auto db = newbnom - bnom_; + // rotate the covariance to this new basis: this is still work in progress TODO + // PSMAT dpdpdb =ROOT::Math::SMatrixIdentity(); + // PSMAT dpdpdb = dPardPardB(time,db); + // std::cout << "dpdpdb = " << dpdpdb << std::endl; + // pars_.covariance() = ROOT::Math::Similarity(dpdpdb,pars_.covariance()); + pars_.parameters() += dPardB(time,db); + resetBNom(newbnom); + } + + void LoopHelix::resetBNom(VEC3 const& newbnom) { + bnom_ = newbnom; + setTransforms(); + } + LoopHelix::LoopHelix(LoopHelix const& other, VEC3 const& bnom, double tref) : LoopHelix(other) { setBNom(tref,bnom); } @@ -89,10 +96,8 @@ LoopHelix::LoopHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, VEC3 const LoopHelix::LoopHelix( Parameters const& pars, double mass, int charge, VEC3 const& bnom, TimeRange const& trange ) : trange_(trange), pars_(pars), mass_(mass), charge_(charge), bnom_(bnom) { - // set the transforms - g2l_ = Rotation3D(AxisAngle(VEC3(sin(bnom_.Phi()),-cos(bnom_.Phi()),0.0),bnom_.Theta())); - l2g_ = g2l_.Inverse(); - } + setTransforms(); + } LoopHelix::LoopHelix(ParticleState const& pstate, VEC3 const& bnom, TimeRange const& range) : LoopHelix(pstate.position4(),pstate.momentum4(),pstate.charge(),bnom,range) @@ -111,6 +116,29 @@ LoopHelix::LoopHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, VEC3 const return ROOT::Math::Similarity(dMomdP,params().covariance()); } + double LoopHelix::positionVariance(double time, MomBasis::Direction mdir) const { + auto dxdpvec = dXdPar(time); + auto momdir = direction(time); + auto posdir = MomBasis::direction(mdir, momdir); + SVEC3 sdir(posdir.X(),posdir.Y(),posdir.Z()); + DVEC dxdp = sdir*dxdpvec; + return ROOT::Math::Similarity(dxdp,params().covariance()); + } + + PMAT LoopHelix::planeCovariance(double time,Plane const& plane) const { + // project covariance onto the U, V direction of the given plane + // particle direction cannot be orthogonal to the plane normal + auto momdir = direction(time); + if(fabs(plane.normal().Dot(momdir)) < 1.0e-10)throw invalid_argument("Momentum direction lies in the plane"); + auto dxdpvec = dXdPar(time); + SVEC3 uvec(plane.uDirection().X(),plane.uDirection().Y(),plane.uDirection().Z()); + SVEC3 vvec(plane.vDirection().X(),plane.vDirection().Y(),plane.vDirection().Z()); + PPMAT dPlanedPar; + dPlanedPar.Place_in_row(uvec*dxdpvec,0,0); + dPlanedPar.Place_in_row(vvec*dxdpvec,1,0); + return ROOT::Math::Similarity(dPlanedPar,params().covariance()); + } + VEC4 LoopHelix::position4(double time) const { VEC3 temp = position3(time); return VEC4(temp.X(),temp.Y(),temp.Z(),time); @@ -133,6 +161,12 @@ LoopHelix::LoopHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, VEC3 const return direction(time)*speed(time); } + VEC3 LoopHelix::acceleration(double time) const { + auto phival = phi(time); + auto locacc = acceleration()*VEC3(-sin(phival),cos(phival),0.0); + return l2g_(locacc); + } + VEC3 LoopHelix::localDirection(double time, MomBasis::Direction mdir) const { double phival = phi(time); double invpb = -sign()/pbar(); // need to sign @@ -228,33 +262,60 @@ LoopHelix::LoopHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, VEC3 const return dPardMLoc(time)*g2lmat; } - DVEC LoopHelix::dPardB(double time) const { + PSMAT LoopHelix::dPardPardB(double time,VEC3 const& db) const { + auto newbnom = bnom_ + db; + double bfrac = bnom_.R()/newbnom.R(); + auto xvec = localPosition(time); double phival = phi(time); - DVEC retval; - retval[rad_] = -rad(); - retval[lam_] = -lam(); - retval[cx_] = rad()*sin(phival); - retval[cy_] = -rad()*cos(phival); - retval[phi0_] = -dphi(time); - retval[t0_] = 0.0; - return (1.0/bnom_.R())*retval; + double cphi = cos(phival); + double sphi = sin(phival); + // compute the rows + DVEC dpdpdb_rad; dpdpdb_rad[rad_] = bfrac; + DVEC dpdpdb_lam; dpdpdb_lam[lam_] = bfrac; + DVEC dpdpdb_cx; dpdpdb_cx[cx_] = 1.0; dpdpdb_cx[rad_] = sphi*(1-bfrac); + DVEC dpdpdb_cy; dpdpdb_cy[cy_] = 1.0; dpdpdb_cy[rad_] = -cphi*(1-bfrac); + DVEC dpdpdb_phi0; dpdpdb_phi0[phi0_] = 1.0; dpdpdb_phi0[lam_] = -xvec.Z()*(1-1.0/bfrac)/(lam()*lam()); + DVEC dpdpdb_t0; dpdpdb_t0[t0_] = 1.0; + // build the matrix from these rows + PSMAT dpdpdb; + dpdpdb.Place_in_row(dpdpdb_rad,rad_,0); + dpdpdb.Place_in_row(dpdpdb_lam,lam_,0); + dpdpdb.Place_in_row(dpdpdb_cx,cx_,0); + dpdpdb.Place_in_row(dpdpdb_cy,cy_,0); + dpdpdb.Place_in_row(dpdpdb_phi0,phi0_,0); + dpdpdb.Place_in_row(dpdpdb_t0,t0_,0); + return dpdpdb; } - DVEC LoopHelix::dPardB(double time, VEC3 const& BPrime) const { - // rotate new B field difference into local coordinate system - VEC3 dB = g2l_(BPrime-bnom_); - // find the parameter change due to BField magnitude change usng component parallel to the local nominal Bfield (always along z) - DVEC retval = dPardB(time)*dB.Z(); - // find the change in (local) position and momentum due to the rotation implied by the B direction change - // work in local coordinate system to avoid additional matrix mulitplications + DVEC LoopHelix::dPardB(double time, VEC3 const& db) const { + // record position and momentum in local coordinates; these are constant auto xvec = localPosition(time); auto mvec = localMomentum(time); - VEC3 BxdB =VEC3(0.0,0.0,1.0).Cross(dB)/bnomR(); + // translate newbnom to local coordinates + VEC3 dbloc = g2l_(db); + // find the parameter change due to bnom magnitude change. These are exact + double br = bnom_.R(); + auto zhat = VEC3(0.0,0.0,1.0); + auto bloc = br*zhat; + auto newbloc = dbloc + bloc; + double nbr = newbloc.R(); + DVEC retval; + double dbf = (br - nbr)/nbr; + retval[rad_] = rad()*dbf; + retval[lam_] = lam()*dbf; + retval[phi0_] = xvec.Z()*(br -nbr)/br/lam(); + retval[t0_] = 0.0; + retval[cx_] = -sin(mvec.Phi())*retval[rad_]; + retval[cy_] = cos(mvec.Phi())*retval[rad_]; + + // find the change in local position and momentum due to the rotation implied by the B direction change + // work in local coordinate system to avoid additional matrix mulitplications + VEC3 BxdB = zhat.Cross(dbloc)/br; VEC3 dx = xvec.Cross(BxdB); VEC3 dm = mvec.Cross(BxdB); - // convert these to a full state vector change + // convert these to the state vector change ParticleState dstate(dx,dm,time,mass(),charge()); - // convert the change in (local) state due to rotation to parameter space + // convert the (local) state due to rotation to parameter space retval += dPardStateLoc(time)*dstate.state(); return retval; } @@ -355,6 +416,32 @@ LoopHelix::LoopHelix( VEC4 const& pos0, MOM4 const& mom0, int charge, VEC3 const return ParticleStateEstimate(state(time),ROOT::Math::Similarity(dsdp,pars_.covariance())); } + VEC3 LoopHelix::center(double time) const { + // local transverse position is at the center. Use the time to define the Z position + VEC3 cpos(cx(),cy(),dphi(time)*lam()); + // transform to global coordinates + auto gcpos = l2g_(cpos); + return gcpos; + } + + Ray LoopHelix::axis(double time) const { + // direction is along Bnom, signed by pz. Note Bnom is in global coordinates + VEC3 adir = bnom_.Unit(); + auto pzsign = -lam()*sign(); + if(pzsign*adir.Z() < 0) adir*= -1.0; + return Ray(adir,center(time)); + } + + double LoopHelix::sagitta(double trange) const { + double tlen = fabs(trange*transverseSpeed()); + double brad = bendRadius(); + if(tlen < M_PI*brad){ + double drunit = (1.0-cos(0.5*tlen/brad)); // unit circle + return 0.125*brad*drunit*drunit; + } + return brad; // maximum possible sagitta + } + void LoopHelix::print(ostream& ost, int detail) const { auto pvar = params().covariance().Diagonal(); ost << " LoopHelix " << range() << " parameters: "; diff --git a/Trajectory/LoopHelix.hh b/Trajectory/LoopHelix.hh index 45734db7..c0ca7b11 100644 --- a/Trajectory/LoopHelix.hh +++ b/Trajectory/LoopHelix.hh @@ -14,6 +14,8 @@ #include "KinKal/General/BFieldMap.hh" #include "KinKal/General/ParticleStateEstimate.hh" #include "KinKal/General/PhysicalConstants.h" +#include "KinKal/Geometry/Plane.hh" +#include "KinKal/Geometry/Ray.hh" #include "Math/Rotation3D.h" #include #include @@ -27,6 +29,7 @@ namespace KinKal { // classes implementing the Kalman fit // define the indices and names of the parameters enum ParamIndex {rad_=0,lam_=1,cx_=2,cy_=3,phi0_=4,t0_=5,npars_=6}; + constexpr static ParamIndex phi0Index() { return phi0_; } constexpr static ParamIndex t0Index() { return t0_; } static std::vector const& paramNames(); @@ -43,7 +46,7 @@ namespace KinKal { // This also requires the nominal BField, which can be a vector (3d) or a scalar (B along z) LoopHelix(VEC4 const& pos, MOM4 const& mom, int charge, VEC3 const& bnom, TimeRange const& range=TimeRange()); LoopHelix(VEC4 const& pos, MOM4 const& mom, int charge, double bnom, TimeRange const& range=TimeRange()); // do I really need this? - // construct from the particle state at a given time, plus mass and charge + // construct from the particle state at a given time, plus mass and charge. Parameter covariance matrix is undefined explicit LoopHelix(ParticleState const& pstate, VEC3 const& bnom, TimeRange const& range=TimeRange()); // same, including covariance information explicit LoopHelix(ParticleStateEstimate const& pstate, VEC3 const& bnom, TimeRange const& range=TimeRange()); @@ -53,22 +56,31 @@ namespace KinKal { LoopHelix( Parameters const& pars, double mass, int charge, VEC3 const& bnom, TimeRange const& trange=TimeRange() ); // copy payload and override the parameters; Is this used? LoopHelix(Parameters const& pdata, LoopHelix const& other); + // synchronize phi0, which has a 2pi wrapping + void syncPhi0(LoopHelix const& other); VEC4 position4(double time) const; VEC3 position3(double time) const; VEC3 velocity(double time) const; double speed(double time=0.0) const { return CLHEP::c_light*beta(); } + double transverseSpeed() const { return fabs(CLHEP::c_light*lam()/ebar()); } // speed perpendicular to the axis + double acceleration() const { return rad()*CLHEP::c_light*CLHEP::c_light/ebar2(); } + VEC3 acceleration(double time) const; void print(std::ostream& ost, int detail) const; TimeRange const& range() const { return trange_; } TimeRange& range() { return trange_; } void setRange(TimeRange const& trange) { trange_ = trange; } // allow resetting the BField. Note this is time-dependent void setBNom(double time, VEC3 const& bnom); + // change the BField. This also resets the transforms + void resetBNom(VEC3 const& bnom); bool inRange(double time) const { return trange_.inRange(time); } VEC3 momentum3(double time) const; MOM4 momentum4(double time) const; double momentum(double time=0) const { return mass_*betaGamma(); } double momentumVariance(double time=0) const; - double energy(double time=0) const { return ebar()*Q(); } + double positionVariance(double time,MomBasis::Direction dir) const; + PMAT planeCovariance(double time,Plane const& plane) const; + double energy(double time=0) const { return fabs(ebar()*Q()); } VEC3 direction(double time, MomBasis::Direction mdir= MomBasis::momdir_) const; double mass() const { return mass_;} // mass int charge() const { return charge_;} // charge in proton charge units @@ -88,6 +100,7 @@ namespace KinKal { ParticleStateEstimate stateEstimate(double time) const; // simple functions double sign() const { return copysign(1.0,charge()); } // charge sign + double parameterSign() const { return copysign(1.0,rad()); } // helicity is defined as the sign of the projection of the angular momentum vector onto the linear momentum vector double helicity() const { return copysign(1.0,lam()); } double pbar2() const { return rad()*rad() + lam()*lam(); } @@ -97,6 +110,7 @@ namespace KinKal { double mbar() const { return fabs(mass_/Q()); } // mass in mm double Q() const { return -BFieldMap::cbar()*charge()*bnom_.R(); } // reduced charge double omega() const { return -CLHEP::c_light*sign()/ ebar(); } // rotational velocity, sign set by magnetic force + double omegaZ() const { return 1.0/lam(); } // dPhi/dz double beta() const { return pbar()/ebar(); } // relativistic beta double gamma() const { return ebar()/mbar(); } // relativistic gamma double betaGamma() const { return pbar()/mbar(); } // relativistic betagamma @@ -119,9 +133,20 @@ namespace KinKal { PSMAT dStatedPar(double time) const; // derivative of global state WRT parameters DVEC momDeriv(double time, MomBasis::Direction mdir) const; // projection of M derivatives onto direction basis // package the above for full (global) state - // Parameter derivatives given a change in BField - DVEC dPardB(double time) const; // parameter derivative WRT change in BField magnitude - DVEC dPardB(double time, VEC3 const& BPrime) const; // parameter change given a new BField vector + DVEC dPardB(double time, VEC3 const& db) const; // parameter change given a change in BField vector; this includes the magnitude and direction changes + PSMAT dPardPardB(double time,VEC3 const& db) const; // Parameter covariance rotation for a change in BField + // helix interface + VEC3 center(double time) const; // helix center in global coordinates + Ray axis(double time) const; // helix axis in global coordinates + // linear approximation + Ray linearize(double time) const { return axis(time); } + // convenience accessors + VEC2 center() const { return VEC2(cx(),cy()); } + double minAxisDist() const { return fabs(center().R()-fabs(rad())); } // minimum distance to the axis + double maxAxisDist() const { return center().R()+fabs(rad()); } // maximum distance to the axis + double axisSpeed() const; // speed along the axis direction (always positive) + double bendRadius() const { return fabs(rad());} + double sagitta(double range) const; // compute maximum sagitta over a time range private : // local coordinate system functions, used internally VEC3 localDirection(double time, MomBasis::Direction mdir= MomBasis::momdir_) const; @@ -130,6 +155,7 @@ namespace KinKal { DPDV dPardXLoc(double time) const; // return the derivative of the parameters WRT the local (unrotated) position vector DPDV dPardMLoc(double time) const; // return the derivative of the parameters WRT the local (unrotated) momentum vector PSMAT dPardStateLoc(double time) const; // derivative of parameters WRT local state + void setTransforms(); // define global to local and local to global given BNom TimeRange trange_; Parameters pars_; // parameters diff --git a/Trajectory/POCAUtil.cc b/Trajectory/POCAUtil.cc index 6ef3e350..017671e2 100644 --- a/Trajectory/POCAUtil.cc +++ b/Trajectory/POCAUtil.cc @@ -1,15 +1,3 @@ -/* - - code based on MuteUtilities/src/TwoLinePCA: - - Given two lines in 3D, compute the distance of closest - approach between the two lines. The lines are - specified in point-slope form. - - Original author Rob Kutschke - - */ - #include "KinKal/Trajectory/POCAUtil.hh" #include #include diff --git a/Trajectory/POCAUtil.hh b/Trajectory/POCAUtil.hh index 7ddc9975..e20930f6 100644 --- a/Trajectory/POCAUtil.hh +++ b/Trajectory/POCAUtil.hh @@ -1,5 +1,16 @@ #ifndef KinKal_POCAUtil_hh #define KinKal_POCAUtil_hh +/* + + code based on MuteUtilities/src/TwoLinePCA: + + Given two lines in 3D, compute the distance of closest + approach between the two lines. The lines are + specified in point-slope form. + + Original author Rob Kutschke + + */ #include "KinKal/General/Vectors.hh" #include "Math/Rotation3D.h" diff --git a/Trajectory/ParticleTrajectory.cc b/Trajectory/ParticleTrajectory.cc new file mode 100644 index 00000000..5f2ee351 --- /dev/null +++ b/Trajectory/ParticleTrajectory.cc @@ -0,0 +1,40 @@ +// +// specialization for LoopHelix to search for reflections +// +#include "KinKal/Trajectory/LoopHelix.hh" +#include "KinKal/Trajectory/ParticleTrajectory.hh" +namespace KinKal { + template<> std::shared_ptr ParticleTrajectory::reflection(double tstart) const { + std::shared_ptr retval; // null by default + // record the direction of the particle WRT the bfield at the start + auto starttraj = this->nearestTraj(tstart); + double startdir = starttraj->momentum3(tstart).Dot(starttraj->bnom()); + // go to the end to see if there's a possible reflection + auto endtraj = this->backPtr(); + double tend = endtraj->range().mid(); + double enddir = endtraj->momentum3(tend).Dot(endtraj->bnom()); + if(startdir*enddir < 0.0){ + // reflection. Binary search to find the trajectory piece just after reflection + double tdiff = 0.5*(endtraj->range().end() - starttraj->range().begin()); + double ttest = tend - tdiff; + auto testtraj = this->nearestTraj(ttest); + while(testtraj != endtraj){ + double testdir = testtraj->momentum3(ttest).Dot(testtraj->bnom()); + tdiff = 0.5*std::max(tdiff, testtraj->range().range()); + if(testdir*enddir > 0){ + endtraj = testtraj; + ttest -= tdiff; + } else { + ttest += tdiff; + } + testtraj = this->nearestTraj(ttest); + } + // final test + double testdir = endtraj->momentum3(endtraj->range().mid()).Dot(endtraj->bnom()); + if(testdir*startdir > 0.0) throw std::invalid_argument("Inconsistant reflection search"); + retval = endtraj; + } + return retval; + } +} + diff --git a/Trajectory/ParticleTrajectory.hh b/Trajectory/ParticleTrajectory.hh index ba510f0d..d4743f86 100644 --- a/Trajectory/ParticleTrajectory.hh +++ b/Trajectory/ParticleTrajectory.hh @@ -11,6 +11,7 @@ namespace KinKal { template class ParticleTrajectory : public PiecewiseTrajectory { public: + using KTRAJPTR = std::shared_ptr; using PTTRAJ = PiecewiseTrajectory; // base class implementation @@ -28,7 +29,7 @@ namespace KinKal { if(fabs(newpiece.mass()-mass())>1e-6 || newpiece.charge() != charge()) throw std::invalid_argument("Invalid particle parameters"); PTTRAJ::prepend(newpiece,allowremove); } - VEC3 position3(double time) const { return PTTRAJ::nearestPiece(time).position3(time); } + // kinematic interface VEC4 position4(double time) const { return PTTRAJ::nearestPiece(time).position4(time); } VEC3 momentum3(double time) const { return PTTRAJ::nearestPiece(time).momentum3(time); } MOM4 momentum4(double time) const { return PTTRAJ::nearestPiece(time).momentum4(time); } @@ -41,7 +42,14 @@ namespace KinKal { VEC3 const& bnom(double time) const { return PTTRAJ::nearestPiece(time).bnom(); } ParticleState state(double time) const { return PTTRAJ::nearestPiece(time).state(time); } ParticleStateEstimate stateEstimate(double time) const { return PTTRAJ::nearestPiece(time).stateEstimate(time); } + // search for and return the piece just following a reflection + KTRAJPTR reflection(double time) const; }; + + // generic implemetnation; just return a null ptr + template std::shared_ptr ParticleTrajectory::reflection(double time) const { + return std::shared_ptr(); + } } #endif diff --git a/Trajectory/PiecewiseClosestApproach.hh b/Trajectory/PiecewiseClosestApproach.hh index b1bb310d..fc0b04be 100644 --- a/Trajectory/PiecewiseClosestApproach.hh +++ b/Trajectory/PiecewiseClosestApproach.hh @@ -8,94 +8,50 @@ #include namespace KinKal { - template class PiecewiseClosestApproach { + template class PiecewiseClosestApproach : public ClosestApproach,STRAJ> { public: - using PKTRAJ = ParticleTrajectory; + using PTRAJ = ParticleTrajectory; using KTCA = ClosestApproach; - // the constructor is the only non-inherited function - PiecewiseClosestApproach(PKTRAJ const& pktraj, STRAJ const& straj, CAHint const& hint, double precision); - // copy the TCA interface. This is ugly and a maintenance burden, but avoids inheritance problems - ClosestApproachData::TPStat status() const { return tpdata_.status(); } - std::string const& statusName() const { return tpdata_.statusName(); } - double doca() const { return tpdata_.doca(); } - double docaVar() const { return tpdata_.docaVar(); } - double tocaVar() const { return tpdata_.tocaVar(); } - double dirDot() const { return tpdata_.dirDot(); } - double deltaT() const { return tpdata_.deltaT(); } - bool usable() const { return tpdata_.usable(); } - double particleToca() const { return tpdata_.particleToca(); } - double sensorToca() const { return tpdata_.sensorToca(); } - double lSign() const { return tpdata_.lsign_; } // sign of angular momentum - VEC4 const& particlePoca() const { return tpdata_.particlePoca(); } - VEC4 const& sensorPoca() const { return tpdata_.sensorPoca(); } - VEC4 delta() const { return tpdata_.delta(); } - VEC3 const& particleDirection() const { return tpdata_.particleDirection(); } - VEC3 const& sensorDirection() const { return tpdata_.sensorDirection(); } - ClosestApproachData const& tpData() const { return tpdata_; } - PKTRAJ const& particleTraj() const { return pktraj_; } + using KTRAJPTR = std::shared_ptr; + PiecewiseClosestApproach(PTRAJ const& ptraj, STRAJ const& straj, CAHint const& hint, double precision); + // provide access to the local (non-piecewise) information implicit in this class size_t particleTrajIndex() const { return pindex_; } - STRAJ const& sensorTraj() const { return straj_; } - DVEC const& dDdP() const { return dDdP_; } - DVEC const& dTdP() const { return dTdP_; } - bool inRange() const { return particleTraj().inRange(particleToca()) && sensorTraj().inRange(sensorToca()); } - double precision() const { return precision_; } - void print(std::ostream& ost=std::cout,int detail=0) const; + KTRAJ const& localParticleTraj() const { return this->particleTraj().piece(pindex_); } + KTRAJPTR const& localTraj() const { return this->particleTraj().indexTraj(pindex_); } + KTCA localClosestApproach() const { return KTCA(localTraj(),this->sensorTraj(),this->precision(),this->tpData(),this->dDdP(),this->dTdP()); } private: - double precision_; // precision used to define convergence - ClosestApproachData tpdata_; // data payload of CA calculation - PKTRAJ const& pktraj_; - STRAJ const& straj_; size_t pindex_; // indices to the local traj used in TCA calculation - DVEC dDdP_; - DVEC dTdP_; }; - template PiecewiseClosestApproach::PiecewiseClosestApproach(PKTRAJ const& pktraj, STRAJ const& straj, CAHint const& hint, double prec) : precision_(prec), pktraj_(pktraj), straj_(straj) { + template PiecewiseClosestApproach::PiecewiseClosestApproach(ParticleTrajectory const& ptraj, STRAJ const& straj, CAHint const& hint, double prec) : ClosestApproach,STRAJ>(ptraj,straj,prec) { // iteratively find the nearest piece, and CA for that piece. Start at hints if availalble, otherwise the middle - static const unsigned maxiter=10; // don't allow infinite iteration. This should be a parameter FIXME! + static const unsigned maxiter=10; // don't allow infinite iteration. This should be a parameter TODO unsigned niter=0; - size_t oldindex= pktraj_.pieces().size(); - pindex_ = pktraj_.nearestIndex(hint.particleToca_); + size_t oldindex= this->particleTraj().pieces().size(); + this->pindex_ = this->particleTraj().nearestIndex(hint.particleToca_); // copy over the hint: it needs to evolve CAHint phint = hint; // iterate until TCA is on the same piece do{ - KTCA tpoca(pktraj_.piece(pindex_),straj,phint,prec); + KTCA tpoca(this->particleTraj().piece(pindex_),this->sensorTraj(),phint,prec); // copy the state - tpdata_ = tpoca.tpData(); - dDdP_ = tpoca.dDdP(); - dTdP_ = tpoca.dTdP(); + this->tpdata_ = tpoca.tpData(); + this->dDdP_ = tpoca.dDdP(); + this->dTdP_ = tpoca.dTdP(); // inrange = tpoca.inRange(); // update the hint phint.particleToca_ = tpoca.particleToca(); phint.sensorToca_ = tpoca.sensorToca(); // update the piece (if needed) oldindex = pindex_; - pindex_ = pktraj_.nearestIndex(tpoca.particlePoca().T()); - } while( pindex_ != oldindex && usable() && niter++ < maxiter); + pindex_ = this->particleTraj().nearestIndex(tpoca.particlePoca().T()); + } while( pindex_ != oldindex && this->usable() && niter++ < maxiter); // overwrite the status if we oscillated on the piece - if(tpdata_.status() == ClosestApproachData::converged && niter >= maxiter) - tpdata_.status_ = ClosestApproachData::unconverged; + if(this->tpdata_.status() == ClosestApproachData::converged && niter >= maxiter) + this->tpdata_.status_ = ClosestApproachData::unconverged; // should test explicitly for piece oscillation FIXME! // test if the solution is on a cusp and if so, chose the one with the smallest DOCA TODO } - - - template void PiecewiseClosestApproach::print(std::ostream& ost,int detail) const { - ost << "PiecewiseClosestApproach status " << statusName() << " Doca " << doca() << " +- " << sqrt(docaVar()) - << " dToca " << deltaT() << " +- " << sqrt(tocaVar()) << " cos(theta) " << dirDot() << " Precision " << precision() << std::endl; - if(detail > 0) - ost << "Particle Poca " << particlePoca() << " Sensor Poca " << sensorPoca() << std::endl; - if(detail > 1) - ost << "dDdP " << dDdP() << " dTdP " << dTdP() << std::endl; - if(detail > 2){ - ost << "Particle "; - particleTraj().print(ost,0); - ost << "Sensor "; - sensorTraj().print(ost,0); - } - - } } #endif diff --git a/Trajectory/PiecewiseTrajectory.hh b/Trajectory/PiecewiseTrajectory.hh index 51479a1d..08236fec 100644 --- a/Trajectory/PiecewiseTrajectory.hh +++ b/Trajectory/PiecewiseTrajectory.hh @@ -4,73 +4,94 @@ // class describing a piecewise trajectory. Templated on a simple time-based trajectory // used as part of the kinematic kalman fit // +#include "KinKal/General/CloneContext.hh" #include "KinKal/General/TimeDir.hh" #include "KinKal/General/Vectors.hh" #include "KinKal/General/MomBasis.hh" #include "KinKal/General/TimeRange.hh" #include +#include +#include #include #include #include namespace KinKal { - template class PiecewiseTrajectory { + template class PiecewiseTrajectory { public: - using DTTRAJ = std::deque; + using KTRAJPTR = std::shared_ptr; + using DKTRAJ = std::deque; + using DKTRAJITER = typename DKTRAJ::iterator; + using DKTRAJCITER = typename DKTRAJ::const_iterator; // forward calls to the pieces - void position3(VEC4& pos) const {nearestPiece(pos.T()).position3(pos); } VEC3 position3(double time) const { return nearestPiece(time).position3(time); } VEC3 velocity(double time) const { return nearestPiece(time).velocity(time); } double speed(double time) const { return nearestPiece(time).speed(time); } VEC3 direction(double time, MomBasis::Direction mdir=MomBasis::momdir_) const { return nearestPiece(time).direction(time,mdir); } VEC3 const& bnom(double time) const { return nearestPiece(time).bnom(time); } double t0() const; - TimeRange range() const { if(pieces_.size() > 0) return TimeRange(pieces_.front().range().begin(),pieces_.back().range().end()); else return TimeRange(); } + TimeRange range() const { if(pieces_.size() > 0) return TimeRange(pieces_.front()->range().begin(),pieces_.back()->range().end()); else return TimeRange(); } void setRange(TimeRange const& trange, bool trim=false); // construct without any content. Any functions except append or prepend will throw in this state PiecewiseTrajectory() {} // construct from an initial piece - PiecewiseTrajectory(TTRAJ const& piece); + PiecewiseTrajectory(KTRAJ const& piece); + // clone op for reinstantiation + PiecewiseTrajectory(PiecewiseTrajectory const&); + std::shared_ptr< PiecewiseTrajectory > clone(CloneContext&) const; + PiecewiseTrajectory& operator=(PiecewiseTrajectory const&) = default; // append or prepend a piece, at the time of the corresponding end of the new trajectory. The last // piece will be shortened or extended as necessary to keep time contiguous. // Optionally allow truncate existing pieces to accomodate this piece. // If appending requires truncation and allowremove=false, the piece is not appended and the return code is false - void append(TTRAJ const& newpiece, bool allowremove=false); - void prepend(TTRAJ const& newpiece, bool allowremove=false); - void add(TTRAJ const& newpiece, TimeDir tdir=TimeDir::forwards, bool allowremove=false); + void append(KTRAJ const& newpiece, bool allowremove=false); + void prepend(KTRAJ const& newpiece, bool allowremove=false); + void add(KTRAJ const& newpiece, TimeDir tdir=TimeDir::forwards, bool allowremove=false); + // literally append a piece. + void add(KTRAJPTR const& pieceptr, TimeDir tdir=TimeDir::forwards); // Find the piece associated with a particular time - TTRAJ const& nearestPiece(double time) const { return pieces_[nearestIndex(time)]; } - TTRAJ const& piece(size_t index) const { return pieces_[index]; } - TTRAJ const& front() const { return pieces_.front(); } - TTRAJ const& back() const { return pieces_.back(); } - TTRAJ& front() { return pieces_.front(); } - TTRAJ& back() { return pieces_.back(); } + KTRAJPTR const& nearestTraj(double time) const { return pieces_[nearestIndex(time)]; } + KTRAJPTR const& indexTraj(size_t index) const { return pieces_[index]; } + KTRAJ const& nearestPiece(double time) const { return *pieces_[nearestIndex(time)]; } + KTRAJ const& piece(size_t index) const { return *pieces_[index]; } + KTRAJ const& front() const { return *pieces_.front(); } + KTRAJ const& back() const { return *pieces_.back(); } + KTRAJ& front() { return *pieces_.front(); } + KTRAJ& back() { return *pieces_.back(); } + KTRAJPTR const& frontPtr() const { return pieces_.front(); } + KTRAJPTR const& backPtr() const { return pieces_.back(); } + void pieceRange(TimeRange const& range, DKTRAJCITER& first, DKTRAJCITER& last ) const; + void pieceRange(TimeRange const& range,DKTRAJITER& first, DKTRAJITER& last ); size_t nearestIndex(double time) const; - DTTRAJ const& pieces() const { return pieces_; } + DKTRAJ const& pieces() const { return pieces_; } // test for spatial gaps double gap(size_t ihigh) const; void gaps(double& largest, size_t& ilargest, double& average) const; void print(std::ostream& ost, int detail) const ; private: - DTTRAJ pieces_; // constituent pieces + DKTRAJ pieces_; // constituent pieces }; - template void PiecewiseTrajectory::setRange(TimeRange const& trange, bool trim) { + template void PiecewiseTrajectory::setRange(TimeRange const& trange, bool trim) { // trim pieces as necessary if(trim){ - while(pieces_.size() > 1 && trange.begin() > pieces_.front().range().end() ) pieces_.pop_front(); - while(pieces_.size() > 1 && trange.end() < pieces_.back().range().begin() ) pieces_.pop_back(); - } else if(trange.begin() > pieces_.front().range().end() || trange.end() < pieces_.back().range().begin()) - throw std::invalid_argument("Invalid Range"); - // update piece range - pieces_.front().setRange(TimeRange(trange.begin(),pieces_.front().range().end())); - pieces_.back().setRange(TimeRange(pieces_.back().range().begin(),trange.end())); + auto ipiece = pieces_.begin(); + while (ipiece != pieces_.end() && !(trange.overlaps((*ipiece)->range())))++ipiece; + if(ipiece != pieces_.begin())pieces_.erase(pieces_.begin(),--ipiece); + auto jpiece=pieces_.rbegin(); + while(jpiece != pieces_.rend() && !(trange.overlaps((*jpiece)->range())))++jpiece; + pieces_.erase(jpiece.base(),pieces_.end()); + } else if(trange.begin() > pieces_.front()->range().end() || trange.end() < pieces_.back()->range().begin()) + throw std::invalid_argument("PiecewiseTrajectory::setRange; Invalid Range"); + // update end piece range + pieces_.front()->range().extend(trange.begin(),TimeDir::backwards); + pieces_.back()->range().extend(trange.end(),TimeDir::forwards); } - template PiecewiseTrajectory::PiecewiseTrajectory(TTRAJ const& piece) : pieces_(1,piece) + template PiecewiseTrajectory::PiecewiseTrajectory(KTRAJ const& piece) : pieces_(1,std::make_shared(piece)) {} - template void PiecewiseTrajectory::add(TTRAJ const& newpiece, TimeDir tdir, bool allowremove){ + template void PiecewiseTrajectory::add(KTRAJ const& newpiece, TimeDir tdir, bool allowremove){ switch (tdir) { case TimeDir::forwards: append(newpiece,allowremove); @@ -79,22 +100,22 @@ namespace KinKal { prepend(newpiece,allowremove); break; default: - throw std::invalid_argument("Invalid direction"); + throw std::invalid_argument("PiecewiseTrajectory::add; Invalid direction"); } } - template void PiecewiseTrajectory::prepend(TTRAJ const& newpiece, bool allowremove) { + template void PiecewiseTrajectory::prepend(KTRAJ const& newpiece, bool allowremove) { // new piece can't have null range - if(newpiece.range().null())throw std::invalid_argument("Can't prepend null range traj"); + if(newpiece.range().null())throw std::invalid_argument("PiecewiseTrajectory::prepend; Can't prepend null range traj"); if(pieces_.empty()){ - pieces_.push_back(newpiece); + pieces_.push_back(std::make_shared(newpiece)); } else { // if the new piece completely contains the existing pieces, overwrite or fail if(newpiece.range().contains(range())){ if(allowremove) *this = PiecewiseTrajectory(newpiece); else - throw std::invalid_argument("range overlap"); + throw std::invalid_argument("PiecewiseTrajector::prepend; range overlap"); } else { // find the piece that needs to be modified size_t ipiece = nearestIndex(newpiece.range().end()); @@ -107,29 +128,29 @@ namespace KinKal { // if we're at the start, prepend if(ipiece == 0){ // update ranges and add the piece - double tmin = std::min(newpiece.range().begin(),pieces_.front().range().begin()); - pieces_.front().range() = TimeRange(newpiece.range().end(),pieces_.front().range().end()); - pieces_.push_front(newpiece); - pieces_.front().range() = TimeRange(tmin,pieces_.front().range().end()); + double tmin = std::min(newpiece.range().begin(),pieces_.front()->range().begin()); + pieces_.front()->range() = TimeRange(newpiece.range().end(),pieces_.front()->range().end()); + pieces_.push_front(std::make_shared(newpiece)); + pieces_.front()->range() = TimeRange(tmin,pieces_.front()->range().end()); } else { - throw std::invalid_argument("range error"); + throw std::invalid_argument("PiecewiseTrajectory::prepend; range error"); } } } } - template void PiecewiseTrajectory::append(TTRAJ const& newpiece, bool allowremove) { + template void PiecewiseTrajectory::append(KTRAJ const& newpiece, bool allowremove) { // new piece can't have null range - if(newpiece.range().null())throw std::invalid_argument("Can't append null range traj"); + if(newpiece.range().null())throw std::invalid_argument("PiecewiseTrajectory::append; Can't append null range traj"); if(pieces_.empty()){ - pieces_.push_back(newpiece); + pieces_.push_back(std::make_shared(newpiece)); } else { // if the new piece completely contains the existing pieces, overwrite or fail if(newpiece.range().begin() < range().begin()){ if(allowremove) *this = PiecewiseTrajectory(newpiece); else - throw std::invalid_argument("range overlap"); + throw std::invalid_argument("PiecewiseTrajectory::append; range overlap"); } else { // find the piece that needs to be modified size_t ipiece = nearestIndex(newpiece.range().begin()); @@ -143,19 +164,37 @@ namespace KinKal { if(ipiece == pieces_.size()-1){ // update ranges and add the piece. // first, make sure we don't loose range - double tmax = std::max(newpiece.range().end(),pieces_.back().range().end()); + double tmax = std::max(newpiece.range().end(),pieces_.back()->range().end()); // truncate the range of the current back to match with the start of the new piece. - pieces_.back().range() = TimeRange(pieces_.back().range().begin(),newpiece.range().begin()); - pieces_.push_back(newpiece); - pieces_.back().range() = TimeRange(pieces_.back().range().begin(),tmax); + pieces_.back()->range() = TimeRange(pieces_.back()->range().begin(),newpiece.range().begin()); + pieces_.push_back(std::make_shared(newpiece)); + pieces_.back()->range() = TimeRange(pieces_.back()->range().begin(),tmax); } else { - throw std::invalid_argument("range error"); + throw std::invalid_argument("PiecewiseTrajectory::append; range error"); } } } } - template size_t PiecewiseTrajectory::nearestIndex(double time) const { + template void PiecewiseTrajectory::add(KTRAJPTR const& pieceptr, TimeDir tdir) { + if(pieces_.empty()){ + pieces_.push_back(pieceptr); + } else if(!(this->range().contains(pieceptr->range()))){ + if(tdir == TimeDir::forwards){ + // synchronize phase variables + pieceptr->syncPhi0(*pieces_.back()); + // fine-adjust the time to have 0 gap + pieceptr->setRange(TimeRange(pieces_.back()->range().end(),pieceptr->range().end())); + pieces_.push_back(pieceptr); + } else { + pieceptr->syncPhi0(*pieces_.front()); + pieceptr->setRange(TimeRange(pieceptr->range().begin(),pieces_.front()->range().begin())); + pieces_.push_front(pieceptr); + } + } + } + + template size_t PiecewiseTrajectory::nearestIndex(double time) const { size_t retval; if(pieces_.empty())throw std::length_error("Empty PiecewiseTrajectory!"); if(time <= range().begin()){ @@ -165,7 +204,7 @@ namespace KinKal { } else { // scan retval = 0; - while(retval < pieces_.size() && (!pieces_[retval].range().inRange(time)) && time > pieces_[retval].range().end()){ + while(retval < pieces_.size() && (!pieces_[retval]->range().inRange(time)) && time > pieces_[retval]->range().end()){ retval++; } if(retval == pieces_.size())throw std::range_error("Failed PTraj range search"); @@ -173,19 +212,19 @@ namespace KinKal { return retval; } - template double PiecewiseTrajectory::gap(size_t ihigh) const { + template double PiecewiseTrajectory::gap(size_t ihigh) const { double retval(0.0); if(ihigh>0 && ihigh < pieces_.size()){ - double jtime = pieces_[ihigh].range().begin(); // time of the junction of this piece with its preceeding piece + double jtime = pieces_[ihigh]->range().begin(); // time of the junction of this piece with its preceeding piece VEC3 p0,p1; - p0 = pieces_[ihigh].position3(jtime); - p1 = pieces_[ihigh-1].position3(jtime); + p0 = pieces_[ihigh]->position3(jtime); + p1 = pieces_[ihigh-1]->position3(jtime); retval = (p1 - p0).R(); } return retval; } - template void PiecewiseTrajectory::gaps(double& largest, size_t& ilargest, double& average) const { + template void PiecewiseTrajectory::gaps(double& largest, size_t& ilargest, double& average) const { largest = average = 0.0; ilargest =0; // loop over adjacent pairs @@ -201,27 +240,27 @@ namespace KinKal { average /= (pieces_.size()-1); } - template double PiecewiseTrajectory::t0() const { + template double PiecewiseTrajectory::t0() const { // find a self-consistent t0 if(pieces_.empty())throw std::length_error("Empty PiecewiseTrajectory!"); - double t0 = pieces_.front().t0(); + double t0 = pieces_.front()->t0(); auto index = nearestIndex(t0); int oldindex = -1; unsigned niter(0); while(static_cast(index) != oldindex && niter < 10){ ++niter; - t0 = pieces_[index].t0(); + t0 = pieces_[index]->t0(); oldindex = index; index = nearestIndex(t0); } return t0; } - template void PiecewiseTrajectory::print(std::ostream& ost, int detail) const { + template void PiecewiseTrajectory::print(std::ostream& ost, int detail) const { double maxgap, avggap; size_t igap; gaps(maxgap,igap,avggap); - ost << "PiecewiseTrajectory of " << TTRAJ::trajName() << " with " << range() << " pieces " << pieces().size() << " gaps max "<< maxgap << " avg " << avggap << std::endl; + ost << "PiecewiseTrajectory of " << KTRAJ::trajName() << " with " << range() << " pieces " << pieces().size() << " gaps max "<< maxgap << " avg " << avggap << std::endl; if(detail ==1 && pieces().size() > 0){ ost << "Front "; front().print(ost,detail); @@ -233,16 +272,63 @@ namespace KinKal { unsigned ipiece(0); for (auto const& piece : pieces_) { ost << "Piece " << ipiece++ << " "; - piece.print(ost,detail); + piece->print(ost,detail); } } } - template std::ostream& operator <<(std::ostream& ost, PiecewiseTrajectory const& pttraj) { + template std::ostream& operator <<(std::ostream& ost, PiecewiseTrajectory const& pttraj) { pttraj.print(ost,0); return ost; } + template void PiecewiseTrajectory::pieceRange(TimeRange const& range, + typename std::deque>::const_iterator &first, + typename std::deque>::const_iterator &last) const { + first = last = pieces_.end(); + // check for no overlap + if(this->range().overlaps(range)){ + // find the first and last pieces which overlap with the range. They can be the same piece. + first = pieces_.cbegin(); + while(first != pieces_.cend() && !((*first)->range().overlaps(range))) ++first; + auto rlast = pieces_.crbegin(); + while(rlast != pieces_.crend() && !((*rlast)->range().overlaps(range))) ++rlast; + last = (rlast+1).base(); // convert back to forwards-iterator + } + } + + template void PiecewiseTrajectory::pieceRange(TimeRange const& range, + typename std::deque>::iterator &first, + typename std::deque>::iterator &last) { + first = last = pieces_.end(); + // check for no overlap + if(this->range().overlaps(range)){ + first = pieces_.begin(); + while(first != pieces_.end() && !((*first)->range().overlaps(range))) ++first; + auto rlast = pieces_.rbegin(); + while(rlast != pieces_.rend() && !((*rlast)->range().overlaps(range))) ++rlast; + last= (rlast+1).base(); + } + } + + // clone op for reinstantiation + template + PiecewiseTrajectory::PiecewiseTrajectory(PiecewiseTrajectory const& rhs){ + for (const auto& ptr: rhs.pieces()){ + auto piece = std::make_shared(*ptr); + pieces_.push_back(piece); + } + } + + template + std::shared_ptr< PiecewiseTrajectory > PiecewiseTrajectory::clone(CloneContext& context) const { + auto rv = std::make_shared< PiecewiseTrajectory >(); + for (auto const& ptr : pieces_){ + auto piece = context.get(ptr); + rv.pieces_.push_back(*piece); + } + return rv; + } } #endif diff --git a/Trajectory/PointClosestApproach.hh b/Trajectory/PointClosestApproach.hh index 070a2478..8e7fb21f 100644 --- a/Trajectory/PointClosestApproach.hh +++ b/Trajectory/PointClosestApproach.hh @@ -23,9 +23,12 @@ namespace KinKal { // construct from the particle and sensor trajectories; TCA is computed on construction, given a hint as to where // to start looking, which disambiguates functions with multiple solutions PointClosestApproach(KTRAJ const& ktraj, VEC4 const& point, PCAHint const& hint, double precision); - // construct without a hint: TCA isn't calculated, state is invalid + // construct without a hint PointClosestApproach(KTRAJ const& ptraj, VEC4 const& point, double precision); - // accessors + // construct with full data + PointClosestApproach(KTRAJ const& ptraj, VEC4 const& point, double precision, + ClosestApproachData const& tpdata, DVEC const& dDdP, DVEC const& dTdP); + // accessors ClosestApproachData const& tpData() const { return tpdata_; } KTRAJ const& particleTraj() const { return ktraj_; } // derviatives of TOCA and DOCA WRT particle trajectory parameters @@ -55,8 +58,8 @@ namespace KinKal { // calculate CA given the hint, and fill the state private: double precision_; // precision used to define convergence - ClosestApproachData tpdata_; // data payload of CA calculation KTRAJ const& ktraj_; // kinematic particle trajectory + ClosestApproachData tpdata_; // data payload of CA calculation DVEC dDdP_; // derivative of DOCA WRT Parameters DVEC dTdP_; // derivative of TOCA WRT Parameters }; @@ -64,6 +67,10 @@ namespace KinKal { template PointClosestApproach::PointClosestApproach(KTRAJ const& ktraj, VEC4 const& point, double prec) : PointClosestApproach(ktraj,point,PCAHint(point.T()), prec) {} + template PointClosestApproach::PointClosestApproach(KTRAJ const& ktraj, VEC4 const& point, double prec, + ClosestApproachData const& tpdata, DVEC const& dDdP, DVEC const& dTdP) : + precision_(prec), ktraj_(ktraj), tpdata_(tpdata), dDdP_(dDdP), dTdP_(dTdP) {} + template PointClosestApproach::PointClosestApproach(KTRAJ const& ktraj, VEC4 const& point, PCAHint const& hint, double prec) : precision_(prec), ktraj_(ktraj) { // sensor CA is fixed to the point diff --git a/Trajectory/PointPiecewiseClosestApproach.hh b/Trajectory/PointPiecewiseClosestApproach.hh index 5de70574..24c88da2 100644 --- a/Trajectory/PointPiecewiseClosestApproach.hh +++ b/Trajectory/PointPiecewiseClosestApproach.hh @@ -8,92 +8,53 @@ #include namespace KinKal { - template class PointPiecewiseClosestApproach { + template class PointPiecewiseClosestApproach : public PointClosestApproach> { public: - using PKTRAJ = ParticleTrajectory; + using PTRAJ = ParticleTrajectory; using KTCA = PointClosestApproach; // the constructor is the only non-inherited function - PointPiecewiseClosestApproach(PKTRAJ const& pktraj, VEC4 const& point, PCAHint const& hint, double precision); - PointPiecewiseClosestApproach(PKTRAJ const& pktraj, VEC4 const& point, double precision); - // copy the TCA interface. This is ugly and a maintenance burden, but avoids inheritance problems - ClosestApproachData::TPStat status() const { return tpdata_.status(); } - std::string const& statusName() const { return tpdata_.statusName(); } - double doca() const { return tpdata_.doca(); } - double docaVar() const { return tpdata_.docaVar(); } - double tocaVar() const { return tpdata_.tocaVar(); } - double dirDot() const { return tpdata_.dirDot(); } - double deltaT() const { return tpdata_.deltaT(); } - bool usable() const { return tpdata_.usable(); } - double particleToca() const { return tpdata_.particleToca(); } - double sensorToca() const { return tpdata_.sensorToca(); } - double lSign() const { return tpdata_.lsign_; } // sign of angular momentum - VEC4 const& particlePoca() const { return tpdata_.particlePoca(); } - VEC4 const& point() const { return tpdata_.sensorPoca(); } - VEC4 delta() const { return tpdata_.delta(); } - VEC3 const& particleDirection() const { return tpdata_.particleDirection(); } - VEC3 const& pointDirection() const { return tpdata_.sensorDirection(); } - ClosestApproachData const& tpData() const { return tpdata_; } - PKTRAJ const& particleTraj() const { return pktraj_; } + PointPiecewiseClosestApproach(PTRAJ const& ptraj, VEC4 const& point, PCAHint const& hint, double precision); + PointPiecewiseClosestApproach(PTRAJ const& ptraj, VEC4 const& point, double precision); + // provide access to the local (non-piecewise) information implicit in this class size_t particleTrajIndex() const { return pindex_; } - DVEC const& dDdP() const { return dDdP_; } - DVEC const& dTdP() const { return dTdP_; } - bool inRange() const { return particleTraj().inRange(particleToca()); } - double precision() const { return precision_; } - void print(std::ostream& ost=std::cout,int detail=0) const; - private: - double precision_; // precision used to define convergence - ClosestApproachData tpdata_; // data payload of CA calculation - PKTRAJ const& pktraj_; + KTRAJ const& localParticleTraj() const { return this->particleTraj().piece(pindex_); } + KTCA localClosestApproach() const { return KTCA(localParticleTraj(),this->point(),this->precision(),this->tpData(),this->dDdP(),this->dTdP()); } + private: size_t pindex_; // indices to the local traj used in TCA calculation - DVEC dDdP_; - DVEC dTdP_; }; - template PointPiecewiseClosestApproach::PointPiecewiseClosestApproach(PKTRAJ const& pktraj, VEC4 const& point, double prec) : - PointPiecewiseClosestApproach(pktraj,point, PCAHint(point.T()), prec) {} + template PointPiecewiseClosestApproach::PointPiecewiseClosestApproach(PTRAJ const& ptraj, VEC4 const& point, double prec) : + PointPiecewiseClosestApproach(ptraj,point, PCAHint(point.T()), prec) {} // iteratively find the nearest piece, and CA for that piece. Start at hints if availalble, otherwise the middle - template PointPiecewiseClosestApproach::PointPiecewiseClosestApproach(PKTRAJ const& pktraj, VEC4 const& point, PCAHint const& hint, double prec) : precision_(prec), pktraj_(pktraj){ + template PointPiecewiseClosestApproach::PointPiecewiseClosestApproach(PTRAJ const& ptraj, VEC4 const& point, PCAHint const& hint, double prec) : + KTCA(ptraj,point,prec) { // iteratively find the nearest piece, and CA for that piece. Start at hints if availalble, otherwise the middle static const unsigned maxiter=10; // don't allow infinite iteration. This should be a parameter FIXME! unsigned niter=0; - size_t oldindex= pktraj_.pieces().size(); - pindex_ = pktraj_.nearestIndex(hint.particleToca_); + size_t oldindex= this->particleTraj().pieces().size(); + pindex_ = this->particleTraj().nearestIndex(hint.particleToca_); // copy over the hint: it needs to evolve PCAHint phint = hint; // iterate until TCA is on the same piece do{ - KTCA tpoca(pktraj_.piece(pindex_),point,phint,prec); + KTCA tpoca(this->particleTraj().piece(pindex_),this->point(),phint,this->precision()); // copy the state - tpdata_ = tpoca.tpData(); - dDdP_ = tpoca.dDdP(); - dTdP_ = tpoca.dTdP(); + this->tpdata_ = tpoca.tpData(); + this->dDdP_ = tpoca.dDdP(); + this->dTdP_ = tpoca.dTdP(); // inrange = tpoca.inRange(); // update the hint phint.particleToca_ = tpoca.particleToca(); // update the piece (if needed) oldindex = pindex_; - pindex_ = pktraj_.nearestIndex(tpoca.particlePoca().T()); + pindex_ = this->particleTraj().nearestIndex(tpoca.particlePoca().T()); } while( pindex_ != oldindex && usable() && niter++ < maxiter); // overwrite the status if we oscillated on the piece - if(tpdata_.status() == ClosestApproachData::converged && niter >= maxiter) - tpdata_.status_ = ClosestApproachData::unconverged; + if(this->tpdata_.status() == ClosestApproachData::converged && niter >= maxiter) + this->tpdata_.status_ = ClosestApproachData::unconverged; // should test explicitly for piece oscillation FIXME! // test if the solution is on a cusp and if so, chose the one with the smallest DOCA TODO - } - - - template void PointPiecewiseClosestApproach::print(std::ostream& ost,int detail) const { - ost << "PointPiecewiseClosestApproach status " << statusName() << " Doca " << doca() << " +- " << sqrt(docaVar()) - << " dToca " << deltaT() << " +- " << sqrt(tocaVar()) << " cos(theta) " << dirDot() << " Precision " << precision() << std::endl; - if(detail > 0) - ost << "Particle Poca " << particlePoca() << " Point " << point() << std::endl; - if(detail > 1) - ost << "dDdP " << dDdP() << " dTdP " << dTdP() << std::endl; - if(detail > 2){ - ost << "Particle "; - particleTraj().print(ost,0); - } } } diff --git a/Trajectory/SensorLine.cc b/Trajectory/SensorLine.cc new file mode 100644 index 00000000..f5fab7ff --- /dev/null +++ b/Trajectory/SensorLine.cc @@ -0,0 +1,53 @@ +#include "KinKal/Trajectory/SensorLine.hh" +#include "KinKal/Trajectory/ConstantDistanceToTime.hh" +#include +#include +#include +#include +#include + +using namespace std; +using namespace ROOT::Math; + +namespace KinKal { + + SensorLine::SensorLine(VEC4 const& mpos, VEC3 const& svel, double length ) : SensorLine(mpos.Vect(), mpos.T(), svel, length) {} + +// note the measurement position is at the END of the directed line segment, since that points in the direction of signal propagation + SensorLine::SensorLine(VEC3 const& mpos, double mtime , VEC3 const& svel, double length ) : + mtime_(mtime), d2t_(new ConstantDistanceToTime(svel.R())), lineseg_(mpos-svel.unit()*length, mpos) {} + + SensorLine::SensorLine(VEC3 const& mpos, VEC3 const& endpos, double mtime, double speed ) : mtime_(mtime), + d2t_(new ConstantDistanceToTime(speed)), lineseg_(endpos,mpos) {} + + SensorLine::SensorLine(VEC3 const& mpos, double mtime, VEC3 const& svel, double length, std::shared_ptr d) : mtime_(mtime), + d2t_(d), lineseg_(mpos-svel.unit()*length, mpos) {} + + VEC3 SensorLine::position3(double time) const { + return lineseg_.endPosition(d2t_->distance(mtime_ - time)); + } + + VEC4 SensorLine::position4(double time) const { + VEC3 pos3 = position3(time); + return VEC4(pos3.X(),pos3.Y(),pos3.Z(),time); + } + + VEC3 SensorLine::velocity(double time) const { + return direction(time)*speed(time); + } + + double SensorLine::timeTo(VEC3 const& point) const { + double s = lineseg_.distanceTo(point); + return s/speed(s) - measurementTime(); + } + + void SensorLine::print(std::ostream& ost, int detail) const { + ost << *this << endl; + } + + std::ostream& operator <<(std::ostream& ost, SensorLine const& sline) { + ost << "SensorLine " << sline.line() << " measurement time " << sline.measurementTime(); + return ost; + } + +} diff --git a/Trajectory/SensorLine.hh b/Trajectory/SensorLine.hh new file mode 100644 index 00000000..9e8eadc6 --- /dev/null +++ b/Trajectory/SensorLine.hh @@ -0,0 +1,49 @@ +#ifndef KinKal_SensorLine_hh +#define KinKal_SensorLine_hh +// +// Class describing linear signal propagation in a sensor with an arbitrary distance-to-time relationship +// +#include +#include "KinKal/General/Vectors.hh" +#include "KinKal/Trajectory/DistanceToTime.hh" +#include "KinKal/Geometry/LineSegment.hh" + +namespace KinKal { + class SensorLine { + public: + // construct from the measurement position and time and signal propagation velocity (mm/ns). Note the velocity points TOWARDS the sensor + SensorLine(VEC4 const& mpos, VEC3 const& svel, double length); + SensorLine(VEC3 const& mpos, double mtime, VEC3 const& svel, double length); + // construct from measurement position and time, and opposite (far) end of the sensor. Signals propagate from the far end to the measurement position with the given speed + SensorLine(VEC3 const& mpos, VEC3 const& endpos, double mtime, double speed ); + SensorLine(VEC3 const& mpos, double mtime, VEC3 const& svel, double length, std::shared_ptr d2t); + // accessors + auto const& measurementPosition() const { return lineseg_.end(); } // measurement is at the end (latest time) + double measurementTime() const { return mtime_; } + double& measurementTime() { return mtime_; } // Fit updates need to refine this + // LineSegment positions + auto const& start() const { return lineseg_.start(); } + auto const& end() const { return lineseg_.end(); } + auto middle() const { return lineseg_.middle(); } + // + auto const& line() const { return lineseg_; } + double speed(double time) const { return d2t_->speed(d2t_->distance(mtime_ - time)); } // D2T is relative to measurement end + double length() const { return lineseg_.length(); } + VEC3 const& direction() const { return lineseg_.direction(); } + // time to a point + double timeTo(VEC3 const& point) const; + // geometric accessors + VEC3 position3(double time) const; + VEC4 position4(double time) const; + VEC3 velocity(double time) const; // signal velocity + VEC3 const& direction(double time) const { return lineseg_.direction(); } + void print(std::ostream& ost, int detail) const; + double timeAtMidpoint() const { return mtime_ - d2t_->time(0.5*length()); } + private: + double mtime_; // measurement time, at the far end of the directed line segment + std::shared_ptr d2t_; // distance to time relationship along the sensor + LineSegment lineseg_; // linear representation of the sensor + }; + std::ostream& operator <<(std::ostream& ost, SensorLine const& sline); +} +#endif diff --git a/cmake/KinKalConfig.cmake.in b/cmake/KinKalConfig.cmake.in new file mode 100755 index 00000000..e6740cac --- /dev/null +++ b/cmake/KinKalConfig.cmake.in @@ -0,0 +1,8 @@ +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/KinKalTargets.cmake") + +include(CMakeFindDependencyMacro) +find_dependency(ROOT) + +check_required_components(KinKal) \ No newline at end of file