Got invariant mass calculation working