• Advertisement
Sign in to follow this  

Preventing a camera's roll from drifting

This topic is 3223 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Recommended Posts

I had actually thought I got my 6DOF camera working properly, but only recently I've noticed that whenever I rotate the yaw and pitch 'too much', the roll seems to drift apart and the three basis vectors are not orthogonal any more. So my guess is that I have to orthogonalise the matrix (which is generated using a FromYawPitchRoll() method) because I'm storing the rotations incrementally, however I'm unsure how to actually do this. I've looked around and the only thing I could find is the Gram-Schmidt algorithm. My math background being very weak, I couldn't really understand how the whole thing worked. Also, just to be sure, checking the dot products of all three vectors should return 0 if they are mutually perpendicular right? Since that's how I'm checking if the three vectors are orthogonal. Any help would be greatly appreciated. :)

Share this post


Link to post
Share on other sites
Advertisement
If you renormalize the matrix periodically this shouldn't happen, or be noticeable. But you could just reorthogonalize the matrix with cross products. This will be biased towards a particular axis of your choosing.

Ideally though wouldn't make incremental corrections on the output value, the matrix. You'd increment the source values, such as the angles like you mentioned, or a quaterion, and use that to regenerate the matrix.

Share this post


Link to post
Share on other sites
Quote:
Ideally though wouldn't make incremental corrections on the output value, the matrix. You'd increment the source values, such as the angles like you mentioned, or a quaternion , and use that to regenerate the matrix.


Thanks for replying.

The matrix used to get the camera's three basis vectors is actually converted from a quaternion which represents the total rotation. Every frame, a matrix is created using yaw pitch and roll values, converted to a quaternion which is multiplied by the total rotation. Sorry, I forgot to explain that.

For clarity, it goes like this:

Take the change of rotation in yaw, pitch and roll values
Build a matrix representing the change
Convert that matrix to a quaternion
Multiply the converted quaternion with the totalRotation quaternion

Whenever the RotateYawPitchRoll() is called and then:

Convert totalRotation to matrix
Orthoganalise the matrix here????
Retrieve the camera's three basis vectors from that matrix
Normalise those vectors

Whenever Update() is called, which is every frame.

[Edited by - -Datriot- on April 22, 2009 8:12:27 AM]

Share this post


Link to post
Share on other sites
Why do you create a matrix only to convert it to a quaternion? Why not just create the quaternion directly?

After calling CreateFromYawPitchRoll() a matrix should be returned which is completely orthogonal, there should be no reorthogonalization required after that. Though i dont know how you're using both quaternions and YawPitchRoll.. there's far too much conversion going on here.

it shouldbe:

Quat dQ = GetIncrementalRotation();
finalQ *= dQ;

finalMatrix = Matrix(finalQ);

Share this post


Link to post
Share on other sites
I'm not sure about other methods, but the way I orthonormalize matrices is this:


void OrthoNormalize(Matrix3& m)
{
m[0] /= m[0].Length();
m[1] /= m[1].Length();
m[2] = m[0].CrossProduct(m[1]);
m[2] /= m[2].Length();
m[0] = m[1].CrossProduct(m[2]);
}


Assuming your 0 and 1 columns are more or less correct, you can automatically get the correct 2 column using the cross product. The final step ensures that 0 column is orthogonal to 1 column.

edit:
Added m[2] normalize, might be required.

Share this post


Link to post
Share on other sites
Quote:
For clarity, it goes like this:

Take the change of rotation in yaw, pitch and roll values
Build a matrix representing the change
Convert that matrix to a quaternion
Multiply the converted quaternion with the totalRotation quaternion

Whenever the RotateYawPitchRoll() is called and then:

Convert totalRotation to matrix
Orthoganalise the matrix here????
Retrieve the camera's three basis vectors from that matrix
Normalise those vectors

