Ajouter un vecteur 3D au quaternion

Question

J'ai utilisé un livre traitant des moteurs physiques. Il utilise C++, mais j'utilise Java, donc le copier-coller ne fonctionnerait pas (et je ne le ferais pas non plus). Un des domaines problématiques que j'ai remarqué est dans ma fonction add (Vector3D) pour la classe Quaternion, et je n'arrive pas à comprendre le bug. Je viens d'apprendre des quaternions du livre (qui a également fait des calculs à la main), donc mon expérience avec Quaternions n'est pas géniale.

Voici le problème:

1. J'ai un objet qui obtient son orientation représentée par une normalisée (magnitude = 1) Quaternion.
2. I appliquer ce couple constant [0, 0, 1] (si seulement un couple de rotation dans la direction z) sur l'objet
3. Le couple provoque une accélération angulaire, ce qui provoque une vitesse angulaire, ce qui provoque un changement de position angulaire, qui est modifié en ajoutant un vecteur 3D à son orientation. L'objet semble tourner bien pour 0 à ~ 60 degrés
4. A ~ 60 degrés, la rotation ralentit
5. Lorsque l'objet a tourné d'environ 90 degrés, il cesse de tourner
6. Les états println() montrent que la rotation s'approche de 90 degrés, l'orientation quaternion de l'objet se rapproche de [sqrt (2), 0, 0, -sqrt (2)] et s'y coince. La propriété changeInAngularPosition est illimitée (car il y a un couple constant, donc la vitesse angulaire et donc la dtheta est illimitée). Lorsque le bloc cesse de tourner, la vitesse angulaire est de la puissance E-4, de sorte que je ne pense pas que ce soit en raison de l'imprécision de virgule flottante

Si je place appliqué le couple constant [1, 0, 0] ou [0, 1, 0], tout fonctionne parfaitement. Cela m'amène à croire que quelque part dans mon cours de Quaternion, j'ai raté avec une valeur Z. Cependant, après plusieurs heures, je n'ai pas pu trouver une erreur.

Remarque: Dans le code suivant, j'utilise un objet de type Real, qui contient des flottants et des méthodes pour les ajouter et les soustraire. (Il est juste pour le rendre pratique si je veux toujours mettre à niveau flotteur double)

est l'add ici (Vector3D) Fonction:

/** 
* updates the angular position using the formula 
* <p> 
* theta_f = theta_i + dt/2 * omega * theta_i 
* <p> 
* where theta is position and omega is angular velocity multiplied by a dt (dt = 1/1000 currently). 
* 
* @param omega    angular velocity times a change in time (dt) 
* @return 
*/ 
public Quaternion add(Vector3D omega) { 

    //convert the omega vector into a Quaternion 
    Quaternion quaternionOmega = new Quaternion(Real.ZERO , omega.getX() , omega.getY() , omega.getZ()); 

    //calculate initial theta 
    Quaternion initialAngularPosition = this; 

    //calculate delta theta, which is dt/2 * omega * theta_i 
    Quaternion changeInAngularPosition = quaternionOmega.multiply(initialAngularPosition).divide(Real.TWO); 

    //System.out.println("dtheta = " + changeInAngularPosition); 
    //System.out.println("theta = " + this); 
    //System.out.println("Quaternion omega = " + quaternionOmega); 

    //add initial theta to delta theta 
    Quaternion finalAngularPosition = initialAngularPosition.add(changeInAngularPosition); 
    //System.out.println(finalAngularPosition); 

    //return the result 
    return finalAngularPosition; 
} 

La méthode add (Vector3D) utilise d'autres méthodes:

• Je suis certain que la division par une méthode scalaire est implémentée correctement car elle est la même que la division vectorielle par scalaires.
• La méthode multiply (Quaternion) a été alimentée à la cuillère par le livre et est inférieure à
• La méthode add (Quaternion) est ci-dessous.Il ajoute des composants respectifs les uns aux autres (w w, x à x, y pour y et z à z)

se multiplient (Quaternion):

Vous pouvez trouver la formule ici: http://en.wikipedia.org/wiki/Quaternion#Ordered_list_form (I ont également vérifié cette fonction dizaine de fois pour vous assurer que j'ai appliqué correctement la formule)

