Alla ricerca di un metodo migliore per fare differenziazione Quaternion

Question

Ho un quaternione (4x1) e un vettore di velocità angolare (3x1) e chiamo una funzione per calcola il quaternione differenziale come spiegato in questo web . Gli sguardi codice come questo:

    float wx = w.at<float>(0);
float wy = w.at<float>(1);
float wz = w.at<float>(2);
float qw = q.at<float>(3); //scalar component 
float qx = q.at<float>(0);
float qy = q.at<float>(1);
float qz = q.at<float>(2);

q.at<float>(0) = 0.5f * (wx*qw + wy*qz - wz*qy);    // qdiffx
q.at<float>(1) = 0.5f * (wy*qw + wz*qx - wx*qz);    // qdiffy
q.at<float>(2) = 0.5f * (wz*qw + wx*qy - wy*qx);    // qdiffz
q.at<float>(3) = -0.5f * (wx*qx + wy*qy + wz*qz);   // qdiffw

Così ora ho il quaternione differenziale memorizzato in q e poi aggiorno il quaternione da semplicemente aggiungendo questo quaternione differenziale.

Questo metodo è adatto per prevedere movimento di oggetti rigidi o c'è un metodo migliore per predire quaternione con velocità angolare? Questo funziona, ma non sto ottenendo i risultati sperati.

Answer 1

Ci sono un paio di cose che potrebbero essere in corso. Non si parla di ri-normalizzazione che quaternione. Le cose brutte sarà sicuramente accadrà se non stai facendo quello. Inoltre non dire che si moltiplicano i componenti delta-quaternioni per la quantità di tempo che è passato dt prima di aggiungerli al quaternione originale. Se la velocità angolare è in radianti al secondo, ma si sta solo facendo un passo avanti per una frazione di secondo, ti passo troppo lontano. Tuttavia, anche così, dal momento che si sta passando attraverso una discreta quantità di tempo e cercando di far finta che è infinitesima, cose strane stanno per accadere, in particolare se il passo temporale o la velocità angolare è di grandi dimensioni.

Il motore fisico, ODE, offre la possibilità di aggiornare la rotazione di un corpo dalla sua velocità angolare come se fosse intraprende un passo infinitesimale o aggiornamento utilizzando un passo finita dimensioni. Il passo finita è molto più preciso, ma coinvolge alcuni trigonometria. funzioni e così è un po 'più lento. Il codice sorgente ODE rilevante può essere visto qui, le linee 300 -321 , con il codice di trovare il delta-quaternione qui, la linea 310 .

float wMag = sqrt(wx*wx + wy*wy + wz*wz);
float theta = 0.5f*wMag*dt;
q[0] = cos(theta);  // Scalar component
float s = sinc(theta)*0.5f*dt;
q[1] = wx * s; 
q[2] = wy * s;
q[3] = wz * s;

Dove sinc(x) è:

if (fabs(x) < 1.0e-4) return (1.0) - x*x*(0.166666666666666666667);
else return sin(x)/x;

Questo consente di evitare il problema di divisione per zero ed è ancora molto preciso.

Il q quaternione viene quindi pre-moltiplicato sulla rappresentazione quaternione esistente dell'orientamento del corpo. Poi, ri-normalizzazione.

---

Modifica - Dove questa formula viene da:

Si consideri q0 quaternione iniziale e finale che q1 quaternione risultati dopo rotante con angolare w velocità per quantità dt di tempo. Tutto quello che stiamo facendo qui sta cambiando il vettore velocità angolare in un quaternione e poi ruotando il primo orientamento da quel quaternione. Entrambi i quaternioni e velocità angolari sono variazioni sul Asse Angolo. Un corpo che è ruotata rispetto all'orientamento canonica theta attorno all'asse unità [x,y,z] avrà la seguente rappresentazione quaternione del suo orientamento: q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]. Un corpo che è rotazione theta/s attorno all'asse unità [x,y,z] avrà angolare w=[theta*x theta*y theta*z] velocità. Così, al fine di decidere quanto la rotazione avverrà nel corso secondi dt, per prima cosa estraiamo la grandezza della velocità angolare: theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2). Poi troviamo l'angolo reale moltiplicando per dt (e contemporaneamente dividere per 2 per comodità nel trasformare questo in un quaternione). Dal momento che abbiamo bisogno di normalizzare il [x y z] asse, stiamo anche dividendo per theta. Ecco dove la parte sinc(theta) proviene. (Dal momento che ha un theta 0.5*dt in più in esso dall'essere grandezza, moltiplichiamo che indietro). La funzione sinc(x) è solo utilizzando la serie di Taylor approssimazione della funzione quando x è piccola perché è numericamente stabile e più accurata di farlo. La possibilità di utilizzare questa funzione utile è il motivo per cui non abbiamo solo dividere per il wMag grandezza reale. Corpi che non ruotano molto veloce avranno molto piccole velocità angolari. Dal momento che ci aspettiamo che questo sia abbastanza comune, abbiamo bisogno di una soluzione stabile. Quello che si finisce con è un quaternione che rappresenta una sola volta passo passo dt di rotazione.

Answer 2

C'è un metodo descritto con ottimo compromesso tra velocità e precisione come incremento un quaterniom rappresenta stato rotazionale (cioè integrare equazione differenziale del moto rotatorio) da piccole vettore incremento dell'angolo dphi (che è vettore velocità angolare omega mulptipliad da tempo passo dt).

Il metodo Exact (e lento) di rotazione del quaternione da vettore :

void rotate_quaternion_by_vector_vec ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;
  double norm = Math.sqrt( r2 );

  double halfAngle = norm * 0.5d;
  double sa = Math.sin( halfAngle )/norm; // we normalize it here to save multiplications
  double ca = Math.cos( halfAngle );
  x*=sa; y*=sa; z*=sa;  

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;
}