Whenever Update() is called, which is every frame.
Just to echo what bzroom said, it sounds like you're doing some unnecessary conversions and normalizations here. Here's a stripped-down version with the redundancies removed:
- Build a quaternion from the yaw, pitch, and roll delta values
- Multiply the delta quaternion with the totalRotation quaternion
- Normalize the totalRotation quaternion
- Convert the totalRotation quaternion to matrix form if/as needed
Let me know if that requires any clarification.

Share this post


Link to post
Share on other sites
The roll of the camera still persists. I reduced the number of conversions by creating the deltaQuaternion directly from the yaw, pitch and roll angles instead of converting it from a matrix like you said, using the following member function.

void FromYawPitchRoll(float yaw, float pitch, float roll)
{
yaw = DegreesToRadians(yaw);
pitch = DegreesToRadians(pitch);
roll = DegreesToRadians(roll);

Quaternion<T> y, p, r;
y = Quaternion<T>(0, sin(-yaw / 2), 0, cos(-yaw / 2));
p = Quaternion<T>(sin(-pitch / 2), 0, 0, cos(-pitch / 2));
r = Quaternion<T>(0, 0, sin(-roll / 2), cos(-roll / 2));

// Could the order of multiplication matter here???
*this = (y * p * r);
Normalise();
}



After that, I used Thrump's method of orthoganalising the three basis vectors, just in case, like so:

// Gets the vectors from the matrix created from totalRotation
right = vector3f(mat(0, 0), mat(1, 0), mat(2, 0));
up = vector3f(mat(0, 1), mat(1, 1), mat(2, 1));
// Orthoganalises them
right.Normalise();
up.Normalise();
forward = -vector3f::Cross(right, up);
forward.Normalise();
right = vector3f::Cross(up, forward);




Rotating the pitch of the camera keeps the dot products as (0, 0, 0), but any change the yaw or roll knocks it all off balance and the three vectorsaren't orthogonal anymore.

Could it be that the operations I'm doing is biased to a certain axis? Since changing the pitch doesn't 'break' the orthoganal vectors.

[Edited by - -Datriot- on April 22, 2009 9:33:05 AM]

Share this post


Link to post
Share on other sites
Quote:
The roll of the camera still persists.
Can you clarify what you mean by 'the roll of the camera persists'? Earlier you mentioned that the orientation is drifting out of orthogonality, but 'the roll still persists' seems like it probably means something different.
Quote:
        void FromYawPitchRoll(float yaw, float pitch, float roll)
{
yaw = DegreesToRadians(yaw);
pitch = DegreesToRadians(pitch);
roll = DegreesToRadians(roll);

Quaternion<T> y, p, r;
y = Quaternion<T>(0, sin(-yaw / 2), 0, cos(-yaw / 2));
p = Quaternion<T>(sin(-pitch / 2), 0, 0, cos(-pitch / 2));
r = Quaternion<T>(0, 0, sin(-roll / 2), cos(-roll / 2));

// Could the order of multiplication matter here???
*this = (y * p * r);
Normalise();
}
Couple of tips here. It would be a good idea to create a 'from axis-angle' function for creating your rotation functions. Doing it manually like this is laborious and highly error-prone. (This applies in general, as well, for any oft-repeated bit of code.)

Assuming the deltas are small, the order of multiplication does not matter (much) here. In any case, it's probably not the cause of the problem. Finally, there's no need to call Normalize() at the end of the function (the quaternion will already be sufficiently close to unit length).
Quote:
After that, I used Thrump's method of orthoganalising the three basis vectors, just in case, like so:
This is completely unnecessary and out of place in the given context. The only 're-orthogonalization' you should need to do in the given context is to call Normalize() on your orientation quaternion at the end of each update. That's it - that will be sufficient to prevent any noticeable numerical error from accumulating.
Quote:
Rotating the pitch of the camera keeps the dot products as (0, 0, 0), but any change the yaw or roll knocks it all off balance and the three vectorsaren't orthogonal anymore.
Are you actually seeing incorrect behavior in your simulation, or is you observation based only on the dot product values that you're seeing? If the latter, can you post an example of a set of values that looks incorrect to you (i.e. something other than zeros)?

