In general, you need to use a profiler to figure out which step in your code is the slowest. See this thread for an example of using profiler output to systematically optimise code:
Also note that LAMMPS provides a compute com/chunk that calculates the centre of mass of a “chunk” of atoms. If you can work out the chunking syntax (for example by giving each group of atoms the same molecular ID) you can calculate the centre of mass of each group of atoms, “spread” that back to each atom, and then calculate each atom’s distance from its own chunk COM (and therefore each central atom’s distance in the process).
Since you can write correct C++ code (which is more than I could when I started!), you may find it easier to copy and modify chunk computes to suit your needs instead of starting from scratch.