diff --git a/project/src/GSTTargetAcceleration.gd b/project/src/GSTTargetAcceleration.gd index 82d7553..180f7f7 100644 --- a/project/src/GSTTargetAcceleration.gd +++ b/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()) diff --git a/project/src/Utils.gd b/project/src/Utils.gd index 07f0181..fa2a4e5 100644 --- a/project/src/Utils.gd +++ b/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: diff --git a/project/src/behaviors/GSTBlend.gd b/project/src/behaviors/GSTBlend.gd new file mode 100644 index 0000000..de1783d --- /dev/null +++ b/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 diff --git a/project/src/behaviors/GSTPriority.gd b/project/src/behaviors/GSTPriority.gd new file mode 100644 index 0000000..3f5a867 --- /dev/null +++ b/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