1
0
forked from Onek8/LNXSDK
Files
LNXSDK/leenkx/Sources/leenkx/trait/NavAgent.hx
2026-10-27 15:58:22 -07:00

163 lines
3.1 KiB
Haxe

package leenkx.trait;
import iron.Trait;
import iron.math.Vec4;
import iron.math.Quat;
import iron.system.Time;
#if lnx_navigation
import leenkx.trait.navigation.Navigation;
#end
class NavAgent extends Trait {
@prop
public var navMeshId: String = 'NavMesh';
@prop
public var speed: Float = 5;
@prop
public var turnDuration: Float = 0.4;
@prop
public var heightOffset: Float = 0.0;
@prop
public var pathCheckTolerance: Float = 0.5;
var path: Array<Vec4> = null;
var index = 0;
var isUpdating: Bool = false;
var activeNavMesh: NavMesh = null;
public function new() {
super();
notifyOnUpdate(initNavAgent);
}
function initNavAgent() {
#if lnx_navigation
activeNavMesh = Navigation.active.navMeshes.get(navMeshId);
if(activeNavMesh == null || !activeNavMesh.ready) return;
removeUpdate(initNavAgent);
#end
}
public function setPath(path: Array<Vec4>) {
this.path = path;
index = 1;
if (!isUpdating) {
notifyOnUpdate(update);
isUpdating = true;
}
}
public function stop() {
if (isUpdating) {
removeUpdate(update);
isUpdating = false;
}
path = null;
}
function update() {
if (activeNavMesh == null || path == null || index >= path.length) {
stop();
return;
}
var currentLoc = object.transform.loc;
var p = path[index];
var dir = new Vec4().subvecs(p, currentLoc);
var normal = activeNavMesh.getNavMeshNormal(currentLoc);
normal.normalize();
var normalDot = dir.dot(normal);
dir.x -= normal.x * normalDot;
dir.y -= normal.y * normalDot;
dir.z -= normal.z * normalDot;
var dist = dir.length();
if (dist < pathCheckTolerance) {
index++;
if (index >= path.length) {
stop();
return;
}
p = path[index];
dir.subvecs(p, currentLoc);
normal = activeNavMesh.getNavMeshNormal(currentLoc);
normal.normalize();
normalDot = dir.dot(normal);
dir.x -= normal.x * normalDot;
dir.y -= normal.y * normalDot;
dir.z -= normal.z * normalDot;
dist = dir.length();
}
dir.normalize();
var step = speed * Time.delta;
if (step > dist) step = dist;
var nextPos = new Vec4(
currentLoc.x + dir.x * step,
currentLoc.y + dir.y * step,
currentLoc.z + dir.z * step
);
normal = activeNavMesh.getNavMeshNormal(nextPos);
normal.normalize();
object.transform.loc.set(
nextPos.x,
nextPos.y,
nextPos.z
);
var targetRot = new Quat().fromTo(new Vec4(0, 0, 1, 0), normal);
var forward = new Vec4().setFrom(dir);
var ndot = forward.dot(normal);
forward.x -= normal.x * ndot;
forward.y -= normal.y * ndot;
forward.z -= normal.z * ndot;
if (forward.length() > 0.0001) {
forward.normalize();
var yAxis = new Vec4(0, -1, 0, 0);
yAxis.applyQuat(targetRot);
var currForward = new Vec4().setFrom(yAxis);
var twist = new Quat().fromTo(currForward, forward);
targetRot = twist.mult(targetRot);
}
var currentRot = new Quat().setFrom(object.transform.rot);
var t = turnDuration > 0 ? Time.delta / turnDuration : 1.0;
if (t > 1.0) t = 1.0;
object.transform.rot = new Quat().lerp(currentRot, targetRot, t);
object.transform.buildMatrix();
}
}