1

Problem:

I have an object in 3D space that exists at a given orientation. I need to reorient the object to a new orientation. I'm currently representing the orientations as quaternions, though this is not strictly necessary.

I essentially need to determine the angular velocity needed to orient the body into the desired orientation.

What I'm currently working with looks something like the following:

Psuedocode: 

// 4x4 Matrix containing rotation and translation
Matrix4 currentTransform = GetTransform();

// Grab the 3x3 matrix containing orientation only
Matrix3 currentOrientMtx = currentTransform.Get3x3();

// Build a quat based on the rotation matrix
Quaternion currentOrientation(currentOrientMtx);
currentOrientation.Normalize();

// Build a new matrix describing our desired orientation
Vector3f zAxis = desiredForward;
Vector3f yAxis = desiredUp;
Vector3f xAxis = yAxis.Cross(zAxis);
Matrix3 desiredOrientMtx(xAxis, yAxis, zAxis);

// Build a quat from our desired roation matrix
Quaternion desiredOrientation(desiredOrientMtx);
desiredOrientation.Normalize();

// Slerp from our current orientation to the new orientation based on our turn rate and time delta
Quaternion slerpedQuat = currentOrientation.Slerp(desiredOrientation, turnRate * deltaTime);

// Determine the axis and angle of rotation
Vector3f rotationAxis = slerpedQuat.GetAxis();
float rotationAngle = slerpedQuat.GetAngle();

// Determine angular displacement and angular velocity
Vector3f angularDisplacement = rotationAxis * rotationAngle;    
Vector3f angularVelocity = angularDisplacement / deltaTime;

SetAngularVelocity(angularVelocity);

This essentially just sends my object spinning to oblivion. I have verified that the desiredOrientMtx I constructed via the axes is indeed the correct final rotation transformation. I feel like I'm missing something silly here.

Thoughts?

DanielS
  • 11
  • 1
  • 8
  • Are you using angular velocity to rotate the object (via physics engine) or just 'setting' it for others to observe? Your angular velocity should really be ...constant til you reach the point you wish to reach so. – Rollen Apr 07 '15 at 19:23
  • Yes, I'm using angular velocity to rotate via a physics engine. My assumption here is that I am overshooting the desired orientation causing the slerp to basically keep going. However if I am correct in this assumption I'm not entirely sure how to set a threshold in order to measure whether or not the current orientation is "close enough." Perhaps by looking at the rotation angle? – DanielS Apr 07 '15 at 19:34
  • Do you have a stop condition? – Rollen Apr 07 '15 at 19:34

1 Answers1

2

To calculate angular velocity, your turnRatealready provides the magnitude (rads/sec), so all you really need is the axis of rotation. That is just given by GetAxis( B * Inverse(A) ). GetAngle of that same quantity would give the total angle to travel between the two. See 'Difference' between two quaternions for further explanation.

SetAngularVelocity( Normalize( GetAxis( B * Inverse(A)) ) * turnRate )

You need to set the angular velocity to 0 at some point (when you reach your goal orientation). One way to do this is by using a quaternion distance. Another simpler way is by checking against the amount of time taken. Finally, you can check the angle between two quats (as discussed above) and check if that is close to 0. 

float totalAngle = GetAngle( Normalize( endingPose * Inverse( startingPose ) ) );
if( fabs( totalAngle ) > 0.0001 )  // some epsilon
{
     // your setting angular velocity code here 
     SetAngularVelocity(angularVelocity);
}
else
{
     SetAngularVelocity( Vector3f(0) );
     // Maybe, if you want high accuracy, call SetTransform here too
}

But, really, I don't see why you don't just use the Slerp to its fullest. Instead of relying on the physics integrator (which can be imprecise) and relying on knowing when you've reached your destination (which is somewhat awkward), you could just move the object frame-by-frame since you know the motion. 

Quaternion startingPose;
Quaternion endingPose;
// As discussed earlier...
Quaternion totalAngle = Quaternion.AngleBetween( startingPose, endingPose ); 

// t is set to 0 whenever you start a new motion
t += deltaT;

float howFarIn = (turnRate * t) / totalAngle; 
SetCurrentTransform( startingPose.Slerp( endingPose, howFarIn ) );

See Smooth rotation with quaternions for some discussion on that.

Community
  • 1
  • 1
Rollen
  • 1,212
  • 11
  • 15
  • If I understand that correctly, Normalize( GetAxis( B * Inverse(A)) ) is giving me the axis of rotation in order to rotate from A to B, correct? – DanielS Apr 07 '15 at 20:41
  • To answer your other questions, I don't know that using time in this context will work as the object is "always" in motion, in one way or another. Additionally, using the physics sim is not optional in this case. To catch the case of needing to zero out my angular velocity, could I simply dot the current orientation vs the desired orientation and look a the acos in order to determine "closeness?" – DanielS Apr 07 '15 at 20:44
  • No, quaternions aren't eulerian vectors. You will have to look at the [Math Exchange Post](http://math.stackexchange.com/questions/90081/quaternion-distance) for what is best to use. Or you could just use the 'total angle between' as that is effectively the same thing as 'distance'. – Rollen Apr 07 '15 at 20:47
  • Hmm...even if the object is consistently changing its motion, you can still just reset the motion variables appropriately (as you any ways do); if it is not optional, the stop condition can still be used. :) – Rollen Apr 07 '15 at 20:49
  • Looking at the Math Exchange Post you linked, I *should* be able to determine theta between two quats via ACos( 2 * (Q1 dot Q2)^2 - 1 ), so at the very least I can use that to determine whether I have reached some sort of epsilon. – DanielS Apr 07 '15 at 20:56