java - 向四元数添加3D vector

Question

我一直在用一本讨论物理引擎的书。它使用C++，但我使用Java，所以复制粘贴不起作用（我也不会这么做）。
我注意到的一个有问题的地方是在四元数类的add（vector3d）函数中，我无法找出这个bug。我刚从这本书中学到了四元数（这本书还手上挥舞了一些数学知识），所以我对四元数的经验并不好。
问题是：
我有一个对象，它的方向由一个规格化的（大小=1）四元数表示。
我在物体上施加恒定的力矩[0，0，1]（所以只有z方向的力矩）
转矩引起角加速度，角速度，角位置的变化，通过向其方向添加三维矢量来修改。这个物体看起来旋转了0到60度
约60度时，旋转速度减慢
当物体旋转大约90度时，它停止转动。
println（）语句表明，当旋转接近90度时，对象的方向四元数接近[sqrt（2），0，0，-sqrt（2）]并卡在那里。
调节位置的变化是无界的（因为有一个恒定的扭矩，所以角速度，因此dtheta是无界的）。当块停止旋转时，角速度为e-4的幂，所以我不认为这是由于浮点不精确造成的。
如果我改为应用恒定扭矩[1，0，0]或[0，1，0]，一切都工作得很好。这让我相信，在我的四元数课的某个地方，我搞砸了z值。然而，过了好几个小时，我还是找不到错误。
注意：在下面的代码中，我使用了一个real类型的对象，它包含浮点数和用于加减浮点数的方法。（只是为了方便我把float升级到double）
下面是add（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;
}

add（vector3d）方法使用一些其他方法：
我确信标量除法是正确实现的，因为它与标量向量除法相同。
乘法（四元数）的方法公式是用勺子喂的书，它如下
下面是add（四元数）方法。它将各个组件彼此相加（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 );
}

添加（四元数）：
我花了几个小时才找到一个“修复”程序。如果我在下面的方法中减去而不是加上相应的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”指的是刚体对象。反惯性矩是一个矩阵（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 );

它碰巧纠正了错误并通过了我的其他测试。然而，我很好奇为什么用乘法器切换被乘数可以解决这个问题，或者如果它没有解决这个问题，我的测试用例漏掉了一些东西。

Answer 1

仔细看一下乘法函数最后一行的结尾：

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

括号有问题。相反，请尝试：

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