PTC Conformity for Misalignment

src/mad_dynmap.cpp

@@ -378,14 +378,27 @@ template <typename M, typename R=M::R>
 inline void misalignent (cflw<M> &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>(m,  1, R(m.dthe));
     xrotation<M>(m, -1, R(m.dphi));
     srotation<M>(m,  1, R(m.dpsi));
   }
 
-  if (m.trn)
+  if (m.rot && m.trn) {
+    translate<M>(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, m.sdir, R(m.dx), R(m.dy), R(m.ds));
+  }
 
   if (m.rot && m.sdir < 0) {
     srotation<M>(m, -1, R(m.dpsi));
@@ -400,32 +413,32 @@ inline void misalignexi (cflw<M> &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>(m,  1, R(m.dpsi)-(a[2]-v[2]));
-    xrotation<M>(m,  1,-R(m.dphi)+(a[0]+v[0]));
-    yrotation<M>(m,  1, R(m.dthe)-(a[1]-v[1]));
+    srotation<M>(m,  1, R(m.dpsi)-m.edir*(a[2]-v[2]));
+    xrotation<M>(m,  1,-R(m.dphi)+m.edir*(a[0]+v[0]));
+    yrotation<M>(m,  1, R(m.dthe)-m.edir*(a[1]-v[1]));
   }
 
-  if (m.trn) translate<M>(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, -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>(m,  1,-R(m.dthe)+(a[1]-v[1]));
-    xrotation<M>(m,  1, R(m.dphi)-(a[0]+v[0]));
-    srotation<M>(m,  1,-R(m.dpsi)+(a[2]-v[2]));
+    yrotation<M>(m,  1,-R(m.dthe)+m.edir*(a[1]-v[1]));
+    xrotation<M>(m,  1, R(m.dphi)-m.edir*(a[0]+v[0]));
+    srotation<M>(m,  1,-R(m.dpsi)+m.edir*(a[2]-v[2]));
   }
   mdump(1);
 }

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 (theta . -phi . 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