Slightly optimized equals()
Added matrix pops to prevent openGL stack overflows