LNXSDK/leenkx/Sources/leenkx/trait/physics/jolt/KinematicCharacterController.hx

package leenkx.trait.physics.jolt;

#if lnx_jolt

import iron.Trait;
import iron.math.Vec4;
import iron.math.Quat;
import iron.object.Transform;
import iron.object.MeshObject;

class KinematicCharacterController extends Trait {

	var shape:ControllerShape;
	public var physics:PhysicsWorld;
	public var transform:Transform = null;
	public var mass:Float;
	public var friction:Float;
	public var restitution:Float;
	public var collisionMargin:Float;
	public var animated:Bool;
	public var group = 1;
	var bodyScaleX:Float;
	var bodyScaleY:Float;
	var bodyScaleZ:Float;
	var currentScaleX:Float;
	var currentScaleY:Float;
	var currentScaleZ:Float;
	var jumpSpeed:Float;

	public var body:jolt.Jt.Body;
	public var bodyId:jolt.Jt.BodyID;
	public var ready = false;
	static var nextId = 0;
	public var id = 0;
	public var onReady:Void->Void = null;

	static var nullvec = true;
	static var vec1:jolt.Jt.Vec3;
	static var quat1:jolt.Jt.Quat;
	static var quat = new Quat();

	var walkDirection:Vec4 = new Vec4();
	var gravityEnabled = true;
	var gravityFactor = 1.0;

	public function new(mass = 1.0, shape = ControllerShape.Capsule, jumpSpeed = 8.0, friction = 0.5, restitution = 0.0,
			collisionMargin = 0.0, animated = false, group = 1) {
		super();

		this.mass = mass;
		this.jumpSpeed = jumpSpeed;
		this.shape = shape;
		this.friction = friction;
		this.restitution = restitution;
		this.collisionMargin = collisionMargin;
		this.animated = animated;
		this.group = group;

		notifyOnAdd(init);
		notifyOnLateUpdate(lateUpdate);
		notifyOnRemove(removeFromWorld);
	}

	inline function withMargin(f:Float):Float {
		return f + f * collisionMargin;
	}

	public function notifyOnReady(f:Void->Void) {
		onReady = f;
		if (ready)
			onReady();
	}

	public function init() {
		if (ready)
			return;

		transform = object.transform;
		physics = PhysicsWorld.active;

		if (physics == null) {
			new PhysicsWorld();
			physics = PhysicsWorld.active;
		}

		#if js
		// Check if Jolt is initialized - defer if not
		if (!physics.physicsReady) {
			haxe.Timer.delay(init, 16);
			return;
		}
		#end

		ready = true;

		if (nullvec) {
			nullvec = false;
			vec1 = new jolt.Jt.Vec3(0, 0, 0);
			quat1 = new jolt.Jt.Quat(0, 0, 0, 1);
		}

		var joltShape:jolt.Jt.Shape = createShape();

		var pos = transform.world.getLoc();
		var rot = new iron.math.Quat();
		rot.fromMat(transform.world);

		// Jolt uses RVec3 for world positions
		var jPos = new jolt.Jt.RVec3(pos.x, pos.y, pos.z);
		var jRot = new jolt.Jt.Quat(rot.x, rot.y, rot.z, rot.w);
		var settings = new jolt.Jt.BodyCreationSettings(joltShape, jPos, jRot, 1, 1);

		// Use kinematic body for character controller
		settings.mFriction = friction;
		settings.mRestitution = restitution;

		body = physics.bodyInterface.CreateBody(settings);
		bodyId = body.GetID();

		#if hl
		settings.delete();
		jPos.delete();
		jRot.delete();
		#end

		physics.bodyInterface.AddBody(bodyId, 1);

		bodyScaleX = currentScaleX = transform.scale.x;
		bodyScaleY = currentScaleY = transform.scale.y;
		bodyScaleZ = currentScaleZ = transform.scale.z;

		id = nextId;
		nextId++;

		if (onReady != null)
			onReady();
	}

