Try to check return values of trigonometric routines. For example, if you use atan2, make sure that if returned angle is negative you take an absolute value of it (not just add 2 * PI).

Good luck!

Mine was failing WA6 due to integer overflow -- remember to convert the data to float early :)