Small performance enhancements

Get rid of half filled heuristic and use unsafe array indexing because
the indices should always be correct.