FP utils functions need optimisation #111
Description
FP's utils functions need optimisation. Using points for cosine and sine functions, distance having a square root (maybe we can have both versions, as distance comparison can be done using the squared value, which is cheaper to get), etc.