Fix stack overflow bug

Use vector math...