Java 向四元数添加三维矢量

我一直在用一本讨论物理引擎的书。它使用C++，但是我使用java，所以复制粘贴不能工作（我也不会这样做）。p> 我注意到的一个问题是四元数类的add（Vector3D）函数，我无法找出错误所在。我刚从书中学习了四元数（书中还用手挥动了一些数学），所以我对四元数的体验并不好

问题是：

我有一个对象，它的方向由一个标准化（幅值=1）四元数表示

我在物体上施加恒定的力矩[0,0,1]（因此只有z方向的力矩）

扭矩会产生角加速度，角速度，角位置会发生变化，通过向其方向添加3D矢量可以对其进行修改。对象似乎可以精细旋转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）

以下是添加（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;
}

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

添加（四元数）：

在我花了几个小时试图找到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”指的是刚体对象。反惯性矩是一个矩阵（3乘3）

`

`

编辑2：

我相信我现在已经修复了错误。我把误差缩小到乘法（四元数）函数。这是第一个位置，在该位置反转z组件的符号可以更正错误。我知道四元数乘法是不可交换的，所以我试着把它们转换过来。我变了

四元数变化在正则位置=四元数mega.multiply（initialAngularPosition）。divide（Real.TWO）；

到

四元数变化角位置=初始角位置。乘（四元数兆）。除（实.2）；

---

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

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

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

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

有个括号问题。请尝试：

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

---

你能在代码中添加入口点吗？步骤1-3（你的7步问题描述）的实现在哪里？我想不出任何情况下，将3向量分量逐个分量添加到四元数是有意义的。你可能想重新思考代码的这一部分应该做什么，我怀疑是某种概念上的错误…@Jim Lewis如果我不清楚的话，很抱歉。add（Vector3D）方法通过应用我在上面编写的Javadoc add（Vector3D）中的公式，将Vector3D添加到四元数中。这做了一些乘法之类的运算，结果是四元数的方向发生了变化。然后，add（四元数）方法通过-component@NathanielWaisbrot I在编辑中添加了入口点代码。让我知道如果我修好了程序错误，我相信我现在已经修复了这个bug，但我还是不明白为什么我的修复程序工作了。请看编辑2哇，接球不错。这确实是问题所在。我很高兴你指出了这一点。这是一个更好的修复比随机一个我发现乱搞的代码。