Implement fast squareroot function and use it throughout mathlib.c