Minor fixes to compile under C89 (INFINITY define, moved some var declarations to function head etc).