Il problema è che si deve calcolare funzioni lenti come cos, sin, sqrt. Invece è possibile ottenere un notevole aumento di velocità e precisione ragionevole per piccoli angoli (che è il caso se il passo temporale della simulazione è ragionevole piccola) approssimando sin e cos dall'espansione di Taylor espresso usando solo norm^2 invece di norm.

Ti piace questa Metodo rapido di rotazione del quaternion dal vettore :

void rotate_quaternion_by_vector_Fast ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;

  // derived from second order taylor expansion
  // often this is accuracy is sufficient
  final double c3 = 1.0d/(6 * 2*2*2 )      ; // evaulated in compile time
  final double c2 = 1.0d/(2 * 2*2)         ; // evaulated in compile time
  double sa =    0.5d - c3*r2              ; 
  double ca =    1    - c2*r2              ; 

  x*=sa;
  y*=sa;
  z*=sa;

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

}

è possibile aumentare la precisione facendo la metà o angolo che più 5 moltiplicazioni:

  final double c3 = 1.0d/( 6.0 *4*4*4  ) ; // evaulated in compile time
  final double c2 = 1.0d/( 2.0 *4*4    ) ; // evaulated in compile time
  double sa_ =    0.25d - c3*r2          ;  
  double ca_ =    1     - c2*r2          ;  
  double ca  = ca_*ca_ - sa_*sa_*r2      ;
  double sa  = 2*ca_*sa_                 ;

o anche più accurata da parte di un altro angolo spliting a halfs:

  final double c3 = 1.0d/( 6 *8*8*8 ); // evaulated in compile time
  final double c2 = 1.0d/( 2 *8*8   ); // evaulated in compile time
  double sa = (  0.125d - c3*r2 )      ;
  double ca =    1      - c2*r2        ;
  double ca_ = ca*ca - sa*sa*r2;
  double sa_ = 2*ca*sa;
         ca = ca_*ca_ - sa_*sa_*r2;
         sa = 2*ca_*sa_;

---

Nota: Se si utilizza più sofisticato schema di integrazione di un semplice Verlet (come Runge-Kutta) si avrebbe probabilmente bisogno di un differenziale del quaternione , piuttosto che il nuovo ( aggiornato) quaternione.

questo è possibile vedere nel codice qui

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

potrebbe essere interpretata come la moltiplicazione del vecchio (non aggiornato) quaternione da ca (coseno di angolo di mezzo) che è approximativelly ca ~ 1 per piccoli angoli e aggiungendo il resto (alcune interazioni trasversali). Così il differenziale semplicemente:

  dq[0] =  x*qw + y*qz - z*qy + (1-ca)*qx;
  dq[1] = -x*qz + y*qw + z*qx + (1-ca)*qy;
  dq[2] =  x*qy - y*qx + z*qw + (1-ca)*qz;
  dq[3] = -x*qx - y*qy - z*qz + (1-ca)*qw;

dove ( 1 - ca ) ~ 0 termine per piccoli angoli e potrebbe essere a volte trascurato (in pratica solo il normalizzarsi quternion).

Answer 3

Semplice conversione da "mappa esponenziale" per quaternione. (Mappa esponenziale uguale alla velocità angolare applica tempo delta). quaternione risultato è la rotazione delta per superato tempo delta e la velocità angolare "w".

Vector3 em = w*deltaTime; // exponential map
{
Quaternion q;
Vector3 ha = em/2.0; // vector of half angle

double l = ha.norm();
if(l > 0)
{
    double ss = sin(l)/l;
    q = Quaternion(cos(l), ha.x()*ss, ha.y()*ss, ha.z()*ss);
}else{
    // if l is too small, its norm can be equal 0 but norm_inf greater 0
    q = Quaternion(1.0, ha.x(), ha.y(), ha.z());
}