我一直在使用一本物理引擎的书。它使用的是C++，但我使用的是Java，所以复制粘贴不起作用（而且我也不会这样做）。
我注意到其中一个问题出现在我的Quaternion类的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]，则一切都可以完美地运行。这使我相信，在我的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)方法使用了其他方法：
- 我确信除以标量的方法已经正确实现，因为它与向量除以标量相同。
- multiply(Quaternion)方法的公式是从书上获取的，如下所示：
- add(Quaternion)方法如下。它将各个组件相加（w加到w，x加到x，y加到y，z加到z）
multiply(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(四元数):
在我花费几个小时寻找错误时，我发现了一个“解决”方法。如果我在下面的方法中减去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 );
}
Quaternion类中还有其他简单的方法，我认为不可能出现错误（例如：getters、setters），但如果您想看到它们，请让我知道。
感谢您抽出时间阅读这篇长长的文章。我很感激。如果有任何不清楚的地方，请告诉我。帮助找到错误并解释我哪里做错了将会非常棒！
编辑 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：
我相信我已经修复了这个bug。我把错误缩小到了multiply（Quaternion）函数。这是第一个位置，其中反转z分量的符号可以纠正该bug。我知道四元数乘法不是交换律，所以我尝试交换它们。我将
Quaternion changeInAngularPosition = quaternionOmega.multiply( initialAngularPosition ).divide( Real.TWO );
改为
Quaternion changeInAngularPosition = initialAngularPosition.multiply( quaternionOmega ).divide( Real.TWO );
这恰好纠正了该bug并通过了我的其他测试。然而，我想知道为什么交换被乘数和乘积会解决问题，或者如果它没有解决问题，我的测试用例是否遗漏了什么。