Are the values small? Are they displayed in scientific notation? It may be that you're simply misinterpreting numerical error as non-orthogonality.

Share this post


Link to post
Share on other sites
Quote:
Can you clarify what you mean by 'the roll of the camera persists'? Earlier you mentioned that the orientation is drifting out of orthogonality, but 'the roll still persists' seems like it probably means something different.


Sorry about that, I should have said 'the problem with the roll still persists'.

Quote:
Are you actually seeing incorrect behavior in your simulation, or is you observation based only on the dot product values that you're seeing?


It is very noticable on the simulation itself, that's how I first become aware of the problem. I just printed the dot products to make sure that the problem was with the camera. The dot products printed off usually range from -10 to 10.

I'm sure I'm doing everything right, I'll check all the relevant functions to make sure there aren't any errors in the calculations.

Share this post


Link to post
Share on other sites
Quote:
The dot products printed off usually range from -10 to 10.
Well, that's not right :)

If you're seeing values of that magnitude, then the problem is not drift or accumulated numerical error but rather an error in your code somewhere, in which case double-checking the various support functions for correctness is indeed a good idea.

Share this post


Link to post
Share on other sites
It's very likely that the order of rotations is causing it to have a different out come than you expect. Is there by chance a specific input you can give the system that introduces roll really fast? Like say, look up 90deg, look left 90deg, look down 90deg and you come out with 90deg roll?

You'll want to make sure that when your rotations about different axes combine that they add up to the desired behavior.

I recommend also that you create a quaternion constructor which uses an axis-angle pair. If you need 3 independent rotations, then you should be creating 3 quaternions each with a specific axis and combining these in the order required for the desired affect.

Q change = rollQ * yawQ * pitchQ; //or what ever order you'd like..

In my example nothing was wrong with the math, it just was not very user friendly, all because the wrong rotation axis was being used.

Share this post


Link to post
Share on other sites
Quote:
It's very likely that the order of rotations is causing it to have a different out come than you expect. Is there by chance a specific input you can give the system that introduces roll really fast? Like say, look up 90deg, look left 90deg, look down 90deg and you come out with 90deg roll?
Since the OP is applying the rotations incrementally (or at least appears to be), the order or rotations *within* the FromYawPitchRoll() function is, for all practical purposes, irrelevant.

The example you give above is correct, however, and highlights an inherent aspect of 6DOF motion: accumulated pitch and yaw can result in perceived roll with respect to a given reference plane (e.g. a 'ground' plane).

@The OP: I don't know that you've stated in your posts exactly how the simulation is misbehaving, but if the only problem is that the orientation picks up some roll relative to its initial orientation as you pitch and yaw, then that's perfectly normal. (If the dot product values that you mentioned earlier are accurate, however, then there are probably other problems as well.)

Share this post


Link to post
Share on other sites
Quote:
Is there by chance a specific input you can give the system that introduces roll really fast? Like say, look up 90deg, look left 90deg, look down 90deg and you come out with 90deg roll?


Yes, it does come out with a 90deg roll, after which the dot products of (right, up), (right, forward) and (up, forward) are 4.04, 6.80 and 1.43 respectively.

Quote:

I don't know that you've stated in your posts exactly how the simulation is misbehaving, but if the only problem is that the orientation picks up some roll relative to its initial orientation as you pitch and yaw, then that's perfectly normal. (If the dot product values that you mentioned earlier are accurate, however, then there are probably other problems as well.)


Just to be sure, I double-checked all the relevent methods (yaw/pitch/roll to quaternion, quaternion multiplication, quatertion to matrix, quaternion normalisation and the vector dot product) and I believe they're all in order, I even checked them with a calculater, so I'm confident the dot product values generated are correct.

