RealOpenMM** velData = const_cast<RealOpenMM**>(((BrookFloatStreamImpl&) velocities.getImpl()).getData()); // Brook code needs to be made const correct
double energy = 0.0;
for (size_t i = 0; i < masses.size(); ++i)
energy += masses[i]*(velData[i][0]*velData[i][0]+velData[i][1]*velData[i][1]+velData[i][2]*velData[i][2]);