squeeze out some performance

[Robert Voigtländer]

Hi,

I try to squeeze out some performance of the code pasted on the link below.
http://pastebin.com/gMnqprST

The code will be used to continuously analyze sonar sensor data. I set this up to calculate all coordinates in a sonar cone without heavy use of trigonometry (assuming that this way is faster in the end).

I optimized as much as I could. Maybe one of you has another bright idea to squeeze out a bit more?

Thanks
Robert