I want an object to follow a realistic path to a destination, taking into account a radius that it can make. I found out about Dubins Curves, and I’ve seen some (non-Godot) examples. Then, because I am completely bad at math, I asked CoPilot to implement an example in Gdscript.
The problem is that the code is always generating a straight line and doesn’t calculate any turns.
Those familiar with Dubins, know that you have 6 feasible methods to arrive at a destination, for example Left-Straight-Left or Right-Straight-Right, etc…
I have this function for the LSL as an example:
static func _dubins_LSR(start_pos, start_angle, end_pos, end_angle, turning_radius):
var dx = end_pos.x - start_pos.x
var dy = end_pos.y - start_pos.y
var dz = end_pos.z - start_pos.z
var D = sqrt(dx * dx + dy * dy + dz * dz)
var d = D / turning_radius
var theta = atan2(dy, dx)
var alpha = fmod(start_angle.y - theta, 2 * PI)
var beta = fmod(end_angle.y - theta, 2 * PI)
var sa = sin(alpha)
var sb = sin(beta)
var ca = cos(alpha)
var cb = cos(beta)
var c_ab = cos(alpha - beta)
var tmp0 = -2 + d * d + 2 * c_ab + 2 * d * (sa + sb)
if tmp0 < 0:
return { "type": PathType.LSR, "length": INF, "points": [] }
var p = sqrt(tmp0)
var tmp1 = atan2(-ca - cb, d + sa + sb) - atan2(-2.0, p)
var t = fmod(alpha - tmp1, 2 * PI)
var q = fmod(beta - tmp1, 2 * PI)
var length = t + p + q
var points = []
points.append(start_pos)
# Calculate the points along the path
var current_pos = start_pos
var current_angle = start_angle
# First left turn
for i in range(0, int(t * turning_radius)):
current_angle.y += 1 / turning_radius
current_pos += Vector3(cos(current_angle.y), sin(current_angle.y), 0)
points.append(current_pos)
# Straight segment
for i in range(0, int(p * turning_radius)):
current_pos += Vector3(cos(current_angle.y), sin(current_angle.y), 0)
points.append(current_pos)
# Second right turn
for i in range(0, int(q * turning_radius)):
current_angle.y -= 1 / turning_radius
current_pos += Vector3(cos(current_angle.y), sin(current_angle.y), 0)
points.append(current_pos)
# Debugging information
print("Start Position: ", start_pos)
print("End Position: ", end_pos)
print("Calculated Path Points: ", points)
return { "type": PathType.LSR, "length": length, "points": points }
This function calculates a Dubins path of type LSL (Left-Straight-Left) between two points in 3D space, considering a given turning radius. It returns a dictionary containing the path type, length, and points.
Like I said, my problem is, that it always generates a straight line, it never makes any turn.
I’m hoping someone who knows math can help me in the right direction.
Here’s a breakdown of what the function does, since is has a lot of poorly named vars:
Calculate Differences and Distance:
- dx, dy, dz: Differences in x, y, and z coordinates between the start and end positions.
- D: Euclidean distance between the start and end positions.
- d: Normalized distance by dividing D by the turning radius.
Angles Calculation:
- theta: Angle between the start and end positions in the x-y plane.
- alpha, beta: Adjusted start and end angles relative to theta.
Trigonometric Values:
- sa, sb, ca, cb: Sine and cosine of alpha and beta.
- c_ab: Cosine of the difference between alpha and beta.
Path Feasibility Check:
- tmp0: A value used to check if the path is feasible. If tmp0 is negative, the function returns an infinite length and an empty points list.
Path Parameters:
- tmp1: Intermediate angle calculation.
- t, p, q: Path parameters representing the lengths of the left turn, straight segment, and second left turn, respectively.
- length: Total length of the path.
Path Points Calculation:
The function calculates the points along the path, including the first left turn, straight segment, and second left turn. It updates the current position and angle at each step and appends the points to the points list.