Cubic Bezier Curve: Maximum Gradient and Collision Avoidance?

Question

I'm using bezier curves as paths for my spaceships to travel along when they are coming into dock at a station. I have a simple algorithm to calculate where the ship should be at time t along a cubic bezier curve:

public class BezierMovement{
    public BezierMovement(){
        // start docking straight away in this test version
        initDocking();
    }

    private Vector3 p0;
    private Vector3 p1;
    private Vector3 p2;
    private Vector3 p3;

    private double tInc = 0.001d;
    private double t = tInc;

    protected void initDocking(){

        // get current location
        Vector3 location = getCurrentLocation();

        // get docking point
        Vector3 dockingPoint = getDockingPoint();

        // ship's normalised direction vector
        Vector3 direction = getDirection();

        // docking point's normalised direction vector
        Vector3 dockingDirection = getDockingDirection();

        // scalars to multiply normalised vectors by 
        // The higher the number, the "curvier" the curve
        float curveFactorShip = 10000.0f;
        float curveFactorDock = 2000.0f;

        p0 = new Vector3(location.x,location.y,location.z);

        p1 = new Vector3(location.x + (direction.x * curveFactorShip),
                         location.y + (direction.y * curveFactorShip),
                         location.z + (direction.z * curveFactorShip));

        p2 = new Vector3(dockingPoint.x + (dockingDirection.x * curveFactorDock),
                         dockingPoint.y + (dockingDirection.y * curveFactorDock),
                         dockingPoint.z + (dockingDirection.z * curveFactorDock));

        p3 = new Vector3(dockingPoint.x, dockingPoint.y, dockingPoint.z);


    }

    public void incrementPosition() {

        bezier(p0, p1, p2, p3, t, getCurrentLocation());

        // make ship go back and forth along curve for testing              
        t += tInc;

        if(t>=1){
            tInc = 0-tInc;
        } else if(t<0){
            tInc = 0-tInc;
        }

    }

    protected void bezier(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, double t, Vector3 outputVector){

        double a = (1-t)*(1-t)*(1-t);
        double b = 3*((1-t)*(1-t))*t;
        double c = 3*(1-t)*(t*t);
        double d = t*t*t;

        outputVector.x = a*p0.x + b*p1.x + c*p2.x + d*p3.x;
        outputVector.y = a*p0.y + b*p1.y + c*p2.y + d*p3.y;
        outputVector.z = a*p0.z + b*p1.z + c*p2.z + d*p3.z;

    }
}

The curve start point is the spaceship location, and end point is the entrance to the docking bay (red dots on diagram). The spaceship has a normalised vector for its direction, and the docking bay has another normalised vector to indicate the direction the ship must be traveling in so as to be aligned straight on to the docking bay when it arrives (the yellow lines on the diagram)

The green line is a possible path of the spaceship, and the purple circle, the spaceship's radius. Finally, the black box is the bounding box for the station.

I have two problems:

1. The spaceship is supposed to only be able to turn at r radians per second
2. The spaceship can't fly through the station

I assume that this translates into:

a). Finding the "curve factors" (control point lengths) that will give a path where the ship doesn't have to turn too tightly

b). Finding the spaceship location/direction from which it can't avoid colliding with the station (and creating a path to guide it out of that state, so it can get on with part a))

However, with both of these, I haven't had much luck finding a solution. I already have code to detect intersections between vectors, boxes, points and spheres, but not bezier curves yet. I also have functions to let me find the distance between two points.

Any help would be most appreciated

Thanks, James

Answer 1

Finding the exact intersections of a Cubic Bezier Curve involves solving a 5th or 6th degree polynomial. More feasible solutions are either using numerical methods, or subdividing the Bezier Curve.

protected void subdivide(
        Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3,
        Vector3 q0, Vector3 q1, Vector3 q2, Vector3 q3,
        Vector3 q4, Vector3 q5, Vector3 q6) {

    q0.x = p0.x; q0.y = p0.y; q0.z = p0.z;
    q6.x = p3.x; q6.y = p3.y; q6.z = p3.z;

    q1.x = (p0.x + p1.x) * 0.5;
    q1.y = (p0.y + p1.y) * 0.5;
    q1.z = (p0.z + p1.z) * 0.5;

    q5.x = (p2.x + p3.x) * 0.5;
    q5.y = (p2.y + p3.y) * 0.5;
    q5.z = (p2.z + p3.z) * 0.5;

    double x3 = (p1.x + p2.x) * 0.5;
    double y3 = (p1.y + p2.y) * 0.5;
    double z3 = (p1.z + p2.z) * 0.5;

    q2.x = (q1.x + x3) * 0.5;
    q2.y = (q1.y + y3) * 0.5;
    q2.z = (q1.z + z3) * 0.5;

    q4.x = (x3 + q1.x) * 0.5;
    q4.y = (y3 + q1.y) * 0.5;
    q4.z = (z3 + q1.z) * 0.5;

    q3.x = (q2.x + q4.x) * 0.5;
    q3.y = (q2.y + q4.y) * 0.5;
    q3.z = (q2.z + q4.z) * 0.5;
}

q1..q3 becomes the first segment. q3..q6 becomes the second segment.

Subdivide the curve 2-5 times, and use the control-points as a polyline.

---

The curvature could be calculated at the end-points of each segment:

protected double curvatureAtStart(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3) {
    double dx1 = p1.x - p0.x;
    double dy1 = p1.y - p0.y;
    double dz1 = p1.z - p0.z;

    double A = dx1 * dx1 + dy1 * dy1 + dz1 * dz1;

    double dx2 = p0.x - 2*p1.x + p2.x;
    double dy2 = p0.y - 2*p1.y + p2.y;
    double dz2 = p0.z - 2*p1.z + p2.z;

    double B = dx1 * dx2 + dy1 * dy2 + dz1 * dz2;

    double Rx = (dx2 - dx1*B/A)/A*2/3;
    double Ry = (dy2 - dy1*B/A)/A*2/3;
    double Rz = (dz2 - dz1*B/A)/A*2/3;

    return Math.sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
}

---

To solve Problem 1, subdivide the curve a few times, and calculate the curvature at each segment's endpoint. This will just be an approximation, but you could selectively subdivide segments with high curvature to get a better approximation in that region.

---

To solve Problem 2, you could subdivide three curves:

• One with velocity zero at both endpoints (C0). This would produce a straight line.
• One with velocity zero at the first endpoint, and one at the second (C1).
• One with velocity one at the first endpoint, and zero at the second (C2).

If you subdivide all curves in the same way, you could quickly evaluate the control-points of the final curve. You blend the corresponding control-points, parametrized by the velocities at the end-points:

C[i] = C0[i] + (C1[i] - C0[i])*v1 + (C2[i] - C0[i])*v2

You could with this find valid parameter-ranges, so that no segment (evaluated as a straight line-segment) intersects the station. (v1 and v2 can go above 1.0).