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);
}