	function createShape():jolt.Jt.Shape {
		var t = transform;
		if (shape == ControllerShape.Box) {
			var halfExtent = new jolt.Jt.Vec3(withMargin(t.dim.x / 2), withMargin(t.dim.y / 2), withMargin(t.dim.z / 2));
			return new jolt.Jt.BoxShape(halfExtent);
		} else if (shape == ControllerShape.Sphere) {
			var width = Math.max(t.dim.x, Math.max(t.dim.y, t.dim.z));
			return new jolt.Jt.SphereShape(withMargin(width / 2));
		} else if (shape == ControllerShape.Cylinder) {
			var radius = Math.max(t.dim.x, t.dim.y) / 2;
			var halfHeight = t.dim.z / 2;
			return new jolt.Jt.CylinderShape(withMargin(halfHeight), withMargin(radius));
		} else if (shape == ControllerShape.Capsule) {
			var r = t.dim.x / 2;
			var halfHeight = (t.dim.z - r * 2) / 2;
			if (halfHeight < 0.01) halfHeight = 0.01;
			return new jolt.Jt.CapsuleShape(withMargin(halfHeight), withMargin(r));
		} else if (shape == ControllerShape.Cone) {
			var radius = Math.max(t.dim.x, t.dim.y) / 2;
			var halfHeight = t.dim.z / 2;
			return new jolt.Jt.CylinderShape(withMargin(halfHeight), withMargin(radius));
		}
		// Default capsule
		var r = t.dim.x / 2;
		var halfHeight = (t.dim.z - r * 2) / 2;
		if (halfHeight < 0.01) halfHeight = 0.01;
		return new jolt.Jt.CapsuleShape(withMargin(halfHeight), withMargin(r));
	}

	function lateUpdate() {
		if (!ready)
			return;
		if (object.animation != null || animated) {
			syncTransform();
		} else {
			var p = physics.bodyInterface.GetPosition(bodyId);
			var q = physics.bodyInterface.GetRotation(bodyId);

			#if js
			transform.loc.set(cast p.GetX(), cast p.GetY(), cast p.GetZ());
			#else
			transform.loc.set(p.GetX(), p.GetY(), p.GetZ());
			#end
			transform.rot.set(q.GetX(), q.GetY(), q.GetZ(), q.GetW());

			#if hl
			p.delete();
			q.delete();
			#end

			if (object.parent != null) {
				var ptransform = object.parent.transform;
				transform.loc.x -= ptransform.worldx();
				transform.loc.y -= ptransform.worldy();
				transform.loc.z -= ptransform.worldz();
			}
			transform.buildMatrix();
		}
	}

	public function canJump():Bool {
		// Simple ground check - could be improved with raycast
		return onGround();
	}

	public function onGround():Bool {
		// Perform downward raycast to check ground
		var pos = transform.world.getLoc();
		var from = new Vec4(pos.x, pos.y, pos.z);
		var to = new Vec4(pos.x, pos.y, pos.z - 0.2);
		var hit = physics.rayCast(from, to);
		return hit != null;
	}

	public function setJumpSpeed(jumpSpeed:Float) {
		this.jumpSpeed = jumpSpeed;
	}

	public function setFallSpeed(fallSpeed:Float) {
		// Jolt handles this through gravity
	}

	public function setMaxSlope(slopeRadians:Float) {
		// Would need CharacterVirtual for proper slope handling
	}

	public function getMaxSlope():Float {
		return Math.PI / 4; // 45 degrees default
	}

	public function setMaxJumpHeight(maxJumpHeight:Float) {
		// Calculate jump speed from height: v = sqrt(2 * g * h)
		var g = physics.getGravity().length();
		jumpSpeed = Math.sqrt(2 * g * maxJumpHeight);
	}

	public function setWalkDirection(dir:Vec4) {
		walkDirection.setFrom(dir);
		var vel = new jolt.Jt.Vec3(dir.x, dir.y, dir.z);
		physics.bodyInterface.SetLinearVelocity(bodyId, vel);
		#if hl vel.delete(); #end
	}

	public function setUpInterpolate(value:Bool) {
		// Not directly applicable in Jolt kinematic body
	}

	public function jump() {
		var currentVel = physics.bodyInterface.GetLinearVelocity(bodyId);
		var vel = new jolt.Jt.Vec3(currentVel.GetX(), currentVel.GetY(), jumpSpeed);
		physics.bodyInterface.SetLinearVelocity(bodyId, vel);
		#if hl currentVel.delete(); vel.delete(); #end
	}

