Optimizing operations in heap.c

While making this file, I hadn't noticed that ( (i+1) * 2) - 1 = (i * 2) + 1
This change fixes it.