function M = MassMatrix(p)
%  Return Mass Matrix.
M = zeros(6,6);

M(1,1) = p.mass;  
M(2,2) = p.mass; 
M(3,3) = p.mass; 
M(1,5) = -p.mass*p.xG(3);
M(2,4) = p.mass*p.xG(3);
M(4,2) = M(2,4);
M(5,1) = M(1,5);

for i = 1:3
  for j = 1:3
    M(i+3,j+3) = p.I(i,j); 
  end
end
