Help me with local avoidance

Kequc

That's my actor. It's a purple capsule with a nose and two raycasts sticking out of the front. I currently have working navigation and steering so that when it changes direction it doesn't look too sudden.

func _physics_process(_delta: float) -> void:
    var position: Vector3 = actor.global_transform.origin
    var desired_velocity: Vector3 = (verts[index] - position).normalized() * actor.traits.MAX_SPEED
    var steering: Vector3 = (desired_velocity - actor.velocity) / actor.traits.MASS
    # Force
    actor.velocity = actor.move_and_slide_with_snap(actor.velocity + steering, SNAP, Vector3.UP, true)
    # Rotation
    actor.rotation.y = atan2(-actor.velocity.x, -actor.velocity.z)

That works.

I'm now trying to add local avoidance

Kequc

I'm getting confused about where I would put any additional calculation in the above code. I've tried detecting collision on either raycast and then adding/removing from the x-axis of the steering Vector3. But that doesn't seem correct because the actor behaves very strangely.

I'm trying out using normalized() in more than one place.

But vector math is not my forte and I'm getting frustrated with myself. Any help appreciated!

Kequc

More example code from what I'm trying to do:

func _physics_process(_delta: float) -> void:
	var position: Vector3 = actor.global_transform.origin
	var direction: Vector3 = (verts[index] - position).normalized()
	var avoidance: Vector3 = calc_avoidance() * AVOIDANCE_WEIGHT
	var desired_velocity: Vector3 = (direction + avoidance).normalized() * actor.traits.MAX_SPEED
	var steering: Vector3 = (desired_velocity - actor.velocity) / actor.traits.MASS
	# Force
	actor.velocity = actor.move_and_slide_with_snap(actor.velocity + steering, SNAP, Vector3.UP, true)
	# Rotation
	actor.rotation.y = atan2(-actor.velocity.x, -actor.velocity.z)

func calc_avoidance() -> Vector3:
	var left: float = 1.0 if actor.ray_left.is_colliding() else 0.0
	var right: float = -1.0 if actor.ray_right.is_colliding() else 0.0
	return Vector3(left + right, 0, 0)

Kequc

The above will be used for avoiding static obstacles like walls and objects. I'll also want to add avoidance for other actors something like this diagram:

Where just a little bit of force is added opposite to the position of nearby actors. So that got me thinking I should just add up all the forces together, including from the collision locations on my raycasts above. But what I end up working with is a lot of Vector3's that I honestly don't really know what to do with.

Anyone have recommendations of where to look for implementation of these types of features in Godot? Or just a paper on algorithms in general about the subject?

I think it would really help me.