Add RMSD calculation routine