public void updateState() { double time = System.currentTimeMillis() / 1000d; double dt = time - lastTime; lastTime = time; vf = calcForwardSpeed(vf, dt, time); vs = calcSideSpeed(vs, dt, time); vu = calcUpSpeed(vu, dt, time); wh = calcHorizontallySpeed(wh, dt, time); Vector3d speed = new Vector3d(vf, vu, vs); speed = PointUtil.rotateZ3d(speed, angle.z); speed = PointUtil.rotateY3d(speed, angle.y); Vector3d dx = speed; dx.scale(dt); point.add(dx); Vector3d angleSpeed = new Vector3d(0, wh, 0); Vector3d dOmega = angleSpeed; dOmega.scale(dt); angle.add(dOmega); }