Optimize some functions using hand-written versions of them. Also use a lookup table for cos values