Minor fixes to compile under C89 (INFINITY define, moved some var declarations to function head etc).
Merge pull request #3 from andsve/master

Minor fixes to compile under C89