Optimize

[Taeir]

• Optimized some modes
• Use square distance instead of actual distance, to skip the square root.
• Optimized vector3f calculations (avoid creating new vectors).

[ChielBruin]

Looks good, can be merged

[Geweldig]

Should be merged.