	public function removeFromWorld() {
		if (physics != null && ready) {
			physics.bodyInterface.RemoveBody(bodyId);
			physics.bodyInterface.DestroyBody(bodyId);
		}
	}

	public function activate() {
		physics.bodyInterface.ActivateBody(bodyId);
	}

	public function disableGravity() {
		gravityEnabled = false;
		physics.bodyInterface.SetGravityFactor(bodyId, 0.0);
	}

	public function enableGravity() {
		gravityEnabled = true;
		physics.bodyInterface.SetGravityFactor(bodyId, gravityFactor);
	}

	public function setGravity(f:Float) {
		gravityFactor = f / 9.81; // Normalize
		if (gravityEnabled) {
			physics.bodyInterface.SetGravityFactor(bodyId, gravityFactor);
		}
	}

	public function setActivationState(newState:Int) {
		if (newState == ControllerActivationState.NoDeactivation) {
			// Keep active - Jolt handles this differently
			activate();
		}
	}

	public function setFriction(f:Float) {
		physics.bodyInterface.SetFriction(bodyId, f);
		this.friction = f;
	}

	public function syncTransform() {
		var t = transform;
		t.buildMatrix();
		var pos = t.world.getLoc();
		var rot = new iron.math.Quat();
		rot.fromMat(t.world);

		// Jolt uses RVec3 for world positions
		var p = new jolt.Jt.RVec3(pos.x, pos.y, pos.z);
		var q = new jolt.Jt.Quat(rot.x, rot.y, rot.z, rot.w);
		physics.bodyInterface.SetPosition(bodyId, p, 0);
		physics.bodyInterface.SetRotation(bodyId, q, 0);
		#if hl p.delete(); q.delete(); #end

		activate();
	}

	public function getLinearVelocity():Vec4 {
		var vel = physics.bodyInterface.GetLinearVelocity(bodyId);
		var result = new Vec4(vel.GetX(), vel.GetY(), vel.GetZ());
		#if hl vel.delete(); #end
		return result;
	}

	public function setLinearVelocity(velocity:Vec4) {
		var vel = new jolt.Jt.Vec3(velocity.x, velocity.y, velocity.z);
		physics.bodyInterface.SetLinearVelocity(bodyId, vel);
		#if hl vel.delete(); #end
	}

	public function getPosition():Vec4 {
		var pos = physics.bodyInterface.GetPosition(bodyId);
		var result = new Vec4(pos.GetX(), pos.GetY(), pos.GetZ());
		#if hl pos.delete(); #end
		return result;
	}

	public function setPosition(position:Vec4) {
		var p = new jolt.Jt.RVec3(position.x, position.y, position.z);
		physics.bodyInterface.SetPosition(bodyId, p, 0);
		#if hl p.delete(); #end
	}

	public function warp(position:Vec4) {
		setPosition(position);
		var zeroVel = new jolt.Jt.Vec3(0, 0, 0);
		physics.bodyInterface.SetLinearVelocity(bodyId, zeroVel);
		#if hl zeroVel.delete(); #end
	}

	public function move(direction:Vec4, speed:Float) {
		var moveVel = new Vec4(direction.x * speed, direction.y * speed, direction.z * speed);
		
		// Preserve vertical velocity for jumping/falling
		var currentVel = physics.bodyInterface.GetLinearVelocity(bodyId);
		var vel = new jolt.Jt.Vec3(moveVel.x, moveVel.y, currentVel.GetZ());
		physics.bodyInterface.SetLinearVelocity(bodyId, vel);
		#if hl currentVel.delete(); vel.delete(); #end
	}

	public function getGroundState():Int {
		if (onGround()) {
			return 0; // OnGround
		}
		return 3; // InAir
	}

	public function isSupported():Bool {
		return onGround();
	}
}

@:enum abstract ControllerShape(Int) from Int to Int {
	var Box = 0;
	var Sphere = 1;
	var ConvexHull = 2;
	var Cone = 3;
	var Cylinder = 4;
	var Capsule = 5;
}

@:enum abstract ControllerActivationState(Int) from Int to Int {
	var Active = 1;
	var NoDeactivation = 4;
	var NoSimulation = 5;
}

#end