將三維矢量添加到四元數

Question

我一直在使用一本討論物理引擎的書。它使用C++，但我使用Java，所以複製粘貼不起作用（我也不會這麼做）。

我注意到的問題領域之一是在我爲Quaternion類添加（Vector3D）函數，並且我找不到該錯誤。我剛剛從這本書中學到了四元數（這也是一些數學手勢），所以我對四元數的經驗並不好。

這裏的問題是：

1. 我有一個對象，該對象獲取通過歸一化（大小= 1）表示的四元數其取向。
2. 我在物體上應用此恆定轉矩[0,0,1]（因此只有z方向上的轉矩）
3. 轉矩引起角加速度，引起角速度，引起角位置變化通過向其方向添加3D矢量進行修改。對象似乎爲0旋轉細至〜60度
4. 在〜60度時，旋轉速度變慢
5. 當物體已經旋轉大約90度時，它停止轉動
6. 所述的println（）語句表明，如旋轉接近90度，物體的方向四元數接近[sqrt（2），0,0，-sqrt（2）]並卡在那裏。
7. changeInAngularPosition是無界的（因爲有一個恆定的轉矩，所以角速度，因此dtheta是無界的）。當塊停止旋轉時，角速度在功率E-4上，所以我不相信這是由於浮點不準確造成的

如果我改用恆定轉矩[1,0,0]或[0，1，0]，一切正常。這讓我相信，在我的四元數課程中，我用Z值搞砸了。但是，幾個小時後，我找不到一個錯誤。

注意：在下面的代碼中，我使用了一個Real類型的對象，其中包含用於加入和減去它們的浮點數和方法。 （這只是爲了方便，如果我曾經想升級浮到雙）

這裏是加（的Vector3D）功能：

/** 
* 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; 
} 

添加（的Vector3D）方法使用其他一些方法：

• 我確定標量方法的分割是正確實現的，因爲它與標量的矢量分割相同。
• 乘法（Quaternion）方法公式是由本書提供的，它低於
• 加法（Quaternion）方法如下。它增加了各部件彼此（W爲w，x到x，y以y和z到z）

乘（四元）：

您可以在這裏找到公式：http://en.wikipedia.org/wiki/Quaternion#Ordered_list_form （我還檢查該功能的十倍左右，以確保我能夠正確的運用公式）

/** 
* @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); 
} 

加（四元）：

我發現在我試着花了一個小時的「修復」 g找到bug。如果我減去而不是在下面的方法中添加各自的z值，那麼旋轉完全正常 - 但是它在一次旋轉多個維度時會弄亂東西。這可能表明出現符號錯誤，但主要發生在您放棄負號的手工計算中。我理解物理學（本書很簡單），但不是四元數。我幾乎可以肯定，這個班上有錯誤。

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

有在四元數類，我不相信會包含錯誤等簡單的方法（即getter方法，setter方法），但讓我知道，如果你想看到他們。

感謝您花時間閱讀這個巨大的文本塊。我很感激。如果有什麼不清楚，請告訴我。任何幫助發現錯誤並解釋我做錯了什麼都會很棒！

編輯1：

入口點代碼： 基本上，我有一個剛體對象，它有一個反覆調用方法。以下代碼是該方法中的相關角動量代碼。其中，「this」是指RigidBody對象。反慣性矩是矩陣（3乘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)); 

`

編輯2：

我相信我現在已經修復了這一錯誤。我將誤差縮小到乘法（四元數）函數。這是扭轉z組件符號的第一個位置糾正了錯誤。我知道四元數乘法不是可交換的，所以我試着將它們切換。我改變

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

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

剛巧糾正錯誤，並通過我的其他測試。不過，我很好奇爲什麼用multiplier切換乘數會解決問題，或者如果它沒有解決它，並且我的測試案例漏掉了一些東西。

Answer 1

在你的乘法函數的最後一行的末尾仔細看：

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

有一個括號問題。嘗試，而不是：

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