Geant4 Cross Reference |
1 // -*- C++ -*- 1 // -*- C++ -*- >> 2 // $Id:$ 2 // ------------------------------------------- 3 // --------------------------------------------------------------------------- 3 4 4 #include <cmath> 5 #include <cmath> 5 #include <float.h> 6 #include <float.h> 6 #include <iostream> 7 #include <iostream> 7 #include "CLHEP/Geometry/BasicVector3D.h" 8 #include "CLHEP/Geometry/BasicVector3D.h" 8 9 9 namespace HepGeom { 10 namespace HepGeom { 10 //------------------------------------------ 11 //-------------------------------------------------------------------------- 11 template<> 12 template<> 12 float BasicVector3D<float>::pseudoRapidity() 13 float BasicVector3D<float>::pseudoRapidity() const { 13 float ma = mag(), dz = z(); 14 float ma = mag(), dz = z(); 14 if (ma == 0) return 0; 15 if (ma == 0) return 0; 15 if (ma == dz) return FLT_MAX; 16 if (ma == dz) return FLT_MAX; 16 if (ma == -dz) return -FLT_MAX; 17 if (ma == -dz) return -FLT_MAX; 17 return 0.5*std::log((ma+dz)/(ma-dz)); 18 return 0.5*std::log((ma+dz)/(ma-dz)); 18 } 19 } 19 20 20 //------------------------------------------ 21 //-------------------------------------------------------------------------- 21 template<> 22 template<> 22 void BasicVector3D<float>::setEta(float a) { 23 void BasicVector3D<float>::setEta(float a) { 23 double ma = mag(); 24 double ma = mag(); 24 if (ma == 0) return; 25 if (ma == 0) return; 25 double tanHalfTheta = std::exp(-a); 26 double tanHalfTheta = std::exp(-a); 26 double tanHalfTheta2 = tanHalfTheta * tanH 27 double tanHalfTheta2 = tanHalfTheta * tanHalfTheta; 27 double cosTheta1 = (1 - tanHalfTheta2 28 double cosTheta1 = (1 - tanHalfTheta2) / (1 + tanHalfTheta2); 28 double rh = ma * std::sqrt(1 - 29 double rh = ma * std::sqrt(1 - cosTheta1*cosTheta1); 29 double ph = phi(); 30 double ph = phi(); 30 set(rh*std::cos(ph), rh*std::sin(ph), ma*c 31 set(rh*std::cos(ph), rh*std::sin(ph), ma*cosTheta1); 31 } 32 } 32 33 33 //------------------------------------------ 34 //-------------------------------------------------------------------------- 34 template<> 35 template<> 35 float BasicVector3D<float>::angle(const Basi 36 float BasicVector3D<float>::angle(const BasicVector3D<float> & v) const { 36 double cosa = 0; 37 double cosa = 0; 37 double ptot = mag()*v.mag(); 38 double ptot = mag()*v.mag(); 38 if(ptot > 0) { 39 if(ptot > 0) { 39 cosa = dot(v)/ptot; 40 cosa = dot(v)/ptot; 40 if(cosa > 1) cosa = 1; 41 if(cosa > 1) cosa = 1; 41 if(cosa < -1) cosa = -1; 42 if(cosa < -1) cosa = -1; 42 } 43 } 43 return std::acos(cosa); 44 return std::acos(cosa); 44 } 45 } 45 46 46 //------------------------------------------ 47 //-------------------------------------------------------------------------- 47 template<> 48 template<> 48 BasicVector3D<float> & BasicVector3D<float>: 49 BasicVector3D<float> & BasicVector3D<float>::rotateX(float a) { 49 double sina = std::sin(a), cosa = std::cos 50 double sina = std::sin(a), cosa = std::cos(a), dy = y(), dz = z(); 50 setY(dy*cosa-dz*sina); 51 setY(dy*cosa-dz*sina); 51 setZ(dz*cosa+dy*sina); 52 setZ(dz*cosa+dy*sina); 52 return *this; 53 return *this; 53 } 54 } 54 55 55 //------------------------------------------ 56 //-------------------------------------------------------------------------- 56 template<> 57 template<> 57 BasicVector3D<float> & BasicVector3D<float>: 58 BasicVector3D<float> & BasicVector3D<float>::rotateY(float a) { 58 double sina = std::sin(a), cosa = std::cos 59 double sina = std::sin(a), cosa = std::cos(a), dz = z(), dx = x(); 59 setZ(dz*cosa-dx*sina); 60 setZ(dz*cosa-dx*sina); 60 setX(dx*cosa+dz*sina); 61 setX(dx*cosa+dz*sina); 61 return *this; 62 return *this; 62 } 63 } 63 64 64 //------------------------------------------ 65 //-------------------------------------------------------------------------- 65 template<> 66 template<> 66 BasicVector3D<float> & BasicVector3D<float>: 67 BasicVector3D<float> & BasicVector3D<float>::rotateZ(float a) { 67 double sina = std::sin(a), cosa = std::cos 68 double sina = std::sin(a), cosa = std::cos(a), dx = x(), dy = y(); 68 setX(dx*cosa-dy*sina); 69 setX(dx*cosa-dy*sina); 69 setY(dy*cosa+dx*sina); 70 setY(dy*cosa+dx*sina); 70 return *this; 71 return *this; 71 } 72 } 72 73 73 //------------------------------------------ 74 //-------------------------------------------------------------------------- 74 template<> 75 template<> 75 BasicVector3D<float> & 76 BasicVector3D<float> & 76 BasicVector3D<float>::rotate(float a, const 77 BasicVector3D<float>::rotate(float a, const BasicVector3D<float> & v) { 77 if (a == 0) return *this; 78 if (a == 0) return *this; 78 double cx = v.x(), cy = v.y(), cz = v.z(); 79 double cx = v.x(), cy = v.y(), cz = v.z(); 79 double ll = std::sqrt(cx*cx + cy*cy + cz*c 80 double ll = std::sqrt(cx*cx + cy*cy + cz*cz); 80 if (ll == 0) { 81 if (ll == 0) { 81 std::cerr << "BasicVector<float>::rotate 82 std::cerr << "BasicVector<float>::rotate() : zero axis" << std::endl; 82 return *this; 83 return *this; 83 } 84 } 84 double cosa = std::cos(a), sina = std::sin 85 double cosa = std::cos(a), sina = std::sin(a); 85 cx /= ll; cy /= ll; cz /= ll; 86 cx /= ll; cy /= ll; cz /= ll; 86 87 87 double xx = cosa + (1-cosa)*cx*cx; 88 double xx = cosa + (1-cosa)*cx*cx; 88 double xy = (1-cosa)*cx*cy - sina*c 89 double xy = (1-cosa)*cx*cy - sina*cz; 89 double xz = (1-cosa)*cx*cz + sina*c 90 double xz = (1-cosa)*cx*cz + sina*cy; 90 91 91 double yx = (1-cosa)*cy*cx + sina*c 92 double yx = (1-cosa)*cy*cx + sina*cz; 92 double yy = cosa + (1-cosa)*cy*cy; 93 double yy = cosa + (1-cosa)*cy*cy; 93 double yz = (1-cosa)*cy*cz - sina*c 94 double yz = (1-cosa)*cy*cz - sina*cx; 94 95 95 double zx = (1-cosa)*cz*cx - sina*c 96 double zx = (1-cosa)*cz*cx - sina*cy; 96 double zy = (1-cosa)*cz*cy + sina*c 97 double zy = (1-cosa)*cz*cy + sina*cx; 97 double zz = cosa + (1-cosa)*cz*cz; 98 double zz = cosa + (1-cosa)*cz*cz; 98 99 99 cx = x(); cy = y(); cz = z(); 100 cx = x(); cy = y(); cz = z(); 100 set(xx*cx+xy*cy+xz*cz, yx*cx+yy*cy+yz*cz, 101 set(xx*cx+xy*cy+xz*cz, yx*cx+yy*cy+yz*cz, zx*cx+zy*cy+zz*cz); 101 return *this; 102 return *this; 102 } 103 } 103 104 104 //------------------------------------------ 105 //-------------------------------------------------------------------------- 105 std::ostream & 106 std::ostream & 106 operator<<(std::ostream & os, const BasicVec 107 operator<<(std::ostream & os, const BasicVector3D<float> & a) 107 { 108 { 108 return os << "(" << a.x() << "," << a.y() 109 return os << "(" << a.x() << "," << a.y() << "," << a.z() << ")"; 109 } 110 } 110 111 111 //------------------------------------------ 112 //-------------------------------------------------------------------------- 112 std::istream & 113 std::istream & 113 operator>> (std::istream & is, BasicVector3D 114 operator>> (std::istream & is, BasicVector3D<float> & a) 114 { 115 { 115 // Required format is ( a, b, c ) that is, 116 // Required format is ( a, b, c ) that is, three numbers, preceded by 116 // (, followed by ), and separated by comm 117 // (, followed by ), and separated by commas. The three numbers are 117 // taken as x, y, z. 118 // taken as x, y, z. 118 119 119 float x, y, z; 120 float x, y, z; 120 char c; 121 char c; 121 122 122 is >> std::ws >> c; 123 is >> std::ws >> c; 123 // ws is defined to invoke eatwhite(istrea 124 // ws is defined to invoke eatwhite(istream & ) 124 // see (Stroustrup gray book) page 333 and 125 // see (Stroustrup gray book) page 333 and 345. 125 if (is.fail() || c != '(' ) { 126 if (is.fail() || c != '(' ) { 126 std::cerr 127 std::cerr 127 << "Could not find required opening parenthe 128 << "Could not find required opening parenthesis " 128 << "in input of a BasicVector3D<float>" 129 << "in input of a BasicVector3D<float>" 129 << std::endl; 130 << std::endl; 130 return is; 131 return is; 131 } 132 } 132 133 133 is >> x >> std::ws >> c; 134 is >> x >> std::ws >> c; 134 if (is.fail() || c != ',' ) { 135 if (is.fail() || c != ',' ) { 135 std::cerr 136 std::cerr 136 << "Could not find x value and required trai 137 << "Could not find x value and required trailing comma " 137 << "in input of a BasicVector3D<float>" 138 << "in input of a BasicVector3D<float>" 138 << std::endl; 139 << std::endl; 139 return is; 140 return is; 140 } 141 } 141 142 142 is >> y >> std::ws >> c; 143 is >> y >> std::ws >> c; 143 if (is.fail() || c != ',' ) { 144 if (is.fail() || c != ',' ) { 144 std::cerr 145 std::cerr 145 << "Could not find y value and required trai 146 << "Could not find y value and required trailing comma " 146 << "in input of a BasicVector3D<float>" 147 << "in input of a BasicVector3D<float>" 147 << std::endl; 148 << std::endl; 148 return is; 149 return is; 149 } 150 } 150 151 151 is >> z >> std::ws >> c; 152 is >> z >> std::ws >> c; 152 if (is.fail() || c != ')' ) { 153 if (is.fail() || c != ')' ) { 153 std::cerr 154 std::cerr 154 << "Could not find z value and required clos 155 << "Could not find z value and required close parenthesis " 155 << "in input of a BasicVector3D<float>" 156 << "in input of a BasicVector3D<float>" 156 << std::endl; 157 << std::endl; 157 return is; 158 return is; 158 } 159 } 159 160 160 a.setX(x); 161 a.setX(x); 161 a.setY(y); 162 a.setY(y); 162 a.setZ(z); 163 a.setZ(z); 163 return is; 164 return is; 164 } 165 } 165 166 166 //------------------------------------------ 167 //-------------------------------------------------------------------------- 167 template<> 168 template<> 168 double BasicVector3D<double>::pseudoRapidity 169 double BasicVector3D<double>::pseudoRapidity() const { 169 double ma = mag(), dz = z(); 170 double ma = mag(), dz = z(); 170 if (ma == 0) return 0; 171 if (ma == 0) return 0; 171 if (ma == dz) return DBL_MAX; 172 if (ma == dz) return DBL_MAX; 172 if (ma == -dz) return -DBL_MAX; 173 if (ma == -dz) return -DBL_MAX; 173 return 0.5*std::log((ma+dz)/(ma-dz)); 174 return 0.5*std::log((ma+dz)/(ma-dz)); 174 } 175 } 175 176 176 //------------------------------------------ 177 //-------------------------------------------------------------------------- 177 template<> 178 template<> 178 void BasicVector3D<double>::setEta(double a) 179 void BasicVector3D<double>::setEta(double a) { 179 double ma = mag(); 180 double ma = mag(); 180 if (ma == 0) return; 181 if (ma == 0) return; 181 double tanHalfTheta = std::exp(-a); 182 double tanHalfTheta = std::exp(-a); 182 double tanHalfTheta2 = tanHalfTheta * tanH 183 double tanHalfTheta2 = tanHalfTheta * tanHalfTheta; 183 double cosTheta1 = (1 - tanHalfTheta2 184 double cosTheta1 = (1 - tanHalfTheta2) / (1 + tanHalfTheta2); 184 double rh = ma * std::sqrt(1 - 185 double rh = ma * std::sqrt(1 - cosTheta1*cosTheta1); 185 double ph = phi(); 186 double ph = phi(); 186 set(rh*std::cos(ph), rh*std::sin(ph), ma*c 187 set(rh*std::cos(ph), rh*std::sin(ph), ma*cosTheta1); 187 } 188 } 188 189 189 //------------------------------------------ 190 //-------------------------------------------------------------------------- 190 template<> 191 template<> 191 double BasicVector3D<double>::angle(const Ba 192 double BasicVector3D<double>::angle(const BasicVector3D<double> & v) const { 192 double cosa = 0; 193 double cosa = 0; 193 double ptot = mag()*v.mag(); 194 double ptot = mag()*v.mag(); 194 if(ptot > 0) { 195 if(ptot > 0) { 195 cosa = dot(v)/ptot; 196 cosa = dot(v)/ptot; 196 if(cosa > 1) cosa = 1; 197 if(cosa > 1) cosa = 1; 197 if(cosa < -1) cosa = -1; 198 if(cosa < -1) cosa = -1; 198 } 199 } 199 return std::acos(cosa); 200 return std::acos(cosa); 200 } 201 } 201 202 202 //------------------------------------------ 203 //-------------------------------------------------------------------------- 203 template<> 204 template<> 204 BasicVector3D<double> & BasicVector3D<double 205 BasicVector3D<double> & BasicVector3D<double>::rotateX(double a) { 205 double sina = std::sin(a), cosa = std::cos 206 double sina = std::sin(a), cosa = std::cos(a), dy = y(), dz = z(); 206 setY(dy*cosa-dz*sina); 207 setY(dy*cosa-dz*sina); 207 setZ(dz*cosa+dy*sina); 208 setZ(dz*cosa+dy*sina); 208 return *this; 209 return *this; 209 } 210 } 210 211 211 //------------------------------------------ 212 //-------------------------------------------------------------------------- 212 template<> 213 template<> 213 BasicVector3D<double> & BasicVector3D<double 214 BasicVector3D<double> & BasicVector3D<double>::rotateY(double a) { 214 double sina = std::sin(a), cosa = std::cos 215 double sina = std::sin(a), cosa = std::cos(a), dz = z(), dx = x(); 215 setZ(dz*cosa-dx*sina); 216 setZ(dz*cosa-dx*sina); 216 setX(dx*cosa+dz*sina); 217 setX(dx*cosa+dz*sina); 217 return *this; 218 return *this; 218 } 219 } 219 220 220 //------------------------------------------ 221 //-------------------------------------------------------------------------- 221 template<> 222 template<> 222 BasicVector3D<double> & BasicVector3D<double 223 BasicVector3D<double> & BasicVector3D<double>::rotateZ(double a) { 223 double sina = std::sin(a), cosa = std::cos 224 double sina = std::sin(a), cosa = std::cos(a), dx = x(), dy = y(); 224 setX(dx*cosa-dy*sina); 225 setX(dx*cosa-dy*sina); 225 setY(dy*cosa+dx*sina); 226 setY(dy*cosa+dx*sina); 226 return *this; 227 return *this; 227 } 228 } 228 229 229 //------------------------------------------ 230 //-------------------------------------------------------------------------- 230 template<> 231 template<> 231 BasicVector3D<double> & 232 BasicVector3D<double> & 232 BasicVector3D<double>::rotate(double a, cons 233 BasicVector3D<double>::rotate(double a, const BasicVector3D<double> & v) { 233 if (a == 0) return *this; 234 if (a == 0) return *this; 234 double cx = v.x(), cy = v.y(), cz = v.z(); 235 double cx = v.x(), cy = v.y(), cz = v.z(); 235 double ll = std::sqrt(cx*cx + cy*cy + cz*c 236 double ll = std::sqrt(cx*cx + cy*cy + cz*cz); 236 if (ll == 0) { 237 if (ll == 0) { 237 std::cerr << "BasicVector<double>::rotat 238 std::cerr << "BasicVector<double>::rotate() : zero axis" << std::endl; 238 return *this; 239 return *this; 239 } 240 } 240 double cosa = std::cos(a), sina = std::sin 241 double cosa = std::cos(a), sina = std::sin(a); 241 cx /= ll; cy /= ll; cz /= ll; 242 cx /= ll; cy /= ll; cz /= ll; 242 243 243 double xx = cosa + (1-cosa)*cx*cx; 244 double xx = cosa + (1-cosa)*cx*cx; 244 double xy = (1-cosa)*cx*cy - sina*c 245 double xy = (1-cosa)*cx*cy - sina*cz; 245 double xz = (1-cosa)*cx*cz + sina*c 246 double xz = (1-cosa)*cx*cz + sina*cy; 246 247 247 double yx = (1-cosa)*cy*cx + sina*c 248 double yx = (1-cosa)*cy*cx + sina*cz; 248 double yy = cosa + (1-cosa)*cy*cy; 249 double yy = cosa + (1-cosa)*cy*cy; 249 double yz = (1-cosa)*cy*cz - sina*c 250 double yz = (1-cosa)*cy*cz - sina*cx; 250 251 251 double zx = (1-cosa)*cz*cx - sina*c 252 double zx = (1-cosa)*cz*cx - sina*cy; 252 double zy = (1-cosa)*cz*cy + sina*c 253 double zy = (1-cosa)*cz*cy + sina*cx; 253 double zz = cosa + (1-cosa)*cz*cz; 254 double zz = cosa + (1-cosa)*cz*cz; 254 255 255 cx = x(); cy = y(); cz = z(); 256 cx = x(); cy = y(); cz = z(); 256 set(xx*cx+xy*cy+xz*cz, yx*cx+yy*cy+yz*cz, 257 set(xx*cx+xy*cy+xz*cz, yx*cx+yy*cy+yz*cz, zx*cx+zy*cy+zz*cz); 257 return *this; 258 return *this; 258 } 259 } 259 260 260 //------------------------------------------ 261 //-------------------------------------------------------------------------- 261 std::ostream & 262 std::ostream & 262 operator<< (std::ostream & os, const BasicVe 263 operator<< (std::ostream & os, const BasicVector3D<double> & a) 263 { 264 { 264 return os << "(" << a.x() << "," << a.y() 265 return os << "(" << a.x() << "," << a.y() << "," << a.z() << ")"; 265 } 266 } 266 267 267 //------------------------------------------ 268 //-------------------------------------------------------------------------- 268 std::istream & 269 std::istream & 269 operator>> (std::istream & is, BasicVector3D 270 operator>> (std::istream & is, BasicVector3D<double> & a) 270 { 271 { 271 // Required format is ( a, b, c ) that is, 272 // Required format is ( a, b, c ) that is, three numbers, preceded by 272 // (, followed by ), and separated by comm 273 // (, followed by ), and separated by commas. The three numbers are 273 // taken as x, y, z. 274 // taken as x, y, z. 274 275 275 double x, y, z; 276 double x, y, z; 276 char c; 277 char c; 277 278 278 is >> std::ws >> c; 279 is >> std::ws >> c; 279 // ws is defined to invoke eatwhite(istrea 280 // ws is defined to invoke eatwhite(istream & ) 280 // see (Stroustrup gray book) page 333 and 281 // see (Stroustrup gray book) page 333 and 345. 281 if (is.fail() || c != '(' ) { 282 if (is.fail() || c != '(' ) { 282 std::cerr 283 std::cerr 283 << "Could not find required opening parenthe 284 << "Could not find required opening parenthesis " 284 << "in input of a BasicVector3D<double>" 285 << "in input of a BasicVector3D<double>" 285 << std::endl; 286 << std::endl; 286 return is; 287 return is; 287 } 288 } 288 289 289 is >> x >> std::ws >> c; 290 is >> x >> std::ws >> c; 290 if (is.fail() || c != ',' ) { 291 if (is.fail() || c != ',' ) { 291 std::cerr 292 std::cerr 292 << "Could not find x value and required trai 293 << "Could not find x value and required trailing comma " 293 << "in input of a BasicVector3D<double>" 294 << "in input of a BasicVector3D<double>" 294 << std::endl; 295 << std::endl; 295 return is; 296 return is; 296 } 297 } 297 298 298 is >> y >> std::ws >> c; 299 is >> y >> std::ws >> c; 299 if (is.fail() || c != ',' ) { 300 if (is.fail() || c != ',' ) { 300 std::cerr 301 std::cerr 301 << "Could not find y value and required trai 302 << "Could not find y value and required trailing comma " 302 << "in input of a BasicVector3D<double>" 303 << "in input of a BasicVector3D<double>" 303 << std::endl; 304 << std::endl; 304 return is; 305 return is; 305 } 306 } 306 307 307 is >> z >> std::ws >> c; 308 is >> z >> std::ws >> c; 308 if (is.fail() || c != ')' ) { 309 if (is.fail() || c != ')' ) { 309 std::cerr 310 std::cerr 310 << "Could not find z value and required clos 311 << "Could not find z value and required close parenthesis " 311 << "in input of a BasicVector3D<double>" 312 << "in input of a BasicVector3D<double>" 312 << std::endl; 313 << std::endl; 313 return is; 314 return is; 314 } 315 } 315 316 316 a.setX(x); 317 a.setX(x); 317 a.setY(y); 318 a.setY(y); 318 a.setZ(z); 319 a.setZ(z); 319 return is; 320 return is; 320 } 321 } 321 } /* namespace HepGeom */ 322 } /* namespace HepGeom */ 322 323