Switched to using looking-at vectors instead of Euler angles

Also fixed camera jittering and added some vector functions

src/main/java/ru/windcorp/progressia/client/graphics/world/Camera.java

@@ -29,6 +29,9 @@ import glm.mat._4.Mat4;
 import glm.vec._3.Vec3;
 import ru.windcorp.progressia.client.graphics.backend.GraphicsInterface;
 import ru.windcorp.progressia.client.graphics.world.Camera.Anchor.Mode;
+import ru.windcorp.progressia.client.world.entity.NPedModel;
+import ru.windcorp.progressia.common.util.Matrices;
+import ru.windcorp.progressia.common.util.Vectors;
 
 public class Camera {
 
@@ -60,13 +63,13 @@ public class Camera {
 			}
 		}
 
-		void getCameraPosition(Vec3 output);
+		Vec3 getCameraPosition(Vec3 output);
 
-		void getCameraVelocity(Vec3 output);
+		Vec3 getCameraVelocity(Vec3 output);
 
-		float getCameraYaw();
+		Vec3 getLookingAt(Vec3 output);
 		
-		float getCameraPitch();
+		Vec3 getUpVector(Vec3 output);
 
 		Collection<Mode> getCameraModes();
 
@@ -84,14 +87,11 @@ public class Camera {
 	 */
 
 	private final Vec3 lastAnchorPosition = new Vec3();
-	private float lastAnchorYaw;
+	private final Vec3 lastAnchorLookingAt = new Vec3();
-	private float lastAnchorPitch;
+	private final Vec3 lastAnchorUpVector = new Vec3();
 
 	private final Mat4 lastCameraMatrix = new Mat4();
 
-	private final Vec3 lastAnchorLookingAt = new Vec3();
-	private final Vec3 lastAnchorUp = new Vec3();
-
 	{
 		invalidateCache();
 	}
@@ -108,6 +108,9 @@ public class Camera {
 	 */
 
 	public void apply(WorldRenderHelper helper) {
+		if (NPedModel.flag) {
+//			System.out.println("Camera.apply()");
+		}
 		applyPerspective(helper);
 		rotateCoordinateSystem(helper);
 
@@ -149,26 +152,34 @@ public class Camera {
 	}
 
 	private void applyDirection(WorldRenderHelper helper) {
-		float pitch = anchor.getCameraPitch();
+		anchor.getLookingAt(lastAnchorLookingAt);
-		float yaw = anchor.getCameraYaw();
+		anchor.getUpVector(lastAnchorUpVector);
 
-		helper.pushViewTransform()
+		lookAt(helper.pushViewTransform());
-			.rotateY(-pitch)
+	}
-			.rotateZ(-yaw);
 
-		this.lastAnchorYaw = yaw;
+	private void lookAt(Mat4 result) {
-		this.lastAnchorPitch = pitch;
+		Vec3 f = this.lastAnchorLookingAt;
+		Vec3 s = Vectors.grab3();
+		Vec3 u = Vectors.grab3();
 		
-		this.lastAnchorLookingAt.set(
+		f.cross(this.lastAnchorUpVector, s);
-			cos(pitch) * cos(yaw),
+		s.normalize();
-			cos(pitch) * sin(yaw),
+		
-			sin(pitch)
+		s.cross(f, u);
-		);
+		
-		this.lastAnchorUp.set(
+		Mat4 workspace = Matrices.grab4();
-			cos(pitch + PI_F / 2) * cos(yaw),
+		workspace.set(
-			cos(pitch + PI_F / 2) * sin(yaw),
+			+f.x, -s.x, +u.x,    0,
-			sin(pitch + PI_F / 2)
+			+f.y, -s.y, +u.y,    0,
+			+f.z, -s.z, +u.z,    0,
+			   0,    0,    0,    1
 		);
+		result.mul(workspace);
+		Matrices.release(workspace);
+		
+		Vectors.release(s);
+		Vectors.release(u);
 	}
 
 	private void applyPosition(WorldRenderHelper helper) {
@@ -247,8 +258,6 @@ public class Camera {
 
 	private void invalidateCache() {
 		this.lastAnchorPosition.set(Float.NaN);
-		this.lastAnchorYaw = Float.NaN;
-		this.lastAnchorPitch = Float.NaN;
 
 		this.lastCameraMatrix.set(
 			Float.NaN,
@@ -270,7 +279,7 @@ public class Camera {
 		);
 
 		this.lastAnchorLookingAt.set(Float.NaN);
-		this.lastAnchorUp.set(Float.NaN);
+		this.lastAnchorUpVector.set(Float.NaN);
 	}
 
 	public Anchor.Mode getMode() {
@@ -289,14 +298,6 @@ public class Camera {
 		return currentModeIndex;
 	}
 
-	public float getLastAnchorYaw() {
-		return lastAnchorYaw;
-	}
-
-	public float getLastAnchorPitch() {
-		return lastAnchorPitch;
-	}
-
 	public Vec3 getLastAnchorPosition() {
 		return lastAnchorPosition;
 	}
@@ -310,7 +311,7 @@ public class Camera {
 	}
 
 	public Vec3 getLastAnchorUp() {
-		return lastAnchorUp;
+		return lastAnchorUpVector;
 	}
 
 }

src/main/java/ru/windcorp/progressia/client/graphics/world/EntityAnchor.java

@@ -59,24 +59,32 @@ public class EntityAnchor implements Anchor {
 	}
 
 	@Override
-	public void getCameraPosition(Vec3 output) {
+	public Vec3 getCameraPosition(Vec3 output) {
+		if (output == null) output = new Vec3();
 		model.getViewPoint(output);
-		output.add(entity.getPosition());
+		output.add(model.getPosition());
+		return output;
 	}
 
 	@Override
-	public void getCameraVelocity(Vec3 output) {
+	public Vec3 getCameraVelocity(Vec3 output) {
+		if (output == null) output = new Vec3();
 		output.set(entity.getVelocity());
+		return output;
 	}
 
 	@Override
-	public float getCameraYaw() {
+	public Vec3 getLookingAt(Vec3 output) {
-		return entity.getYaw();
+		if (output == null) output = new Vec3();
+		model.getLookingAt(output);
+		return output;
 	}
 	
 	@Override
-	public float getCameraPitch() {
+	public Vec3 getUpVector(Vec3 output) {
-		return entity.getPitch();
+		if (output == null) output = new Vec3();
+		model.getUpVector(output);
+		return output;
 	}
 
 	@Override

src/main/java/ru/windcorp/progressia/client/graphics/world/Selection.java

@@ -38,10 +38,9 @@ public class Selection {
 	private BlockRay ray = new BlockRay();
 
 	public void update(WorldRender world, EntityData player) {
-		Vec3 direction = new Vec3();
 		Vec3 start = new Vec3();
+		Vec3 direction = player.getLookingAt();
 		
-		player.getLookingAtVector(direction);
 		world.getEntityRenderable(player).getViewPoint(start);
 		start.add(player.getPosition());
 

src/main/java/ru/windcorp/progressia/client/world/entity/EntityRenderable.java

@@ -19,7 +19,9 @@
 package ru.windcorp.progressia.client.world.entity;
 
 import glm.vec._3.Vec3;
+import ru.windcorp.progressia.client.graphics.backend.GraphicsInterface;
 import ru.windcorp.progressia.client.graphics.model.Renderable;
+import ru.windcorp.progressia.client.graphics.model.ShapeRenderHelper;
 import ru.windcorp.progressia.common.world.entity.EntityData;
 import ru.windcorp.progressia.common.world.generic.GenericEntity;
 
@@ -27,10 +29,35 @@ public abstract class EntityRenderable implements Renderable, GenericEntity {
 
 	private final EntityData data;
 	
+	private long stateComputedForFrame = -1;
+
 	public EntityRenderable(EntityData data) {
 		this.data = data;
 	}
 
+	/**
+	 * Updates the state of this model. This method is invoked exactly once per
+	 * renderable per frame before this entity is queried for the first time.
+	 */
+	protected void update() {
+		// Do nothing
+	}
+	
+	private void updateIfNecessary() {
+		if (stateComputedForFrame != GraphicsInterface.getFramesRendered()) {
+			update();
+			stateComputedForFrame = GraphicsInterface.getFramesRendered();
+		}
+	}
+	
+	@Override
+	public final void render(ShapeRenderHelper renderer) {
+		updateIfNecessary();
+		doRender(renderer);
+	}
+	
+	protected abstract void doRender(ShapeRenderHelper renderer);
+
 	public EntityData getData() {
 		return data;
 	}
@@ -45,7 +72,36 @@ public abstract class EntityRenderable implements Renderable, GenericEntity {
 		return getData().getId();
 	}
 
-	public void getViewPoint(Vec3 output) {
+	public final Vec3 getLookingAt(Vec3 output) {
+		if (output == null) output = new Vec3();
+		updateIfNecessary();
+		doGetLookingAt(output);
+		return output;
+	}
+	
+	protected void doGetLookingAt(Vec3 output) {
+		output.set(getData().getLookingAt());
+	}
+
+	public final Vec3 getUpVector(Vec3 output) {
+		if (output == null) output = new Vec3();
+		updateIfNecessary();
+		doGetUpVector(output);
+		return output;
+	}
+	
+	protected void doGetUpVector(Vec3 output) {
+		output.set(getData().getUpVector());
+	}
+
+	public final Vec3 getViewPoint(Vec3 output) {
+		if (output == null) output = new Vec3();
+		updateIfNecessary();
+		doGetViewPoint(output);
+		return output;
+	}
+	
+	protected void doGetViewPoint(Vec3 output) {
 		output.set(0, 0, 0);
 	}
 

src/main/java/ru/windcorp/progressia/client/world/entity/HumanoidModel.java

@@ -43,6 +43,7 @@ public class HumanoidModel extends NPedModel {
 
 		@Override
 		protected void applyTransform(Mat4 mat, NPedModel model) {
+			super.applyTransform(mat, model);
 			float phase = model.getWalkingFrequency() * model.getWalkingParameter() + animationOffset;
 			float value = sin(phase);
 			float amplitude = getSwingAmplitude((HumanoidModel) model) * model.getVelocityParameter();

src/main/java/ru/windcorp/progressia/client/world/entity/NPedModel.java

@@ -18,11 +18,8 @@
  
 package ru.windcorp.progressia.client.world.entity;
 
-import static java.lang.Math.atan2;
-import static java.lang.Math.min;
 import static java.lang.Math.pow;
 import static java.lang.Math.toRadians;
-import static ru.windcorp.progressia.common.util.FloatMathUtil.normalizeAngle;
 
 import glm.Glm;
 import glm.mat._4.Mat4;
@@ -32,6 +29,9 @@ import ru.windcorp.progressia.client.graphics.backend.GraphicsInterface;
 import ru.windcorp.progressia.client.graphics.model.Renderable;
 import ru.windcorp.progressia.client.graphics.model.ShapeRenderHelper;
 import ru.windcorp.progressia.common.Units;
+import ru.windcorp.progressia.common.util.FloatMathUtil;
+import ru.windcorp.progressia.common.util.VectorUtil;
+import ru.windcorp.progressia.common.util.Vectors;
 import ru.windcorp.progressia.common.world.entity.EntityData;
 
 public abstract class NPedModel extends EntityRenderable {
@@ -51,29 +51,30 @@ public abstract class NPedModel extends EntityRenderable {
 			ShapeRenderHelper renderer,
 			NPedModel model
 		) {
-			renderer.pushTransform().translate(translation);
 			applyTransform(renderer.pushTransform(), model);
 			renderable.render(renderer);
 			renderer.popTransform();
-			renderer.popTransform();
 		}
 
-		protected abstract void applyTransform(Mat4 mat, NPedModel model);
+		protected void applyTransform(Mat4 mat, NPedModel model) {
+			mat.translate(getTranslation());
+		}
 
 		public Vec3 getTranslation() {
 			return translation;
 		}
+		
+		public Mat4 getTransform(Mat4 output, NPedModel model) {
+			if (output == null) output = new Mat4().identity();
+			applyTransform(output, model);
+			return output;
+		}
 	}
 
 	public static class Body extends BodyPart {
 		public Body(Renderable renderable) {
 			super(renderable, null);
 		}
-
-		@Override
-		protected void applyTransform(Mat4 mat, NPedModel model) {
-			// Do nothing
-		}
 	}
 
 	public static class Head extends BodyPart {
@@ -97,7 +98,8 @@ public abstract class NPedModel extends EntityRenderable {
 
 		@Override
 		protected void applyTransform(Mat4 mat, NPedModel model) {
-			mat.rotateZ(model.getHeadYaw()).rotateY(model.getHeadPitch());
+			super.applyTransform(mat, model);
+			mat.rotateZ(-model.getHeadYaw()).rotateY(-model.getHeadPitch());
 		}
 
 		public Vec3 getViewPoint() {
@@ -105,6 +107,8 @@ public abstract class NPedModel extends EntityRenderable {
 		}
 	}
 
+	public static boolean flag;
+
 	protected final Body body;
 	protected final Head head;
 
@@ -128,7 +132,9 @@ public abstract class NPedModel extends EntityRenderable {
 
 	private float walkingFrequency;
 
-	private float bodyYaw = Float.NaN;
+	private final Vec3 bodyLookingAt = new Vec3().set(0);
+	private final Mat4 bodyTransform = new Mat4(); 
+	
 	private float headYaw;
 	private float headPitch;
 
@@ -138,17 +144,14 @@ public abstract class NPedModel extends EntityRenderable {
 		this.head = head;
 		this.scale = scale;
 
-		evaluateAngles();
+		computeRotations();
 	}
 
 	@Override
-	public void render(ShapeRenderHelper renderer) {
+	protected void doRender(ShapeRenderHelper renderer) {
-		renderer.pushTransform().scale(scale).rotateZ(bodyYaw);
+		renderer.pushTransform().scale(scale).mul(bodyTransform);
 		renderBodyParts(renderer);
 		renderer.popTransform();
-
-		accountForVelocity();
-		evaluateAngles();
 	}
 
 	protected void renderBodyParts(ShapeRenderHelper renderer) {
@@ -156,50 +159,107 @@ public abstract class NPedModel extends EntityRenderable {
 		head.render(renderer, this);
 	}
 	
-	private void evaluateAngles() {
+	@Override
-		float globalYaw = normalizeAngle(getData().getYaw());
+	protected void update() {
-
+		advanceTime();
-		if (Float.isNaN(bodyYaw)) {
+		computeRotations();
-			bodyYaw = globalYaw;
-			headYaw = 0;
-		} else {
-			headYaw = normalizeAngle(globalYaw - bodyYaw);
-
-			if (headYaw > +head.maxYaw) {
-				bodyYaw += headYaw - +head.maxYaw;
-				headYaw = +head.maxYaw;
-			} else if (headYaw < -head.maxYaw) {
-				bodyYaw += headYaw - -head.maxYaw;
-				headYaw = -head.maxYaw;
-			}
-		}
-
-		bodyYaw = normalizeAngle(bodyYaw);
-
-		headPitch = Glm.clamp(
-			getData().getPitch(),
-			-head.maxPitch,
-			head.maxPitch
-		);
 	}
 
-	private void accountForVelocity() {
+	private void computeRotations() {
-		Vec3 horizontal = new Vec3(getData().getVelocity());
+		if (!bodyLookingAt.any()) {
-		horizontal.z = 0;
+			getData().getForwardVector(bodyLookingAt);
+			headYaw = 0;
+		} else {
+			ensureBodyLookingAtIsPerpendicularToUpVector();
+			computeDesiredHeadYaw();
+			clampHeadYawAndChangeBodyLookingAt();
+		}
+		
+		recomputeBodyTransform();
+
+		setHeadPitch();
+	}
+
+	private void ensureBodyLookingAtIsPerpendicularToUpVector() {
+		Vec3 up = getData().getUpVector();
+		if (up.dot(bodyLookingAt) > 1 - 1e-4) return;
+		
+		Vec3 tmp = Vectors.grab3();
+		
+		tmp.set(up).mul(-up.dot(bodyLookingAt)).add(bodyLookingAt);
+		
+		float tmpLength = tmp.length();
+		if (tmpLength > 1e-4) {
+			bodyLookingAt.set(tmp).div(tmpLength);
+		} else {
+			// bodyLookingAt is suddenly parallel to up vector -- PANIC! ENTERING RESCUE MODE!
+			getData().getForwardVector(bodyLookingAt);
+		}
+		
+		Vectors.release(tmp);
+	}
+
+	private void computeDesiredHeadYaw() {
+		Vec3 newDirection = getData().getForwardVector(null);
+		Vec3 oldDirection = bodyLookingAt;
+		Vec3 up = getData().getUpVector();
+		
+		headYaw = (float) VectorUtil.getAngle(oldDirection, newDirection, up);
+	}
+
+	private void clampHeadYawAndChangeBodyLookingAt() {
+		float bodyYawChange = 0;
+		
+		if (headYaw > +head.maxYaw) {
+			bodyYawChange = headYaw - +head.maxYaw;
+			headYaw = +head.maxYaw;
+		} else if (headYaw < -head.maxYaw) {
+			bodyYawChange = headYaw - -head.maxYaw;
+			headYaw = -head.maxYaw;
+		}
+		
+		if (bodyYawChange != 0) {
+			VectorUtil.rotate(bodyLookingAt, getData().getUpVector(), bodyYawChange);
+		}
+	}
+
+	private void recomputeBodyTransform() {
+		Vec3 u = getData().getUpVector();
+		Vec3 f = bodyLookingAt;
+		Vec3 s = Vectors.grab3();
+		
+		s.set(u).cross(f);
+		
+		bodyTransform.identity().set(
+			+f.x, -s.x, +u.x,    0,
+			+f.y, -s.y, +u.y,    0,
+			+f.z, -s.z, +u.z,    0,
+			   0,    0,    0,    1
+		);
+		
+		Vectors.release(s);
+	}
+
+	private void setHeadPitch() {
+		headPitch = Glm.clamp((float) getData().getPitch(), -head.maxPitch, +head.maxPitch);
+	}
+
+	private void advanceTime() {
+		Vec3 horizontal = getData().getUpVector()
+			.mul_(-getData().getUpVector().dot(getData().getVelocity()))
+			.add(getData().getVelocity());
 
 		velocity = horizontal.length();
 
-		evaluateVelocityCoeff();
+		computeVelocityParameter();
 
 		// TODO switch to world time
 		walkingParameter += velocity * GraphicsInterface.getFrameLength() * 1000;
 		
-		bodyYaw += velocityParameter * normalizeAngle(
+		rotateBodyWithMovement(horizontal);
-			(float) (atan2(horizontal.y, horizontal.x) - bodyYaw)
-		) * min(1, GraphicsInterface.getFrameLength() * 10);
 	}
 
-	private void evaluateVelocityCoeff() {
+	private void computeVelocityParameter() {
 		if (velocity > maxEffectiveVelocity) {
 			velocityParameter = 1;
 		} else {
@@ -207,17 +267,39 @@ public abstract class NPedModel extends EntityRenderable {
 		}
 	}
 
+	private void rotateBodyWithMovement(Vec3 target) {
+		if (velocityParameter == 0 || !target.any() || Glm.equals(target, bodyLookingAt)) {
+			return;
+		}
+		
+		Vec3 axis = getData().getUpVector();
+		
+		float yawDifference = FloatMathUtil.normalizeAngle(
+			(float) VectorUtil.getAngle(
+				bodyLookingAt,
+				target.normalize_(),
+				axis
+			)
+		);
+		
+		float bodyYawChange =
+			velocityParameter *
+			yawDifference *
+			(float) Math.expm1(GraphicsInterface.getFrameLength() * 10);
+		
+		VectorUtil.rotate(bodyLookingAt, axis, bodyYawChange);
+	}
+
 	@Override
-	public void getViewPoint(Vec3 output) {
+	protected void doGetViewPoint(Vec3 output) {
 		Mat4 m = new Mat4();
 		Vec4 v = new Vec4();
 
 		m.identity()
 			.scale(scale)
-			.rotateZ(bodyYaw)
+			.mul(bodyTransform);
-			.translate(head.getTranslation())
+		
-			.rotateZ(headYaw)
+		head.getTransform(m, this);
-			.rotateY(headPitch);
 
 		v.set(head.getViewPoint(), 1);
 		m.mul(v);
@@ -233,8 +315,8 @@ public abstract class NPedModel extends EntityRenderable {
 		return head;
 	}
 	
-	public float getBodyYaw() {
+	public Vec3 getBodyLookingAt() {
-		return bodyYaw;
+		return bodyLookingAt;
 	}
 
 	public float getHeadYaw() {

src/main/java/ru/windcorp/progressia/client/world/entity/QuadripedModel.java

@@ -44,6 +44,7 @@ public class QuadripedModel extends NPedModel {
 
 		@Override
 		protected void applyTransform(Mat4 mat, NPedModel model) {
+			super.applyTransform(mat, model);
 			float phase = model.getWalkingFrequency() * model.getWalkingParameter() + animationOffset;
 			float value = sin(phase);
 			float amplitude = ((QuadripedModel) model).getWalkingSwing() * model.getVelocityParameter();

src/main/java/ru/windcorp/progressia/common/util/VectorUtil.java

@@ -20,6 +20,8 @@ package ru.windcorp.progressia.common.util;
 
 import java.util.function.Consumer;
 
+import glm.Glm;
+import glm.mat._3.Mat3;
 import glm.mat._4.Mat4;
 import glm.vec._2.Vec2;
 import glm.vec._2.d.Vec2d;
@@ -135,12 +137,72 @@ public class VectorUtil {
 	}
 
 	public static void applyMat4(Vec3 inOut, Mat4 mat) {
-		Vec4 vec4 = Vectors.grab4();
+		applyMat4(inOut, mat, inOut);
-		vec4.set(inOut, 1f);
+	}
 	
-		mat.mul(vec4);
+	public static void rotate(Vec3 in, Vec3 axis, float angle, Vec3 out) {
+		Mat3 mat = Matrices.grab3();
 		
-		inOut.set(vec4.x, vec4.y, vec4.z);
+		mat.identity().rotate(angle, axis);
+		mat.mul(in, out);
+		
+		Matrices.release(mat);
+	}
+	
+	public static void rotate(Vec3 inOut, Vec3 axis, float angle) {
+		rotate(inOut, axis, angle, inOut);
+	}
+	
+	public static double getAngle(Vec3 from, Vec3 to, Vec3 normal) {
+		Vec3 left = Vectors.grab3();
+		
+		left.set(normal).cross(from);
+		double sign = Math.signum(left.dot(to));
+		
+		double result = (float) Math.acos(Glm.clamp(from.dot(to), -1, +1)) * sign;
+		
+		Vectors.release(left);
+		return result;
+	}
+	
+	public static Vec3 projectOnSurface(Vec3 in, Vec3 normal, Vec3 out) {
+		if (in == out) {
+			return projectOnSurface(in, normal);
+		}
+		
+		if (out == null) {
+			out = new Vec3();
+		}
+		
+		out.set(normal).mul(-normal.dot(in)).add(in);
+		
+		return out;
+	}
+	
+	public static Vec3 projectOnSurface(Vec3 inOut, Vec3 normal) {
+		Vec3 buffer = Vectors.grab3();
+		
+		projectOnSurface(inOut, normal, buffer);
+		inOut.set(buffer);
+		
+		Vectors.release(buffer);
+		
+		return inOut;
+	}
+	
+	public static Vec3 projectOnVector(Vec3 in, Vec3 vector, Vec3 out) {
+		if (out == null) {
+			out = new Vec3();
+		}
+		
+		float dot = vector.dot(in);
+		out.set(vector).mul(dot);
+		
+		return out;
+	}
+	
+	public static Vec3 projectOnVector(Vec3 inOut, Vec3 vector) {
+		return projectOnVector(inOut, vector);
 	}
 
 	public static Vec3 linearCombination(

src/main/java/ru/windcorp/progressia/common/world/BlockRay.java

@@ -39,10 +39,18 @@ public class BlockRay {
 	private boolean isValid = false;
 
 	public void start(Vec3 position, Vec3 direction) {
-		if (!direction.any()) {
+		if (direction.x == 0 && direction.y == 0 && direction.z == 0) {
 			throw new IllegalArgumentException("Direction is a zero vector");
 		}
 		
+		if (Float.isNaN(direction.x) || Float.isNaN(direction.y) || Float.isNaN(direction.z)) {
+			throw new IllegalArgumentException("Direction contains NaN: " + direction);
+		}
+		
+		if (Float.isNaN(position.x) || Float.isNaN(position.y) || Float.isNaN(position.z)) {
+			throw new IllegalArgumentException("Position contains NaN: " + position);
+		}
+
 		isValid = true;
 		this.position.set(position).sub(0.5f); // Make sure lattice points are
 												// block vertices, not centers
@@ -76,6 +84,8 @@ public class BlockRay {
 			axis = Axis.Z;
 		}
 		
+		assert tMin > 0 : "tMin is not positive (" + tMin + ")";
+
 		// block.(axis) += signum(direction.(axis))
 		VectorUtil.set(block, axis, VectorUtil.get(block, axis) + (int) signum(VectorUtil.get(direction, axis)));
 

src/main/java/ru/windcorp/progressia/common/world/entity/EntityData.java

@@ -22,7 +22,6 @@ import java.io.DataInput;
 import java.io.DataOutput;
 import java.io.IOException;
 
-import glm.vec._2.Vec2;
 import glm.vec._3.Vec3;
 import ru.windcorp.jputil.chars.StringUtil;
 import ru.windcorp.progressia.common.collision.Collideable;
@@ -36,7 +35,8 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 	private final Vec3 position = new Vec3();
 	private final Vec3 velocity = new Vec3();
 
-	private final Vec2 direction = new Vec2();
+	private final Vec3 lookingAt = new Vec3(1, 0, 0);
+	private final Vec3 upVector = new Vec3(0, 0, 1);
 
 	/**
 	 * The unique {@code long} value guaranteed to never be assigned to an
@@ -79,22 +79,6 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 		this.velocity.set(velocity);
 	}
 
-	public Vec2 getDirection() {
-		return direction;
-	}
-
-	public void setDirection(Vec2 direction) {
-		this.direction.set(direction.x, direction.y);
-	}
-
-	public float getYaw() {
-		return getDirection().x;
-	}
-
-	public float getPitch() {
-		return getDirection().y;
-	}
-
 	public long getEntityId() {
 		return entityId;
 	}
@@ -152,16 +136,90 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 		getVelocity().add(velocityChange);
 	}
 
-	public Vec3 getLookingAtVector(Vec3 output) {
+	public Vec3 getLookingAt() {
-		output.set(
+		return lookingAt;
-			Math.cos(getPitch()) * Math.cos(getYaw()),
+	}
-			Math.cos(getPitch()) * Math.sin(getYaw()),
-			-Math.sin(getPitch())
-		);
 
+	public void setLookingAt(Vec3 lookingAt) {
+		float lengthSq = lookingAt.x * lookingAt.x + lookingAt.y * lookingAt.y + lookingAt.z * lookingAt.z;
+		if (lengthSq == 1) {
+			this.lookingAt.set(lookingAt);
+		} else if (lengthSq == 0) {
+			throw new IllegalArgumentException("lookingAt is zero-length");
+		} else {
+			float length = (float) Math.sqrt(lengthSq);
+			this.lookingAt.set(
+				lookingAt.x / length,
+				lookingAt.y / length,
+				lookingAt.z / length
+			);
+		}
+	}
+
+	public Vec3 getUpVector() {
+		return upVector;
+	}
+
+	/**
+	 * Sets this entity's up vector without updating looking at-vector.
+	 * 
+	 * @param upVector the Vec3 to copy up vector from
+	 * @see #changeUpVector(Vec3)
+	 */
+	public void setUpVector(Vec3 upVector) {
+		float lengthSq = upVector.x * upVector.x + upVector.y * upVector.y + upVector.z * upVector.z;
+		if (lengthSq == 1) {
+			this.upVector.set(upVector);
+		} else if (lengthSq == 0) {
+			throw new IllegalArgumentException("upVector is zero-length");
+		} else {
+			float length = (float) Math.sqrt(lengthSq);
+			this.upVector.set(
+				upVector.x / length,
+				upVector.y / length,
+				upVector.z / length
+			);
+		}
+	}
+
+	/**
+	 * Computes the forward vector of this entity. An entity's forward vector is
+	 * defined as a normalized projection of the looking at-vector onto the
+	 * plane perpendicular to up vector, or {@code (NaN; NaN; NaN)} if looking
+	 * at-vector is parallel to the up vector.
+	 * 
+	 * @param output a {@link Vec3} where the result is stored. May be
+	 *               {@code null}.
+	 * @return the computed forward vector or {@code (NaN; NaN; NaN)}
+	 */
+	public Vec3 getForwardVector(Vec3 output) {
+		if (output == null)
+			output = new Vec3();
+		output.set(getUpVector()).mul(-getUpVector().dot(getLookingAt())).add(getLookingAt()).normalize();
 		return output;
 	}
 
+	public double getPitch() {
+		return -Math.acos(getLookingAt().dot(getUpVector())) + Math.PI / 2;
+	}
+
+	/**
+	 * Updates this entity's up vector and alters looking at-vector to match the
+	 * rotation of the up vector.
+	 * <p>
+	 * This method assumes that the up vector has changed due to rotation around
+	 * some axis. The axis and the angle are computed, after which the same
+	 * rotation is applied to the looking at-vector.
+	 * 
+	 * @param newUpVector the Vec3 to copy up vector from. May be equal to
+	 *                    current up vector
+	 * @see #setLookingAt(Vec3)
+	 */
+	public void changeUpVector(Vec3 newUpVector) {
+		// TODO
+		this.upVector.set(newUpVector);
+	}
+
 	@Override
 	public String toString() {
 		return new StringBuilder(super.toString())
@@ -189,8 +247,13 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 		output.writeFloat(getVelocity().y);
 		output.writeFloat(getVelocity().z);
 
-		output.writeFloat(getDirection().x);
+		output.writeFloat(getLookingAt().x);
-		output.writeFloat(getDirection().y);
+		output.writeFloat(getLookingAt().y);
+		output.writeFloat(getLookingAt().z);
+
+		output.writeFloat(getUpVector().x);
+		output.writeFloat(getUpVector().y);
+		output.writeFloat(getUpVector().z);
 
 		super.write(output, context);
 	}
@@ -209,14 +272,22 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 			input.readFloat()
 		);
 
-		Vec2 direction = new Vec2(
+		Vec3 lookingAt = new Vec3(
+			input.readFloat(),
+			input.readFloat(),
+			input.readFloat()
+		);
+
+		Vec3 upVector = new Vec3(
+			input.readFloat(),
 			input.readFloat(),
 			input.readFloat()
 		);
 
 		setPosition(position);
 		setVelocity(velocity);
-		setDirection(direction);
+		setLookingAt(lookingAt);
+		setUpVector(upVector);
 
 		super.read(input, context);
 	}

src/main/java/ru/windcorp/progressia/server/PlayerManager.java

@@ -22,7 +22,7 @@ import java.util.ArrayList;
 import java.util.Collection;
 import java.util.Collections;
 
-import glm.vec._2.Vec2;
+import glm.vec._3.Vec3;
 import ru.windcorp.progressia.common.util.crash.CrashReports;
 import ru.windcorp.progressia.common.world.entity.EntityData;
 import ru.windcorp.progressia.common.world.entity.EntityDataRegistry;
@@ -61,12 +61,9 @@ public class PlayerManager {
 
 		player.setEntityId(TestContent.PLAYER_ENTITY_ID);
 		player.setPosition(TestContent.SPAWN);
-		player.setDirection(
+		
-			new Vec2(
+		player.setUpVector(new Vec3(0, 0, 1));
-				Math.toRadians(40),
+		player.setLookingAt(new Vec3(2, 1, 0));
-				Math.toRadians(10)
-			)
-		);
 
 		getServer().getWorld().getData().addEntity(player);
 

src/main/java/ru/windcorp/progressia/test/LayerTestUI.java

@@ -24,7 +24,6 @@ import com.google.common.eventbus.Subscribe;
 
 import glm.mat._4.Mat4;
 import glm.vec._4.Vec4;
-import ru.windcorp.progressia.client.ClientState;
 import ru.windcorp.progressia.client.graphics.Colors;
 import ru.windcorp.progressia.client.graphics.backend.GraphicsInterface;
 import ru.windcorp.progressia.client.graphics.flat.AssembledFlatLayer;
@@ -34,7 +33,6 @@ import ru.windcorp.progressia.client.graphics.input.bus.Input;
 import ru.windcorp.progressia.client.graphics.model.LambdaModel;
 import ru.windcorp.progressia.client.graphics.texture.SimpleTextures;
 import ru.windcorp.progressia.client.graphics.texture.Texture;
-import ru.windcorp.progressia.client.graphics.world.Camera;
 
 public class LayerTestUI extends AssembledFlatLayer {
 
@@ -46,9 +44,12 @@ public class LayerTestUI extends AssembledFlatLayer {
 
 	private boolean flag = false;
 
-	private static final int WIDTH = 80;
+	private static final int SCALE = 4;
-	private static final int HEIGHT = 80;
+	private static final int TEX_SIZE = 16 * SCALE;
+	private static final int TEX_SHADOW = SCALE / 2;
 	private static final int BORDER = 5;
+	private static final int HEIGHT = TEX_SIZE + 4 * BORDER;
+	private static final int WIDTH = HEIGHT;
 
 	@Override
 	protected void assemble(RenderTarget target) {
@@ -64,25 +65,22 @@ public class LayerTestUI extends AssembledFlatLayer {
 		target.fill(x, y, WIDTH, HEIGHT, borderColor);
 		target.fill(x + BORDER, y + BORDER, WIDTH - 2 * BORDER, HEIGHT - 2 * BORDER, boxColor);
 
-		final int texShadow = 2;
-		final int texSize = HEIGHT - 4 * BORDER;
-
 		target.pushTransform(new Mat4().identity().translate(x + 2 * BORDER, y + 2 * BORDER, 0));
 
 		final Texture compassBg = SimpleTextures.get("compass_icon");
 		final Texture compassFg = SimpleTextures.get("compass_icon_arrow");
 
-		target.drawTexture(texShadow, -texShadow, texSize, texSize, Colors.BLACK, compassBg);
+		target.drawTexture(TEX_SHADOW, -TEX_SHADOW, TEX_SIZE, TEX_SIZE, Colors.BLACK, compassBg);
-		target.drawTexture(0, 0, texSize, texSize, compassBg);
+		target.drawTexture(0, 0, TEX_SIZE, TEX_SIZE, compassBg);
 
 		target.addCustomRenderer(
 			new LambdaModel(
 				LambdaModel.lambdaBuilder()
 					.addDynamicPart(
-						target.createRectagle(0, 0, texSize, texSize, Colors.WHITE, compassFg),
+						target.createRectagle(0, 0, TEX_SIZE, TEX_SIZE, Colors.WHITE, compassFg),
-						mat -> mat.translate(texSize / 2, texSize / 2, 0)
+						mat -> mat.translate(TEX_SIZE / 2, TEX_SIZE / 2, 0)
 							.rotateZ(getCompassRotation())
-							.translate(-texSize / 2, -texSize / 2, 0)
+							.translate(-TEX_SIZE / 2, -TEX_SIZE / 2, 0)
 					)
 			)
 		);
@@ -92,12 +90,7 @@ public class LayerTestUI extends AssembledFlatLayer {
 	}
 
 	private double getCompassRotation() {
-		Camera.Anchor anchor = ClientState.getInstance().getCamera().getAnchor();
+		return 0;
-
-		if (anchor == null)
-			return 0;
-
-		return -anchor.getCameraYaw();
 	}
 
 	private void drawCross(RenderTarget target) {

src/main/java/ru/windcorp/progressia/test/TestEntityRenderStatie.java

@@ -44,7 +44,7 @@ public class TestEntityRenderStatie extends EntityRender {
 	public EntityRenderable createRenderable(EntityData entity) {
 		return new EntityRenderable(entity) {
 			@Override
-			public void render(ShapeRenderHelper renderer) {
+			public void doRender(ShapeRenderHelper renderer) {
 				renderer.pushTransform().scale(
 					((TestEntityDataStatie) entity).getSize() / 24.0f
 				);

src/main/java/ru/windcorp/progressia/test/TestPlayerControls.java

@@ -20,7 +20,7 @@ package ru.windcorp.progressia.test;
 
 import glm.Glm;
 import glm.mat._3.Mat3;
-import glm.vec._2.Vec2;
+import glm.mat._4.Mat4;
 import glm.vec._3.Vec3;
 import org.lwjgl.glfw.GLFW;
 import ru.windcorp.progressia.client.ClientState;
@@ -36,7 +36,9 @@ import ru.windcorp.progressia.client.graphics.input.bus.Input;
 import ru.windcorp.progressia.client.graphics.world.LocalPlayer;
 import ru.windcorp.progressia.client.localization.Localizer;
 import ru.windcorp.progressia.common.Units;
-import ru.windcorp.progressia.common.util.FloatMathUtil;
+import ru.windcorp.progressia.common.util.Matrices;
+import ru.windcorp.progressia.common.util.VectorUtil;
+import ru.windcorp.progressia.common.util.Vectors;
 import ru.windcorp.progressia.common.world.block.BlockData;
 import ru.windcorp.progressia.common.world.entity.EntityData;
 import ru.windcorp.progressia.common.world.tile.TileData;
@@ -108,17 +110,17 @@ public class TestPlayerControls {
 			authority = WALKING_CONTROL_AUTHORITY;
 		}
 
-		Mat3 angMat = new Mat3().identity().rotateZ(player.getYaw());
+		Mat3 movementTransform = getMovementTransform(player, null);
-		Vec3 desiredVelocity = new Vec3(movementForward, -movementRight, 0);
+		Vec3 desiredVelocity = new Vec3(movementForward, movementRight, 0);
-
+		if (movementForward != 0 && movementRight != 0) {
-		if (movementForward != 0 && movementRight != 0)
 			desiredVelocity.normalize();
-		angMat.mul_(desiredVelocity); // bug in jglm, .mul() and mul_() are
+		}
-										// swapped
 		desiredVelocity.z = movementUp;
+		movementTransform.mul_(desiredVelocity); // bug in jglm, .mul() and mul_() are
+										         // swapped
 		desiredVelocity.mul(speed);
 
-		Vec3 change = new Vec3()
+		Vec3 newVelocity = new Vec3()
 			.set(desiredVelocity)
 			.sub(player.getVelocity())
 			.mul((float) Math.exp(-authority * GraphicsInterface.getFrameLength()))
@@ -126,10 +128,12 @@ public class TestPlayerControls {
 			.add(desiredVelocity);
 
 		if (!isFlying) {
-			change.z = player.getVelocity().z;
+			Vec3 up = player.getUpVector();
+			Vec3 wantedVertical = VectorUtil.projectOnVector(player.getVelocity(), up, null);
+			VectorUtil.projectOnSurface(newVelocity, up).add(wantedVertical);
 		}
 
-		player.getVelocity().set(change);
+		player.getVelocity().set(newVelocity);
 
 		// THIS IS TERRIBLE TEST
 		EntityData serverEntity = ServerState.getInstance().getWorld().getData()
@@ -140,6 +144,22 @@ public class TestPlayerControls {
 
 	}
 
+	private Mat3 getMovementTransform(EntityData player, Mat3 mat) {
+		if (mat == null) {
+			mat = new Mat3();
+		}
+		
+		Vec3 f = player.getForwardVector(null);
+		Vec3 u = player.getUpVector();
+		Vec3 s = u.cross_(f);
+		
+		return mat.set(
+			+f.x, -s.x, +u.x,
+			+f.y, -s.y, +u.y,
+			+f.z, -s.z, +u.z
+		);
+	}
+
 	public void handleInput(Input input) {
 		InputEvent event = input.getEvent();
 
@@ -318,36 +338,44 @@ public class TestPlayerControls {
 	}
 
 	private void onMouseMoved(CursorMoveEvent event) {
-		if (!captureMouse)
+		if (!captureMouse) {
 			return;
+		}
 
 		if (ClientState.getInstance() == null || !ClientState.getInstance().isReady()) {
 			return;
 		}
 
-		final float yawScale = -0.002f;
+		final double yawScale = -0.002f;
-		final float pitchScale = yawScale;
+		final double pitchScale = -yawScale;
+		final double pitchExtremum = Math.PI/2 * 0.95f;
+		
+		double yawChange = event.getChangeX() * yawScale;
+		double pitchChange = event.getChangeY() * pitchScale;
 
 		EntityData player = getEntity();
 		
-		normalizeAngles(
+		double startPitch = player.getPitch();
-			player.getDirection().add(
+		double endPitch = startPitch + pitchChange;
-				(float) (event.getChangeX() * yawScale),
+		endPitch = Glm.clamp(endPitch, -pitchExtremum, +pitchExtremum);
-				(float) (event.getChangeY() * pitchScale)
+		pitchChange = endPitch - startPitch;
-			)
-		);
-	}
 		
-	private void normalizeAngles(Vec2 dir) {
+		Mat4 mat = Matrices.grab4();
-		// Normalize yaw
+		Vec3 lookingAt = Vectors.grab3();		
-		dir.x = FloatMathUtil.normalizeAngle(dir.x);
+		Vec3 rightVector = Vectors.grab3();
 
-		// Clamp pitch
+		rightVector.set(player.getLookingAt()).cross(player.getUpVector()).normalize();
-		dir.y = Glm.clamp(
+		
-			dir.y,
+		mat.identity()
-			-FloatMathUtil.PI_F / 2,
+			.rotate((float) yawChange, player.getUpVector())
-			+FloatMathUtil.PI_F / 2
+			.rotate((float) pitchChange, rightVector);
-		);
+		
+		VectorUtil.applyMat4(player.getLookingAt(), mat, lookingAt);
+		player.setLookingAt(lookingAt);
+		
+		Vectors.release(rightVector);
+		Vectors.release(lookingAt);
+		Matrices.release(mat);
 	}
 
 	private void onWheelScroll(WheelScrollEvent event) {