Curva Bezier cubica: massima pendenza e prevenzione delle collisioni?

Question

Sto usando le curve di Bezier come percorsi per le mie astronavi che viaggiano quando arrivano al molo di una stazione. Ho un semplice algoritmo per calcolare dove la nave dovrebbe essere al tempo t lungo una curva di Bézier cubica:

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; 

    } 
} 

La curva punto di partenza è la posizione nave spaziale, e il punto finale è l'ingresso alla baia di attracco (punti rossi sullo schema). L'astronave ha un vettore normalizzato per la sua direzione e la baia di attracco ha un altro vettore normalizzato per indicare la direzione in cui la nave deve viaggiare in modo da essere allineata direttamente all'alloggiamento di attracco quando arriva (le linee gialle sullo schema)

La linea verde è un possibile percorso dell'astronave e il cerchio viola, il raggio dell'astronave. Infine, la scatola nera è il riquadro di delimitazione della stazione.

ho due problemi:

1. L'astronave dovrebbe essere solo in grado di girare a r radianti al secondo
2. L'astronave non può volare attraverso la stazione di

Presumo che ciò si traduca in:

a). Individuazione dei "fattori di curva" (lunghezze dei punti di controllo) che daranno un percorso in cui la nave non deve girare troppo stretto

b). Trovare la posizione/direzione dell'astronave da cui non può evitare di entrare in collisione con la stazione (e creare un percorso per guidarlo da quello stato, in modo che possa andare avanti con la parte a))

Tuttavia, con entrambi questi , Non ho avuto molta fortuna nel trovare una soluzione. Ho già il codice per rilevare le intersezioni tra vettori, riquadri, punti e sfere, ma non ancora curve più precise. Ho anche funzioni per farmi trovare la distanza tra due punti.

Qualsiasi aiuto sarebbe apprezzato

Grazie, James

Answer 1

Trovare le intersezioni esatte di una cubica di Bézier Curve comporta la soluzione di un 5 ° o 6 ° polinomio di grado. Soluzioni più fattibili sono usando metodi numerici o suddividendo la curva di Bezier.

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 diventa il primo segmento. q3 .. q6 diventa il secondo segmento.

Suddividere la curva 2-5 volte e utilizzare i punti di controllo come polilinea.

---

Il curvature può essere calcolato nei punti finali di ogni segmento:

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); 
} 

---

Per risolvere Problema 1, suddividono la curva alcune volte, e calcolare la curvatura a endpoint di ogni segmento. Questa sarà solo un'approssimazione, ma potresti dividere selettivamente i segmenti con un'alta curvatura per ottenere un'approssimazione migliore in quella regione.

---

Per risolvere Problema 2, si potrebbe suddividere tre curve:

• Uno con velocità zero in entrambi gli endpoint (C0). Ciò produrrebbe una linea retta.
• Uno con velocità zero al primo estremo e uno al secondo (C1).
• Uno con velocità uno al primo estremo e zero al secondo (C2).

Se suddividere tutte le curve nello stesso modo, si potrebbe valutare rapidamente i controllo-punti della curva finale. Si fondono i corrispondenti di controllo punti, parametrizzata le velocità nei punti finali:

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

Si potrebbe a questo trovare validi parametri-range, in modo che nessun segmento (valutata come una linea-segmento retto) interseca la stazione. (v1 e v2 possono andare oltre 1,0).