Pathfinding: staying on the right of a series of line segments

[HundredTai]

Hello,
I have a graph nodes that are connected by line segments. These lines dictate the center line which objects will generaly follow. The problem is that I’d like the objects to always stay on the right side of the line by s amount of units. This means for every node I have to calculate a point s units to the right of that node. The problem being that the line segments are not straight. For lack of better explanation I added the following image:

The black line indicates the center line I have the data for. The Nodes, (A,B,C,D) only have a Vector3 for position. The blue circles are the points I’d like to calcuate with an offset of s from the corresponding node. In theory it comes down to caculating a point on a circle around the node where the radius is s. How should I go about getting the angle on that circle which depends on the neighbouring nodes?

Thanks.

[ChefBaas]

I haven’t fully worked out a solution, but perhaps this intuition is useful to you.

.

The way I see it there are two situations; one in which your object needs to move right (taking a path shorter than the path described by the nodes) and one in which your object needs to move left (taking a path longer than the path described by the nodes. I’ve created a scrappy image explaining what my approach would be for both situations.

.

For both situations, we can imagine a circle around our point, with radius s. If I’m not mistaken this circle should always touch the lines between the nodes (so that the point is always distance s from the nodes).

.

.

In the situation where our point moves to the right, we can let it move in the same direction as the line of the node (direction = v3 of the destination - v3 of the starting point; e.g. B - A if moving from A to B). We then need to check when a circle with radius s centered on our point would have collision with the line BC and as soon as it does, have it change direction (C - B) and so forth. You could do line segment vs. circle collision as follows:

.

Vector2 pointPosition = [insert your point's position]
Vector2 pointAtTop = one of your nodes
Vector2 pointAtBottom = the other node
        // here we calculate the closest point on the (infinite) line through the line segment described by our two nodes we want to check collision with.
        // dot product gives us a number that represents the closest point on the (infinite) line.
        // if between 0.0 and 1.0, the closest point is actually on the line segment.
        // else it lies outside it and we clamp it to the range [0.0, 1.0] to obtain either edge of the line segment as the closest point
        float t = Mathf.Clamp(Vector2.Dot(blobPosition - pointAtTop, pointAtBottom - pointAtTop) / lengthOfLineSegmentSquared, 0.0f, 1.0f);
        Vector2 projection = pointAtTop + t * (pointAtBottom - pointAtTop);
        // then we simply check whether the distance from our point to that point is smaller than the distance (the radius of ouc circle)
        if (Vector2.SqrMagnitude(projection - pointPosition) < radius * radius)
        {
            return true;
        }

        return false;

.

.

Now the other case is slightly more complicated. Moving from one point to another isn’t too difficult; from A to B for example we can move the length of |AB| in the right direction (B - A) and as soon we are in the right position to move from B to C we can simply move the length of |BC| in the right direction (C - B). However to move to that right position we will have to make a circular movement (see the image) around B to keep the same distance from B. We will need to move our point an amount of degrees equal to 180 - α (in both cases in the image 45 degrees, keep in mind that at point C our point should start at 45 degrees rotation and go towards 90 rotation). The math for this should be something like this:

.

ourPoint.x = nodeToMoveAround.x + radius * cos(angle)

ourPoint.y = nodeToMoveAround.y + radius * sin(angle)

Make sure you convert to radians before doing the calculation.

.

Hopefully this is clear and helps you, otherwise I’ll do my best to clarify anything.

[JVene]

There’s more to this than you’ve described. A preface point that occurs to me is that triangles and circles are highly related in math, and it can be easy to confuse.

The more important point to get out of the way, though, is that the dotted lines in your graph, especially illustrated in the right diagram (more so than the left one), are not all s units long if your intent is for the blue line to remain s units away from the black line.

To further detail this, I’ll say that at point A on the black line is Asquare, while the blue circle attached to A by a dotted line is Acircle. Line BsquareBcircle is a dotted line. Line AsqaureBsquare is a black line. Line AcircleBcircle is a blue line, etc.

Hold on, this is, shall we say, interesting (which is why I bothered in the first place). I’m a bit rushed on time toward the end of this, so pardon if I’m off a tad here or there. The general notions are taken from links included to law of cosines and right angle trigonometry. The rest is just logic.

Notice on the left graph how the blue line BcircleCcircle isn’t parallel to the black line from BsquareCsquare, which violates your stated goal of the blue line staying s units away from the black line.

You’ve drawn the graph in the right to better reflect the stated intent, but you marked the dotted lines as length s, but they aren’t (not all of them).

On the graph to the right:

At Asquare, AsquareAcircle happens to be s in length, because, it appears, that dotted line is at a right angle from Asquare. That is not the case at point Bsquare. Bcircle should be s units from the point on the black line AsquareBsquare (a point before Bsquare) that forms a similar 90 degree angle. The dotted line BsquareBcircle is the hypotenuse of a right triangle implied at Bcircle to a point on black line AsquareBsquare meeting at 90 degrees. It can’t be s in length, it must be a little longer, as any hypotenuse would be.

It isn’t exactly to scale, but the implication of the drawing is that the dotted lines at Bsquare and Csquare should bisect the angles implied. That is, the angle of dotted line BsquareBcircle should bisect the angle formed at point Bsquare, formed by lines AsquareBsquare and BsquareCsquare.

You’ll need to use trigonometry to solve for these positions.

I must leave to you how you decide what is left or right. Most of the math doesn’t actually care.

We must start by deciding the angles of the dotted lines. We don’t know their length until we do.

The start and end points are self described as 90 degree projections from their square point to the right, and will be s in length. If there were an implied triangle at such a point, the interior angle would be zero, and bisection such an angle produces zero, meaning there is no trig to calculate, and s is the resulting dotted line’s length.

However, on the right diagram, BsquareBcircle is longer than s, but that depends on the angle at Bsquare. That line, BsquareBcircle, should bisect that implied by the angle at Bsquare where AsquareBsquare meet with BsquareCsquare. We must now calculate that angle. The implied triangle involves an imaginary line not depicted, AsquareCsquare. The law of cosines could then be used to flesh out the triangle. First, calculate the length of AsquareCsquare. You might use:

Vector3 Asq = new Vector3( Asq.x, Asq.y, Asq.z );
Vector3 Csq = new Vector3( Csq.x, Csq.y, Csq.z );

float lenAsqCsq = ( Csq - Asq ).magnitude;

Similarly, if you calculate lenAsqBsq and lenBsqCsq, you have 3 sides of a triangle from which the law of cosines can give you the angles. Referencing The Law of Cosines for the law of cosines, inferring that line c of the formula is lenAsqCsq, angle C is our target of interest. The cosine of angle C is found with something like:

float asquared = lenBsqCsq * lenBsqCsq;
float bsquared = lenAsqBsq * lenAsqBsq;
float csquared = lenAsqCsq * lenAsqCsq;

float cosAngleAtBsquare = ( asquared + bsquared - csquared ) / ( 2 * lenBsqCsq * lenAsqBsq );

float angleAtBSquare = Mathf.Acos( cosAngleAtBsquare ) * Mathf.Rad2Deg;
float angleOfBSquareDottedLine = angleAtBSquare / 2f;

Unpacking a bit, this is a tad pedantic to be clear. This “maps” the values of our code to that of the reference for the law of cosines, where sides of the triangle are labeled in lower case a, b and c, while angles of the triangle are upper case A, B and C. In this situation we assume that the imaginary line from AsquareCsquare is the law of cosines’ line c, and we want the law of cosines’ angle C (opposite this imaginary line), which is at point Bsquare. The formula uses the lengths squared, so to make that fit better and read more clearly, I fashion floats that are the squares of the 3 known sides of the triangle, which also shows how those lines map to the sides of the law of cosines’ formula.

Next, we calculate the cosine of the angle C (which is the angle at Bsquare). The angle itself is give by Mathf.Acos (the inverse trigonometric function giving the angle from a cosine), which is returned in radians. You and I more comfortably recognize degrees, so that is converted into degrees with Mathf.Rad2Deg.

The angle of the triangle implied by the dotted line must bisect this angle, so that is half of the calculated angle.

This now forms a triangle implied by an imaginary point on AsquareBsquare, meeting at 90 degrees, with an angle of angleOfBSquareDottedLine, and by logic tells us that the angle at Bcircle of this imaginary triangle is 180 - ( 90 + angleOfBsquareDottedLine ).

The length of the imaginary side connecting Bcircle to the imaginary point on AsquareBsquare is, by definition of the code requirement, s. Using right angle trigonometry, we can now say how long the dotted line BsquareBcircle is, and where it is.

The object now is to calculate how far the imaginary point in AsquareBsqure is from Bsquare. It is the missing leg of the triangle (we don’t yet know the length of the dotted line or the leg in question, but we know all 3 angles and that one leg, from the imaginary point to Bcircle, is s in length. From Right Angle Trig, scrolling down to “Sine, Cosine and Tangent”, you note that we know the “Adjacent” leg (it’s s), and we know the angle between the “Adjacent” leg and the hypotenuse (the dotted line). That angle is adjacentAngle = 180 - ( 90 + angleOfBSquareDottedLine ). Trig says that tan( adjacentAngle ) = Opposite / Adjacent (their lengths). Algebra allows us to spin that around to find Opposite with Opposite = tan( adjacentAngle ) * Adjacent. Code would be something like:

float opposite_len = Mathf.Tan(angleOfBSquareDottedLine * Mathf.Deg2Rad ) * s;

Reminder: s is adjacent. adjacentAngle is angleOfBSquareDottedLine, but was stored in degrees, but Tan needs radians.

You can now position Bcircle. It’s y is Bsquare.y - opposite_len. This assumes you’re working on the xy plane, adjust accordingly. Bcircle’s x is Bsquare.x + s.

While you don’t need to know the length of BsquareBcircle, you can find it with Pythagoras, which will prove to you that it was never s, and couldn’t be.

All of what I just walked you through assumes that Asquare is at the origin, and AsquareBsquare is aligned with the Y axis (on an XY plane, you might be in XZ, but it is otherwise the same thing). Indeed, for each segment you must work in a local coordinate system. Line BsquareCsquare must be oriented such that Bsquare is at the origin and BsquareCsquare is aligned with the Y axis, to repeat all of this for the imaginary line of BsquareDsquare. Don’t fret, though - you can start by just assuming so. The result you get (as I calculated Bcircle above) is merely a vector in local space. You’ll transform that (rotate it and translate it) into the global coordinate system of each segment.

One final note here, in the left diagram, the angle of CsquareCcircle is 135 degrees. The lines BsquareCsquare and CsquareDsquare describe a 270 degree rotation, which bisected is 135. Consequently, the resulting calculation for Ccircle’s Y position will be added to Csquare’s Y, not subtracted.

Keep your wits about you. If you think that’s complicated, we’re standing on the shoulders of ancient giants, from the ancient Greeks through the brilliant Arabs who gave us algebra over 1,000 years ago. Math is software built for the human mind to solve puzzles like this.