Moved methods / functions

[vyjur]

Har egentlig bare flyttet den ene metoden fra pathfinding til environment siden den legger bare til objekter Og så flyttet alt med vinkler og catmull og sånt inn i Pathfinding

[vyjur]

Kan teste med example.py. Har selv testet med den. Så ligger det også tester for det