For some reason, the camera now seems to 'flicker' and show the original orientation (when total yaw, pitch and roll is 0) whenever I rotate it. I have no idea where it's come from since I haven't changed any of the code. I'm at a complete loss as to why all these errors are occuring and I'm sure what I should do next.

Share this post


Link to post
Share on other sites
The dot product of two unit length vectors will be in the range of [-1, 1].. So you definitely need to check your math.

As for the flickering, that's another area you're just going to check out.

As for the 90deg roll. Like we mentioned, that's perfectly expected behavior, it's doing exactly what you're telling it to.

The problem is that it's not what you want it to do :P You're yaw'ing about the view up axis. When you pitch the view back 90degrees, the view up axis is now pointing in the original "back" direction, rather than up. So when you "yaw" here, you're actually rolling. since the roll and yaw axes are now aligned.

If you're trying to make a fly camera, it's very typical to always Yaw about the world Up axis, rather than the view up axis. This will eliminate any chance of roll being introduced.

Quat pitch(viewRight, pitchAngle), yaw(worldUp, yawAngle);
Quat change = pitch * yaw;
final *= change;
final.Normalize();

Matrix output(final);

Share this post


Link to post
Share on other sites
I would post some code - basically everything that's relevant to the problem (within reason), which might include the various support functions, the code where you compute and display the dot products, and the code where you set up the view matrix and submit it to the graphics API. The dot product values you're seeing indicate clearly that something is way off, and I wouldn't rule out the possibility that there's an error in one of your support functions somewhere that perhaps you've overlooked.

