Question

我一直在用一本讨论物理引擎的书。它使用C ++，但我使用Java，因此复制粘贴不起作用（我也不会这样做）。

我注意到的一个有问题的区域是我的Quaternion类的add（Vector3D）函数，我无法弄清楚这个bug。我刚从书中学到了四元数（这也是一些数学手册），所以我对四元数的经验并不是很好。

问题在于：

我有一个对象，它的方向由标准化（幅度= 1）四元数表示。
我在物体上施加恒定扭矩[0,0,1]（因此只有z方向的扭矩）
扭矩导致角加速度，引起角速度，引起角位置的变化，通过向其方向添加3D矢量来修改角位置。对象看起来可以精细旋转0到~60度
在~60度时，旋转减慢
当物体旋转大约90度时，它会停止旋转
println（）语句显示，当旋转接近90度时，对象的方向四元数接近[sqrt（2），0,0，-sqrt（2）]并卡在那里。
changeInAngularPosition是无界的（因为有一个恒定的扭矩，所以角速度因此dtheta是无界的）。当挡块停止旋转时，角速度在E-4的功率上，所以我不认为这是由浮点不准确造成的。

如果我改为使用恒定扭矩[1,0,0]或[0,1,0]，一切都能正常工作。这让我相信在我的Quaternion类的某个地方，我搞砸了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）方法使用其他一些方法：

我确信通过标量方法进行的除法是正确实现的，因为它与标量的向量除法相同。
乘法（Quaternion）方法公式由书本用勺子喂食，它位于
add（Quaternion）方法如下。它将相应的组件相互添加（w到w，x到x，y到y和z到z）

乘（Quaternion）：

您可以在此处找到公式：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 );
}

add（Quaternion）：

我在尝试查找错误的几个小时内找到了“修复”。如果我减去而不是在下面的方法中添加相应的z值，旋转工作完全正常 - 但是当一次旋转多个维度时它会混乱。这可能表示符号错误，但这主要发生在您放弃负号的手工计算中。我理解物理学（这本书很简单），但不是Quaternions。我几乎可以肯定这个错误就在这个课程中。

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

Quaternion类中还有其他一些我认为不能包含错误的简单方法（即getter，setter），但是如果你想看到它们，请告诉我。

感谢您花时间阅读这个巨大的文本块。我很感激。如果有什么不清楚，请告诉我。任何帮助发现错误并解释我做错了什么都会很棒！

编辑1：

入口点代码：基本上，我有一个RigidBody对象，它有一个反复调用的方法。以下代码是该方法中的相关角动量代码。在其中，“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 );

碰巧纠正了错误并通过了我的其他测试。但是，我很好奇为什么用倍增器切换被乘数会解决问题，或者如果它没有修复它并且我的测试用例遗漏了什么。

Answer 1

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

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

有一个括号问题。请尝试改为：

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

将3D矢量添加到四元数

1 个答案: