Fixed a bunch of issues with gravity and implemented gravity changes

Also added DebugGraphics and made VectorUtil comply with the general Vec
contract

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

@@ -42,6 +42,7 @@ import ru.windcorp.progressia.common.collision.Collideable;
 import ru.windcorp.progressia.common.collision.colliders.Collider;
 import ru.windcorp.progressia.common.util.FloatMathUtil;
 import ru.windcorp.progressia.common.util.Vectors;
+import ru.windcorp.progressia.common.world.GravityModel;
 import ru.windcorp.progressia.common.world.entity.EntityData;
 import ru.windcorp.progressia.test.CollisionModelRenderer;
 import ru.windcorp.progressia.test.TestPlayerControls;
@@ -199,12 +200,19 @@ public class LayerWorld extends Layer {
 	}
 
 	private void tmp_applyGravity(EntityData entity, float tickLength) {
+		GravityModel gm = ClientState.getInstance().getWorld().getData().getGravityModel();
+		
+		Vec3 upVector = Vectors.grab3();
+		gm.getUp(entity.getPosition(), upVector);
+		entity.changeUpVector(upVector);
+		Vectors.release(upVector);
+		
 		if (ClientState.getInstance().getLocalPlayer().getEntity() == entity && tmp_testControls.isFlying()) {
 			return;
 		}
 
 		Vec3 gravitationalAcceleration = Vectors.grab3();
-		ClientState.getInstance().getWorld().getData().getGravityModel().getGravity(entity.getPosition(), gravitationalAcceleration);
+		gm.getGravity(entity.getPosition(), gravitationalAcceleration);
 		
 		gravitationalAcceleration.mul(tickLength);
 		entity.getVelocity().add(gravitationalAcceleration);

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

@@ -231,9 +231,9 @@ public abstract class NPedModel extends EntityRenderable {
 		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,
+			+f.x, +f.y, +f.z,    0,
+			-s.x, -s.y, -s.z,    0,
+			+u.x, +u.y, +u.z,    0,
 			   0,    0,    0,    1
 		);
 		
@@ -253,7 +253,6 @@ public abstract class NPedModel extends EntityRenderable {
 
 		computeVelocityParameter();
 
-		// TODO switch to world time
 		walkingParameter += velocity * GraphicsInterface.getFrameLength() * 1000;
 		
 		rotateBodyWithMovement(horizontal);

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

@@ -126,7 +126,11 @@ public class VectorUtil {
 		iterateCuboidAround(center.x, center.y, center.z, diameter, action);
 	}
 
-	public static void applyMat4(Vec3 in, Mat4 mat, Vec3 out) {
+	public static Vec3 applyMat4(Vec3 in, Mat4 mat, Vec3 out) {
+		if (out == null) {
+			out = new Vec3();
+		}
+		
 		Vec4 vec4 = Vectors.grab4();
 		vec4.set(in, 1f);
 
@@ -134,23 +138,31 @@ public class VectorUtil {
 
 		out.set(vec4.x, vec4.y, vec4.z);
 		Vectors.release(vec4);
+		
+		return out;
 	}
 
-	public static void applyMat4(Vec3 inOut, Mat4 mat) {
-		applyMat4(inOut, mat, inOut);
+	public static Vec3 applyMat4(Vec3 inOut, Mat4 mat) {
+		return applyMat4(inOut, mat, inOut);
+	}
+	
+	public static Vec3 rotate(Vec3 in, Vec3 axis, float angle, Vec3 out) {
+		if (out == null) {
+			out = new Vec3();
 		}
 		
-	public static void rotate(Vec3 in, Vec3 axis, float angle, Vec3 out) {
 		Mat3 mat = Matrices.grab3();
 		
 		mat.identity().rotate(angle, axis);
 		mat.mul(in, out);
 		
 		Matrices.release(mat);
+		
+		return out;
 	}
 	
-	public static void rotate(Vec3 inOut, Vec3 axis, float angle) {
-		rotate(inOut, axis, angle, inOut);
+	public static Vec3 rotate(Vec3 inOut, Vec3 axis, float angle) {
+		return rotate(inOut, axis, angle, inOut);
 	}
 	
 	public static double getAngle(Vec3 from, Vec3 to, Vec3 normal) {
@@ -212,6 +224,10 @@ public class VectorUtil {
 		float kb,
 		Vec3 output
 	) {
+		if (output == null) {
+			output = new Vec3();
+		}
+		
 		output.set(
 			va.x * ka + vb.x * kb,
 			va.y * ka + vb.y * kb,
@@ -229,6 +245,10 @@ public class VectorUtil {
 		float kc,
 		Vec3 output
 	) {
+		if (output == null) {
+			output = new Vec3();
+		}
+		
 		output.set(
 			va.x * ka + vb.x * kb + vc.x * kc,
 			va.y * ka + vb.y * kb + vc.y * kc,

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

@@ -24,7 +24,11 @@ import ru.windcorp.progressia.common.util.crash.CrashReports;
 import ru.windcorp.progressia.common.util.namespaces.Namespaced;
 
 /**
- * Gravity model specifies the gravitational acceleration field. A gravity model may be queried for the vector of gravitational acceleration that should affect an object. This vector is, generally speaking, a function of space: gravity in two different locations may vary. Gravity may also be a zero vector.
+ * Gravity model specifies the gravitational acceleration field. A gravity model
+ * may be queried for the vector of gravitational acceleration that should
+ * affect an object. This vector is, generally speaking, a function of space:
+ * gravity in two different locations may vary. Gravity may also be a zero
+ * vector.
  * 
  * @author javapony
  */
@@ -35,12 +39,14 @@ public abstract class GravityModel extends Namespaced {
 	}
 
 	/**
-	 * Computes the vector of gravitational acceleration at the provided location.
+	 * Computes the vector of gravitational acceleration at the provided
+	 * location.
 	 * 
 	 * @param pos    the position to compute gravity at
-	 * @param output a {@link Vec3} where the result is stored. May be {@code null}.
-	 * 
-	 * @return the vector of gravitational acceleration. The returned object will match {@code output} parameter is it is non-null.
+	 * @param output a {@link Vec3} where the result is stored. May be
+	 *               {@code null}.
+	 * @return the vector of gravitational acceleration. The returned object
+	 *         will match {@code output} parameter is it is non-null.
 	 */
 	public Vec3 getGravity(Vec3 pos, Vec3 output) {
 		Objects.requireNonNull(pos, "pos");
@@ -59,24 +65,31 @@ public abstract class GravityModel extends Namespaced {
 	}
 
 	/**
-	 * Computes the up direction at the provided location. Up vector is defined as the normalized gravitational acceleration vector or {@code (0; 0; 0)} if there is no gravity. 
+	 * Computes the up direction at the provided location. Up vector is defined
+	 * as the additive inverse of the normalized gravitational acceleration
+	 * vector or {@code (0; 0; 0)} if there is no gravity.
 	 * 
 	 * @param pos    the position to compute up vector at
-	 * @param output a {@link Vec3} where the result is stored. May be {@code null}.
-	 * 
-	 * @return the up vector. The returned object will match {@code output} parameter is it is non-null.
+	 * @param output a {@link Vec3} where the result is stored. May be
+	 *               {@code null}.
+	 * @return the up vector. The returned object will match {@code output}
+	 *         parameter is it is non-null.
 	 */
 	public Vec3 getUp(Vec3 pos, Vec3 output) {
 		output = getGravity(pos, output);
-		if (output.any()) output.normalize();
+		if (output.any())
+			output.normalize().negate();
 		return output;
 	}
 
 	/**
-	 * Computes the gravitational acceleration vector at the provided location. Actual computation of gravity is delegated to this method by the other methods in this class.
+	 * Computes the gravitational acceleration vector at the provided location.
+	 * Actual computation of gravity is delegated to this method by the other
+	 * methods in this class.
 	 * 
 	 * @param pos    the position to compute gravity at
-	 * @param output a {@link Vec3} where the result must be stored. Never {@code null}.
+	 * @param output a {@link Vec3} where the result must be stored. Never
+	 *               {@code null}.
 	 */
 	protected abstract void doGetGravity(Vec3 pos, Vec3 output);
 

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

@@ -21,13 +21,16 @@ package ru.windcorp.progressia.common.world.entity;
 import java.io.DataInput;
 import java.io.DataOutput;
 import java.io.IOException;
+import java.util.Objects;
 
+import glm.mat._3.Mat3;
 import glm.vec._3.Vec3;
 import ru.windcorp.jputil.chars.StringUtil;
 import ru.windcorp.progressia.common.collision.Collideable;
 import ru.windcorp.progressia.common.collision.CollisionModel;
 import ru.windcorp.progressia.common.state.IOContext;
 import ru.windcorp.progressia.common.state.StatefulObject;
+import ru.windcorp.progressia.common.util.Matrices;
 import ru.windcorp.progressia.common.world.generic.GenericEntity;
 
 public class EntityData extends StatefulObject implements Collideable, GenericEntity {
@@ -146,6 +149,8 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 			this.lookingAt.set(lookingAt);
 		} else if (lengthSq == 0) {
 			throw new IllegalArgumentException("lookingAt is zero-length");
+		} else if (!Float.isFinite(lengthSq)) {
+			throw new IllegalArgumentException("lookingAt is not finite: " + lookingAt);
 		} else {
 			float length = (float) Math.sqrt(lengthSq);
 			this.lookingAt.set(
@@ -172,6 +177,8 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 			this.upVector.set(upVector);
 		} else if (lengthSq == 0) {
 			throw new IllegalArgumentException("upVector is zero-length");
+		} else if (!Float.isFinite(lengthSq)) {
+			throw new IllegalArgumentException("upVector is not finite: " + upVector);
 		} else {
 			float length = (float) Math.sqrt(lengthSq);
 			this.upVector.set(
@@ -216,8 +223,76 @@ public class EntityData extends StatefulObject implements Collideable, GenericEn
 	 * @see #setLookingAt(Vec3)
 	 */
 	public void changeUpVector(Vec3 newUpVector) {
-		// TODO
-		this.upVector.set(newUpVector);
+		Objects.requireNonNull(newUpVector, "newUpVector");
+
+		Vec3 u0 = upVector;
+		Vec3 u1 = newUpVector;
+
+		if (u1.x == 0 && u1.y == 0 && u1.z == 0) {
+			// Entering weightlessness, not changing anything
+			return;
+		}
+
+		if (u0.x == u1.x && u0.y == u1.y && u0.z == u1.z) {
+			// Nothing changed
+			return;
+		}
+
+		if (u0.x == -u1.x && u0.y == -u1.y && u0.z == -u1.z) {
+			// Welp, don't do anything stupid then
+			upVector.set(newUpVector);
+			return;
+		}
+
+		float u1LengthSq = u1.x*u1.x + u1.y*u1.y + u1.z*u1.z;
+		float u1Length = 1;
+		
+		if (!Float.isFinite(u1LengthSq)) {
+			throw new IllegalArgumentException("newUpVector is not finite: " + newUpVector);
+		} else if (u1LengthSq != 1) {
+			u1Length = (float) Math.sqrt(u1LengthSq);
+		}
+
+		// u0 and u1 are now both definitely two different usable vectors
+		
+		if (rotateLookingAtToMatchUpVectorRotation(u0, u1, u1Length, lookingAt)) {
+			return;
+		}
+		
+		upVector.set(newUpVector).div(u1Length);
+	}
+
+	private static boolean rotateLookingAtToMatchUpVectorRotation(Vec3 u0, Vec3 u1, float u1Length, Vec3 lookingAt) {
+		// Determine rotation parameters
+		Vec3 axis = u0.cross_(u1);
+		float cos = u0.dot(u1) / u1Length;
+		float sin = axis.length() / u1Length;
+		
+		if (sin == 0) {
+			return true;
+		}
+		
+		axis.div(sin * u1Length); // normalize axis
+
+		float x = axis.x;
+		float y = axis.y;
+		float z = axis.z;
+
+		Mat3 matrix = Matrices.grab3();
+
+		// Don't format. @formatter:off
+		matrix.set(
+			 cos + (1 - cos)*x*x,    (1 - cos)*x*y - sin*z,   (1 - cos)*x*z + sin*y,
+			(1 - cos)*y*x + sin*z,    cos + (1 - cos)*y*y,    (1 - cos)*y*z - sin*x,
+			(1 - cos)*z*x - sin*y,   (1 - cos)*z*y + sin*x,    cos + (1 - cos)*z*z
+		);
+		// @formatter:on
+
+		matrix.mul_(lookingAt); // bug in jglm, .mul() and .mul_() are swapped
+
+		Matrices.release(matrix);
+		
+		return false;
 	}
 
 	@Override

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

@@ -0,0 +1,95 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package ru.windcorp.progressia.test;
+
+import glm.mat._4.Mat4;
+import glm.vec._3.Vec3;
+import glm.vec._4.Vec4;
+import ru.windcorp.progressia.client.graphics.Colors;
+import ru.windcorp.progressia.client.graphics.model.Renderable;
+import ru.windcorp.progressia.client.graphics.model.ShapeRenderHelper;
+import ru.windcorp.progressia.client.graphics.model.Shapes;
+import ru.windcorp.progressia.client.graphics.model.StaticModel;
+import ru.windcorp.progressia.client.graphics.texture.Texture;
+import ru.windcorp.progressia.client.graphics.world.WorldRenderProgram;
+import ru.windcorp.progressia.common.util.Vectors;
+
+public class DebugGraphics {
+	
+	private static final float TAIL_THICKNESS = 0.03f;
+	private static final float HEAD_SIZE = 0.1f;
+	
+	private static final Renderable THE_VECTOR = StaticModel.builder().addPart(
+		new Shapes.PppBuilder(WorldRenderProgram.getDefault(), (Texture) null)
+			.setSize(1.0f, TAIL_THICKNESS, TAIL_THICKNESS)
+			.setOrigin(0, -TAIL_THICKNESS / 2, -TAIL_THICKNESS / 2)
+			.create()
+	).addPart(
+		new Shapes.PppBuilder(WorldRenderProgram.getDefault(), (Texture) null)
+			.setSize(HEAD_SIZE, HEAD_SIZE, HEAD_SIZE)
+			.setOrigin((1 - HEAD_SIZE / 2), -HEAD_SIZE / 2, -HEAD_SIZE / 2)
+			.create()
+	).build();
+	
+	public static void drawVector(Vec3 vector, Vec4 color, Vec3 origin, float scale, ShapeRenderHelper renderer) {
+		float length = vector.length();
+		if (length == 0) return;
+		
+		if (scale == 0) scale = 1 / length;
+		
+		Mat4 mat = renderer.pushTransform();
+		
+		mat.translate(origin);
+		
+		Vec3 somePerpendicular = new Vec3();
+		
+		if (Math.abs(vector.z) > (1 - 1e-4f) * length) {
+			somePerpendicular.set(1, 0, 0);
+		} else {
+			somePerpendicular.set(0, 0, 1);
+		}
+		
+		Vec3 f = vector;
+		Vec3 s = somePerpendicular.cross_(f).normalize();
+		Vec3 u = somePerpendicular.set(f).cross(s).normalize();
+		
+		// @formatter:off
+		mat.mul(new Mat4(
+			+f.x * scale, +f.y * scale, +f.z * scale,    0,
+			        -s.x,         -s.y,         -s.z,    0,
+			        +u.x,         +u.y,         +u.z,    0,
+			           0,            0,            0,    1
+		));
+		// @formatter:on
+		
+		renderer.pushColorMultiplier().mul(color);
+		THE_VECTOR.render(renderer);
+		renderer.popColorMultiplier();
+
+		renderer.popTransform();
+	}
+	
+	public static void drawVector(Vec3 vector, ShapeRenderHelper renderer) {
+		drawVector(vector, Colors.GRAY_A, Vectors.ZERO_3, 1, renderer);
+	}
+	
+	public static void drawDirection(Vec3 vector, ShapeRenderHelper renderer) {
+		drawVector(vector, Colors.GRAY_A, Vectors.ZERO_3, 0, renderer);
+	}
+
+}

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

@@ -116,7 +116,7 @@ public class TestPlayerControls {
 			desiredVelocity.normalize();
 		}
 		desiredVelocity.z = movementUp;
-		movementTransform.mul_(desiredVelocity); // bug in jglm, .mul() and mul_() are
+		movementTransform.mul_(desiredVelocity); // bug in jglm, .mul() and .mul_() are
 										         // swapped
 		desiredVelocity.mul(speed);
 
@@ -154,9 +154,9 @@ public class TestPlayerControls {
 		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
+			+f.x, +f.y, +f.z,
+			-s.x, -s.y, -s.z,
+			+u.x, +u.y, +u.z
 		);
 	}
 
@@ -282,7 +282,13 @@ public class TestPlayerControls {
 			return;
 		}
 
-		getEntity().getVelocity().add(0, 0, JUMP_VELOCITY);
+		Vec3 up = getEntity().getUpVector();
+		
+		getEntity().getVelocity().add(
+			up.x * JUMP_VELOCITY,
+			up.y * JUMP_VELOCITY,
+			up.z * JUMP_VELOCITY
+		);
 	}
 
 	private void handleShift(int multiplier) {

src/main/java/ru/windcorp/progressia/test/gen/TestGravityModel.java

@@ -28,7 +28,14 @@ public class TestGravityModel extends GravityModel {
 
 	@Override
 	protected void doGetGravity(Vec3 pos, Vec3 output) {
-		output.set(0, 0, -9.8);
+		output.set(pos);
+		
+		if (output.length() < 10) {
+			output.set(0);
+			return;
+		}
+		
+		output.normalize().mul(-9.8f);
 	}
 
 }