function M=trans(p)
M=[ 1 0 0 p(1,1)
    0 1 0 p(2,1)
    0 0 1 p(3,1)
    0 0 0  1];
  