I use quaternions for rotations in OpenGL engine.Currently , in order to create rotation matrix for x ,y and z rotations I create a quaternion per axis rotation.Then I multiply these to get the final quaternion:
void RotateTo3(const float xr ,const float yr ,const float zr){
quat qRotX=angleAxis(xr, X_AXIS);
quat qRotY=angleAxis(yr, Y_AXIS);
quat qRotZ=angleAxis(zr, Z_AXIS);
quat resQuat=normalize(qRotX * qRotY * qRotZ);
resQuat=normalize(resQuat);
_rotMatrix= mat4_cast(resQuat);
}
Now it's all good but I want to create only one quaternion from all 3 axis angles and skip the final multiplication.One of the quat constructors has params for euler angles vector which goes like this:
quat resQuat(vec3(yr,xr,zr))
So if I try this the final rotation is wrong.(Also tried quat(vec3(xr,yr,zr)) ) .Isn't there a way in GLM to fill the final quaternion from all 3 axis in one instance ?
Now , one more thing:
As Nicol Bolas suggested , I could use glm::eulerAngleYXZ() to fill a rotation matrix right away as by his opinion it is pointless to do the intermediate quaternion step.. But what I found is that the function doesn't work properly , at least for me .For example :
This :
mat4 ex= eulerAngleX(radians(xr));
mat4 ey= eulerAngleY(radians(yr));
mat4 ez= eulerAngleZ(radians(zr));
rotMatrix= ex * ey * ez;
Doesn't return the same as this :
rotMatrix= eulerAngleYXZ(radians(yr),radians(xr),radians(zr));
And from my comparisons to the correct rotation state ,the first way gives the correct rotations while the second wrong .
Contrary to popular belief, quaternions are not magical "solve the Gimbal lock" devices, such that any uses of quaternions make Euler angles somehow not Euler angles.
Your RotateTo3 function takes 3 Euler angles and converts them into a rotation matrix. It doesn't matter how you perform this process; whether you use 3 matrices, 3 quaternions or glm::eulerAngleYXZ. The result will still be a matrix composed from 3 axial rotations. It will have all of the properties and failings of Euler angles. Because it is Euler angles.
Using quaternions as intermediaries here is pointless. It gains you nothing; you may as well just use matrices built from successive glm::rotate calls.
If you want to do orientation without Gimbal lock or the other Euler angle problems, then you need to stop representing your orientation as Euler angles.
In answer to the question you actually asked, you can use glm::eulerAngleYXZ to compute
Do you mean something like this:
quat formQuaternion(double x, double y, double z, double angle){
quat out;
//x, y, and z form a normalized vector which is now the axis of rotation.
out.w = cosf( fAngle/2)
out.x = x * sinf( fAngle/2 )
out.y = y * sinf( fAngle/2 )
out.z = z * sinf( fAngle/2 )
return out;
}
Sorry I don't actually know the quat class you are using, but it should still have some way to set the 4 dimensions. Source: Quaternion tutorial
eulerAngleYXZ gives one possible set of euler angles which, if recombined in the order indicated by the api name, will yield the same orientation as the given quaternion.
It's not a wrong result - it's one of several correct results.
Use a quaternion to store your orientation internally - to rotate it, multiply your orientation quat by another quat representing the amount to rotate by, which can be built from angle/axis to achieve what you want.
Related
To control a robotic arm, I have a controller with 6 dimensions (x, y, z position, and roll, pitch, yaw rotation). I am using a position (x, y, z) and quaternion (x, y, z, w) to represent the desired position and orientation of the robot's gripper.
I am using some libraries that people might not be familiar with (namely, geometry_msgs from ROS), so here is what I'm doing in pseudocode:
while(true):
new_position = last_position + (joy.x, joy.y, joy.z)
new_quaternion = last_quaternion * quaternionFromRPY(joy.roll, joy.pitch, joy.yaw)
// (Compute inverse kinematics using position and orientation, then send to robot arm)
last_position = new_position
last_quaternion = new_quaternion
delay(dt)
I am able to set the x, y, and z of the position part of the pose just fine. Technically, I am also able to set the quaternion just fine as well, but my problem is that controlling the quaternion is extremely non-intuitive, since rotation is not commutative. i.e., if I rotate 180 degrees about the x-axis, then the control of the rotation about the z-axis gets inverted.
Is there any way that I can control the rotation from a sort of "global reference frame," if that makes any sense? I would like it so that if I twist the joystick clockwise about z, the pose will rotate accordingly, instead of "sometimes rotating clockwise, sometimes rotating counterclockwise."
Isn't this just as simple as exchanging the order of the factors in the quaternion product?
A unit quaternion q transforms a vector v in local coordinates to the rotated vector q*v*q' in global coordinates. A modified quaternion a*q*b (a, b also unit quaternions) transforms v as
a*(q*(b*v*b')*q')*a',
where the b part can be interpreted as rotation in local coordinates and the a part as rotation in global coordinates.
So to apply a rotation in global coordinates only, set b=1, i.e., leave it out, and put the desired rotation in the a factor.
Solved
I'm making a 3D portal system in my engine (like Portal game). Each of the portals has its own orientation saved in a quaternion. To render the virtual scene in one of the portals I need to calculate the difference between the two quaternions, and the result use to rotate the virtual scene.
When creating the first portal on the left wall, and second one on the right wall, the rotation from one to another will take place in only one axis, but for example when the first portal will be created on the floor, and the second one on the right wall, the rotation from one to another could be in two axis, and that's the problem, because the rotation goes wrong.
I think the problem exists because the orientation for example X axis and Z axis are stored together in one quaternion and I need it separately to manualy multiply X * Z (or Z * X), but how to do it with only one quaternion, (the difference quaternion)? Or is there other way to correct rotate the scene?
EDIT:
Here on this picture are two portals P1 and P2, the arrows show how are they rotated. As I am looking into P1 I will see what sees P2. To find the rotation which I need to rotate the main scene to be like the virtual scene in this picture I'm doing following:
Getting difference from quaternion P2 to quaternion P1
Rotating result by 180 degrees in Y axis (portal's UP)
Using the result to rotate the virtual scene
This method above works only when the difference takes place in only one axis. When one portal will be on the floor, or on te ceiling, this will not work because the difference quaternion is build in more than one axis. As suggested I tried to multiply P1's quaternion to P2's quaternion, and inversely but this isn't working.
EDIT 2:
To find the difference from P2 to P1 I'm doing following:
Quat q1 = P1->getOrientation();
Quat q2 = P2->getOrientation();
Quat diff = Quat::diff(q2, q1); // q2 * diff = q1 //
Here's the Quat::diff function:
GE::Quat GE::Quat::diff(const Quat &a, const Quat &b)
{
Quat inv = a;
inv.inverse();
return inv * b;
}
Inverse:
void GE::Quat::inverse()
{
Quat q = (*this);
q.conjugate();
(*this) = q / Quat::dot((*this), (*this));
}
Conjugate:
void GE::Quat::conjugate()
{
Quat q;
q.x = -this->x;
q.y = -this->y;
q.z = -this->z;
q.w = this->w;
(*this) = q;
}
Dot product:
float GE::Quat::dot(const Quat &q1, const Quat &q2)
{
return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
}
Operator*:
const GE::Quat GE::Quat::operator* ( const Quat &q) const
{
Quat qu;
qu.x = this->w*q.x + this->x*q.w + this->y*q.z - this->z*q.y;
qu.y = this->w*q.y + this->y*q.w + this->z*q.x - this->x*q.z;
qu.z = this->w*q.z + this->z*q.w + this->x*q.y - this->y*q.x;
qu.w = this->w*q.w - this->x*q.x - this->y*q.y - this->z*q.z;
return qu;
}
Operator/:
const GE::Quat GE::Quat::operator/ (float s) const
{
Quat q = (*this);
return Quat(q.x / s, q.y / s, q.z / s, q.w / s);
}
All this stuff works, because I have tested it with GLM library
If you want to find a quaternion diff such that diff * q1 == q2, then you need to use the multiplicative inverse:
diff * q1 = q2 ---> diff = q2 * inverse(q1)
where: inverse(q1) = conjugate(q1) / abs(q1)
and: conjugate( quaternion(re, i, j, k) ) = quaternion(re, -i, -j, -k)
If your quaternions are rotation quaternions, they should all be unit quaternions. This makes finding the inverse easy: since abs(q1) = 1, your inverse(q1) = conjugate(q1) can be found by just negating the i, j, and k components.
However, for the kind of scene-based geometric configuration you describe, you probably don't actually want to do the above, because you also need to compute the translation correctly.
The most straightforward way to do everything correctly is to convert your quaternions into 4x4 rotation matrices, and multiply them in the appropriate order with 4x4 translation matrices, as described in most introductory computer graphics texts.
It is certainly possible to compose Euclidean transformations by hand, keeping your rotations in quaternion form while applying the quaternions incrementally to a separate translation vector. However, this method tends to be technically obscure and prone to coding error: there are good reasons why the 4x4 matrix form is conventional, and one of the big ones is that it appears to be easier to get it right that way.
I solved my problem. As it turned out I don't need any difference between two rotations. Just multiply one rotation by rotation in 180 degrees, and then multiply by inverse of second rotation that way (using matrices):
Matrix m1 = p1->getOrientation().toMatrix();
Matrix m2 = p2->getOrientation().toMatrix();
Matrix model = m1 * Matrix::rotation(180, Vector3(0,1,0)) * Matrix::inverse(m2);
and translation calculating this way:
Vector3 position = -p2->getPosition();
position = model * position + p1->getPosition();
model = Matrix::translation(position) * model;
No, you have to multiply two quaternions together to get the final quaternion you desire.
Let's say that your first rotation is q1 and the second is q2. You want to apply them in that order.
The resulting quaternion will be q2 * q1, which will represent your composite rotation (recall that quaternions use left-hand multiplication, so q2 is being applied to q1 by multiplying from the left)
Reference
For a brief tutorial on computing a single quaternion, refer to my previous stack overflow answer
Edit:
To clarify, you'd face a similar problem with rotation matrices and Euler angles. You define your transformations about X, Y, and Z, and then multiply them together to get the resulting transformation matrix (wiki). You have the same issue here. Rotation matrices and Quaternions are equivalent in most ways for representing rotations. Quaternions are preferred mostly because they're a bit easier to represent (and easier for addressing gimbal lock)
Quaternions work the following way: the local frame of reference is represented as the imaginary quaternion directions i,j,k. For instance, for an observer standing in the portal door 1 and looking in the direction of the arrow, direction i may represent the direction of the arrow, j is up and k=ij points to the right of the observer. In global coordinates represented by the quaternion q1, the axes in 3D coordinates are
q1*(i,j,k)*q1^-1=q1*(i,j,k)*q1',
where q' is the conjugate, and for unit quaternions, the conjugate is the inverse.
Now the task is to find a unit quaternion q so that directions q*(i,j,k)*q' in local frame 1 expressed in global coordinates coincide with the rotated directions of frame 2 in global coordinates. From the sketch that means forwards becomes backwards and left becomes right, that is
q1*q*(i,j,k)*q'*q1'=q2*(-i,j,-k)*q2'
=q2*j*(i,j,k)*j'*q2'
which is readily achieved by equating
q1*q=q2*j or q=q1'*q2*j.
But details may be different, mainly that another axis may represent the direction "up" instead of j.
If the global system of the sketch is from the bottom, so that global-i points forward in the vertical direction, global-j up and global-k to the right, then local1-(i,j,k) is global-(-i,j,-k), giving
q1=j.
local2-(i,j,k) is global-(-k,j,i) which can be realized by
q2=sqrt(0.5)*(1+j),
since
(1+j)*i*(1-j)=i*(1-j)^2=-2*i*j=-2*k and
(1+j)*k*(1-j)=(1+j)^2*k= 2*j*k= 2*i
Comparing this to the actual values in your implementation will indicate how the assignment of axes and quaternion directions has to be changed.
Check https://www.emis.de/proceedings/Varna/vol1/GEOM09.pdf
Imagine to get dQ from Q1 to Q2, I'll explain why dQ = Q1*·Q2, instead of Q2·Q1*
This rotates the frame, instead of an object. For any vector v in R3, the rotation action of operator
L(v) = Q*·v·Q
It's not Q·v·Q*, which is object rotation action.
If you rotates Q1 and then Q1* and then Q2, you can write
(Q1·Q1*·Q2)*·v·(Q1·Q1*·Q2) = (Q1*·Q2)*·Q1*·v·Q1·(Q1*·Q2) = dQ*·Q1*·v·Q1·dQ
So dQ = Q1*·Q2
I have a unit vector in 3D space whose direction I wish to perturb by some angle within the range 0 to theta, with the position of the vector remaining the same. What is a way I can accomplish this?
Thanks.
EDIT: After thinking about the way I posed the question, it seems to be a bit too general. I'll attempt to make it more specific: Assume that the vector originates from the surface of an object (i.e. sphere, circle, box, line, cylinder, cone). If there are different methods to finding the new direction for each of those objects, then providing help for the sphere one is fine.
EDIT 2: I was going to type this in a comment but it was too much.
So I have orig_vector, which I wish to perturb in some direction between 0 and theta. The theta can be thought of as forming a cone around my vector (with theta being the angle between the center and one side of the cone) and I wish to generate a new vector within that cone. I can generate a point lying on the plane that is tangent to my vector and thus creating a unit vector in the direction of the point, call it rand_vector. At this time, I orig_vector and trand_vector are two unit vectors perpendicular to each other.
I generate my first angle, angle1 between 0 and 2pi and I rotate rand_vector around orig_vector by angle1, forming rand_vector2. I looked up a resource online and it said that the second angle, angle2 should be between 0 and sin(theta) (where theta is the original "cone" angle). Then I rotate rand_vector2 by acos(angle2) around the vector defined by the cross product between rand_vector2 and orig_vector.
When I do this, I don't obtain the desired results. That is, when theta=0, I still get perturbed vectors, and I expect to get orig_vector. If anyone can explain the reason for the angles and why they are the way they are, I would greatly appreciate it.
EDIT 3: This is the final edit, I promise =). So I fixed my bug and everything that I described above works (it was an implementation bug, not a theory bug). However, my question about the angles (i.e. why is angle2 = sin(theta)*rand() and why is perturbed_vector = rand_vector2.Rotate(rand_vector2.Cross(orig_vector), acos(angle2)). Thanks so much!
Here's the algorithm that I've used for this kind of problem before. It was described in Ray Tracing News.
1) Make a third vector perpendicular to the other two to build an orthogonal basis:
cross_vector = unit( cross( orig_vector, rand_vector ) )
2) Pick two uniform random numbers in [0,1]:
s = rand( 0, 1 )
r = rand( 0, 1 )
3) Let h be the cosine of the cone's angle:
h = cos( theta )
4) Modify uniform sampling on a sphere to pick a random vector in the cone around +Z:
phi = 2 * pi * s
z = h + ( 1 - h ) * r
sinT = sqrt( 1 - z * z )
x = cos( phi ) * sinT
y = sin( phi ) * sinT
5) Change of basis to reorient it around the original angle:
perturbed = rand_vector * x + cross_vector * y + orig_vector * z
If you have another vector to represent an axis of rotation, there are libraries that will take the axis and the angle and give you a rotation matrix, which can then be multiplied by your starting vector to get the result you want.
However, the axis of rotation should be at right angles to your starting vector, to get the amount of rotation you expect. If the axis of rotation does not lie in the plane perpendicular to your vector, the result will be somewhat different than theta.
That being said, if you already have a vector at right angles to the one you want to perturb, and you're not picky about the direction of the perturbation, you can just as easily take a linear combination of your starting vector with the perpendicular one, adjust for magnitude as needed.
I.e., if P and Q are vectors having identical magnitude, and are perpendicular, and you want to rotate P in the direction of Q, then the vector R given by R = [Pcos(theta)+Qsin(theta)] will satisfy the constraints you've given. If P and Q have differing magnitude, then there will be some scaling involved.
You may be interested in 3D-coordinate transformations to change your vector angle.
I don't know how many directions you want to change your angle in, but transforming your Cartesian coordinates to spherical coordinates should allow you to make your angle change as you like.
Actually, it is very easy to do that. All you have to do is multiply your vector by the correct rotation matrix. The resulting vector will be your rotated vector. Now, how do you obtain such rotation matrix? That depends on the 3d framework/engine you are using. Any 3d framework must provide functions for obtaining rotation matrices, normally as static methods of the Matrix class.
Good luck.
Like said in other comments you can rotate your vector using a rotation matrix.
The rotation matrix has two angles you rotate your vector around. You can pick them with a random number generator, but just picking two from a flat generator is not correct. To ensure that your rotation vector is generated flat, you have to pick one random angle φ from a flat generator and the other one from a generator flat in cosθ ;this ensures that your solid angle element dcos(θ)dφ is defined correctly (φ and θ defined as usual for spherical coordinates).
Example: picking a random direction with no restriction on range, random() generates flat in [0,1]
angle1 = acos(random())
angle2 = 2*pi*random()
My code in unity - tested and working:
/*
* this is used to perturb given vector 'direction' by changing it by angle not more than 'angle' vector from
* base direction. Used to provide errors for player playing algorithms
*
*/
Vector3 perturbDirection( Vector3 direction, float angle ) {
// division by zero protection
if( Mathf.Approximately( direction.z, 0f )) {
direction.z = 0.0001f;
}
// 1 get some orthogonal vector to direction ( solve direction and orthogonal dot product = 0, assume x = 1, y = 1, then z = as below ))
Vector3 orthogonal = new Vector3( 1f, 1f, - ( direction.x + direction.y ) / direction.z );
// 2 get random vector from circle on flat orthogonal to direction vector. get full range to assume all cone space randomization (-180, 180 )
float orthoAngle = UnityEngine.Random.Range( -180f, 180f );
Quaternion rotateTowardsDirection = Quaternion.AngleAxis( orthoAngle, direction );
Vector3 randomOrtho = rotateTowardsDirection * orthogonal;
// 3 rotate direction towards random orthogonal vector by vector from our available range
float perturbAngle = UnityEngine.Random.Range( 0f, angle ); // range from (0, angle), full cone cover guarantees previous (-180,180) range
Quaternion rotateDirection = Quaternion.AngleAxis( perturbAngle, randomOrtho );
Vector3 perturbedDirection = rotateDirection * direction;
return perturbedDirection;
}
I need to calculate the 2 angles (yaw and pitch) for a 3D object to face an arbitrary 3D point. These rotations are known as "Euler" rotations simply because after the first rotation, (lets say Z, based on the picture below) the Y axis also rotates with the object.
This is the code I'm using but its not working fully. When on the ground plane (Y = 0) the object correctly rotates to face the point, but as soon as I move the point upwards in Y, the rotations don't look correct.
// x, y, z represent a fractional value between -[1] and [1]
// a "unit vector" of the point I need to rotate towards
yaw = Math.atan2( y, x )
pitch = Math.atan2( z, Math.sqrt( x * x + y * y ) )
Do you know how to calculate the 2 Euler angles given a point?
The picture below shows the way I rotate. These are the angles I need to calculate.
(The only difference is I'm rotating the object in the order X,Y,Z and not Z,Y,X)
This is my system.
coordinate system is x = to the right, y = downwards, z = further back
an object is by default at (0,0,1) which is facing backward
rotations are in the order X, Y, Z where rotation upon X is pitch, Y is yaw and Z is roll
Here are my working assumptions:
The coordinate system (x,y,z) is such that positive x is to the right, positive y is down, and z is the remaining direction. In particular, y=0 is the ground plane.
An object at (0,0,0) currently facing towards (0,0,1) is being turned to face towards (x,y,z).
In order to accomplish this, there will be a rotation about the x-axis followed by one around the y-axis. Finally, there is a rotation about the z-axis in order to have things upright.
(The terminology yaw, pitch, and roll can be confusing, so I'd like to avoid using it, but roughly speaking the correspondence is x=pitch, y=yaw, z=roll.)
Here is my attempt to solve your problem given this setup:
rotx = Math.atan2( y, z )
roty = Math.atan2( x * Math.cos(rotx), z )
rotz = Math.atan2( Math.cos(rotx), Math.sin(rotx) * Math.sin(roty) )
Hopefully this is correct up to signs. I think the easiest way to fix the signs is by trial and error. Indeed, you appear to have gotten the signs on rotx and roty correct -- including a subtle issue with regards to z -- so you only need to fix the sign on rotz.
I expect this to be nontrivial (possibly depending on which octant you're in), but please try a few possibilities before saying it's wrong. Good luck!
Here is the code that finally worked for me.
I noticed a "flip" effect that occurred when the object moved from any front quadrant (positive Z) to any back quadrant. In the front quadrants the front of the object would always face the point. In the back quadrants the back of the object always faces the point.
This code corrects the flip effect so the front of the object always faces the point. I encountered it through trial-and-error so I don't really know what's happening!
rotx = Math.atan2( y, z );
if (z >= 0) {
roty = -Math.atan2( x * Math.cos(rotx), z );
}else{
roty = Math.atan2( x * Math.cos(rotx), -z );
}
Rich Seller's answer shows you how to rotate a point from one 3-D coordinate system to another system, given a set of Euler angles describing the rotation between the two coordinate systems.
But it sounds like you're asking for something different:
You have: 3-D coordinates of a single point
You want: a set of Euler angles
If that's what you're asking for, you don't have enough information. To find the Euler angles,
you'd need coordinates of at least two points, in both coordinate systems, to determine the rotation from one coordinate system into the other.
You should also be aware that Euler angles can be ambiguous: Rich's answer assumes the
rotations are applied to Z, then X', then Z', but that's not standardized. If you have to interoperate with some other code using Euler angles, you need to make sure you're using the same convention.
You might want to consider using rotation matrices or quaternions instead of Euler angles.
This series of rotations will give you what you're asking for:
About X: 0
About Y: atan2(z, x)
About Z: atan2(y, sqrt(x*x + z*z))
I cannot tell you what these are in terms of "roll", "pitch" and "yaw" unless you first define how you are using these terms. You are not using them in the standard way.
EDIT:
All right, then try this:
About X: -atan2(y, z)
About Y: atan2(x, sqrt(y*y + z*z))
About Z: 0
Talking about the rotation of axes, I think step 3 should have been the rotation of X'-, Y''-, and Z'-axes about the Y''-axis.
I have an input 3D vector, along with the pitch and yaw of the camera. Can anyone describe or provide a link to a resource that will help me understand and implement the required transformation and matrix mapping?
The world-to-camera transformation matrix is the inverse of the camera-to-world matrix. The camera-to-world matrix is the combination of a translation to the camera's position and a rotation to the camera's orientation. Thus, if M is the 3x3 rotation matrix corresponding to the camera's orientation and t is the camera's position, then the 4x4 camera-to-world matrix is:
M00 M01 M02 tx
M10 M11 M12 ty
M20 M21 M22 tz
0 0 0 1
Note that I've assumed that vectors are column vectors which are multiplied on the right to perform transformations. If you use the opposite convention, make sure to transpose the matrix.
To find M, you can use one of the formulas listed on Wikipedia, depending on your particular convention for roll, pitch, and yaw. Keep in mind that those formulas use the convention that vectors are row vectors which are multiplied on the left.
Instead of computing the camera-to-world matrix and inverting it, a more efficient (and numerically stable) alternative is to calculate the world-to-camera matrix directly. To do so, just invert the camera's position (by negating all 3 coordinates) and its orientation (by negating the roll, pitch, and yaw angles, and adjusting them to be in their proper ranges), and then compute the matrix using the same algorithm.
If we have a structure like this to describe a 4x4 matrix:
class Matrix4x4
{
public:
union
{
struct
{
Type Xx, Xy, Xz, Xw;
Type Yx, Yy, Yz, Yw;
Type Zx, Zy, Zz, Zw;
Type Wx, Wy, Wz, Ww;
};
struct
{
Vector3<Type> Right;
Type XW;
Vector3<Type> Up;
Type YW;
Vector3<Type> Look;
Type ZW;
Vector3<Type> Pos;
Type WW;
};
Type asDoubleArray[4][4];
Type asArray[16];
};
};
If all you have is Euler angles, that is an angles representing the yaw, pitch, and roll and a point in 3d space for the position, you can calculate the Right, Up, and Look vectors. Note that Right, Up, and Look are just the X,Y,Z Vectors, but since this is a camera, I find it easier to name it so. The simplest way to apply your roations to the camera matrix is to build a series of rotation matrices and multiply our camera matrix by each rotation matrix.
A good reference for that is here: http://www.euclideanspace.com
Once you have applied all the needed rotations, you can set the vector Pos to the camera's position in the world space.
Lastly, before you apply the camera's transformation, you need to take the camera's inverse of its matrix. This is what you are going to multiply your modelview matrix by before you start drawing polygons. For the matrix class above, the inverse is calculated like this:
template <typename Type>
Matrix4x4<Type> Matrix4x4<Type>::OrthoNormalInverse(void)
{
Matrix4x4<Type> OrthInv;
OrthInv = Transpose();
OrthInv.Xw = 0;
OrthInv.Yw = 0;
OrthInv.Zw = 0;
OrthInv.Wx = -(Right*Pos);
OrthInv.Wy = -(Up*Pos);
OrthInv.Wz = -(Look*Pos);
return OrthInv;
}
So finally, with all our matrix constuction out of the way, you would be doing something like this:
Matrix4x4<float> cameraMatrix, rollRotation, pitchRotation, yawRotation;
Vector4<float> cameraPosition;
cameraMatrix = cameraMatrix * rollRotation * pitchRotation * yawRotation;
Matrix4x4<float> invCameraMat;
invCameraMat = cameraMatrix.OrthoNormalInverse();
glMultMatrixf(invCameraMat.asArray);
Hope this helps.
What you are describing is called 'Perspective Projection' and there are reams of resources on the web that explain the matrix math and give the code necessary to do this. You could start with the wikipedia page