/** 
* @param multiplier  <code>Quaternion</code> by which to multiply 
* @return     <code>this * Quaternion</code> 
*/ 
public Quaternion multiply(Quaternion multiplier) { 
    Real w1 = this.m_w; 
    Real w2 = multiplier.getW(); 
    Real x1 = this.m_x; 
    Real x2 = multiplier.getX(); 
    Real y1 = this.m_y; 
    Real y2 = multiplier.getY(); 
    Real z1 = this.m_z; 
    Real z2 = multiplier.getZ(); 

    Real productW = w1.multiply(w2).subtract(x1.multiply(x2)).subtract(y1.multiply(y2)).subtract(z1.multiply(z2)); 
    Real productX = w1.multiply(x2).add(x1.multiply(w2)).add(y1.multiply(z2)).subtract(z1.multiply(y2)); 
    Real productY = w1.multiply(y2).subtract(x1.multiply(z2)).add(y1.multiply(w2)).add(z1.multiply(x2)); 
    Real productZ = w1.multiply(z2).add(x1.multiply(y2)).subtract(y1.multiply(x2).add(z1.multiply(w2))); 

    return new Quaternion(productW , productX , productY , productZ); 
} 

add (Quaternion):

J'ai trouvé une « solution » dans les heures passées tryin g pour trouver le bug. Si je soustrais au lieu d'ajouter les valeurs z respectives dans la méthode suivante, la rotation fonctionne complètement bien - mais cela gâche les choses lors de la rotation dans plusieurs dimensions à la fois. Cela peut suggérer une erreur de signe, mais cela se produit principalement dans les calculs faits à la main où vous laissez tomber un signe négatif. Je comprends la physique (que le livre rend assez simple), mais pas les quaternions. Je suis presque certain que l'erreur est dans cette classe.

/** 
* adds this <code>Quaternion</code> to the <code>augend</code> by 
* adding respective components 
* 
* [ w1 , x1 , y1 , z1 ] + [ w2 , x2 , y2 , z2 ] = [ w1 + w2 , x1 + x2 , y1 + y2 , z1 + z2 ] 
* 
* @param augend  <code>Quaternion</code> to add 
* @return    <code>this + augend </code> 
*/ 
public Quaternion add(Quaternion augend) { 
    Real newW = this.m_w.add(augend.getW()); 
    Real newX = this.m_x.add(augend.getX()); 
    Real newY = this.m_y.add(augend.getY()); 

    //TODO UNEXPLAINABLE - why must this be subtract 
    Real newZ = this.m_z.add(augend.getZ()); 

    return new Quaternion(newW , newX , newY , newZ); 
} 

Il existe d'autres méthodes simples dans la classe Quaternion que je ne crois pas que pourrait contenir l'erreur (à savoir, getters setters), mais laissez-moi savoir si vous voulez les voir.

Merci d'avoir pris le temps de lire ce bloc de texte géant. Je vous en suis reconnaissant. Faites-moi savoir si quelque chose n'était pas clair. Toute aide pour repérer le bug et expliquer ce que j'ai fait de mal serait merveilleux!

Edit 1:

code de point d'entrée: Fondamentalement, j'avoir un objet modèle du solide indéformable et il a une méthode appelée à plusieurs reprises. Le code suivant est le code d'impulsion angulaire pertinent dans cette méthode. Dans celui-ci, "ceci" fait référence à l'objet RigidBody. Le moment d'inertie inverse est une matrice (3 par 3).

`

//calculate angular acceleration from torque = I * alpha 
    //or alpha = torque/I 
    Vector3D angularAcceleration = this.m_invMomentOfInertia.transform(this.getNetTorque()); 

    //adjust angular velocity 
    this.setAngularVelocity(this.getAngularVelocity().add(angularAcceleration.multiply(duration))); 

    //modify angular position 
    Vector3D deltaTheta = this.getAngularVelocity().multiply(duration); 
    this.setOrientation(this.getOrientation().add(deltaTheta)); 

`

Edit 2:

Je crois avoir maintenant résolu le bug. J'ai réduit l'erreur à la fonction de multiplication (Quaternion). C'était le premier endroit où l'inversion du signe du composant z corrigeait le bug. Je savais que la multiplication de Quaternion n'était pas commutative, alors j'ai essayé de les changer. J'ai changé

Quaternion changeInAngularPosition = quaternionOmega.multiply(initialAngularPosition).divide(Real.TWO);

à Quaternion changeInAngularPosition = initialAngularPosition.multiply(quaternionOmega).divide(Real.TWO);

qui est arrivé à corriger le bug et passer mes autres tests. Cependant, je suis curieux de savoir pourquoi changer le multiplicande avec le multiplicateur réglerait le problème, ou s'il ne l'a pas résolu et mes cas de test ont manqué quelque chose.

Answer 1

regarder de près à la fin de la dernière ligne de votre fonction de multiplication:

.subtract(y1.multiply(x2).add(z1.multiply(w2))); 

Il y a un problème de parenthèse. Essayez, à la place:

.subtract(y1.multiply(x2)).add(z1.multiply(w2));