A WIP Armory3D Character Controller

CharacterController2.hx

package arm;

import kha.graphics4.hxsl.Types.Vec;
import kha.FastFloat;
import iron.math.Quat;
import iron.math.Vec4;
import haxe.Log;
import armory.trait.physics.PhysicsWorld;
import iron.system.Input;
import armory.trait.physics.bullet.RigidBody;

class CharacterController2 extends iron.Trait {
#if arm_bullet
	/**The rigid body of the character object**/
	var body:RigidBody;
	var keyboard = Input.getKeyboard();
	var phys = PhysicsWorld.active;

	// State variables
	/**Whether or not the player is touching the ground ( or any other object below it )**/
	var onGround = false;
	/**Whether or not the player is touching the ceiling ( or any other object above it )**/
	var onCeiling = false;
	/**The slope of the ground in radians that the character is standing on**/
	var groundSlope:FastFloat = 0;
	/**The normal of the ground surface the character is standing on**/
	var groundNorm:Vec4 = new Vec4();
	/**Whether or not the player is rising due to a jump**/
	var isJumping = false;
	/**Whether or not the player is falling due to a jump**/
	var isJumpFalling = false;
	/**The world Z location of the player when last jump was started**/
	private var jumpInitialZ:FastFloat = 0;
	private var lastDirection:Vec4 = new Vec4();

	// Movement parameters

	/**The speed that the characters falls**/
	@prop
	var fallSpeed:FastFloat = 5;
	@prop
	var strafeSpeed:FastFloat = 3;
	@prop
	var walkSpeed:FastFloat = 5.5;
	@prop
	var sprintSpeed:FastFloat = 10;
	@prop
	var stepHeight:FastFloat = 0.4;
	@prop
	var jumpHeight:FastFloat = 1;
	@prop
	var jumpSpeed:FastFloat = 5;
	/**The maximum slope in radians that the player can walk up without sliding.**/
	@prop
	var maxSlope:FastFloat = 1.05; // 60 degrees
	var offGroundFrames = 0;

	public function new() {
		super();
		notifyOnInit(init);
	}

	/**
	 * Called on object initialization.
	 */
	private function init() {
		body = object.getTrait(RigidBody);
		if (body == null) return;

		// Set body physics properties
		body.setGravity(new Vec4(0, 0, 0));
		body.setAngularFactor(0, 0, 0);
		// body.setFriction(1);

		notifyOnUpdate(update);
		// notifyOnLateUpdate(stickToFloor);
		phys.notifyOnPreUpdate(stickToFloor);
	}

	private function stickToFloor() {
		updateState();
		if (!isJumping && !onGround) {
			// Make object "stick" to the ground within a threshold
			final stickyThreshold = 0.05;
			var rayFrom = object.transform.world.getLoc().clone();
			rayFrom.z -= object.transform.dim.z / 2;
			var rayTo = rayFrom.clone();
			rayTo.z -= stickyThreshold;

			// Cast ray to get ground distance
			phys.rayCast(rayFrom, rayTo);
			var hitPoint = phys.hitPointWorld;
			var normal = phys.hitNormalWorld;
			var slope = Math.abs(Math.asin(Math.abs(normal.z) / normal.length()) - Math.PI / 2);

			// Move object to the ground
			var distance = hitPoint.z - rayFrom.z;

			groundNorm = normal;

			if (Math.abs(distance) <= stickyThreshold && normal.z > 0) {
				// Move object down
				object.transform.loc.z += distance;
				object.transform.buildMatrix();
				body.syncTransform();

				// Force another check for contacts
				@:privateAccess phys.updateContacts();

				// Un-penatrate objects
				var contacts = phys.getContactPairs(body);
				var normal;

				if (contacts != null) {
					for (contact in contacts) {

						// If the character is object a
						if (body == phys.rbMap.get(contact.a)) {
							// Normal is the normal on object b
							normal = contact.normOnB.clone();
						
						// If the character is object b
						} else {
							// Normal is the opposite of the normal on object b AKA the normal on object a
							normal = contact.normOnB.clone().mult(-1);
						}

						// If contact is within threshhold
						if (contact.distance < 0) {
							// Move object backup up to avoid penatration
							object.transform.loc.sub(normal.clone().mult(contact.distance * 0.9));
							object.transform.buildMatrix();
							body.syncTransform();
						}
					}
				}
			}
		}
	}