Also, perhaps you could clarify exactly what kind of motion you're trying to implement. Again, accumulated perceived roll is perfectly normal when applying incremental local 'yaw' and 'pitch' rotations; if this isn't what you want, maybe it's not really 6DOF motion that you're after. (Or maybe it's autoleveling that you want?)

Share this post


Link to post
Share on other sites
Let's say camera is initially looking at (1, 0, 0), (0, 1, 0) and (0, 0, 1). Directly in front of it there is a arrow pointing upwards (0, 1, 0). I yaw 90 degrees upwards, pitch 90 degrees to the left. After that, I roll 90 degrees. I then yaw AND pitch 90 degrees to the right and down, making the camera look at the original direction. This means that the arrow must be pointing to the left BECAUSE of the 90 degree roll. If I did the same, but never rolled, when I return to the original direction the arrow should still be pointing upwards.

That's the type of behaviour I'm looking for. Even if 6DOF isn't it, something is definitely wrong because of the dot product values, so here is all the relevant code. Hopefully you can find something I may have overlooked.

Camera:

// NOTE: ACamera just initialises right, up and forward vectors. It does nothing else relevent to the problem
class SixDOFCamera : public ACamera<float>
{


private:

float yaw, pitch, roll;


public:

SixDOFCamera() : ACamera<float>(),
yaw(0.0f), pitch(0.0f), roll(0.0f) // Gives yaw, pitch and roll the default values (no rotation)
{
}
SixDOFCamera(const vector3f& initialPosition, const vector3f& initialRight, const vector3f& initialUp,
const vector3f& initialForward) :
ACamera<float>(initialPosition, initialRight, initialUp, initialForward),
yaw(0.0f), pitch(0.0f), roll(0.0f)
{
}

virtual ~SixDOFCamera() {}


void RotateYawPitchRoll(float yawDegrees, float pitchDegrees, float rollDegrees)
{
quaternionf deltaRotation; // Represents change in orientation from last frame

deltaRotation.FromYawPitchRoll(yawDegrees, pitchDegrees, rollDegrees)

localRotation = deltaRotation * localRotation;
localRotation.Normalise(); // Just in case

yaw += yawDegrees;
pitch += pitchDegrees;
roll += rollDegrees;
}

void Update()
{
matrixf mat = localRotation.ToRotationMatrix();
right = vector3f(mat(0, 0), mat(1, 0), mat(2, 0));
up = vector3f(mat(0, 1), mat(1, 1), mat(2, 1));
forward = -vector3f(mat(0, 2), mat(1, 2), mat(2, 2));

std::cout << vector3f::Dot(right, up) << " " << vector3f::Dot(right, forward)
<< " " << vector3f::Dot(up, forward) << std::endl;
}

const float& GetYaw() const { return yaw; }
const float& GetPitch() const { return pitch; }
const float& GetRoll() const { return roll; }

};



Quaternion:

template<typename T>
class Quaternion
{


public:

union
{
struct
{
T x, y, z, w;
};

T values[4];
};



/* Constructors. */
Quaternion<T>() : x(0), y(0), z(0), w(1) {}

Quaternion<T>(const T& ix, const T& iy, const T& iz, const T& iw)
: x(ix), y(iy), z(iz), w(iw) {}

void ToIdentity()
{
x = y = z = 0;
w = 1;
}

/* Returns the length/mangitude of the quaternion. */
inline T Length() const
{
return sqrt(w * w + x * x + y * y + z * z);
}

inline Quaternion<T>& Normalise()
{
// Used to prevent any divide-by-zero problems
T oneOverMag = 1.0f / Length();
// Normalizes each component, giving the quaternion the length of 1
x *= oneOverMag;
y *= oneOverMag;
z *= oneOverMag;
w *= oneOverMag;

return *this;
}



/* Converts the quaternion into a 4x4 rotation matrix. */
inline Matrix<T> ToRotationMatrix() const
{
if (!IsUnit()) return Matrix<T>::Identity(4);

T array[16];

const T xx = (x * x), yy = (y * y), zz = (z * z), ww = (w * w);
const T xy = (x * y), xz = (x * z);
const T yz = (y * z);
const T wx = (w * x), wy = (w * y), wz = (w * z);

// First column/vector
array[0] = ww + xx - yy - zz;
array[4] = 2.0 * (xy + wz);
array[8] = 2.0 * (xz - wy);
array[12] = 0.0;
// Second column/vector
array[1] = 2.0 * (xy - wz);
array[5] = ww - xx + yy - zz;
array[9] = 2.0 * (yz + wx);
array[13] = 0.0;
// Third column/vector
array[2] = 2.0 * (xz + wy);
array[6] = 2.0 * (yz - wx);
array[10] = ww - xx - yy + zz;
array[14] = 0.0;
// Fourth column/vector. Same regardless of unit quaternion
array[3] = 0.0;
array[7] = 0.0;
array[11] = 0.0;
array[15] = 1.0;

return Matrix<T>(4, 4, array);
}


inline void FromYawPitchRoll(float yaw, float pitch, float roll)
{
// Converts given degrees to radians
yaw = DegreesToRadians(yaw);
pitch = DegreesToRadians(pitch);
roll = DegreesToRadians(roll);

Quaternion<T> y, p, r;
y = Quaternion<T>(0, sin(-yaw / 2), 0, cos(-yaw / 2));
p = Quaternion<T>(sin(-pitch / 2), 0, 0, cos(-pitch / 2));
r = Quaternion<T>(0, 0, sin(-roll / 2), cos(-roll / 2));
// Concatenates the three rotations into one quaternion
*this = (y * p * r);

// NOTE: Will use SetAxisAngle() later, for now, this appears to work
}



/* Quaternion multiplication methods. */
inline Quaternion<T> operator*(const Quaternion<T>& b)
{
Quaternion<T>& a = *this;
Quaternion<T> c;

c.x = (a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y);
c.y = (a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z);
c.z = (a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x);
c.w = (a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);

return c;
}
inline Quaternion<T>& operator*=(const Quaternion<T>& quat)
{
(*this) = (*this) * quat;
return *this;
}


};




Vector Length, Normalisation and Dot Product:

inline T Length() const { return sqrt(x * x + y * y + z * z); }

inline Vector3<T>& Normalise()
{
// Used to prevent divide-by-zero
T oneOverMag = 1.0f / Length();
x *= oneOverMag;
y *= oneOverMag;
z *= oneOverMag;

return *this;
}

static inline T Dot(const Vector3<T>& a1, const Vector3<T> &a2)
{ return (a1.x * a2.x + a1.y * a2.y + a1.z * a2.z); }



Matrix generation, the matrix generated here is multiplied by the identity matrix:

// The camera's position, target and up values are passed to this method
void RenderDevice::SetViewMatrixAsLookAt(const vector3f& position,
const vector3f& target, const vector3f& up)
{
viewMatrix = matrixf::Identity(4); // Makes view matrix a 4x4 identity matrix
vector3f vec1, vec2, vec3;

vec1 = (position - target); // Z
vec1.Normalise();
vec2 = vector3f::Cross(up, vec1); // X
vec2.Normalise();
vec3 = vector3f::Cross(vec1, vec2); // Y

viewMatrix.SetElement(0, 0, vec2.x); // Column 1
viewMatrix.SetElement(1, 0, vec3.x);
viewMatrix.SetElement(2, 0, vec1.x);

viewMatrix.SetElement(0, 1, vec2.y); // Column 2
viewMatrix.SetElement(1, 1, vec3.y);
viewMatrix.SetElement(2, 1, vec1.y);

viewMatrix.SetElement(0, 2, vec2.z); // Column 3
viewMatrix.SetElement(1, 2, vec3.z);
viewMatrix.SetElement(2, 2, vec1.z);

// Now for the translation part of the matrix
viewMatrix.SetElement(3, 0, -vector3f::Dot(vec2, position)); // Column 4
viewMatrix.SetElement(3, 1, -vector3f::Dot(vec3, position));
viewMatrix.SetElement(3, 2, vector3f::Dot(vec1, position));
}



Performed every frame, before rendering scene:

// Converts matrix to array
float a[16];
theGeneratedMatrix.ToArray(a);
// Loads it
glLoadMatrix(a);

Share this post


Link to post
Share on other sites
Code sniplet overload...

Did you see my example? was it not very clear?

Also it seems as though your missusing the terms yaw and pitch and roll..

You can't Yaw up, You can't pitch Left :(
http://en.wikipedia.org/wiki/Tait-Bryan_angles

Also, do you _need_ Roll? Are you sure you want Roll? If you want to allow roll then you have to make another fudge. As you can see in my example I made 1 fudge. I used the worlds Up axis always rather than the view up Axis, this make controls a bit more sane for a human. It how ever completely prevents roll.

If you dont want any roll, then why even include it in the code? And my example should work fine.

If however you do want roll, then you have to understand that what you're seeing is correct. That is really what it's like to maneuver in 6 DOF.. you're just used to something else.

Click here to see 6DOF control. It's a 3d model viewer and you may need to download a widget. When the sketch comes up click play and then pause, then click the rotate button. This will enable camera orbit. As you can see, if you're not extremely careful, roll is introduced immediately.

It's just a side effect of which axes you choose to rotate around. I dont think you're paying enough attention to the axis you're rotating around because you're using euler angles rather than explicitly concatenating rotations using quaternions.

What was the up vector, may not be the up vector after you make another rotation. FromYawPitchRoll sounds like a bad start.

You dont need a constructor like that in your quaternion, you only need one which takes an axis angle pair. And you'll create multiple quaternions, one for each explicit rotation you'd like to enable. And then you'll combine those rotations using quaternion multiplication. The axis you'll choose are not known inside the quaternion, and thus must be found and utilized in your camera object code. The quaternion itself is not aware of enough information to create the proper rotation. You must use multilple quaternions. I wonder how many different ways i can say the same thing. It'd be interesting to know how repetetive i am.

Like this:

Quaternion pitch(AxisAngle(viewRight, pitchChange));
Quaternion yaw(AxisAngle(worldUp, yawChange));
Quaternion change = pitch * yaw;

//final is used to store current camera orienation
final *= change;
final.Normalize();

//output matrix is used to generate view transform
Matrix output(final);


[Edited by - bzroom on April 24, 2009 12:02:13 PM]

Share this post


Link to post
Share on other sites
Quote:
Original post by -Datriot-
Let's say camera is initially looking at (1, 0, 0), (0, 1, 0) and (0, 0, 1). Directly in front of it there is a arrow pointing upwards (0, 1, 0). I yaw 90 degrees upwards, pitch 90 degrees to the left. After that, I roll 90 degrees. I then yaw AND pitch 90 degrees to the right and down, making the camera look at the original direction. This means that the arrow must be pointing to the left BECAUSE of the 90 degree roll. If I did the same, but never rolled, when I return to the original direction the arrow should still be pointing upwards.
I couldn't quite follow that example, but I agree with bzroom that you seem to be misusing the terms 'yaw' and 'pitch'.
Quote:
That's the type of behaviour I'm looking for. Even if 6DOF isn't it, something is definitely wrong because of the dot product values, so here is all the relevant code. Hopefully you can find something I may have overlooked.
Maybe you could post the example again, except with pitch and yaw referenced correctly, and with the direction of the roll specified. I *tried* to step through your example, but I couldn't get it to work :| In any case, I'm still not quite clear on what type of motion you're after (is there a well-known game that uses the type of motion you want that you could refer us to?).

I made a pass through your code and added a few comments:

class SixDOFCamera : public ACamera<float>
{


private:

// It doesn't look like these are ever used, and as it is now they
// really don't accomplish anything. I would remove them.
float yaw, pitch, roll;


public:

SixDOFCamera() : ACamera<float>(), yaw(0.0f), pitch(0.0f), roll(0.0f)
{
}

// Tracking the direction vectors separately seems completely unnecessary
// to me. I would store a quaternion or rotation matrix and a position
// vector, and no more. You can always get the direction vectors from
// the matrix or quaternion when you need them.
SixDOFCamera(const vector3f& initialPosition, const vector3f& initialRight, const vector3f& initialUp,
const vector3f& initialForward) :
ACamera<float>(initialPosition, initialRight, initialUp, initialForward),
yaw(0.0f), pitch(0.0f), roll(0.0f)
{
}

// If ACamera already has a virtual destructor, I don't think you need
// to implement the destructor here if it doesn't do anything.
virtual ~SixDOFCamera() {}


void RotateYawPitchRoll(float yawDegrees, float pitchDegrees, float rollDegrees)
{
quaternionf deltaRotation;

deltaRotation.FromYawPitchRoll(yawDegrees, pitchDegrees, rollDegrees)

// Assuming standard quaternion multiplication order, I think this
// should actually be 'localRotation * deltaRotation'.
localRotation = deltaRotation * localRotation;

// Just to be clear, this isn't 'just in case': without this
// normalization, it's practically inevitable that your orientation
// quaternion will drift over time and become non-unit-length.
// The implications of this drift are a bit different with quaternions
// than with matrices, but it's still to be avoided.
localRotation.Normalise(); // Just in case

// Again, this doesn't seem to accomplish anything. Also, since you're
// using incremental rotations, the accumulated values aren't very
// meaningful.
yaw += yawDegrees;
pitch += pitchDegrees;
roll += rollDegrees;
}

void Update()
{
matrixf mat = localRotation.ToRotationMatrix();
right = vector3f(mat(0, 0), mat(1, 0), mat(2, 0));
up = vector3f(mat(0, 1), mat(1, 1), mat(2, 1));
forward = -vector3f(mat(0, 2), mat(1, 2), mat(2, 2));

// These three values should all be close to zero. If they're not,
// there's something very wrong somewhere (perhaps in your
// ToRotationMatrix() function?).
std::cout << vector3f::Dot(right, up) << " " << vector3f::Dot(right, forward)
<< " " << vector3f::Dot(up, forward) << std::endl;
}

const float& GetYaw() const { return yaw; }
const float& GetPitch() const { return pitch; }
const float& GetRoll() const { return roll; }

};

template<typename T>
class Quaternion
{
public:

union
{
struct
{
T x, y, z, w;
};

T values[4];
};



/* Constructors. */
Quaternion<T>() : x(0), y(0), z(0), w(1) {}

Quaternion<T>(const T& ix, const T& iy, const T& iz, const T& iw)
: x(ix), y(iy), z(iz), w(iw) {}

void ToIdentity()
{
x = y = z = 0;
w = 1;
}

/* Returns the length/mangitude of the quaternion. */
inline T Length() const
{
return sqrt(w * w + x * x + y * y + z * z);
}

inline Quaternion<T>& Normalise()
{
// Your comment here is a bit confusing - inverting and multiplying
// does nothing to prevent divide-by-zero errors. It's basically
// just a minor optimization based on the assumption that a divide
// will be more costly than a multiplication.

// Used to prevent any divide-by-zero problems
T oneOverMag = 1.0f / Length();
x *= oneOverMag;
y *= oneOverMag;
z *= oneOverMag;
w *= oneOverMag;

return *this;
}



/* Converts the quaternion into a 4x4 rotation matrix. */
inline Matrix<T> ToRotationMatrix() const
{
if (!IsUnit()) return Matrix<T>::Identity(4);

T array[16];

// I didn't proof this, but I do have one question. The following
// code suggests that your matrices are laid out so that basis
// vector elements are *not* contiguous, which is different than
// how both OpenGL and Direct3D store their matrices. Are you sure
// the data is being mapped to the Matrix<T> storage correctly?
// And why use a temporary 1-d float array anyway? Why not just
// write the result directly to a Matrix<T>?

const T xx = (x * x), yy = (y * y), zz = (z * z), ww = (w * w);
const T xy = (x * y), xz = (x * z);
const T yz = (y * z);
const T wx = (w * x), wy = (w * y), wz = (w * z);

// First column/vector
array[0] = ww + xx - yy - zz;
array[4] = 2.0 * (xy + wz);
array[8] = 2.0 * (xz - wy);
array[12] = 0.0;
// Second column/vector
array[1] = 2.0 * (xy - wz);
array[5] = ww - xx + yy - zz;
array[9] = 2.0 * (yz + wx);
array[13] = 0.0;
// Third column/vector
array[2] = 2.0 * (xz + wy);
array[6] = 2.0 * (yz - wx);
array[10] = ww - xx - yy + zz;
array[14] = 0.0;
// Fourth column/vector. Same regardless of unit quaternion
array[3] = 0.0;
array[7] = 0.0;
array[11] = 0.0;
array[15] = 1.0;

return Matrix<T>(4, 4, array);
}


inline void FromYawPitchRoll(float yaw, float pitch, float roll)
{
// Converts given degrees to radians
yaw = DegreesToRadians(yaw);
pitch = DegreesToRadians(pitch);
roll = DegreesToRadians(roll);

Quaternion<T> y, p, r;

// You shouldn't have to negate the angles here. Also (I think
// I mentioned this before), write a function to build a quaternion
// from an axis-angle pair, rather than doing it manually as you're
// doing here.
y = Quaternion<T>(0, sin(-yaw / 2), 0, cos(-yaw / 2));
p = Quaternion<T>(sin(-pitch / 2), 0, 0, cos(-pitch / 2));
r = Quaternion<T>(0, 0, sin(-roll / 2), cos(-roll / 2));
*this = (y * p * r);
}

Share this post


Link to post
Share on other sites
Sign in to follow this  

  • Advertisement