File indexing completed on 2023-03-17 10:38:45
0001
0002
0003
0004
0005
0006
0007
0008 #include "Alignment/CocoaUtilities/interface/ALIUtils.h"
0009 #include "Alignment/CocoaUtilities/interface/GlobalOptionMgr.h"
0010
0011 #include <cmath>
0012 #include <cstdlib>
0013 #include <iomanip>
0014
0015 ALIint ALIUtils::debug = 99;
0016 ALIint ALIUtils::report = 1;
0017 ALIdouble ALIUtils::_LengthValueDimensionFactor = 1.;
0018 ALIdouble ALIUtils::_LengthSigmaDimensionFactor = 1.;
0019 ALIdouble ALIUtils::_AngleValueDimensionFactor = 1.;
0020 ALIdouble ALIUtils::_AngleSigmaDimensionFactor = 1.;
0021 ALIdouble ALIUtils::_OutputLengthValueDimensionFactor = 1.;
0022 ALIdouble ALIUtils::_OutputLengthSigmaDimensionFactor = 1.;
0023 ALIdouble ALIUtils::_OutputAngleValueDimensionFactor = 1.;
0024 ALIdouble ALIUtils::_OutputAngleSigmaDimensionFactor = 1.;
0025 time_t ALIUtils::_time_now;
0026 ALIdouble ALIUtils::deg = 0.017453293;
0027 ALIbool ALIUtils::firstTime = false;
0028 ALIdouble ALIUtils::maximum_deviation_derivative = 1.E-6;
0029
0030
0031
0032
0033 int ALIUtils::IsNumber(const ALIstring& str) {
0034 int isnum = 1;
0035 int numE = 0;
0036 for (ALIuint ii = 0; ii < str.length(); ii++) {
0037 if (!isdigit(str[ii]) && str[ii] != '.' && str[ii] != '-' && str[ii] != '+') {
0038
0039 if (str[ii] == 'E' || str[ii] == 'e') {
0040 if (numE != 0 || ii == str.length() - 1) {
0041 isnum = 0;
0042 break;
0043 }
0044 numE++;
0045 } else {
0046 isnum = 0;
0047 break;
0048 }
0049 }
0050 }
0051
0052 return isnum;
0053 }
0054
0055
0056
0057
0058 void ALIUtils::dump3v(const CLHEP::Hep3Vector& vec, const std::string& msg) {
0059
0060 std::cout << msg << std::setprecision(8) << vec;
0061 std::cout << std::endl;
0062
0063
0064
0065
0066 }
0067
0068
0069
0070
0071 void ALIUtils::dumprm(const CLHEP::HepRotation& rm, const std::string& msg, std::ostream& out) {
0072 out << msg << " xx=" << rm.xx() << " xy=" << rm.xy() << " xz=" << rm.xz() << std::endl;
0073 out << msg << " yx=" << rm.yx() << " yy=" << rm.yy() << " yz=" << rm.yz() << std::endl;
0074 out << msg << " zx=" << rm.zx() << " zy=" << rm.zy() << " zz=" << rm.zz() << std::endl;
0075 }
0076
0077
0078
0079
0080
0081 void ALIUtils::SetLengthDimensionFactors() {
0082
0083
0084 GlobalOptionMgr* gomgr = GlobalOptionMgr::getInstance();
0085 ALIint ad = ALIint(gomgr->getGlobalOption("length_value_dimension"));
0086
0087 _LengthValueDimensionFactor = CalculateLengthDimensionFactorFromInt(ad);
0088 ad = ALIint(gomgr->GlobalOptions()[ALIstring("length_error_dimension")]);
0089 _LengthSigmaDimensionFactor = CalculateLengthDimensionFactorFromInt(ad);
0090
0091
0092
0093
0094
0095 if (ALIUtils::debug >= 6)
0096 std::cout << _LengthValueDimensionFactor << " Set Length DimensionFactors " << _LengthSigmaDimensionFactor
0097 << std::endl;
0098 }
0099
0100
0101
0102
0103
0104 void ALIUtils::SetAngleDimensionFactors() {
0105
0106
0107 GlobalOptionMgr* gomgr = GlobalOptionMgr::getInstance();
0108 ALIint ad = ALIint(gomgr->GlobalOptions()[ALIstring("angle_value_dimension")]);
0109 _AngleValueDimensionFactor = CalculateAngleDimensionFactorFromInt(ad);
0110
0111 ad = ALIint(gomgr->GlobalOptions()[ALIstring("angle_error_dimension")]);
0112 _AngleSigmaDimensionFactor = CalculateAngleDimensionFactorFromInt(ad);
0113
0114
0115
0116
0117
0118 if (ALIUtils::debug >= 6)
0119 std::cout << _AngleValueDimensionFactor << "Set Angle DimensionFactors" << _AngleSigmaDimensionFactor << std::endl;
0120 }
0121
0122
0123
0124
0125
0126 void ALIUtils::SetOutputLengthDimensionFactors() {
0127
0128
0129 GlobalOptionMgr* gomgr = GlobalOptionMgr::getInstance();
0130 ALIint ad = ALIint(gomgr->GlobalOptions()[ALIstring("output_length_value_dimension")]);
0131 if (ad == 0)
0132 ad = ALIint(gomgr->GlobalOptions()[ALIstring("length_value_dimension")]);
0133 _OutputLengthValueDimensionFactor = CalculateLengthDimensionFactorFromInt(ad);
0134
0135 ad = ALIint(gomgr->GlobalOptions()[ALIstring("output_length_error_dimension")]);
0136 if (ad == 0)
0137 ad = ALIint(gomgr->GlobalOptions()[ALIstring("length_error_dimension")]);
0138 _OutputLengthSigmaDimensionFactor = CalculateLengthDimensionFactorFromInt(ad);
0139
0140
0141
0142
0143
0144 if (ALIUtils::debug >= 6)
0145 std::cout << _OutputLengthValueDimensionFactor << "Output Length Dimension Factors"
0146 << _OutputLengthSigmaDimensionFactor << std::endl;
0147 }
0148
0149
0150
0151
0152
0153 void ALIUtils::SetOutputAngleDimensionFactors() {
0154
0155
0156 GlobalOptionMgr* gomgr = GlobalOptionMgr::getInstance();
0157 ALIint ad = ALIint(gomgr->GlobalOptions()[ALIstring("output_angle_value_dimension")]);
0158 if (ad == 0)
0159 ad = ALIint(gomgr->GlobalOptions()[ALIstring("angle_value_dimension")]);
0160 _OutputAngleValueDimensionFactor = CalculateAngleDimensionFactorFromInt(ad);
0161
0162 ad = ALIint(gomgr->GlobalOptions()[ALIstring("output_angle_error_dimension")]);
0163 if (ad == 0)
0164 ad = ALIint(gomgr->GlobalOptions()[ALIstring("angle_error_dimension")]);
0165 _OutputAngleSigmaDimensionFactor = CalculateAngleDimensionFactorFromInt(ad);
0166
0167
0168
0169
0170
0171 if (ALIUtils::debug >= 9)
0172 std::cout << _OutputAngleValueDimensionFactor << "Output Angle Dimension Factors"
0173 << _OutputAngleSigmaDimensionFactor << std::endl;
0174 }
0175
0176
0177
0178
0179 ALIdouble ALIUtils::CalculateLengthDimensionFactorFromString(ALIstring dimstr) {
0180 ALIdouble valsig = 1.;
0181 ALIstring internalDim = "m";
0182 if (internalDim == "m") {
0183 if (dimstr == "m") {
0184 valsig = 1.;
0185 } else if (dimstr == "mm") {
0186 valsig = 1.E-3;
0187 } else if (dimstr == "mum") {
0188 valsig = 1.E-6;
0189 } else if (dimstr == "cm") {
0190 valsig = 1.E-2;
0191 } else {
0192 std::cerr << "!!! UNKNOWN DIMENSION SCALING " << dimstr << std::endl
0193 << "VALUE MUST BE BETWEEN 0 AND 3 " << std::endl;
0194 exit(1);
0195 }
0196 } else if (internalDim == "mm") {
0197 if (dimstr == "m") {
0198 valsig = 1.E3;
0199 } else if (dimstr == "mm") {
0200 valsig = 1.;
0201 } else if (dimstr == "mum") {
0202 valsig = 1.E-3;
0203 } else if (dimstr == "cm") {
0204 valsig = 1.E+1;
0205 } else {
0206 std::cerr << "!!! UNKNOWN DIMENSION SCALING: " << dimstr << std::endl
0207 << "VALUE MUST BE A LENGTH DIMENSION " << std::endl;
0208 exit(1);
0209 }
0210 }
0211
0212 return valsig;
0213 }
0214
0215
0216
0217
0218 ALIdouble ALIUtils::CalculateAngleDimensionFactorFromString(ALIstring dimstr) {
0219 ALIdouble valsig;
0220 if (dimstr == "rad") {
0221 valsig = 1.;
0222 } else if (dimstr == "mrad") {
0223 valsig = 1.E-3;
0224 } else if (dimstr == "murad") {
0225 valsig = 1.E-6;
0226 } else if (dimstr == "deg") {
0227 valsig = M_PI / 180.;
0228 } else if (dimstr == "grad") {
0229 valsig = M_PI / 200.;
0230 } else {
0231 std::cerr << "!!! UNKNOWN DIMENSION SCALING: " << dimstr << std::endl
0232 << "VALUE MUST BE AN ANGLE DIMENSION " << std::endl;
0233 exit(1);
0234 }
0235
0236 return valsig;
0237 }
0238
0239
0240
0241
0242 ALIdouble ALIUtils::CalculateLengthDimensionFactorFromInt(ALIint ad) {
0243 ALIdouble valsig;
0244 switch (ad) {
0245 case 0:
0246 valsig = CalculateLengthDimensionFactorFromString("m");
0247 break;
0248 case 1:
0249 valsig = CalculateLengthDimensionFactorFromString("mm");
0250 break;
0251 case 2:
0252 valsig = CalculateLengthDimensionFactorFromString("mum");
0253 break;
0254 case 3:
0255 valsig = CalculateLengthDimensionFactorFromString("cm");
0256 break;
0257 default:
0258 std::cerr << "!!! UNKNOWN DIMENSION SCALING " << ad << std::endl << "VALUE MUST BE BETWEEN 0 AND 3 " << std::endl;
0259 exit(1);
0260 }
0261
0262
0263
0264
0265 return valsig;
0266 }
0267
0268
0269
0270
0271 ALIdouble ALIUtils::CalculateAngleDimensionFactorFromInt(ALIint ad) {
0272 ALIdouble valsig;
0273 switch (ad) {
0274 case 0:
0275 valsig = CalculateAngleDimensionFactorFromString("rad");
0276 break;
0277 case 1:
0278 valsig = CalculateAngleDimensionFactorFromString("mrad");
0279 break;
0280 case 2:
0281 valsig = CalculateAngleDimensionFactorFromString("murad");
0282 break;
0283 case 3:
0284 valsig = CalculateAngleDimensionFactorFromString("deg");
0285 break;
0286 case 4:
0287 valsig = CalculateAngleDimensionFactorFromString("grad");
0288 break;
0289 default:
0290 std::cerr << "!!! UNKNOWN DIMENSION SCALING " << ad << std::endl << "VALUE MUST BE BETWEEN 0 AND 3 " << std::endl;
0291 exit(1);
0292 }
0293
0294
0295
0296
0297 return valsig;
0298 }
0299
0300
0301
0302
0303
0304
0305
0306
0307
0308 void ALIUtils::dumpDimensions(std::ofstream& fout) {
0309 fout << "DIMENSIONS: lengths = ";
0310 ALIstring internalDim = "m";
0311 if (_OutputLengthValueDimensionFactor == 1.) {
0312 fout << "m";
0313 } else if (_OutputLengthValueDimensionFactor == 1.E-3) {
0314 fout << "mm";
0315 } else if (_OutputLengthValueDimensionFactor == 1.E-6) {
0316 fout << "mum";
0317 } else if (_OutputLengthValueDimensionFactor == 1.E-2) {
0318 fout << "cm";
0319 } else {
0320 std::cerr << " !! unknown OutputLengthValueDimensionFactor " << _OutputLengthValueDimensionFactor << std::endl;
0321 exit(1);
0322 }
0323
0324 fout << " +- ";
0325 if (_OutputLengthSigmaDimensionFactor == 1.) {
0326 fout << "m";
0327 } else if (_OutputLengthSigmaDimensionFactor == 1.E-3) {
0328 fout << "mm";
0329 } else if (_OutputLengthSigmaDimensionFactor == 1.E-6) {
0330 fout << "mum";
0331 } else if (_OutputLengthSigmaDimensionFactor == 1.E-2) {
0332 fout << "cm";
0333 } else {
0334 std::cerr << " !! unknown OutputLengthSigmaDimensionFactor " << _OutputLengthSigmaDimensionFactor << std::endl;
0335 exit(1);
0336 }
0337
0338 fout << " angles = ";
0339 if (_OutputAngleValueDimensionFactor == 1.) {
0340 fout << "rad";
0341 } else if (_OutputAngleValueDimensionFactor == 1.E-3) {
0342 fout << "mrad";
0343 } else if (_OutputAngleValueDimensionFactor == 1.E-6) {
0344 fout << "murad";
0345 } else if (_OutputAngleValueDimensionFactor == M_PI / 180.) {
0346 fout << "deg";
0347 } else if (_OutputAngleValueDimensionFactor == M_PI / 200.) {
0348 fout << "grad";
0349 } else {
0350 std::cerr << " !! unknown OutputAngleValueDimensionFactor " << _OutputAngleValueDimensionFactor << std::endl;
0351 exit(1);
0352 }
0353
0354 fout << " +- ";
0355 if (_OutputAngleSigmaDimensionFactor == 1.) {
0356 fout << "rad";
0357 } else if (_OutputAngleSigmaDimensionFactor == 1.E-3) {
0358 fout << "mrad";
0359 } else if (_OutputAngleSigmaDimensionFactor == 1.E-6) {
0360 fout << "murad";
0361 } else if (_OutputAngleSigmaDimensionFactor == M_PI / 180.) {
0362 fout << "deg";
0363 } else if (_OutputAngleSigmaDimensionFactor == M_PI / 200.) {
0364 fout << "grad";
0365 } else {
0366 std::cerr << " !! unknown OutputAngleSigmaDimensionFactor " << _OutputAngleSigmaDimensionFactor << std::endl;
0367 exit(1);
0368 }
0369 fout << std::endl;
0370 }
0371
0372
0373 double ALIUtils::getFloat(const ALIstring& str) {
0374
0375 if (!IsNumber(str)) {
0376 std::cerr << "!!!! EXITING: trying to get the float from a string that is not a number " << str << std::endl;
0377 exit(1);
0378 }
0379
0380 return atof(str.c_str());
0381 }
0382
0383
0384 int ALIUtils::getInt(const ALIstring& str) {
0385
0386 if (!IsNumber(str)) {
0387
0388 std::cerr << "!!!! EXITING: trying to get the integer from a string that is not a number " << str << std::endl;
0389 exit(1);
0390 } else {
0391
0392 bool isFloat = false;
0393 int ch = str.find('.');
0394 ALIuint ii = 0;
0395 if (ch != -1) {
0396 for (ii = ch + 1; ii < str.size(); ii++) {
0397 if (str[ii] != '0')
0398 isFloat = true;
0399 }
0400 }
0401
0402 ch = str.find('E');
0403 if (ch != -1)
0404 ch = str.find('e');
0405 if (ch != -1) {
0406 if (str[ch + 1] == '-')
0407 isFloat = true;
0408 }
0409
0410 if (isFloat) {
0411 std::cerr << "!!!! EXITING: trying to get the integer from a string that is a float: " << str << std::endl;
0412 std::cerr << ii << " ii " << ch << std::endl;
0413 exit(1);
0414 }
0415 }
0416 return int(atof(str.c_str()));
0417 }
0418
0419
0420 bool ALIUtils::getBool(const ALIstring& str) {
0421 bool val;
0422
0423
0424
0425 if (str == "ON" || str == "TRUE") {
0426 val = true;
0427 } else if (str == "OFF" || str == "FALSE") {
0428 val = false;
0429 } else {
0430 std::cerr << "!!!! EXITING: trying to get the float from a string that is not 'ON'/'OFF'/'TRUE'/'FALSE' " << str
0431 << std::endl;
0432 exit(1);
0433 }
0434
0435 return val;
0436 }
0437
0438
0439 ALIstring ALIUtils::subQuotes(const ALIstring& str) {
0440
0441 if (str.find('"') != 0 || str.rfind('"') != str.length() - 1) {
0442 std::cerr << "!!!EXITING trying to substract quotes from a word that has no quotes " << str << std::endl;
0443 exit(1);
0444 }
0445
0446
0447
0448 ALIstring strt = str.substr(1, str.size() - 2);
0449
0450
0451
0452 while (strt[0] == ' ') {
0453 strt = strt.substr(1, strt.size() - 1);
0454 }
0455
0456
0457 while (strt[strt.size() - 1] == ' ') {
0458 strt = strt.substr(0, strt.size() - 1);
0459 }
0460
0461 return strt;
0462 }
0463
0464
0465 void ALIUtils::dumpVS(const std::vector<ALIstring>& wl, const std::string& msg, std::ostream& outs) {
0466 outs << msg << std::endl;
0467 ALIuint siz = wl.size();
0468 for (ALIuint ii = 0; ii < siz; ii++) {
0469 outs << wl[ii] << " ";
0470
0471
0472 }
0473 outs << std::endl;
0474 }
0475
0476
0477 ALIdouble ALIUtils::getDimensionValue(const ALIstring& dim, const ALIstring& dimType) {
0478 ALIdouble value;
0479 if (dimType == "Length") {
0480 if (dim == "mm") {
0481 value = 1.E-3;
0482 } else if (dim == "cm") {
0483 value = 1.E-2;
0484 } else if (dim == "m") {
0485 value = 1.;
0486 } else if (dim == "mum") {
0487 value = 1.E-6;
0488 } else if (dim == "dm") {
0489 value = 1.E-1;
0490 } else if (dim == "nm") {
0491 value = 1.E-9;
0492 } else {
0493 std::cerr << "!!!!FATAL ERROR: ALIUtils::GetDimensionValue. " << dim
0494 << " is a dimension not supported for dimension type " << dimType << std::endl;
0495 abort();
0496 }
0497 } else if (dimType == "Angle") {
0498 if (dim == "rad") {
0499 value = 1.;
0500 } else if (dim == "mrad") {
0501 value = 1.E-3;
0502 } else if (dim == "murad") {
0503 value = 1.E-6;
0504 } else if (dim == "deg") {
0505 value = M_PI / 180.;
0506 } else if (dim == "grad") {
0507 value = M_PI / 200.;
0508 } else {
0509 std::cerr << "!!!!FATAL ERROR: ALIUtils::GetDimensionValue. " << dim
0510 << " is a dimension not supported for dimension type " << dimType << std::endl;
0511 abort();
0512 }
0513 } else {
0514 std::cerr << "!!!!FATAL ERROR: ALIUtils::GetDimensionValue. " << dimType << " is a dimension type not supported "
0515 << std::endl;
0516 abort();
0517 }
0518
0519 return value;
0520 }
0521
0522
0523
0524
0525
0526
0527
0528
0529
0530
0531
0532
0533
0534
0535 std::string ALIUtils::changeName(const std::string& oldName, const std::string& subsstr1, const std::string& subsstr2) {
0536 std::string newName = oldName;
0537 int il = oldName.find(subsstr1);
0538
0539 while (il >= 0) {
0540 newName = newName.substr(0, il) + subsstr2 + newName.substr(il + subsstr1.length(), newName.length());
0541
0542 il = oldName.find(subsstr1, il + 1);
0543 }
0544
0545 return newName;
0546 }
0547
0548
0549 std::vector<double> ALIUtils::getRotationAnglesFromMatrix(const CLHEP::HepRotation& rmLocal,
0550 double origAngleX,
0551 double origAngleY,
0552 double origAngleZ) {
0553 double pii = acos(0.) * 2;
0554 std::vector<double> newang(3);
0555 double angleX = origAngleX;
0556 double angleY = origAngleY;
0557 double angleZ = origAngleZ;
0558
0559 if (ALIUtils::debug >= 4) {
0560 std::cout << " angles as value entries: X= " << angleX << " Y= " << angleY << " Z " << angleZ << std::endl;
0561 }
0562
0563
0564 double rotzx = approxTo0(rmLocal.zx());
0565 double rotzy = approxTo0(rmLocal.zy());
0566 double rotzz = approxTo0(rmLocal.zz());
0567 double rotyx = approxTo0(rmLocal.yx());
0568 double rotxx = approxTo0(rmLocal.xx());
0569 if (rotzy == 0. && rotzz == 0.) {
0570
0571 newang[0] = angleX;
0572
0573 if (eq2ang(rmLocal.zx(), -1.)) {
0574 double aa = asin(rmLocal.xy());
0575 if (diff2pi(angleZ, -aa + newang[0]) < diff2pi(angleZ, -pii + aa + newang[0])) {
0576 newang[2] = -aa + newang[0];
0577 if (ALIUtils::debug >= 5)
0578 std::cout << " newang[0] = -aa + newang[0] " << std::endl;
0579 } else {
0580 newang[2] = -pii + aa + newang[0];
0581 if (ALIUtils::debug >= 5)
0582 std::cout << " newang[0] = -pii + aa + newang[0] " << newang[0] << " " << aa << " " << newang[2] << std::endl;
0583 }
0584 } else {
0585 double aa = asin(-rmLocal.xy());
0586 if (diff2pi(angleZ, aa - newang[0]) < diff2pi(angleZ, pii - aa - newang[0])) {
0587 newang[2] = aa - newang[0];
0588 if (ALIUtils::debug >= 5)
0589 std::cout << " newang[0] = aa - newang[2] " << std::endl;
0590 } else {
0591 newang[2] = pii - aa - newang[0];
0592 if (ALIUtils::debug >= 5)
0593 std::cout << " newang[0] = pii - aa - newang[2] " << newang[0] << " " << aa << " " << newang[2] << std::endl;
0594 }
0595 }
0596 } else {
0597 newang[0] = atan(rotzy / rotzz);
0598 newang[2] = atan(rotyx / rotxx);
0599 }
0600 if (rotzx < -1.) {
0601
0602 rotzx = -1.;
0603 } else if (rotzx > 1.) {
0604
0605 rotzx = 1.;
0606 }
0607 newang[1] = -asin(rotzx);
0608 if (ALIUtils::debug >= 5)
0609 std::cout << "First calculation of angles: " << std::endl
0610 << " newang[0] " << newang[0] << " " << rotzy << " " << rotzz << std::endl
0611 << " newang[1] " << newang[1] << " " << rotzx << std::endl
0612 << " newang[2] " << newang[2] << " " << rotyx << " " << rotxx << std::endl;
0613
0614
0615
0616
0617 double rotnewxx = cos(newang[1]) * cos(newang[2]);
0618 double rotnewzz = cos(newang[0]) * cos(newang[1]);
0619 double rotnewxy = sin(newang[0]) * sin(newang[1]) * cos(newang[2]) - cos(newang[0]) * sin(newang[2]);
0620 double rotnewxz = cos(newang[0]) * sin(newang[1]) * cos(newang[2]) + sin(newang[0]) * sin(newang[2]);
0621 double rotnewyy = sin(newang[0]) * sin(newang[1]) * sin(newang[2]) + cos(newang[0]) * cos(newang[2]);
0622 double rotnewyz = cos(newang[0]) * sin(newang[1]) * sin(newang[2]) - sin(newang[0]) * cos(newang[2]);
0623
0624 bool eqxx = eq2ang(rotnewxx, rmLocal.xx());
0625 bool eqzz = eq2ang(rotnewzz, rmLocal.zz());
0626 bool eqxy = eq2ang(rotnewxy, rmLocal.xy());
0627 bool eqxz = eq2ang(rotnewxz, rmLocal.xz());
0628 bool eqyy = eq2ang(rotnewyy, rmLocal.yy());
0629 bool eqyz = eq2ang(rotnewyz, rmLocal.yz());
0630
0631
0632 if (ALIUtils::debug >= 5) {
0633 std::cout << " pred rm.xx " << rotnewxx << " =? " << rmLocal.xx() << " pred rm.zz " << rotnewzz << " =? "
0634 << rmLocal.zz() << std::endl;
0635 std::cout << " eqxx " << eqxx << " eqzz " << eqzz << std::endl;
0636
0637 }
0638
0639 if (eqxx & !eqzz) {
0640 newang[0] = pii + newang[0];
0641 if (ALIUtils::debug >= 5)
0642 std::cout << " change newang[0] " << newang[0] << std::endl;
0643 } else if (!eqxx & !eqzz) {
0644 newang[1] = pii - newang[1];
0645 if (ALIUtils::debug >= 5)
0646 std::cout << " change newang[1] " << newang[1] << std::endl;
0647 } else if (!eqxx & eqzz) {
0648 newang[2] = pii + newang[2];
0649 if (ALIUtils::debug >= 5)
0650 std::cout << " change newang[2] " << newang[2] << std::endl;
0651 }
0652
0653
0654 if (ALIUtils::debug >= 5) {
0655 std::cout << " pred rm.xy " << rotnewxy << " =? " << rmLocal.xy() << " pred rm.xz " << rotnewxz << " =? "
0656 << rmLocal.xz() << " pred rm.yy " << rotnewyy << " =? " << rmLocal.yy() << " pred rm.yz " << rotnewyz
0657 << " =? " << rmLocal.yz() << std::endl;
0658 std::cout << " eqxy " << eqxy << " eqxz " << eqxz << " eqyy " << eqyy << " eqyz " << eqyz << std::endl;
0659 }
0660
0661 if (!eqxy || !eqxz || !eqyy || !eqyz) {
0662
0663 if (ALIUtils::debug >= 5)
0664 std::cout << " change the 3 newang " << std::endl;
0665 newang[0] = addPii(newang[0]);
0666 newang[1] = pii - newang[1];
0667 newang[2] = addPii(newang[2]);
0668 double rotnewxy = -sin(newang[0]) * sin(newang[1]) * cos(newang[2]) - cos(newang[0]) * sin(newang[2]);
0669 double rotnewxz = -cos(newang[0]) * sin(newang[1]) * cos(newang[2]) - sin(newang[0]) * sin(newang[2]);
0670 if (ALIUtils::debug >= 5)
0671 std::cout << " rotnewxy " << rotnewxy << " = " << rmLocal.xy() << " rotnewxz " << rotnewxz << " = "
0672 << rmLocal.xz() << std::endl;
0673 }
0674 if (diff2pi(angleX, newang[0]) + diff2pi(angleY, newang[1]) + diff2pi(angleZ, newang[2]) >
0675 diff2pi(angleX, pii + newang[0]) + diff2pi(angleY, pii - newang[1]) + diff2pi(angleZ, pii + newang[2])) {
0676
0677 if (ALIUtils::debug >= 5)
0678 std::cout << " change the 3 newang " << std::endl;
0679 newang[0] = addPii(newang[0]);
0680 newang[1] = pii - newang[1];
0681 newang[2] = addPii(newang[2]);
0682 double rotnewxy = -sin(newang[0]) * sin(newang[1]) * cos(newang[2]) - cos(newang[0]) * sin(newang[2]);
0683 double rotnewxz = -cos(newang[0]) * sin(newang[1]) * cos(newang[2]) - sin(newang[0]) * sin(newang[2]);
0684 if (ALIUtils::debug >= 5)
0685 std::cout << " rotnewxy " << rotnewxy << " = " << rmLocal.xy() << " rotnewxz " << rotnewxz << " = "
0686 << rmLocal.xz() << std::endl;
0687 }
0688
0689 for (int ii = 0; ii < 3; ii++) {
0690 newang[ii] = approxTo0(newang[ii]);
0691 }
0692
0693
0694 if (checkMatrixEquations(newang[0], newang[1], newang[2], rmLocal) != 0) {
0695 std::cerr << " wrong rotation matrix " << newang[0] << " " << newang[1] << " " << newang[2] << std::endl;
0696 ALIUtils::dumprm(rmLocal, " matrix is ");
0697 }
0698 if (ALIUtils::debug >= 5) {
0699 std::cout << "Final angles: newang[0] " << newang[0] << " newang[1] " << newang[1] << " newang[2] " << newang[2]
0700 << std::endl;
0701 CLHEP::HepRotation rot;
0702 rot.rotateX(newang[0]);
0703 ALIUtils::dumprm(rot, " new rot after X ");
0704 rot.rotateY(newang[1]);
0705 ALIUtils::dumprm(rot, " new rot after Y ");
0706 rot.rotateZ(newang[2]);
0707 ALIUtils::dumprm(rot, " new rot ");
0708 ALIUtils::dumprm(rmLocal, " rmLocal ");
0709
0710 }
0711
0712
0713 return newang;
0714 }
0715
0716
0717 double ALIUtils::diff2pi(double ang1, double ang2) {
0718 double pii = acos(0.) * 2;
0719 double diff = fabs(ang1 - ang2);
0720 diff = diff - int(diff / 2. / pii) * 2 * pii;
0721 return diff;
0722 }
0723
0724
0725 bool ALIUtils::eq2ang(double ang1, double ang2) {
0726 bool beq = true;
0727
0728 double pii = acos(0.) * 2;
0729 double diff = diff2pi(ang1, ang2);
0730 if (diff > 0.00001) {
0731 if (fabs(diff - 2 * pii) > 0.00001) {
0732
0733 beq = false;
0734 }
0735 } else {
0736 beq = true;
0737 }
0738
0739 return beq;
0740 }
0741
0742
0743 double ALIUtils::approxTo0(double val) {
0744 double precision = 1.e-9;
0745 if (fabs(val) < precision)
0746 val = 0;
0747 return val;
0748 }
0749
0750
0751 double ALIUtils::addPii(double val) {
0752 if (val < M_PI) {
0753 val += M_PI;
0754 } else {
0755 val -= M_PI;
0756 }
0757
0758 return val;
0759 }
0760
0761
0762 int ALIUtils::checkMatrixEquations(double angleX, double angleY, double angleZ, const CLHEP::HepRotation& rot) {
0763 double sx = sin(angleX);
0764 double cx = cos(angleX);
0765 double sy = sin(angleY);
0766 double cy = cos(angleY);
0767 double sz = sin(angleZ);
0768 double cz = cos(angleZ);
0769
0770 double rotxx = cy * cz;
0771 double rotxy = sx * sy * cz - cx * sz;
0772 double rotxz = cx * sy * cz + sx * sz;
0773 double rotyx = cy * sz;
0774 double rotyy = sx * sy * sz + cx * cz;
0775 double rotyz = cx * sy * sz - sx * cz;
0776 double rotzx = -sy;
0777 double rotzy = sx * cy;
0778 double rotzz = cx * cy;
0779
0780 int matrixElemBad = 0;
0781 if (!eq2ang(rot.xx(), rotxx) || !eq2ang(rot.xy(), rotxy) || !eq2ang(rot.xz(), rotxz) || !eq2ang(rot.yx(), rotyx) ||
0782 !eq2ang(rot.yy(), rotyy) || !eq2ang(rot.yz(), rotyz) || !eq2ang(rot.zx(), rotzx) || !eq2ang(rot.zy(), rotzy) ||
0783 !eq2ang(rot.zz(), rotzz)) {
0784 matrixElemBad++;
0785 }
0786
0787 return matrixElemBad;
0788 }