150151152153154155156157158159160
Vect cors = new Vect(oxcor(), oycor(), ozcor()); Vect up = forward.cross(right); Vect xaxis = new Vect(1, 0, 0); Vect upxy = new Vect(up.x(), up.y(), 0); upxy = upxy.normalize(); if (up.z() > 0) { delta = -delta; }
165166167168169170171172173174175
forward = forward.rotateZ(delta); cors = cors.add(rotationPoint); Vect rightxy = new Vect(right.x(), right.y(), 0); rightxy = rightxy.normalize(); heading = StrictMath.toDegrees(rightxy.angleTo(xaxis)); oxyandzcor(cors.x(), cors.y(), cors.z()); }