diff --git a/src/mad_dynmap.cpp b/src/mad_dynmap.cpp index 1528a2953..dac5d1d81 100644 --- a/src/mad_dynmap.cpp +++ b/src/mad_dynmap.cpp @@ -378,14 +378,27 @@ template inline void misalignent (cflw &m) { mdump(0); + num_t tb[3], t[3]={m.edir*fval(m.dx), m.edir*fval(m.dy), fval(m.ds)}; // t needs to be weighted before rotation + + if (m.rot && m.trn){ + num_t r[3*3]; + mad_mat_rotyxz(r, m.edir*fval(m.dphi),-m.edir*fval(m.dthe),-m.edir*fval(m.dpsi), false); + mad_mat_mul(r, t, tb, 3, 1, 3); + } + if (m.rot && m.sdir > 0) { yrotation(m, 1, R(m.dthe)); xrotation(m, -1, R(m.dphi)); srotation(m, 1, R(m.dpsi)); } - if (m.trn) + if (m.rot && m.trn) { + translate(m, m.sdir, + R(m.dx)-m.edir*(t[0]-tb[0]), R(m.dy)-m.edir*(t[1]-tb[1]), R(m.ds)-(t[2]-tb[2])); + } + else if (m.trn) { translate(m, m.sdir, R(m.dx), R(m.dy), R(m.ds)); + } if (m.rot && m.sdir < 0) { srotation(m, -1, R(m.dpsi)); @@ -400,32 +413,32 @@ inline void misalignexi (cflw &m) { mdump(0); num_t rb[3*3], r[3*3], - tb[3] , t[3]={fval(m.dx) , fval(m.dy) , fval(m.ds) }, - a[3]={fval(m.dphi), fval(m.dthe), fval(m.dpsi)}; + tb[3] , t[3]={m.edir*fval(m.dx) , m.edir*fval(m.dy) , fval(m.ds) }, + a[3]={m.edir*fval(m.dphi), m.edir*fval(m.dthe), m.edir*fval(m.dpsi)}; if (m.rot) mad_mat_rotyxz(r, a[0], -a[1], -a[2], true); // compute Rbar, Tbar - mad_mat_rtbar(rb, tb, fabs(m.el), fval(m.mang), fval(m.tlt), m.rot ? r:0, t); + mad_mat_rtbar(rb, tb, fabs(m.el), m.edir*fval(m.mang), fval(m.tlt), m.rot ? r:0, t); if (m.rot && m.sdir > 0) { num_t v[3]; mad_mat_torotyxz(rb, v, true); - srotation(m, 1, R(m.dpsi)-(a[2]-v[2])); - xrotation(m, 1,-R(m.dphi)+(a[0]+v[0])); - yrotation(m, 1, R(m.dthe)-(a[1]-v[1])); + srotation(m, 1, R(m.dpsi)-m.edir*(a[2]-v[2])); + xrotation(m, 1,-R(m.dphi)+m.edir*(a[0]+v[0])); + yrotation(m, 1, R(m.dthe)-m.edir*(a[1]-v[1])); } - if (m.trn) translate(m, -m.sdir, - R(m.dx)-(t[0]-tb[0]), R(m.dy)-(t[1]-tb[1]), R(m.ds)-(t[2]-tb[2])); + translate(m, -m.sdir, + R(m.dx)-m.edir*(t[0]-tb[0]), R(m.dy)-m.edir*(t[1]-tb[1]), R(m.ds)-(t[2]-tb[2])); if (m.rot && m.sdir < 0) { num_t v[3]; mad_mat_torotyxz(rb, v, true); - yrotation(m, 1,-R(m.dthe)+(a[1]-v[1])); - xrotation(m, 1, R(m.dphi)-(a[0]+v[0])); - srotation(m, 1,-R(m.dpsi)+(a[2]-v[2])); + yrotation(m, 1,-R(m.dthe)+m.edir*(a[1]-v[1])); + xrotation(m, 1, R(m.dphi)-m.edir*(a[0]+v[0])); + srotation(m, 1,-R(m.dpsi)+m.edir*(a[2]-v[2])); } mdump(1); } diff --git a/src/madl_dynmap.mad b/src/madl_dynmap.mad index 129e23af7..77199950c 100644 --- a/src/madl_dynmap.mad +++ b/src/madl_dynmap.mad @@ -371,7 +371,7 @@ function misalignent (elm, m, lw) -- -- backward: x1 = R^-1*(x1'-T) -- rotate - if rot and sdir > 0 then + if rot and sdir > 0 then -- MADX survey does Ry(theta) o Rx(-phi) o Rz(psi) (X) if not ptcmodel then yrotation(elm, m, 1, dthe) xrotation(elm, m, 1, -dphi) @@ -434,11 +434,6 @@ function misalignexi (elm, m, lw) -- -- forward : x2 = Rb^-1*(x2'-Tb) -- backward: x2' = Rb*x2+Tb - -- _T:print("T") - -- _R:print("R") - -- _Tb:print("Tb") - -- _Rb:print("Rb") - -- rotate if rot and sdir > 0 then if not ptcmodel then @@ -448,9 +443,9 @@ function misalignexi (elm, m, lw) -- yrotation(elm, m, -edir, ay) else local ax, ay, az = _Rb:torotzyx(true) - srotation(elm, m, edir, az) xrotation(elm, m, edir, ax) yrotation(elm, m, edir, ay) + srotation(elm, m, edir, az) end end