| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #pragma once | ||
| 2 | |||
| 3 | #include "na64sw-config.h" | ||
| 4 | |||
| 5 | #include "na64util/numerical/spatialRotations.h" | ||
| 6 | |||
| 7 | #include <iostream> | ||
| 8 | #include <cmath> | ||
| 9 | #include <cassert> | ||
| 10 | |||
| 11 | /**\file | ||
| 12 | * \brief Basic spatial vectors | ||
| 13 | * | ||
| 14 | * This header defines few simple linear algebra types and operations. The | ||
| 15 | * goal is to facilitate some very basic operations commonly met in the NA64sw, | ||
| 16 | * to minimize dependencies. The subset is not meant to provide | ||
| 17 | * precision or efficiency (use specialized libs for massive algebra) -- where | ||
| 18 | * possible it is recommended to rely on third-party libs or frameworks (GSL, | ||
| 19 | * LAPAC, Eigen3, ROOT, etc) | ||
| 20 | * | ||
| 21 | * \todo Since we anyway strictly depend on GSL, foresee precise routines | ||
| 22 | * | ||
| 23 | * \ingroup numerical-utils | ||
| 24 | * */ | ||
| 25 | |||
| 26 | ///\defgroup numerical-utils Numerical utility types and functions | ||
| 27 | |||
| 28 | namespace na64dp { | ||
| 29 | namespace util { | ||
| 30 | |||
| 31 | template<typename FloatT=Float_t> | ||
| 32 | union Matrix3T; | ||
| 33 | |||
| 34 | ///\brief 3-dim spatial vector representation used in the lib | ||
| 35 | /// | ||
| 36 | ///\ingroup numerical-utils | ||
| 37 | template<typename FloatT=Float_t> | ||
| 38 | union Vec3T { | ||
| 39 | /// Data acessed by index | ||
| 40 | FloatT r[3]; | ||
| 41 | /// Data acessed by component name | ||
| 42 | struct { FloatT x, y, z; } c; | ||
| 43 | |||
| 44 | /// Component getter | ||
| 45 | 54 | FloatT operator[](int i) const { return r[i]; } | |
| 46 | /// Component reference getter | ||
| 47 | FloatT & operator[](int i) { return r[i]; } | ||
| 48 | |||
| 49 | /// Scalar product | ||
| 50 | template<typename FloatT2> | ||
| 51 | FloatT operator*(const Vec3T<FloatT2> & b) const | ||
| 52 | { return c.x*b.c.x + c.y*b.c.y + c.z*b.c.z; } | ||
| 53 | /// Vector multiplied by number | ||
| 54 | template<typename FloatT2> | ||
| 55 | 12 | Vec3T<FloatT> operator*(FloatT2 a) const | |
| 56 | 12 | { return {{ static_cast<FloatT>(a*c.x) | |
| 57 | 12 | , static_cast<FloatT>(a*c.y) | |
| 58 | 12 | , static_cast<FloatT>(a*c.z) | |
| 59 | 12 | }}; } | |
| 60 | /// In-place multiplication by number | ||
| 61 | template<typename FloatT2> | ||
| 62 | Vec3T<FloatT> & operator*=(FloatT2 a) | ||
| 63 | { r[0] *= a; r[1] *= a; r[2] *= a; | ||
| 64 | return *this; } | ||
| 65 | /// Vector divided by number | ||
| 66 | template<typename FloatT2> | ||
| 67 | Vec3T<FloatT> operator/(FloatT2 a) const | ||
| 68 | { return {{c.x/a, c.y/a, c.z/a}}; } | ||
| 69 | /// In-place division by number | ||
| 70 | template<typename FloatT2> | ||
| 71 | Vec3T<FloatT> & operator/=(FloatT2 a) | ||
| 72 | { r[0] /= a; r[1] /= a; r[2] /= a; | ||
| 73 | return *this; } | ||
| 74 | /// Vector sum | ||
| 75 | template<typename FloatT2> | ||
| 76 | 21 | Vec3T<FloatT> operator+(const Vec3T<FloatT2> & b) const | |
| 77 | 21 | { return Vec3T{c.x + b.c.x, c.y + b.c.y, c.z + b.c.z}; } | |
| 78 | /// In-place vector sum | ||
| 79 | template<typename FloatT2> | ||
| 80 | Vec3T<FloatT> & operator+=(const Vec3T<FloatT2> & b) | ||
| 81 | { c.x += b.c.x; c.y += b.c.y; c.z += b.c.z; | ||
| 82 | return *this; } | ||
| 83 | /// Vector sub | ||
| 84 | template<typename FloatT2> | ||
| 85 | 22 | Vec3T<FloatT> operator-(const Vec3T<FloatT2> & b) const | |
| 86 | 22 | { return Vec3T{{c.x - b.c.x, c.y - b.c.y, c.z - b.c.z}}; } | |
| 87 | /// In-place vector sub | ||
| 88 | template<typename FloatT2> | ||
| 89 | Vec3T<FloatT> & operator-=(const Vec3T<FloatT2> & b) | ||
| 90 | { c.x -= b.c.x; c.y -= b.c.y; c.z -= b.c.z; | ||
| 91 | return *this; } | ||
| 92 | /// Vector cross product | ||
| 93 | template<typename FloatT2> | ||
| 94 | Vec3T<FloatT> cross(const Vec3T<FloatT2> & b) const { | ||
| 95 | return Vec3T{{ | ||
| 96 | // a1b2 - a2b1 | ||
| 97 | r[1]*b.r[2] - r[2]*b.r[1], | ||
| 98 | // a2b0 - a0b2 | ||
| 99 | r[2]*b.r[0] - r[0]*b.r[2], | ||
| 100 | // a0b1 - a1b0 | ||
| 101 | r[0]*b.r[1] - r[1]*b.r[0], | ||
| 102 | }}; | ||
| 103 | } | ||
| 104 | /// Returns norm of the vector | ||
| 105 | 36 | FloatT norm() const { | |
| 106 | 36 | return sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]); | |
| 107 | } | ||
| 108 | /// Returns unit (orth) of this vector | ||
| 109 | Vec3T<FloatT> unit() const { | ||
| 110 | Float_t nrm = norm(); | ||
| 111 | if(!nrm) return *this; // zero vector | ||
| 112 | return {{ r[0]/nrm, r[1]/nrm, r[2]/nrm }}; | ||
| 113 | } | ||
| 114 | /// Unary minus (inversion) operator | ||
| 115 | Vec3T<FloatT> operator-() const { | ||
| 116 | return Vec3T{{-c.x, -c.y, -c.z}}; | ||
| 117 | } | ||
| 118 | }; | ||
| 119 | |||
| 120 | ///\brief Print operator | ||
| 121 | template<typename FloatT> std::ostream & | ||
| 122 | operator<<(std::ostream & os, const Vec3T<FloatT> & v) { | ||
| 123 | os << "{" /*<< std::setprecision(2)*/ << v.r[0] | ||
| 124 | << ", " << v.r[1] | ||
| 125 | << ", " << v.r[2] | ||
| 126 | << "}"; | ||
| 127 | return os; | ||
| 128 | } | ||
| 129 | |||
| 130 | ///\brief 3x3 matrix representation | ||
| 131 | /// | ||
| 132 | ///\ingroup numerical-utils | ||
| 133 | template<typename FloatT> | ||
| 134 | union Matrix3T { | ||
| 135 | /// Data representations | ||
| 136 | FloatT cm[3][3]; | ||
| 137 | FloatT m[9]; | ||
| 138 | |||
| 139 | /// Returns RO-ptr to the 1st element in Nth row | ||
| 140 | 705 | const FloatT * operator[](int n) const { | |
| 141 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 705 times.
|
705 | assert(n >= 0); |
| 142 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 705 times.
|
705 | assert(n < 3); |
| 143 | 705 | return m + 3*n; | |
| 144 | } | ||
| 145 | /// Returns RW-ptr to the 1st element in Nth row | ||
| 146 | 99 | FloatT * operator[](int n) { | |
| 147 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 99 times.
|
99 | assert(n >= 0); |
| 148 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 99 times.
|
99 | assert(n < 3); |
| 149 | 99 | return m + 3*n; | |
| 150 | } | ||
| 151 | |||
| 152 | /// Matrix.Vector product | ||
| 153 | template<typename FloatT2> | ||
| 154 | 19 | Vec3T<FloatT> operator*(const Vec3T<FloatT2> & v) const { | |
| 155 | Vec3T<FloatT> p; | ||
| 156 |
2/2✓ Branch 0 taken 57 times.
✓ Branch 1 taken 19 times.
|
76 | for( int j = 0; j < 3; ++j ) { |
| 157 | 57 | p.r[j] = 0.; | |
| 158 |
2/2✓ Branch 0 taken 171 times.
✓ Branch 1 taken 57 times.
|
228 | for( int i = 0; i < 3; ++i ) { |
| 159 | 171 | p.r[j] += v.r[i]*operator[](j)[i]; | |
| 160 | } | ||
| 161 | } | ||
| 162 | 19 | return p; | |
| 163 | } | ||
| 164 | /// Matrix.Matrix product | ||
| 165 | template<typename FloatT2> | ||
| 166 | 8 | Matrix3T<FloatT> operator*(const Matrix3T<FloatT2> & bm) const { | |
| 167 | Matrix3T rm; | ||
| 168 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 8 times.
|
32 | for( int j = 0; j < 3; ++j ) { |
| 169 |
2/2✓ Branch 0 taken 72 times.
✓ Branch 1 taken 24 times.
|
96 | for( int i = 0; i < 3; ++i ) { |
| 170 | 72 | rm.operator[](i)[j] = 0.; | |
| 171 |
2/2✓ Branch 0 taken 216 times.
✓ Branch 1 taken 72 times.
|
288 | for( int k = 0; k < 3; ++k ) { |
| 172 | 216 | rm.cm[i][j] += operator[](i)[k]*bm[k][j]; | |
| 173 | } | ||
| 174 | } | ||
| 175 | } | ||
| 176 | 8 | return rm; | |
| 177 | } | ||
| 178 | /// Matrix determinant | ||
| 179 | /// | ||
| 180 | /// Directly computes matrix determinant based on first line. | ||
| 181 | /// | ||
| 182 | ///\warning Must not be used for general purpose as can be numerically | ||
| 183 | /// unstable for small values in first line. | ||
| 184 | 2 | FloatT det() const { | |
| 185 | 2 | const auto & M = *this; | |
| 186 | // a00(a22a11-a21a12) | ||
| 187 | // - a10(a22a01-a21a02) | ||
| 188 | // + a20(a12a01-a11a02) | ||
| 189 | 2 | return M[0][0]*(M[2][2]*M[1][1] - M[2][1]*M[1][2]) | |
| 190 | 2 | - M[1][0]*(M[2][2]*M[0][1] - M[2][1]*M[0][2]) | |
| 191 | 2 | + M[2][0]*(M[1][2]*M[0][1] - M[1][1]*M[0][2]) | |
| 192 | ; | ||
| 193 | } | ||
| 194 | ///\brief Returns inverted matrix | ||
| 195 | /// | ||
| 196 | ///\warning Uses direct calculus for inversion, not recommended for general | ||
| 197 | /// use as it is numerically unstable. | ||
| 198 | Matrix3T<FloatT> inv() const; | ||
| 199 | /// Returns transposed matrix | ||
| 200 | Matrix3T<FloatT> transposed() const; | ||
| 201 | }; | ||
| 202 | |||
| 203 | // | ||
| 204 | // Matrix method implementation | ||
| 205 | |||
| 206 | template<typename FloatT> | ||
| 207 | Matrix3T<FloatT> | ||
| 208 | 2 | Matrix3T<FloatT>::inv() const { | |
| 209 | 2 | FloatT d = det(); | |
| 210 | 2 | const auto & M = *this; | |
| 211 | // | a22a11-a21a12 -(a22a01-a21a02) a12a01-a11a02 | | ||
| 212 | // |-(a22a10-a20a12) a22a00-a21a02 -(a12a00-a10a02)| | ||
| 213 | // | a21a10-a20a11 -(a21a00-a20a01) a11a00-a10a01 | | ||
| 214 | 2 | return Matrix3T{ {{ ( M[2][2]*M[1][1] - M[2][1]*M[1][2] )/d | |
| 215 | 2 | , -( M[2][2]*M[0][1] - M[2][1]*M[0][2] )/d | |
| 216 | 2 | , ( M[1][2]*M[0][1] - M[1][1]*M[0][2] )/d | |
| 217 | } | ||
| 218 | 2 | , { -( M[2][2]*M[1][0] - M[2][0]*M[1][2] )/d | |
| 219 | 2 | , ( M[2][2]*M[0][0] - M[2][0]*M[0][2] )/d | |
| 220 | 2 | , -( M[1][2]*M[0][0] - M[1][0]*M[0][2] )/d | |
| 221 | } | ||
| 222 | 2 | , { ( M[2][1]*M[1][0] - M[2][0]*M[1][1] )/d | |
| 223 | 2 | , -( M[2][1]*M[0][0] - M[2][0]*M[0][1] )/d | |
| 224 | 2 | , ( M[1][1]*M[0][0] - M[1][0]*M[0][1] )/d | |
| 225 | } | ||
| 226 | 18 | } }; | |
| 227 | } | ||
| 228 | |||
| 229 | // | ||
| 230 | // Common utilities | ||
| 231 | |||
| 232 | /// Returns rotation matrix over `axis` by `angle` | ||
| 233 | template<typename FloatT=Float_t> Matrix3T<FloatT> | ||
| 234 | 3 | arbitrary_rotation_matrix(const Vec3T<FloatT> & axis, FloatT angle) { | |
| 235 | 3 | const FloatT l = axis.r[0] | |
| 236 | 3 | , p = axis.r[1] | |
| 237 | 3 | , n = axis.r[2] | |
| 238 | 3 | , cost = cos(angle) | |
| 239 | 3 | , sint = sin(angle) | |
| 240 | 3 | , cost1 = 1 - cost | |
| 241 | ; | ||
| 242 | Matrix3T<FloatT> om; | ||
| 243 | |||
| 244 | 3 | om[0][0] = l*l*cost1 + cost; | |
| 245 | 3 | om[0][1] = p*l*cost1 - n*sint; | |
| 246 | 3 | om[0][2] = n*l*cost1 + p*sint; | |
| 247 | |||
| 248 | 3 | om[1][0] = l*p*cost1 + n*sint; | |
| 249 | 3 | om[1][1] = p*p*cost1 + cost; | |
| 250 | 3 | om[1][2] = n*p*cost1 - l*sint; | |
| 251 | |||
| 252 | 3 | om[2][0] = l*n*cost1 - p*sint; | |
| 253 | 3 | om[2][1] = p*n*cost1 + l*sint; | |
| 254 | 3 | om[2][2] = n*n*cost1 + cost; | |
| 255 | |||
| 256 | 3 | return om; | |
| 257 | } | ||
| 258 | |||
| 259 | //std::ostream & operator<<(std::ostream & os, const Matrix3T & m); | ||
| 260 | |||
| 261 | /// Framework-wide standard spatial vector type | ||
| 262 | typedef Vec3T<Float_t> Vec3; | ||
| 263 | /// Framework-wide standard 3x3 matrix type | ||
| 264 | typedef Matrix3T<Float_t> Matrix3; | ||
| 265 | |||
| 266 | } // namespace ::na64dp::util | ||
| 267 | } // namespace ::na64dp | ||
| 268 | |||
| 269 |