Add Blend and Priority behaviors

project/src/GSTTargetAcceleration.gd

@@ -13,3 +13,16 @@ func set_zero() -> void:
 	linear.y = 0.0
 	linear.z = 0.0
 	angular = 0.0
+
+
+func add_scaled_accel(accel: GSTTargetAcceleration, scalar: float) -> void:
+	linear += accel.linear * scalar
+	angular += accel.angular * scalar
+
+
+func get_squared_magnitude() -> float:
+	return linear.length_squared() + angular * angular
+
+
+func get_magnitude() -> float:
+	return sqrt(get_squared_magnitude())

project/src/Utils.gd

@@ -4,7 +4,7 @@ Useful math and utility functions to complement Godot's own.
 """
 
 
-static func clmapedv3(vector: Vector3, limit: float) -> Vector3:
+static func clampedv3(vector: Vector3, limit: float) -> Vector3:
 	var len2: = vector.length_squared()
 	var limit2: = limit * limit
 	if len2 > limit2:

project/src/behaviors/GSTBlend.gd

@@ -0,0 +1,53 @@
+extends GSTSteeringBehavior
+class_name GSTBlend
+"""
+Blends multiple steering behaviors into one, and returns acceleration combining all of them.
+
+Each behavior is associated with a weight - a modifier by which the result will be multiplied by,
+then added to a total acceleration.
+"""
+
+
+onready var _behaviors: = []
+onready var _accel: = GSTTargetAcceleration.new()
+
+
+func _init(agent: GSTSteeringAgent).(agent) -> void:
+	pass
+
+
+func add(behavior: GSTSteeringBehavior, weight: float) -> void:
+	behavior.agent = agent
+	_behaviors.append(GSTBehaviorAndWeight.new(behavior, weight))
+
+
+func get_behavior_at(index: int) -> GSTBehaviorAndWeight:
+	if _behaviors.size() > index:
+		return _behaviors[index]
+	printerr("Tried to get index " + str(index) + " in array of size " + str(_behaviors.size()))
+	return null
+
+
+func _calculate_steering(blended_accel: GSTTargetAcceleration) -> GSTTargetAcceleration:
+	blended_accel.set_zero()
+	
+	for i in range(_behaviors.size()):
+		var bw: GSTBehaviorAndWeight = _behaviors[i]
+		bw.behavior.calculate_steering(_accel)
+		
+		blended_accel.add_scaled_accel(_accel, bw.weight)
+	
+	blended_accel.linear = Utils.clampedv3(blended_accel.linear, agent.max_linear_acceleration)
+	if blended_accel.angular > agent.max_angular_acceleration:
+		blended_accel.angular = agent.max_angular_acceleration
+	
+	return blended_accel
+
+
+class GSTBehaviorAndWeight:
+	var behavior: GSTSteeringBehavior
+	var weight: float
+	
+	func _init(behavior: GSTSteeringBehavior, weight: float) -> void:
+		self.behavior = behavior
+		self.weight = weight

project/src/behaviors/GSTPriority.gd

@@ -0,0 +1,48 @@
+extends GSTSteeringBehavior
+class_name GSTPriority
+"""
+Contains multiple steering behaviors and returns only the result of the first that has a non-zero
+ acceleration.
+"""
+
+
+onready var _behaviors: = []
+
+var last_selected_index: int
+var threshold_for_zero: float
+
+
+func _init(agent: GSTSteeringAgent, threshold_for_zero: = 0.001).(agent) -> void:
+	self.threshold_for_zero = threshold_for_zero
+
+
+func add(behavior: GSTSteeringBehavior) -> void:
+	_behaviors.append(behavior)
+
+
+func get_behavior_at(index: int) -> GSTSteeringBehavior:
+	if _behaviors.size() > index:
+		return _behaviors[index]
+	printerr("Tried to get index " + str(index) + " in array of size " + str(_behaviors.size()))
+	return null
+
+
+func _calculate_steering(accel: GSTTargetAcceleration) -> GSTTargetAcceleration:
+	var threshold_squared: = threshold_for_zero * threshold_for_zero
+	
+	last_selected_index = -1
+	
+	var size: = _behaviors.size()
+	
+	if size > 0:
+		for i in range(size):
+			last_selected_index = i
+			var behavior: GSTSteeringBehavior = _behaviors[i]
+			behavior.calculate_steering(accel)
+			
+			if accel.get_squared_magnitude() > threshold_squared:
+				break
+	else:
+		accel.set_zero()
+	
+	return accel