xpos n'est pas un float.

Et je ne vois pas l'optimisation majeur ici
x_pcos=xpos*128 = x_pcos=xpos<<7;
xpos=x_pcos/128 = xpos=xpos>>7;

De plus xpos et ypos sont en short.