	private function preUpdate() {
		stickToFloor();
	}

	private function update() {
		// Update state variables
		updateState();

		// Get user input direction
		var up = keyboard.down('w');
		var down = keyboard.down('s');
		var right = keyboard.down('d');
		var left = keyboard.down('a');
		var sprint = keyboard.down('alt');
		
		// Build velocity
		var velocity = new Vec4();

		// Add user input directions
		if (sprint && up && ! (down || left || right) && onGround) {
			velocity.add(new Vec4(0, -sprintSpeed, 0));
		} else {
			if (up) velocity.add(new Vec4(0, -walkSpeed, 0));
			if (down) velocity.add(new Vec4(0, walkSpeed, 0));
			if (right) velocity.add(new Vec4(-strafeSpeed, 0, 0));
			if (left) velocity.add(new Vec4(strafeSpeed, 0, 0));
		}

		// Make sure you can't walk faster by walking diagonal
		var maxSpeed = if (sprint) sprintSpeed + walkSpeed else walkSpeed;

		if (velocity.length() > maxSpeed) {
			velocity.normalize().mult(maxSpeed);
		}

		// Align velocity with object transform
		velocity.applyQuat(new Quat().fromMat(object.transform.world));

		if (!isJumping) {
			// Set the movement vector to be parallel to ground surface
			var speed = velocity.length();
			var t = velocity.dot(groundNorm.normalize());
			var t2 = new Vec4(
				t * groundNorm.x,
				t * groundNorm.y,
				t * groundNorm.z
			);
			velocity.sub(t2);
			velocity.normalize().mult(speed);

			if (!onGround) {
				// Add gravity vector if not on the ground
				velocity.z -= fallSpeed;
			}

		// isJumping
		} else {
			var jumpDistLeft = jumpHeight - (object.transform.world.getLoc().z - jumpInitialZ);

			// Move object until it reaches jump height or hits the ceiling
			if (jumpDistLeft > 0 && !onCeiling) {
				velocity.add(new Vec4(0, 0, jumpSpeed));
			} else {
				isJumping = false;
				isJumpFalling = true;
			}
		}

		// Set body velocity
		body.activate();
		body.setLinearVelocity(velocity.x, velocity.y, velocity.z);
	}

	//
	// HELPER FUNCTIONS
	//

	private function updateState() {
		// Reset contact states
		onGround = false;
		offGroundFrames++;
		onCeiling = false;

		// Determine state of contacts
		final contactThreshold = 0.01;
		var contacts = phys.getContactPairs(body);
		var normal;
		var slope;
		if (contacts != null) {
			for (contact in contacts) {

				// If the character is object a
				if (body == phys.rbMap.get(contact.a)) {
					// Normal is the normal on object b
					normal = contact.normOnB.clone();
				
				// If the character is object b
				} else {
					// Normal is the opposite of the normal on object b AKA the normal on object a
					normal = contact.normOnB.clone().mult(-1);
				}

				slope = Math.abs(Math.asin(Math.abs(normal.z) / normal.length()) - Math.PI / 2);

				// If contact is within threshhold
				if (contact.distance <= contactThreshold) {
					// If the normal is facing up and the slope is not greater than the max slope
					if (normal.z > 0 && slope <= maxSlope) {
						onGround = true;
						offGroundFrames = 0;
						groundSlope = slope;

					// If the normal is facing down and the slope is not greater than a slope threshhold
					// ( Math.Pi / 2.5 represents a slope slightly less than 90 degrees )
					} else if (normal.z < 0 && slope <= (Math.PI / 2.5)) {
						onCeiling = true;
					}
				}
			}
		}

		// Check jump state
		if (keyboard.started("space") && onGround) {
			isJumping = true;
			jumpInitialZ = object.transform.world.getLoc().z;
		}

		// Check jump falling state
		if (isJumpFalling && onGround) {
			// We are no longer falling from the jump
			isJumpFalling = false;
		}

		// Update ground Normal
		groundNorm = new Vec4(0, 0, 1);
		final groundDetectDistance = 0.1;
		var rayFrom = object.transform.world.getLoc().clone();
		rayFrom.z -= object.transform.dim.z / 2;
		var rayTo = rayFrom.clone();
		rayTo.z -= groundDetectDistance;
		phys.rayCast(rayFrom, rayTo);
		groundNorm = phys.hitNormalWorld;
	}
#end
}