Adding 3D Vector to Quaternion

Question

I have been using a book discussing physics engines. It uses C++, but I use Java, so copy-pasting would not work ( and I wouldn't do that either ).

One of the problematic areas I have noticed is in my add( Vector3D ) function for the Quaternion class, and I can't figure out the bug. I just learned about Quaternions from the book (which also hand-waved some math), so my experience with Quaternions is not great.

Here is the problem:

1. I have an object that gets its orientation represented by a normalized (magnitude = 1 ) Quaternion.
2. I apply this constant torque [ 0 , 0 , 1 ] (so only a torque in the z-direction) on the object
3. The torque causes angular acceleration, causing angular velocity, causing a change in angular position, which is modified by adding a 3D Vector to its orientation. The object appears to rotate fine for 0 to ~60 degrees
4. At ~60 degrees, the rotation slows down
5. When the object has rotated approximately 90 degrees, it stops rotating
6. The println() statements show that as the rotation approaches 90 degrees, the orientation quaternion of the object approaches [ sqrt(2) , 0 , 0 , -sqrt(2) ] and gets stuck there.
7. The changeInAngularPosition is unbounded ( because there is a constant torque, so angular velocity and therefore dtheta is unbounded ). When the block stops rotating, the angular velocity is on the power E-4, so I do not believe it is due to floating point inaccuracy

If I instead applied the constant torque [ 1 , 0 , 0 ] or [ 0 , 1 , 0 ], everything works perfectly. This leads me to believe that somewhere in my Quaternion class, I screwed up with a Z value. However, after many hours, I could not find a mistake.

Note: In the following code, I use a objects of type Real, which contain floats and methods for adding and subtracting them. (It's just to make it convenient if I ever want to upgrade float to double)

Here is the add( Vector3D ) function:

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

The add( Vector3D ) method uses some other methods:

• I am certain that the division by a scalar method is implemented correctly because it is the same as Vector division by scalars.
• The multiply( Quaternion ) method formula was spoon-fed by the book and it is below
• The add( Quaternion ) method is below. It adds respective components to each other ( w to w , x to x , y to y and z to z )

multiply( Quaternion ):

You can find the formula here: http://en.wikipedia.org/wiki/Quaternion#Ordered_list_form (I have also checked this function about ten times to make sure I applied the formula correctly)

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

I did find a "fix" in the hours I spent trying to find the bug. If I subtracted instead of added the respective z values in the following method, the rotation works completely fine - but it messes up things when rotating in multiple dimensions at once. This might suggest a sign error, but that mainly occurs in calculations done by hand where you drop a negative sign. I understand the physics ( which the book makes quite simple ), but not Quaternions. I'm almost certain the error is in this class.

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

There are other simple methods in the Quaternion class that I do not believe could contain the error (i.e. getters, setters ), but let me know if you would like to see them.

Thanks for taking the time to read this giant block of text. I appreciate it. Let me know if anything wasn't clear. Any help with spotting the bug and explaining what I did wrong would be wonderful!

Edit 1:

Entry point code: Basically, I have a RigidBody object and it has a method called over and over. The following code is the relevant angular momentum code in that method. In it, "this" refers to the RigidBody object. The inverse moment of inertia is a Matrix (3 by 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 ) );

`

Edit 2:

I believe I have now fixed the bug. I narrowed the error down to the multiply( Quaternion ) function. That was the first location at which reversing the sign of the z component corrected the bug. I knew Quaternion multiplication is not commutative, so I tried switching them around. I changed

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

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

which happened to correct the bug and pass my other tests. However, I'm curious as to why switching the multiplicand with the multiplier would fix the problem, or if it did not fix it and my test cases missed something.

Answer 1

Take a close look at the end of the last line of your multiplication function:

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

There's a parenthesis problem. Try, instead:

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