| 1 |
|
|---|
| 2 | #include <cmath>
|
|---|
| 3 |
|
|---|
| 4 | #include "bancroft.h"
|
|---|
| 5 | #include "bncconst.h"
|
|---|
| 6 |
|
|---|
| 7 | void bancroft(const Matrix& BBpass, ColumnVector& pos) {
|
|---|
| 8 |
|
|---|
| 9 | if (pos.Nrows() != 4) {
|
|---|
| 10 | pos.ReSize(4);
|
|---|
| 11 | }
|
|---|
| 12 | pos = 0.0;
|
|---|
| 13 |
|
|---|
| 14 | for (int iter = 1; iter <= 2; iter++) {
|
|---|
| 15 | Matrix BB = BBpass;
|
|---|
| 16 | int mm = BB.Nrows();
|
|---|
| 17 | for (int ii = 1; ii <= mm; ii++) {
|
|---|
| 18 | double xx = BB(ii,1);
|
|---|
| 19 | double yy = BB(ii,2);
|
|---|
| 20 | double traveltime = 0.072;
|
|---|
| 21 | if (iter > 1) {
|
|---|
| 22 | double zz = BB(ii,3);
|
|---|
| 23 | double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
|
|---|
| 24 | (yy-pos(2)) * (yy-pos(2)) +
|
|---|
| 25 | (zz-pos(3)) * (zz-pos(3)) );
|
|---|
| 26 | traveltime = rho / t_CST::c;
|
|---|
| 27 | }
|
|---|
| 28 | double angle = traveltime * t_CST::omega;
|
|---|
| 29 | double cosa = cos(angle);
|
|---|
| 30 | double sina = sin(angle);
|
|---|
| 31 | BB(ii,1) = cosa * xx + sina * yy;
|
|---|
| 32 | BB(ii,2) = -sina * xx + cosa * yy;
|
|---|
| 33 | }
|
|---|
| 34 |
|
|---|
| 35 | Matrix BBB;
|
|---|
| 36 | if (mm > 4) {
|
|---|
| 37 | SymmetricMatrix hlp; hlp << BB.t() * BB;
|
|---|
| 38 | BBB = hlp.i() * BB.t();
|
|---|
| 39 | }
|
|---|
| 40 | else {
|
|---|
| 41 | BBB = BB.i();
|
|---|
| 42 | }
|
|---|
| 43 | ColumnVector ee(mm); ee = 1.0;
|
|---|
| 44 | ColumnVector alpha(mm); alpha = 0.0;
|
|---|
| 45 | for (int ii = 1; ii <= mm; ii++) {
|
|---|
| 46 | alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
|
|---|
| 47 | }
|
|---|
| 48 | ColumnVector BBBe = BBB * ee;
|
|---|
| 49 | ColumnVector BBBalpha = BBB * alpha;
|
|---|
| 50 | double aa = lorentz(BBBe, BBBe);
|
|---|
| 51 | double bb = lorentz(BBBe, BBBalpha)-1;
|
|---|
| 52 | double cc = lorentz(BBBalpha, BBBalpha);
|
|---|
| 53 | double root = sqrt(bb*bb-aa*cc);
|
|---|
| 54 |
|
|---|
| 55 | Matrix hlpPos(4,2);
|
|---|
| 56 | hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
|
|---|
| 57 | hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
|
|---|
| 58 |
|
|---|
| 59 | ColumnVector omc(2);
|
|---|
| 60 | for (int pp = 1; pp <= 2; pp++) {
|
|---|
| 61 | hlpPos(4,pp) = -hlpPos(4,pp);
|
|---|
| 62 | omc(pp) = BB(1,4) -
|
|---|
| 63 | sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
|
|---|
| 64 | (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
|
|---|
| 65 | (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
|
|---|
| 66 | hlpPos(4,pp);
|
|---|
| 67 | }
|
|---|
| 68 | if ( fabs(omc(1)) > fabs(omc(2)) ) {
|
|---|
| 69 | pos = hlpPos.Column(2);
|
|---|
| 70 | }
|
|---|
| 71 | else {
|
|---|
| 72 | pos = hlpPos.Column(1);
|
|---|
| 73 | }
|
|---|
| 74 | }
|
|---|
| 75 | }
|
|---|
| 76 |
|
|---|
| 77 | double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
|
|---|
| 78 | return aa(1)*bb(1) + aa(2)*bb(2) + aa(3)*bb(3) - aa(4)*bb(4);
|
|---|
| 79 | }
|
|---|
