| 76082cd6 |
1 | // $Header$ |
| 2 | |
| 3 | // Copyright (C) 1999-2005, Matevz Tadel. All rights reserved. |
| 4 | // This file is part of GLED, released under GNU General Public License version 2. |
| 5 | // For the licensing terms see $GLEDSYS/LICENSE or http://www.gnu.org/. |
| 6 | |
| 7 | //______________________________________________________________________ |
| 8 | // ZTrans |
| 9 | // |
| 10 | // ZTrans is a 4x4 transformation matrix for homogeneous coordinates |
| 11 | // stored internaly in a column-major order to allow direct usage by |
| 12 | // GL. The element type is Double32_t as statically the floats would |
| 13 | // be precise enough but continuous operations on the matrix must |
| 14 | // retain precision of column vectors. |
| 15 | // |
| 16 | // Cartan angles in mA[1-3] (+z, -y, +x) are stored for backward |
| 17 | // compatibility and will probably be removed soon. |
| 18 | // |
| 19 | // Direct element access (first two should be used with care): |
| 20 | // operator[i] direct access to elements, i:0->15 |
| 21 | // CM(i,j) element 4*j + i; i,j:0->3 { CM ~ c-matrix } |
| 22 | // operator(i,j) element 4*(j-1) + i - 1 i,j:1->4 |
| 23 | // |
| 24 | // Column-vector access: |
| 25 | // USet Get/SetBaseVec(), Get/SetPos() and Arr[XYZT]() methods. |
| 26 | // |
| 27 | // For all methods taking the matrix indices: |
| 28 | // 1->X, 2->Y, 3->Z; 4->Position (if applicable). 0 reserved for time. |
| 29 | // |
| 30 | // Shorthands in method-names: |
| 31 | // LF ~ LocalFrame; PF ~ ParentFrame; IP ~ InPlace |
| 32 | |
| 33 | #include "ZTrans.h" |
| 34 | #include "Reve.h" |
| 35 | #include <TMath.h> |
| 0b28fd57 |
36 | #include <TClass.h> |
| 76082cd6 |
37 | |
| 38 | #include <ctype.h> |
| 39 | |
| 40 | #define F00 0 |
| 41 | #define F01 4 |
| 42 | #define F02 8 |
| 43 | #define F03 12 |
| 44 | |
| 45 | #define F10 1 |
| 46 | #define F11 5 |
| 47 | #define F12 9 |
| 48 | #define F13 13 |
| 49 | |
| 50 | #define F20 2 |
| 51 | #define F21 6 |
| 52 | #define F22 10 |
| 53 | #define F23 14 |
| 54 | |
| 55 | #define F30 3 |
| 56 | #define F31 7 |
| 57 | #define F32 11 |
| 58 | #define F33 15 |
| 59 | |
| 60 | using namespace Reve; |
| 61 | |
| 62 | ClassImp(ZTrans) |
| 63 | |
| 64 | /**************************************************************************/ |
| 65 | |
| 3c67f72c |
66 | ZTrans::ZTrans() : |
| 67 | TObject(), |
| 68 | mA1(0), mA2(0), mA3(0), bAsOK(kFALSE), |
| 69 | fUseTrans (kTRUE), |
| 70 | fEditTrans(kFALSE) |
| 71 | { |
| 72 | UnitTrans(); |
| 73 | } |
| 76082cd6 |
74 | |
| 3c67f72c |
75 | ZTrans::ZTrans(const ZTrans& t) : |
| 76 | TObject(), |
| 77 | mA1(t.mA1), mA2(t.mA2), mA3(t.mA3), bAsOK(t.bAsOK), |
| 78 | fUseTrans (t.fUseTrans), |
| 79 | fEditTrans(t.fEditTrans) |
| 80 | { |
| 81 | SetTrans(t, kFALSE); |
| 82 | } |
| 76082cd6 |
83 | |
| 56c08b98 |
84 | ZTrans::ZTrans(const Double_t arr[16]) : |
| 85 | TObject(), |
| 86 | mA1(0), mA2(0), mA3(0), bAsOK(kFALSE), |
| 87 | fUseTrans (kTRUE), |
| 88 | fEditTrans(kFALSE) |
| 89 | { |
| 90 | SetFromArray(arr); |
| 91 | } |
| 92 | |
| 93 | ZTrans::ZTrans(const Float_t arr[16]) : |
| 94 | TObject(), |
| 95 | mA1(0), mA2(0), mA3(0), bAsOK(kFALSE), |
| 96 | fUseTrans (kTRUE), |
| 97 | fEditTrans(kFALSE) |
| 98 | { |
| 99 | SetFromArray(arr); |
| 100 | } |
| 101 | |
| 76082cd6 |
102 | /**************************************************************************/ |
| 103 | |
| 104 | void ZTrans::UnitTrans() |
| 105 | { |
| 106 | // Reset matrix to unity. |
| 107 | |
| 108 | memset(M, 0, 16*sizeof(Double_t)); |
| 109 | M[F00] = M[F11] = M[F22] = M[F33] = 1; |
| 110 | mA1 = mA2 = mA3 = 0; |
| 3c67f72c |
111 | bAsOK = kTRUE; |
| 76082cd6 |
112 | } |
| 113 | |
| 114 | void ZTrans::UnitRot() |
| 115 | { |
| 116 | // Reset rotation part of the matrix to unity. |
| 117 | |
| 118 | memset(M, 0, 12*sizeof(Double_t)); |
| 119 | M[F00] = M[F11] = M[F22] = 1; |
| 120 | mA1 = mA2 = mA3 = 0; |
| 3c67f72c |
121 | bAsOK = kTRUE; |
| 76082cd6 |
122 | } |
| 123 | |
| 3c67f72c |
124 | void ZTrans::SetTrans(const ZTrans& t, Bool_t copyAngles) |
| 76082cd6 |
125 | { |
| 126 | memcpy(M, t.M, sizeof(M)); |
| 3c67f72c |
127 | if (copyAngles && t.bAsOK) { |
| 128 | bAsOK = kTRUE; |
| 129 | mA1 = t.mA1; mA2 = t.mA2; mA3 = t.mA3; |
| 130 | } else { |
| 131 | bAsOK = kFALSE; |
| 132 | } |
| 76082cd6 |
133 | } |
| 134 | |
| 56c08b98 |
135 | void ZTrans::SetFromArray(const Double_t arr[16]) |
| 136 | { |
| 137 | for(Int_t i=0; i<16; ++i) M[i] = arr[i]; |
| 138 | bAsOK = false; |
| 139 | } |
| 140 | |
| 141 | void ZTrans::SetFromArray(const Float_t arr[16]) |
| 142 | { |
| 143 | for(Int_t i=0; i<16; ++i) M[i] = arr[i]; |
| 144 | bAsOK = false; |
| 145 | } |
| 146 | |
| 76082cd6 |
147 | void ZTrans::SetupRotation(Int_t i, Int_t j, Double_t f) |
| 148 | { |
| 149 | // Setup the matrix as an elementary rotation. |
| 150 | // Optimized versions of left/right multiplication with an elementary |
| 151 | // rotation matrix are implemented in RotatePF/RotateLF. |
| 152 | // Expects identity matrix. |
| 153 | |
| 154 | if(i == j) return; |
| 155 | ZTrans& M = *this; |
| 156 | M(i,i) = M(j,j) = TMath::Cos(f); |
| 157 | Double_t s = TMath::Sin(f); |
| 158 | M(i,j) = -s; M(j,i) = s; |
| 3c67f72c |
159 | bAsOK = kFALSE; |
| 76082cd6 |
160 | } |
| 161 | |
| 162 | /**************************************************************************/ |
| 163 | |
| 164 | // OrtoNorm3 and Invert are near the bottom. |
| 165 | |
| 166 | /**************************************************************************/ |
| 167 | |
| 168 | void ZTrans::MultLeft(const ZTrans& t) |
| 169 | { |
| 170 | Double_t B[4]; |
| 171 | Double_t* C = M; |
| 172 | for(int c=0; c<4; ++c, C+=4) { |
| 173 | const Double_t* T = t.M; |
| 174 | for(int r=0; r<4; ++r, ++T) |
| 175 | B[r] = T[0]*C[0] + T[4]*C[1] + T[8]*C[2] + T[12]*C[3]; |
| 176 | C[0] = B[0]; C[1] = B[1]; C[2] = B[2]; C[3] = B[3]; |
| 177 | } |
| 3c67f72c |
178 | bAsOK = kFALSE; |
| 76082cd6 |
179 | } |
| 180 | |
| 181 | void ZTrans::MultRight(const ZTrans& t) |
| 182 | { |
| 183 | Double_t B[4]; |
| 184 | Double_t* C = M; |
| 185 | for(int r=0; r<4; ++r, ++C) { |
| 186 | const Double_t* T = t.M; |
| 187 | for(int c=0; c<4; ++c, T+=4) |
| 188 | B[c] = C[0]*T[0] + C[4]*T[1] + C[8]*T[2] + C[12]*T[3]; |
| 189 | C[0] = B[0]; C[4] = B[1]; C[8] = B[2]; C[12] = B[3]; |
| 190 | } |
| 3c67f72c |
191 | bAsOK = kFALSE; |
| 76082cd6 |
192 | } |
| 193 | |
| 194 | ZTrans ZTrans::operator*(const ZTrans& t) |
| 195 | { |
| 196 | ZTrans b(*this); |
| 197 | b.MultRight(t); |
| 198 | return b; |
| 199 | } |
| 200 | |
| 56c08b98 |
201 | /**************************************************************************/ |
| 202 | |
| 203 | void ZTrans::TransposeRotationPart() |
| 204 | { |
| 205 | Double_t x; |
| 206 | x = M[F01]; M[F01] = M[F10]; M[F10] = x; |
| 207 | x = M[F02]; M[F02] = M[F20]; M[F20] = x; |
| 208 | x = M[F12]; M[F12] = M[F21]; M[F21] = x; |
| 209 | bAsOK = false; |
| 210 | } |
| 211 | |
| 76082cd6 |
212 | /**************************************************************************/ |
| 213 | // Move & Rotate |
| 214 | /**************************************************************************/ |
| 215 | |
| 216 | void ZTrans::MoveLF(Int_t ai, Double_t amount) |
| 217 | { |
| 218 | const Double_t *C = M + 4*--ai; |
| 219 | M[F03] += amount*C[0]; M[F13] += amount*C[1]; M[F23] += amount*C[2]; |
| 220 | } |
| 221 | |
| 222 | void ZTrans::Move3LF(Double_t x, Double_t y, Double_t z) |
| 223 | { |
| 224 | M[F03] += x*M[0] + y*M[4] + z*M[8]; |
| 225 | M[F13] += x*M[1] + y*M[5] + z*M[9]; |
| 226 | M[F23] += x*M[2] + y*M[6] + z*M[10]; |
| 227 | } |
| 228 | |
| 229 | void ZTrans::RotateLF(Int_t i1, Int_t i2, Double_t amount) |
| 230 | { |
| 231 | // Rotate in local frame. Does optimised version of MultRight. |
| 232 | |
| 233 | if(i1 == i2) return; |
| 234 | // Algorithm: ZTrans a; a.SetupRotation(i1, i2, amount); MultRight(a); |
| 235 | // Optimized version: |
| 236 | const Double_t cos = TMath::Cos(amount), sin = TMath::Sin(amount); |
| 237 | Double_t b1, b2; |
| 238 | Double_t* C = M; |
| 239 | --i1 <<= 2; --i2 <<= 2; // column major |
| 240 | for(int r=0; r<4; ++r, ++C) { |
| 241 | b1 = cos*C[i1] + sin*C[i2]; |
| 242 | b2 = cos*C[i2] - sin*C[i1]; |
| 243 | C[i1] = b1; C[i2] = b2; |
| 244 | } |
| 3c67f72c |
245 | bAsOK = kFALSE; |
| 76082cd6 |
246 | } |
| 247 | |
| 248 | /**************************************************************************/ |
| 249 | |
| 250 | void ZTrans::MovePF(Int_t ai, Double_t amount) |
| 251 | { |
| 252 | M[F03 + --ai] += amount; |
| 253 | } |
| 254 | |
| 255 | void ZTrans::Move3PF(Double_t x, Double_t y, Double_t z) |
| 256 | { |
| 257 | M[F03] += x; |
| 258 | M[F13] += y; |
| 259 | M[F23] += z; |
| 260 | } |
| 261 | |
| 262 | void ZTrans::RotatePF(Int_t i1, Int_t i2, Double_t amount) |
| 263 | { |
| 264 | // Rotate in parent frame. Does optimised version of MultLeft. |
| 265 | |
| 266 | if(i1 == i2) return; |
| 267 | // Algorithm: ZTrans a; a.SetupRotation(i1, i2, amount); MultLeft(a); |
| 268 | |
| 269 | // Optimized version: |
| 270 | const Double_t cos = TMath::Cos(amount), sin = TMath::Sin(amount); |
| 271 | Double_t b1, b2; |
| 272 | Double_t* C = M; |
| 273 | --i1; --i2; |
| 274 | for(int c=0; c<4; ++c, C+=4) { |
| 275 | b1 = cos*C[i1] - sin*C[i2]; |
| 276 | b2 = cos*C[i2] + sin*C[i1]; |
| 277 | C[i1] = b1; C[i2] = b2; |
| 278 | } |
| 3c67f72c |
279 | bAsOK = kFALSE; |
| 76082cd6 |
280 | } |
| 281 | |
| 282 | /**************************************************************************/ |
| 283 | |
| 284 | void ZTrans::Move(const ZTrans& a, Int_t ai, Double_t amount) |
| 285 | { |
| 286 | const Double_t* A = a.M + 4*--ai; |
| 287 | M[F03] += amount*A[0]; |
| 288 | M[F13] += amount*A[1]; |
| 289 | M[F23] += amount*A[2]; |
| 290 | } |
| 291 | |
| 292 | void ZTrans::Move3(const ZTrans& a, Double_t x, Double_t y, Double_t z) |
| 293 | { |
| 294 | const Double_t* A = a.M; |
| 295 | M[F03] += x*A[F00] + y*A[F01] + z*A[F02]; |
| 296 | M[F13] += x*A[F10] + y*A[F11] + z*A[F12]; |
| 297 | M[F23] += x*A[F20] + y*A[F21] + z*A[F22]; |
| 298 | } |
| 299 | |
| 300 | void ZTrans::Rotate(const ZTrans& a, Int_t i1, Int_t i2, Double_t amount) |
| 301 | { |
| 302 | if(i1 == i2) return; |
| 303 | ZTrans X(a); |
| 304 | X.Invert(); |
| 305 | MultLeft(X); |
| 306 | RotatePF(i1, i2, amount); |
| 307 | MultLeft(a); |
| 3c67f72c |
308 | bAsOK = kFALSE; |
| 76082cd6 |
309 | } |
| 310 | |
| 311 | /**************************************************************************/ |
| 312 | // Base-vector interface |
| 313 | /**************************************************************************/ |
| 314 | |
| 315 | void ZTrans::SetBaseVec(Int_t b, Double_t x, Double_t y, Double_t z) |
| 316 | { |
| 317 | Double_t* C = M + 4*--b; |
| 318 | C[0] = x; C[1] = y; C[2] = z; |
| 3c67f72c |
319 | bAsOK = kFALSE; |
| 76082cd6 |
320 | } |
| 321 | |
| 322 | void ZTrans::SetBaseVec(Int_t b, const TVector3& v) |
| 323 | { |
| 324 | Double_t* C = M + 4*--b; |
| 325 | v.GetXYZ(C); |
| 3c67f72c |
326 | bAsOK = kFALSE; |
| 76082cd6 |
327 | } |
| 328 | |
| 329 | TVector3 ZTrans::GetBaseVec(Int_t b) const |
| 330 | { return TVector3(&M[4*--b]); } |
| 331 | |
| 332 | void ZTrans::GetBaseVec(Int_t b, TVector3& v) const |
| 333 | { |
| 334 | const Double_t* C = M + 4*--b; |
| 335 | v.SetXYZ(C[0], C[1], C[2]); |
| 336 | } |
| 337 | |
| 338 | /**************************************************************************/ |
| 339 | // Position interface |
| 340 | /**************************************************************************/ |
| 341 | |
| 342 | void ZTrans::SetPos(Double_t x, Double_t y, Double_t z) |
| 343 | { M[F03] = x; M[F13] = y; M[F23] = z; } |
| 344 | |
| 345 | void ZTrans::SetPos(Double_t* x) |
| 346 | { M[F03] = x[0]; M[F13] = x[1]; M[F23] = x[2]; } |
| 347 | |
| 32e219c2 |
348 | void ZTrans::SetPos(Float_t* x) |
| 349 | { M[F03] = x[0]; M[F13] = x[1]; M[F23] = x[2]; } |
| 350 | |
| 76082cd6 |
351 | void ZTrans::SetPos(const ZTrans& t) |
| 352 | { |
| 353 | const Double_t* T = t.M; |
| 354 | M[F03] = T[F03]; M[F13] = T[F13]; M[F23] = T[F23]; |
| 355 | } |
| 356 | |
| 357 | void ZTrans::GetPos(Double_t& x, Double_t& y, Double_t& z) const |
| 358 | { x = M[F03]; y = M[F13]; z = M[F23]; } |
| 359 | |
| 360 | void ZTrans::GetPos(Double_t* x) const |
| 361 | { x[0] = M[F03]; x[1] = M[F13]; x[2] = M[F23]; } |
| 362 | |
| 32e219c2 |
363 | void ZTrans::GetPos(Float_t* x) const |
| 364 | { x[0] = M[F03]; x[1] = M[F13]; x[2] = M[F23]; } |
| 365 | |
| 76082cd6 |
366 | void ZTrans::GetPos(TVector3& v) const |
| 367 | { v.SetXYZ(M[F03], M[F13], M[F23]); } |
| 368 | |
| 369 | TVector3 ZTrans::GetPos() const |
| 370 | { return TVector3(M[F03], M[F13], M[F23]); } |
| 371 | |
| 372 | /**************************************************************************/ |
| 373 | // Cardan angle interface |
| 374 | /**************************************************************************/ |
| 375 | |
| 376 | namespace { |
| 377 | inline void clamp_angle(Float_t& a) { |
| 378 | while(a < -TMath::TwoPi()) a += TMath::TwoPi(); |
| 379 | while(a > TMath::TwoPi()) a -= TMath::TwoPi(); |
| 380 | } |
| 381 | } |
| 382 | |
| 383 | void ZTrans::SetRotByAngles(Float_t a1, Float_t a2, Float_t a3) |
| 384 | { |
| 385 | // Sets Rotation part as given by angles: |
| 386 | // a1 around z, -a2 around y, a3 around x |
| 387 | clamp_angle(a1); clamp_angle(a2); clamp_angle(a3); |
| 388 | |
| 389 | Double_t A, B, C, D, E, F; |
| 390 | A = TMath::Cos(a3); B = TMath::Sin(a3); |
| 391 | C = TMath::Cos(a2); D = TMath::Sin(a2); // should be -sin(a2) for positive direction |
| 392 | E = TMath::Cos(a1); F = TMath::Sin(a1); |
| 393 | Double_t AD = A*D, BD = B*D; |
| 394 | |
| 395 | M[F00] = C*E; M[F01] = -BD*E - A*F; M[F02] = -AD*E + B*F; |
| 396 | M[F10] = C*F; M[F11] = -BD*F + A*E; M[F12] = -AD*F - B*E; |
| 397 | M[F20] = D; M[F21] = B*C; M[F22] = A*C; |
| 398 | |
| 399 | mA1 = a1; mA2 = a2; mA3 = a3; |
| 400 | bAsOK = true; |
| 401 | } |
| 402 | |
| 403 | void ZTrans::SetRotByAnyAngles(Float_t a1, Float_t a2, Float_t a3, |
| 404 | const Text_t* pat) |
| 405 | { |
| 406 | // Sets Rotation part as given by angles a1, a1, a3 and pattern pat. |
| 407 | // Pattern consists of "XxYyZz" characters. |
| 408 | // eg: x means rotate about x axis, X means rotate in negative direction |
| 409 | // xYz -> R_x(a3) * R_y(-a2) * R_z(a1); (standard Gled representation) |
| 410 | // Note that angles and pattern elements have inversed order! |
| 411 | // |
| 412 | // Implements Eulerian/Cardanian angles in a uniform way. |
| 413 | |
| 414 | int n = strspn(pat, "XxYyZz"); if(n > 3) n = 3; |
| 415 | // Build Trans ... assign ... |
| 416 | Float_t a[] = { a3, a2, a1 }; |
| 417 | UnitRot(); |
| 418 | for(int i=0; i<n; i++) { |
| 419 | if(isupper(pat[i])) a[i] = -a[i]; |
| 420 | switch(pat[i]) { |
| 421 | case 'x': case 'X': RotateLF(2, 3, a[i]); break; |
| 422 | case 'y': case 'Y': RotateLF(3, 1, a[i]); break; |
| 423 | case 'z': case 'Z': RotateLF(1, 2, a[i]); break; |
| 424 | } |
| 425 | } |
| 3c67f72c |
426 | bAsOK = kFALSE; |
| 76082cd6 |
427 | } |
| 428 | |
| 429 | void ZTrans::GetRotAngles(Float_t* x) const |
| 430 | { |
| 431 | // Get Cardan rotation angles (pattern xYz above). |
| 432 | |
| 433 | if(!bAsOK) { |
| 434 | Double_t sx, sy, sz; |
| 435 | GetScale(sx, sy, sz); |
| 436 | Double_t d = M[F20]/sx; |
| 437 | if(d>1) d=1; else if(d<-1) d=-1; // Fix numerical errors |
| 438 | mA2 = TMath::ASin(d); |
| 439 | Double_t C = TMath::Cos(mA2); |
| 440 | if(TMath::Abs(C) > 8.7e-6) { |
| 441 | mA1 = TMath::ATan2(M[F10], M[F00]); |
| 442 | mA3 = TMath::ATan2(M[F21]/sy, M[F22]/sz); |
| 443 | } else { |
| 444 | mA1 = TMath::ATan2(M[F10]/sx, M[F11]/sy); |
| 445 | mA3 = 0; |
| 446 | } |
| 447 | bAsOK = true; |
| 448 | } |
| 449 | x[0] = mA1; x[1] = mA2; x[2] = mA3; |
| 450 | } |
| 451 | |
| 452 | /**************************************************************************/ |
| 453 | // Scaling |
| 454 | /**************************************************************************/ |
| 455 | |
| 456 | void ZTrans::Scale(Double_t sx, Double_t sy, Double_t sz) |
| 457 | { |
| 458 | M[F00] *= sx; M[F10] *= sx; M[F20] *= sx; |
| 459 | M[F01] *= sy; M[F11] *= sy; M[F21] *= sy; |
| 460 | M[F02] *= sz; M[F12] *= sz; M[F22] *= sz; |
| 461 | } |
| 462 | |
| 463 | void ZTrans::GetScale(Double_t& sx, Double_t& sy, Double_t& sz) const |
| 464 | { |
| 465 | sx = TMath::Sqrt( M[F00]*M[F00] + M[F10]*M[F10] + M[F20]*M[F20] ); |
| 466 | sy = TMath::Sqrt( M[F01]*M[F01] + M[F11]*M[F11] + M[F21]*M[F21] ); |
| 467 | sz = TMath::Sqrt( M[F02]*M[F02] + M[F12]*M[F12] + M[F22]*M[F22] ); |
| 468 | } |
| 469 | |
| 470 | void ZTrans::Unscale(Double_t& sx, Double_t& sy, Double_t& sz) |
| 471 | { |
| 472 | GetScale(sx, sy, sz); |
| 473 | M[F00] /= sx; M[F10] /= sx; M[F20] /= sx; |
| 474 | M[F01] /= sy; M[F11] /= sy; M[F21] /= sy; |
| 475 | M[F02] /= sz; M[F12] /= sz; M[F22] /= sz; |
| 476 | } |
| 477 | |
| 478 | Double_t ZTrans::Unscale() |
| 479 | { |
| 480 | Double_t sx, sy, sz; |
| 481 | Unscale(sx, sy, sz); |
| 482 | return (sx + sy + sz)/3; |
| 483 | } |
| 484 | |
| 485 | /**************************************************************************/ |
| 486 | // Operations on vectors |
| 487 | /**************************************************************************/ |
| 488 | |
| 489 | void ZTrans::MultiplyIP(TVector3& v, Double_t w) const |
| 490 | { |
| 491 | v.SetXYZ(M[F00]*v.x() + M[F01]*v.y() + M[F02]*v.z() + M[F03]*w, |
| 492 | M[F10]*v.x() + M[F11]*v.y() + M[F12]*v.z() + M[F13]*w, |
| 493 | M[F20]*v.x() + M[F21]*v.y() + M[F22]*v.z() + M[F23]*w); |
| 494 | } |
| 495 | |
| 32e219c2 |
496 | void ZTrans::MultiplyIP(Double_t* v, Double_t w) const |
| 497 | { |
| 498 | Double_t r[3] = { v[0], v[1], v[2] }; |
| 499 | v[0] = M[F00]*r[0] + M[F01]*r[1] + M[F02]*r[2] + M[F03]*w; |
| 500 | v[1] = M[F10]*r[0] + M[F11]*r[1] + M[F12]*r[2] + M[F13]*w; |
| 501 | v[2] = M[F20]*r[0] + M[F21]*r[1] + M[F22]*r[2] + M[F23]*w; |
| 502 | } |
| 503 | |
| 504 | void ZTrans::MultiplyIP(Float_t* v, Double_t w) const |
| 505 | { |
| 506 | Double_t r[3] = { v[0], v[1], v[2] }; |
| 507 | v[0] = M[F00]*r[0] + M[F01]*r[1] + M[F02]*r[2] + M[F03]*w; |
| 508 | v[1] = M[F10]*r[0] + M[F11]*r[1] + M[F12]*r[2] + M[F13]*w; |
| 509 | v[2] = M[F20]*r[0] + M[F21]*r[1] + M[F22]*r[2] + M[F23]*w; |
| 510 | } |
| 511 | |
| 76082cd6 |
512 | TVector3 ZTrans::Multiply(const TVector3& v, Double_t w) const |
| 513 | { |
| 514 | return TVector3(M[F00]*v.x() + M[F01]*v.y() + M[F02]*v.z() + M[F03]*w, |
| 515 | M[F10]*v.x() + M[F11]*v.y() + M[F12]*v.z() + M[F13]*w, |
| 516 | M[F20]*v.x() + M[F21]*v.y() + M[F22]*v.z() + M[F23]*w); |
| 517 | } |
| 518 | |
| 32e219c2 |
519 | void ZTrans::Multiply(const Double_t *vin, Double_t* vout, Double_t w) const |
| 520 | { |
| 521 | vout[0] = M[F00]*vin[0] + M[F01]*vin[1] + M[F02]*vin[2] + M[F03]*w; |
| 522 | vout[1] = M[F10]*vin[0] + M[F11]*vin[1] + M[F12]*vin[1] + M[F13]*w; |
| 523 | vout[2] = M[F20]*vin[0] + M[F21]*vin[1] + M[F22]*vin[1] + M[F23]*w; |
| 524 | } |
| 525 | |
| 76082cd6 |
526 | void ZTrans::RotateIP(TVector3& v) const |
| 527 | { |
| 528 | v.SetXYZ(M[F00]*v.x() + M[F01]*v.y() + M[F02]*v.z(), |
| 529 | M[F10]*v.x() + M[F11]*v.y() + M[F12]*v.z(), |
| 530 | M[F20]*v.x() + M[F21]*v.y() + M[F22]*v.z()); |
| 531 | } |
| 532 | |
| 533 | TVector3 ZTrans::Rotate(const TVector3& v) const |
| 534 | { |
| 535 | return TVector3(M[F00]*v.x() + M[F01]*v.y() + M[F02]*v.z(), |
| 536 | M[F10]*v.x() + M[F11]*v.y() + M[F12]*v.z(), |
| 537 | M[F20]*v.x() + M[F21]*v.y() + M[F22]*v.z()); |
| 538 | } |
| 539 | |
| 540 | /**************************************************************************/ |
| 541 | // Normalization, ortogonalization |
| 542 | /**************************************************************************/ |
| 543 | |
| 544 | Double_t ZTrans::norm3_column(Int_t col) |
| 545 | { |
| 546 | Double_t* C = M + 4*--col; |
| 547 | const Double_t l = TMath::Sqrt(C[0]*C[0] + C[1]*C[1] + C[2]*C[2]); |
| 548 | C[0] /= l; C[1] /= l; C[2] /= l; |
| 549 | return l; |
| 550 | } |
| 551 | |
| 552 | Double_t ZTrans::orto3_column(Int_t col, Int_t ref) |
| 553 | { |
| 554 | Double_t* C = M + 4*--col; |
| 555 | Double_t* R = M + 4*--ref; |
| 556 | const Double_t dp = C[0]*R[0] + C[1]*R[1] + C[2]*R[2]; |
| 557 | C[0] -= R[0]*dp; C[1] -= R[1]*dp; C[2] -= R[2]*dp; |
| 558 | return dp; |
| 559 | } |
| 560 | |
| 561 | void ZTrans::OrtoNorm3() |
| 562 | { |
| 563 | norm3_column(1); |
| 564 | orto3_column(2,1); norm3_column(2); |
| 565 | M[F02] = M[F10]*M[F21] - M[F11]*M[F20]; |
| 566 | M[F12] = M[F20]*M[F01] - M[F21]*M[F00]; |
| 567 | M[F22] = M[F00]*M[F11] - M[F01]*M[F10]; |
| 568 | // cross-product faster. |
| 569 | // orto3_column(3,1); orto3_column(3,2); norm3_column(3); |
| 570 | } |
| 571 | |
| 572 | /**************************************************************************/ |
| 573 | // Inversion |
| 574 | /**************************************************************************/ |
| 575 | |
| 576 | Double_t ZTrans::Invert() |
| 577 | { |
| 578 | // Copied from ROOT's TMatrixFCramerInv. |
| 579 | |
| 580 | static const Exc_t _eh("ZTrans::Invert "); |
| 581 | |
| 582 | // Find all NECESSARY 2x2 dets: (18 of them) |
| 583 | const Double_t det2_12_01 = M[F10]*M[F21] - M[F11]*M[F20]; |
| 584 | const Double_t det2_12_02 = M[F10]*M[F22] - M[F12]*M[F20]; |
| 585 | const Double_t det2_12_03 = M[F10]*M[F23] - M[F13]*M[F20]; |
| 586 | const Double_t det2_12_13 = M[F11]*M[F23] - M[F13]*M[F21]; |
| 587 | const Double_t det2_12_23 = M[F12]*M[F23] - M[F13]*M[F22]; |
| 588 | const Double_t det2_12_12 = M[F11]*M[F22] - M[F12]*M[F21]; |
| 589 | const Double_t det2_13_01 = M[F10]*M[F31] - M[F11]*M[F30]; |
| 590 | const Double_t det2_13_02 = M[F10]*M[F32] - M[F12]*M[F30]; |
| 591 | const Double_t det2_13_03 = M[F10]*M[F33] - M[F13]*M[F30]; |
| 592 | const Double_t det2_13_12 = M[F11]*M[F32] - M[F12]*M[F31]; |
| 593 | const Double_t det2_13_13 = M[F11]*M[F33] - M[F13]*M[F31]; |
| 594 | const Double_t det2_13_23 = M[F12]*M[F33] - M[F13]*M[F32]; |
| 595 | const Double_t det2_23_01 = M[F20]*M[F31] - M[F21]*M[F30]; |
| 596 | const Double_t det2_23_02 = M[F20]*M[F32] - M[F22]*M[F30]; |
| 597 | const Double_t det2_23_03 = M[F20]*M[F33] - M[F23]*M[F30]; |
| 598 | const Double_t det2_23_12 = M[F21]*M[F32] - M[F22]*M[F31]; |
| 599 | const Double_t det2_23_13 = M[F21]*M[F33] - M[F23]*M[F31]; |
| 600 | const Double_t det2_23_23 = M[F22]*M[F33] - M[F23]*M[F32]; |
| 601 | |
| 602 | // Find all NECESSARY 3x3 dets: (16 of them) |
| 603 | const Double_t det3_012_012 = M[F00]*det2_12_12 - M[F01]*det2_12_02 + M[F02]*det2_12_01; |
| 604 | const Double_t det3_012_013 = M[F00]*det2_12_13 - M[F01]*det2_12_03 + M[F03]*det2_12_01; |
| 605 | const Double_t det3_012_023 = M[F00]*det2_12_23 - M[F02]*det2_12_03 + M[F03]*det2_12_02; |
| 606 | const Double_t det3_012_123 = M[F01]*det2_12_23 - M[F02]*det2_12_13 + M[F03]*det2_12_12; |
| 607 | const Double_t det3_013_012 = M[F00]*det2_13_12 - M[F01]*det2_13_02 + M[F02]*det2_13_01; |
| 608 | const Double_t det3_013_013 = M[F00]*det2_13_13 - M[F01]*det2_13_03 + M[F03]*det2_13_01; |
| 609 | const Double_t det3_013_023 = M[F00]*det2_13_23 - M[F02]*det2_13_03 + M[F03]*det2_13_02; |
| 610 | const Double_t det3_013_123 = M[F01]*det2_13_23 - M[F02]*det2_13_13 + M[F03]*det2_13_12; |
| 611 | const Double_t det3_023_012 = M[F00]*det2_23_12 - M[F01]*det2_23_02 + M[F02]*det2_23_01; |
| 612 | const Double_t det3_023_013 = M[F00]*det2_23_13 - M[F01]*det2_23_03 + M[F03]*det2_23_01; |
| 613 | const Double_t det3_023_023 = M[F00]*det2_23_23 - M[F02]*det2_23_03 + M[F03]*det2_23_02; |
| 614 | const Double_t det3_023_123 = M[F01]*det2_23_23 - M[F02]*det2_23_13 + M[F03]*det2_23_12; |
| 615 | const Double_t det3_123_012 = M[F10]*det2_23_12 - M[F11]*det2_23_02 + M[F12]*det2_23_01; |
| 616 | const Double_t det3_123_013 = M[F10]*det2_23_13 - M[F11]*det2_23_03 + M[F13]*det2_23_01; |
| 617 | const Double_t det3_123_023 = M[F10]*det2_23_23 - M[F12]*det2_23_03 + M[F13]*det2_23_02; |
| 618 | const Double_t det3_123_123 = M[F11]*det2_23_23 - M[F12]*det2_23_13 + M[F13]*det2_23_12; |
| 619 | |
| 620 | // Find the 4x4 det: |
| 621 | const Double_t det = M[F00]*det3_123_123 - M[F01]*det3_123_023 + |
| 622 | M[F02]*det3_123_013 - M[F03]*det3_123_012; |
| 623 | |
| 624 | if(det == 0) { |
| 625 | throw(_eh + "matrix is singular."); |
| 626 | } |
| 627 | |
| 628 | const Double_t oneOverDet = 1.0/det; |
| 629 | const Double_t mn1OverDet = - oneOverDet; |
| 630 | |
| 631 | M[F00] = det3_123_123 * oneOverDet; |
| 632 | M[F01] = det3_023_123 * mn1OverDet; |
| 633 | M[F02] = det3_013_123 * oneOverDet; |
| 634 | M[F03] = det3_012_123 * mn1OverDet; |
| 635 | |
| 636 | M[F10] = det3_123_023 * mn1OverDet; |
| 637 | M[F11] = det3_023_023 * oneOverDet; |
| 638 | M[F12] = det3_013_023 * mn1OverDet; |
| 639 | M[F13] = det3_012_023 * oneOverDet; |
| 640 | |
| 641 | M[F20] = det3_123_013 * oneOverDet; |
| 642 | M[F21] = det3_023_013 * mn1OverDet; |
| 643 | M[F22] = det3_013_013 * oneOverDet; |
| 644 | M[F23] = det3_012_013 * mn1OverDet; |
| 645 | |
| 646 | M[F30] = det3_123_012 * mn1OverDet; |
| 647 | M[F31] = det3_023_012 * oneOverDet; |
| 648 | M[F32] = det3_013_012 * mn1OverDet; |
| 649 | M[F33] = det3_012_012 * oneOverDet; |
| 650 | |
| 3c67f72c |
651 | bAsOK = kFALSE; |
| 76082cd6 |
652 | return det; |
| 653 | } |
| 654 | |
| 655 | /**************************************************************************/ |
| 656 | |
| 657 | void ZTrans::Streamer(TBuffer &R__b) |
| 658 | { |
| 659 | // Stream an object of class ZTrans. |
| 660 | |
| 661 | if (R__b.IsReading()) { |
| 662 | ZTrans::Class()->ReadBuffer(R__b, this); |
| 3c67f72c |
663 | bAsOK = kFALSE; |
| 76082cd6 |
664 | } else { |
| 665 | ZTrans::Class()->WriteBuffer(R__b, this); |
| 666 | } |
| 667 | } |
| 668 | |
| 669 | /**************************************************************************/ |
| 670 | /**************************************************************************/ |
| 671 | |
| 672 | void ZTrans::Print(Option_t* /*option*/) const |
| 673 | { |
| 674 | const Double_t* C = M; |
| 675 | for(Int_t i=0; i<4; ++i, ++C) |
| 676 | printf("%8.3f %8.3f %8.3f | %8.3f\n", C[0], C[4], C[8], C[12]); |
| 677 | } |
| 678 | |
| 679 | #include <iomanip> |
| 680 | |
| 681 | ostream& Reve::operator<<(ostream& s, const ZTrans& t) { |
| 682 | s.setf(std::ios::fixed, std::ios::floatfield); |
| 683 | s.precision(3); |
| 684 | for(Int_t i=1; i<=4; i++) |
| 685 | for(Int_t j=1; j<=4; j++) |
| 686 | s << t(i,j) << ((j==4) ? "\n" : "\t"); |
| 687 | return s; |
| 688 | } |
| 689 | |
| 690 | /**************************************************************************/ |
| 691 | // Reve stuff |
| 692 | /**************************************************************************/ |
| 693 | |
| 694 | #include <TGeoMatrix.h> |
| 695 | #include <TBuffer3D.h> |
| 696 | |
| 697 | void ZTrans::SetFrom(Double_t* carr) |
| 698 | { |
| 699 | fUseTrans = kTRUE; |
| 700 | memcpy(M, carr, 16*sizeof(Double_t)); |
| 3c67f72c |
701 | bAsOK = kFALSE; |
| 76082cd6 |
702 | } |
| 703 | |
| 704 | void ZTrans::SetFrom(const TGeoMatrix& mat) |
| 705 | { |
| 706 | fUseTrans = kTRUE; |
| 707 | const Double_t *r = mat.GetRotationMatrix(); |
| 708 | const Double_t *t = mat.GetTranslation(); |
| 709 | const Double_t *s = mat.GetScale(); |
| 710 | Double_t *m = M; |
| 711 | m[0] = r[0]*s[0]; m[1] = r[3]*s[0]; m[2] = r[6]*s[0]; m[3] = 0; m += 4; |
| 712 | m[0] = r[1]*s[1]; m[1] = r[4]*s[1]; m[2] = r[7]*s[1]; m[3] = 0; m += 4; |
| 713 | m[0] = r[2]*s[2]; m[1] = r[5]*s[2]; m[2] = r[8]*s[2]; m[3] = 0; m += 4; |
| 714 | m[0] = t[0]; m[1] = t[1]; m[2] = t[2]; m[3] = 1; |
| 3c67f72c |
715 | bAsOK = kFALSE; |
| 76082cd6 |
716 | } |
| 717 | |
| a3975d20 |
718 | void ZTrans::SetGeoHMatrix(TGeoHMatrix& mat) |
| 719 | { |
| 720 | Double_t *r = mat.GetRotationMatrix(); |
| 721 | Double_t *t = mat.GetTranslation(); |
| 722 | Double_t *s = mat.GetScale(); |
| 723 | if (fUseTrans) |
| 724 | { |
| 725 | mat.SetBit(TGeoMatrix::kGeoGenTrans); |
| 726 | Double_t *m = M; |
| 727 | GetScale(s[0], s[1], s[2]); |
| 728 | r[0] = m[0]/s[0]; r[3] = m[1]/s[0]; r[6] = m[2]/s[0]; m += 4; |
| 729 | r[1] = m[0]/s[1]; r[4] = m[1]/s[1]; r[7] = m[2]/s[1]; m += 4; |
| 730 | r[2] = m[0]/s[2]; r[5] = m[1]/s[2]; r[8] = m[2]/s[2]; m += 4; |
| 731 | t[0] = m[0]; t[1] = m[1]; t[2] = m[2]; |
| 732 | } else { |
| 733 | mat.ResetBit(TGeoMatrix::kGeoGenTrans); |
| 734 | r[0] = 1; r[3] = 0; r[6] = 0; |
| 735 | r[1] = 0; r[4] = 1; r[7] = 0; |
| 736 | r[2] = 0; r[5] = 0; r[8] = 1; |
| 737 | s[0] = s[1] = s[2] = 1; |
| 738 | t[0] = t[1] = t[2] = 0; |
| 739 | } |
| 740 | } |
| 741 | |
| 76082cd6 |
742 | void ZTrans::SetBuffer3D(TBuffer3D& buff) |
| 743 | { |
| 744 | buff.fLocalFrame = fUseTrans; |
| 10497cea |
745 | if (fUseTrans) { |
| 746 | // In phys-shape ctor the rotation part is transposed, due to |
| 747 | // TGeo's convention for rotation matrix. So we have to transpose |
| 748 | // it here, also. |
| 749 | Double_t *B = buff.fLocalMaster; |
| 750 | B[0] = M[0]; B[1] = M[4]; B[2] = M[8]; B[3] = M[3]; |
| 751 | B[4] = M[1]; B[5] = M[5]; B[6] = M[9]; B[7] = M[7]; |
| 752 | B[8] = M[2]; B[9] = M[6]; B[10] = M[10]; B[11] = M[11]; |
| 753 | B[12] = M[12]; B[13] = M[13]; B[14] = M[14]; B[15] = M[15]; |
| 754 | // Otherwise this would do: |
| 755 | // memcpy(buff.fLocalMaster, M, 16*sizeof(Double_t)); |
| 756 | } |
| 76082cd6 |
757 | } |
| 758 | |
| 759 | Bool_t ZTrans::IsScale(Double_t low, Double_t high) const |
| 760 | { |
| 761 | // Test if the transformation is a scale. |
| 762 | // To be used by ROOT TGLObject descendants that potentially need to |
| 763 | // use GL_NORMALIZE. |
| 764 | // The low/high limits are expected to be squares of acutal limits. |
| 765 | // |
| 766 | // Ideally this should be done by the TGLViewer [but is not]. |
| 767 | |
| 768 | if (!fUseTrans) return kFALSE; |
| 769 | Double_t s; |
| 770 | s = M[F00]*M[F00] + M[F10]*M[F10] + M[F20]*M[F20]; |
| 771 | if (s < low || s > high) return kTRUE; |
| 772 | s = M[F01]*M[F01] + M[F11]*M[F11] + M[F21]*M[F21]; |
| 773 | if (s < low || s > high) return kTRUE; |
| 774 | s = M[F02]*M[F02] + M[F12]*M[F12] + M[F22]*M[F22]; |
| 775 | if (s < low || s > high) return kTRUE; |
| 776 | return kFALSE; |
| 777 | } |
git://git.uio.no / u / mrichter / AliRoot.git / blame