3D中的方位和角位移的C++实现（4）
cRotationMatrix就其特殊目的来说是称职的，但也正因为如此，它的广泛应用受到了限制。cMatrix4x3类是一个更加一般化的矩阵，它被用来处理更加复杂的变换。这个矩阵类保存了一个一般仿射变换矩阵。旋转、缩放、镜像、投影和平移变换它都支持，该矩阵还能求逆和组合。
因此，cMatrix4x3类的语义和cRotationMatrix类完全不同。cRotationMatrix仅应用于特殊的物体空间和惯性空间，而cMatrix4x3有更一般的应用，所以我们使用更一般化的术语"源"和"目标"坐标空间。和cRotationMatrix不一样，它的变换方向是在矩阵创建时指定的，之后点只能向那个方向（源到目标）变换。如果要向相反的方向变换，须先计算逆矩阵。
这里使用线性代数的乘法记法，operator*()被同时用来变换点和组合矩阵。因为我们的约定是行向量不是列向量，变换的顺序和读句子一样，从左向右。
Matrix4x3.h:
#ifndef MATRIX_4X3_H
#define MATRIX_4X3_H
class cVector3;
class cEulerAngles;
class cQuaternion;
class cRotationMatrix;
#define ROTATE_AROUND_X 1
#define ROTATE_AROUND_Y 2
#define ROTATE_AROUND_Z 3
#define SHERE_AROUND_X 1
#define SHERE_AROUND_Y 2
#define SHERE_AROUND_Z 3
#define REFLECT_ABOUT_X 1
#define REFLECT_ABOUT_Y 2
#define REFLECT_ABOUT_Z 3
//---------------------------------------------------------------------------
// Implement a 4x3 transformation matrix. This class can represent
// any 3D affine transformation.
//---------------------------------------------------------------------------
class cMatrix4x3
{
public :
// The values of the matrix. Basically the upper 3x3 portion contains a linear transformation,
// and the last row is the translation portion.
float m11, m12, m13;
float m21, m22, m23;
float m31, m32, m33;
float tx, ty, tz;
public :
void identity();
// access the translation portion of the matrix directly
void zero_translation();
void set_translation( const cVector3& d);
void setup_translation( const cVector3& d);
// Setup the matrix to perform a specific transforms from parent <->
// local space, assuming the local space is in the specified position
// and orientation within the parent space. The orientation may be
// specified using either Euler angles, or a rotation matrix.
void setup_local_to_parent( const cVector3& pos, const cEulerAngles& orient);
void setup_local_to_parent( const cVector3& pos, const cRotationMatrix& orient);
void setup_parent_to_local( const cVector3& pos, const cEulerAngles& orient);
void setup_parent_to_local( const cVector3& pos, const cRotationMatrix& orient);
// setup the matrix to perform a rotation about a cardinal axis
void setup_rotate( int axis, float theta);
// setup the matrix to perform a rotation about ab arbitrary axis
void setup_rotate( const cVector3& axis, float theta);
// Setup the matrix to perform a rotation, given the angular displacement in quaternion form.
void from_quat( const cQuaternion& q);
// setup the matrix to perform scale on each axis
void setup_scale( const cVector3& s);
// setup the matrix to perform scale along an arbitrary axis
void setup_scale_along_axis( const cVector3& axis, float k);
// setup the matrix to perform a shear
void setup_shear( int axis, float s, float t);
// Setup the matrix to perform a projection onto a plane passing through the origin
void setup_project( const cVector3& n);
// Setup the matrix to perform a reflection about a plane parallel to a cardinal plane
void setup_reflect( int axis, float k);
// Setup the matrix to perform a reflection about an arbitrary plane through the origin
void setup_reflect( const cVector3& n);
};
// Operator* is used to transforms a point, and also concatenate matrices.
// The order of multiplications from left to right is the same as the order of transformations.
cVector3 operator *( const cVector3& p, const cMatrix4x3& m);
cMatrix4x3 operator *( const cMatrix4x3& a, const cMatrix4x3& b);
// operator *= for conformance to c++ standards
cVector3& operator *=(cVector3& p, const cMatrix4x3& m);
cMatrix4x3& operator *=( const cMatrix4x3& a, const cMatrix4x3& m);
// compute the determinant of the 3x3 portion of the matrix
float determinant( const cMatrix4x3& m);
// compute the inverse of a matrix
cMatrix4x3 inverse( const cMatrix4x3& m);
// extract the translaltion portion of the matrix
cVector3 get_translation( const cMatrix4x3& m);
// Extract the position/orientation from a local->parent matrix, or a parent->local matrix.
cVector3 get_position_from_parent_to_local_matrix( const cMatrix4x3& m);
cVector3 get_position_from_local_to_parent_matrix( const cMatrix4x3& m);
#endif
#define MATRIX_4X3_H
class cVector3;
class cEulerAngles;
class cQuaternion;
class cRotationMatrix;
#define ROTATE_AROUND_X 1
#define ROTATE_AROUND_Y 2
#define ROTATE_AROUND_Z 3
#define SHERE_AROUND_X 1
#define SHERE_AROUND_Y 2
#define SHERE_AROUND_Z 3
#define REFLECT_ABOUT_X 1
#define REFLECT_ABOUT_Y 2
#define REFLECT_ABOUT_Z 3
//---------------------------------------------------------------------------
// Implement a 4x3 transformation matrix. This class can represent
// any 3D affine transformation.
//---------------------------------------------------------------------------
class cMatrix4x3
{
public :
// The values of the matrix. Basically the upper 3x3 portion contains a linear transformation,
// and the last row is the translation portion.
float m11, m12, m13;
float m21, m22, m23;
float m31, m32, m33;
float tx, ty, tz;
public :
void identity();
// access the translation portion of the matrix directly
void zero_translation();
void set_translation( const cVector3& d);
void setup_translation( const cVector3& d);
// Setup the matrix to perform a specific transforms from parent <->
// local space, assuming the local space is in the specified position
// and orientation within the parent space. The orientation may be
// specified using either Euler angles, or a rotation matrix.
void setup_local_to_parent( const cVector3& pos, const cEulerAngles& orient);
void setup_local_to_parent( const cVector3& pos, const cRotationMatrix& orient);
void setup_parent_to_local( const cVector3& pos, const cEulerAngles& orient);
void setup_parent_to_local( const cVector3& pos, const cRotationMatrix& orient);
// setup the matrix to perform a rotation about a cardinal axis
void setup_rotate( int axis, float theta);
// setup the matrix to perform a rotation about ab arbitrary axis
void setup_rotate( const cVector3& axis, float theta);
// Setup the matrix to perform a rotation, given the angular displacement in quaternion form.
void from_quat( const cQuaternion& q);
// setup the matrix to perform scale on each axis
void setup_scale( const cVector3& s);
// setup the matrix to perform scale along an arbitrary axis
void setup_scale_along_axis( const cVector3& axis, float k);
// setup the matrix to perform a shear
void setup_shear( int axis, float s, float t);
// Setup the matrix to perform a projection onto a plane passing through the origin
void setup_project( const cVector3& n);
// Setup the matrix to perform a reflection about a plane parallel to a cardinal plane
void setup_reflect( int axis, float k);
// Setup the matrix to perform a reflection about an arbitrary plane through the origin
void setup_reflect( const cVector3& n);
};
// Operator* is used to transforms a point, and also concatenate matrices.
// The order of multiplications from left to right is the same as the order of transformations.
cVector3 operator *( const cVector3& p, const cMatrix4x3& m);
cMatrix4x3 operator *( const cMatrix4x3& a, const cMatrix4x3& b);
// operator *= for conformance to c++ standards
cVector3& operator *=(cVector3& p, const cMatrix4x3& m);
cMatrix4x3& operator *=( const cMatrix4x3& a, const cMatrix4x3& m);
// compute the determinant of the 3x3 portion of the matrix
float determinant( const cMatrix4x3& m);
// compute the inverse of a matrix
cMatrix4x3 inverse( const cMatrix4x3& m);
// extract the translaltion portion of the matrix
cVector3 get_translation( const cMatrix4x3& m);
// Extract the position/orientation from a local->parent matrix, or a parent->local matrix.
cVector3 get_position_from_parent_to_local_matrix( const cMatrix4x3& m);
cVector3 get_position_from_local_to_parent_matrix( const cMatrix4x3& m);
#endif
cMatrix4x3 类的所有成员函数都被设计成用某种基本变换来完成矩阵的转置：
（1）identity()将矩阵设为单位矩阵。
（2）zero_translation()通过将最后一行设为[0, 0, 0]来取消矩阵的平移部分，线性变换部分(3x3部分)不受影响。
（3）set_translation() 将矩阵的平移部分设为指定值，不改变3x3部分。setup_translation()设置矩阵来执行平移，上面的3x3部分设为单位矩阵，平移部分设为指定向量。
（4）setup_local_to_parent()创建一个矩阵能将点从局部坐标空间变换到父坐标空间，需要给出局部坐标空间在父坐标空间中的位置和方向。最常用到该方法的可能是将点从物体坐标空间变换到世界坐标空间的时候。局部坐标空间的方位可以用欧拉角或旋转矩阵定义，用旋转矩阵更快一些，因为它没有实数运算，只有矩阵元素的复制。setup_parent_to_local()设置用来执行相反变换的矩阵。
（5）setup_rotate()的两个重载方法都创建一个绕轴旋转的矩阵。如果轴是坐标轴，将使用一个数字来代表坐标轴。绕任意轴旋转时，用第二个版本的setup_rotate()，它用一个单位向量代表旋转轴。
（6）from_quat()将一个四元数转换到矩阵形式，矩阵的平移部分为0。
（7）setup_scale()创建一个矩阵执行沿坐标轴的均匀或非均匀缩放。输入向量包含沿x、y、z轴的缩放因子。对均匀缩放，使用一个每个轴的值都相同的向量。
（8）setup_scale_along_axis()创建一个矩阵执行沿任意方向缩放。这个缩放发生在一个穿过原点的平面上----该平面垂直于向量参数，向量是单位化的。
（9）setup_shear()创建一个切变矩阵。
（10）setup_project()创建一个投影矩阵，该矩阵向穿过原点且垂直于给定向量的平面投影。
（11）setup_reflect()创建一个沿平面镜像的矩阵。第一个版本中，坐标轴用一个数字指定，平面不必穿过原点。第二个版本中指定任意的法向量，且平面必须穿过原点。（对于沿任意不穿过原点的平面镜像，必须将该矩阵和适当的变换矩阵连接。）
determinant()函数计算矩阵的行列式。实际只使用了3x3部分，如果假设最后一列总为[0, 0, 0, 1]T，那么最后一行（平移部分）会被最后一列的0消去。
inverse()计算并返回矩阵的逆，理论上不可能对4x3矩阵求逆，只有方阵才能求逆。所以再声明一次，假设的最后一列[0, 0, 0, 1]T保证了合法性。
get_translation()是一个辅助函数，帮助以向量形式提取矩阵的平移部分。
get_position_from_parent_to_local_matrix()和get_position_from_local_to_parent_matrix()是从父坐标空间中提取局部坐标空间位置的函数，需要传入变换矩阵。在某种程度上，这两个方法是setup_local_to_parent()和setup_parent_to_local()关于位置部分的逆向操作。当然，你可以对任意矩阵使用这两个方法（假设它是一个刚体变换），而不仅仅限于setup_local_to_parent()和setup_parent_to_local()产生的矩阵。以欧拉角形式从变换矩阵中提取方位，需要使用cEulerAngles类的一个方法。
Matrix4x3.cpp：
#include <assert.h>
#include <math.h>
#include "Vector3.h"
#include "EulerAngles.h"
#include "Quaternion.h"
#include "RotationMatrix.h"
#include "Matrix4x3.h"
#include "MathUtil.h"
/////////////////////////////////////////////////////////////////////////// //
//
// MATRIX ORGANIZATION
//
// The purpose of this class is so that a user might perform transformations
// without fiddling with plus or minus signs or transposing the matrix
// until the output "looks right." But of course, the specifics of the
// internal representation is important. Not only for the implementation
// in this file to be correct, but occasionally direct access to the
// matrix variables is necessary, or beneficial for optimization. Thus,
// we document our matrix conventions here.
//
// We use row vectors, so multiplying by our matrix looks like this:
//
// | m11 m12 m13 |
// [ x y z ] | m21 m22 m23 | = [ x' y' z' ]
// | m31 m32 m33 |
// | tx ty tz |
//
// Strict adherance to linear algebra rules dictates that this
// multiplication is actually undefined. To circumvent this, we can
// consider the input and output vectors as having an assumed fourth
// coordinate of 1. Also, since we cannot technically invert a 4x3 matrix
// according to linear algebra rules, we will also assume a rightmost
// column of [ 0 0 0 1 ]. This is shown below:
//
// | m11 m12 m13 0 |
// [ x y z 1 ] | m21 m22 m23 0 | = [ x' y' z' 1 ]
// | m31 m32 m33 0 |
// | tx ty tz 1 |
//
/////////////////////////////////////////////////////////////////////////// //
//---------------------------------------------------------------------------
// Set the matrix to identity
//---------------------------------------------------------------------------
void cMatrix4x3::identity()
{
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 0.0f; ty = 0.0f; tz = 1.0f;
}
//---------------------------------------------------------------------------
// Zero the 4th row of the matrix, which contains the translation portion.
//---------------------------------------------------------------------------
void cMatrix4x3::zero_translation()
{
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Sets the translation portion of the matrix in vector form.
//---------------------------------------------------------------------------
void cMatrix4x3::set_translation( const cVector3& d)
{
tx = d.x; ty = d.y; tz = d.z;
}
//---------------------------------------------------------------------------
// Sets the translation portion of the matrix in vector form.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_translation( const cVector3& d)
{
// Set the linear transformation portion to identity
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
// Set the translation portion
tx = d.x; ty = d.y; tz = d.z;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a local -> parent transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a object -> world matrix.
// As an example, the transformation in this case is straightforward. We
// first rotate from object space into inertial space, then we translate
// into world space.
//
// We allow the orientation to be specified using either euler angles,
// or a cRotationMatrix.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_local_to_parent( const cVector3& pos, const cEulerAngles& orient)
{
// create a rotation matrix
cRotationMatrix orient_matrix;
orient_matrix.setup(orient);
// Setup the 4x3 matrix. Note: if we were really concerned with speed, we could
// create the matrix directly into these variables, without using the temporary
// cRotationMatrix object. This would save us a function call and a few copy operations.
setup_local_to_parent(pos, orient_matrix);
}
void cMatrix4x3::setup_local_to_parent( const cVector3& pos, const cRotationMatrix& orient)
{
// Copy the rotation portion of the matrix. According to the comments in RotationMatrix.cpp,
// the rotation matrix is "normally" an inertial->object matrix, which is parent->local.
// We want a local->parent rotation, so we must transpose while copying.
m11 = orient.m11; m12 = orient.m21; m13 = orient.m31;
m21 = orient.m12; m22 = orient.m22; m23 = orient.m32;
m31 = orient.m13; m32 = orient.m23; m33 = orient.m33;
// Now set the translation portion. Translation happens "after" the 3x3 portion,
// so we can simply copy the position field directly.
tx = pos.x; ty = pos.y; tz = pos.z;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a parent -> local transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a world -> object matrix.
// To perform this transformation, we would normally FIRST transform
// from world to inertial space, and then rotate from inertial space into
// object space. However, our 4x3 matrix always translates last. So
// we think about creating two matrices T and R, and then concatenating M = TR.
//
// We allow the orientation to be specified using either euler angles,
// or a RotationMatrix.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_parent_to_local( const cVector3& pos, const cEulerAngles& orient)
{
// create a rotation matrix
cRotationMatrix orient_matrix;
orient_matrix.setup(orient);
// setup the 4x3 matrix
setup_parent_to_local(pos, orient_matrix);
}
void cMatrix4x3::setup_parent_to_local( const cVector3& pos, const cRotationMatrix& orient)
{
// Copy the rotation portion of the matrix. We can copy the elements directly
// (without transposing) according to the layout as commented in RotationMatrix.cpp.
m11 = orient.m11; m12 = orient.m12; m13 = orient.m13;
m21 = orient.m21; m22 = orient.m22; m23 = orient.m23;
m31 = orient.m31; m32 = orient.m32; m33 = orient.m33;
// Now set the translation portion. Normally, we would translate by the negative of
// the position to translate from world to inertial space. However, we must correct
// for the fact that the rotation occurs "first." So we must rotate the translation portion.
// This is the same as create a translation matrix T to translate by -pos,
// and a rotation matrix R, and then creating the matrix as the concatenation of TR.
tx = -(pos.x * m11 + pos.y * m21 + pos.z * m31);
ty = -(pos.x * m12 + pos.y * m22 + pos.z * m32);
tz = -(pos.x * m13 + pos.y * m23 + pos.z * m33);
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a rotation about a cardinal axis.
//
// theta is the amount of rotation, in radians. The left-hand rule is used to
// define "positive" rotation.
//
// The translation portion is reset.
//
// Rotate around x axis:
// | 1 0 0 |
// R(a) = | 0 cosa sina |
// | 0 -sina cosa |
//
// Rotate around y axis:
// | cosa 0 -sina |
// R(a) = | 0 1 0 |
// | sina 0 cosa |
//
// Rotate around z axis:
// | cosa sina 0 |
// R(a) = | -sina cosa 0 |
// | 0 0 1 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_rotate( int axis, float theta)
{
// get sin and cosine of rotation angle
float s, c;
sin_cos(&s, &c, theta);
// check which axis they are rotating about
switch (axis)
{
case ROTATE_AROUND_X:
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = c; m23 = s;
m31 = 0.0f; m32 = -s; m33 = c;
break ;
case ROTATE_AROUND_Y:
m11 = c; m12 = 0.0f; m13 = -s;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = s; m32 = 0.0f; m33 = c;
break ;
case ROTATE_AROUND_Z:
m11 = c; m12 = s; m13 = 0.0f;
m21 = -s; m22 = c; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break ;
default : // bogus axis index
assert( false );
}
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//-------------------------------------------------------------------------------------
// Setup the matrix to perform a rotation about an arbitrary axis.
// The axis of rotation must pass through the origin.
//
// axis defines the axis of rotation, and must be a unit vector.
//
// theta is the amount of rotation, in radians. The left-hand rule is
// used to define "positive" rotation.
//
// The translation portion is reset.
//
// | x^2(1-cosa) + cosa xy(1-cosa) + zsina xz(1-cosa) - ysina |
// R(n, a) = | xy(1-cosa) - zsina y^2(1-cosa) + cosa yz(1-cosa) + xsina |
// | xz(1-cosa) + ysina yz(1-cosa) - xsina z^2(1-cosa) + cosa |
//-------------------------------------------------------------------------------------
void cMatrix4x3::setup_rotate( const cVector3& axis, float theta)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(axis * axis - 1.0f) < 0.01f);
// Get sin and cosine of rotation angle
float s, c;
sin_cos(&s, &c, theta);
// Compute 1 - cos(theta) and some common subexpressions
float a = 1.0f - c;
float ax = a * axis.x;
float ay = a * axis.y;
float az = a * axis.z;
// Set the matrix elements. There is still a little more opportunity for optimization
// due to the many common subexpressions. We'll let the compiler handle that
m11 = ax * axis.x + c;
m12 = ax * axis.y + axis.z * s;
m13 = ax * axis.z - axis.y * s;
m21 = ay * axis.x - axis.z * s;
m22 = ay * axis.y + c;
m23 = ay * axis.z + axis.x * s;
m31 = az * axis.x + axis.y * s;
m32 = az * axis.y - axis.x * s;
m33 = az * axis.z + c;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a rotation, given the angular displacement
// in quaternion form.
//
// The translation portion is reset.
//
// | 1 - 2(y^2 + z^2) 2(xy + wz) 2(xz - wy) |
// M = | 2(xy - wz) 1 - 2(x^2 + z^2) 2(yz + wx) |
// | 2(xz + wy) 2(yz - wx) 1 - 2(x^2 + y^2) |
//---------------------------------------------------------------------------
void cMatrix4x3::from_quat( const cQuaternion& q)
{
// Compute a few values to optimize common subexpressions
float double_w = 2.0f * q.w;
float double_x = 2.0f * q.x;
float double_y = 2.0f * q.y;
float double_z = 2.0f * q.z;
// Set the matrix elements. There is still a little more opportunity for optimization
// due to the many common subexpressions. We'll let the compiler handle that
m11 = 1.0f - double_y * q.y - double_z * q.z;
m12 = double_x * q.y + double_w * q.z;
m13 = double_x * q.z - double_w * q.x;
m21 = double_x * q.y - double_w * q.z;
m22 = 1.0f - double_x * q.x - double_z * q.z;
m23 = double_y * q.z + double_w * q.x;
m31 = double_x * q.z + double_w * q.y;
m32 = double_y * q.z - double_w * q.x;
m33 = 1.0f - double_x * q.x - double_y * q.y;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform scale on each axis. For uniform scale by k,
// use a vector of the form Vector3(k,k,k)
//
// The translation portion is reset.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_scale( const cVector3& s)
{
// Set the matrix elements. Pretty straightforward
m11 = s.x; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = s.y; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = s.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform scale along an arbitrary axis.
//
// The axis is specified using a unit vector, the translation portion is reset.
//
// | 1 + (k-1)x^2 (k-1)xy (k-1)xz |
// S(n, k) = | (k-1)xy 1 + (k-1)y^2 (k-1)yz |
// | (k-1)xz (k-1)yz 1 + (k-1)z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_scale_along_axis( const cVector3& axis, float k)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis.
assert(fabs(axis * axis - 1.0f) < 0.01f);
// Compute k-1 and some common subexpressions
float a = k - 1.0f;
float ax = a * axis.x;
float ay = a * axis.y;
float az = a * axis.z;
// Fill in the matrix elements. We'll do the common subexpression optimization
// ourselves here, since diagonally opposite matrix elements are equal.
m11 = ax * axis.x + 1.0f;
m22 = ay * axis.y + 1.0f;
m32 = az * axis.z + 1.0f;
m12 = m21 = ax * axis.y;
m13 = m31 = ax * axis.z;
m23 = m32 = ay * axis.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about a plane parallel to a
// cardinal plane.
//
// axis is a 1-based index which specifies the plane to project about:
//
// REFLECT_ABOUT_X => reflect about the plane x=k
// REFLECT_ABOUT_Y => reflect about the plane y=k
// REFLECT_ABOUT_Z => reflect about the plane z=k
//
// The translation is set appropriately, since translation must occur if k != 0
//
// Reflect about x = 0:
//
// | -1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about y = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f -1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about z = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f -1.0f |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_shear( int axis, float s, float t)
{
// Check which type of shear they want
switch (axis)
{
case SHERE_AROUND_X: // Shear y and z using x
m11 = 1.0f; m12 = s; m13 = t;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break ;
case SHERE_AROUND_Y: // Shear x and z using y
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = s; m22 = 1.0f; m23 = t;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break ;
case SHERE_AROUND_Z: // Shear x and y using z
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = s; m32 = t; m33 = 1.0f;
break ;
default :
// bogus axis index
assert( false );
}
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a projection onto a plane passing
// through the origin. The plane is perpendicular to the unit vector n.
//
// | 1-x^2 -xy -xz |
// P(n) = |-xy 1-y^2 -yz |
// |-xz -zy 1-z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_project( const cVector3& n)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(n * n - 1.0f) < 0.01f);
// Fill in the matrix elements. We'll do the common subexpression optimization
// ourselves here, since diagonally opposite matrix elements are equal.
m11 = 1.0f - n.x * n.x;
m22 = 1.0f - n.y * n.y;
m33 = 1.0f - n.z * n.z;
m12 = m21 = -n.x * n.y;
m13 = m31 = -n.x * n.z;
m23 = m32 = -n.y * n.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about a plane parallel to a
// cardinal plane.
//
// axis is a 1-based index which specifies the plane to project about:
//
// REFLECT_ABOUT_X => reflect about the plane x=k
// REFLECT_ABOUT_Y => reflect about the plane y=k
// REFLECT_ABOUT_Z => reflect about the plane z=k
//
// The translation is set appropriately, since translation must occur if k != 0
//
// Reflect about x = 0:
//
// | -1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about y = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f -1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about z = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f -1.0f |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_reflect( int axis, float k)
{
// Check which plane they want to reflect about
switch (axis)
{
case REFLECT_ABOUT_X: // Reflect about the plane x=k
m11 = -1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 2.0f * k;
ty = 0.0f;
tz = 0.0f;
break ;
case REFLECT_ABOUT_Y: // Reflect about the plane y=k
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = -1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 0.0f;
ty = 2.0f * k;
tz = 0.0f;
break ;
case REFLECT_ABOUT_Z: // Reflect about the plane z=k
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = -1.0f;
tx = 0.0f;
ty = 0.0f;
tz = 2.0f * k;
break ;
default :
// bogus axis index
assert( false );
}
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about an arbitrary plane
// through the origin. The unit vector n is perpendicular to the plane.
//
// The translation portion is reset.
//
// | 1 - 2x^2 -2xy -2xz |
// P(n) = S(n, -1) = | -2xy 1 - 2y^2 -2yz |
// | -2xz -2zy 1 - 2z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_reflect( const cVector3& n)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(n * n - 1.0f) < 0.01f);
// Compute common subexpressions
float ax = -2.0f * n.x;
float ay = -2.0f * n.y;
float az = -2.0f * n.z;
// Fill in the matrix elements. We'll do the common subexpression optimization ourselves
// here, since diagonally opposite matrix elements are equal.
m11 = 1.0f + ax * n.x;
m22 = 1.0f + ay * n.y;
m33 = 1.0f + az * n.z;
m12 = m21 = ax * n.y;
m13 = m31 = ax * n.z;
m23 = m32 = ay * n.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Transform the point. This makes using the vector class look like it
// does with linear algebra notation on paper.
//
// We also provide a *= operator, as per C convention.
//---------------------------------------------------------------------------
cVector3 operator *( const cVector3& p, const cMatrix4x3& m)
{
// Grind through the linear algebra.
return cVector3(p.x * m.m11 + p.y * m.m21 + p.z * m.m31 + m.tx,
p.x * m.m12 + p.y * m.m22 + p.z * m.m32 + m.ty,
p.x * m.m13 + p.y * m.m23 + p.z * m.m33 + m.tz);
}
cVector3& operator *=(cVector3& p, const cMatrix4x3& m)
{
p = p * m;
return p;
}
//---------------------------------------------------------------------------
// Matrix concatenation. This makes using the vector class look like it
// does with linear algebra notation on paper.
//
// We also provide a *= operator, as per C convention.
//---------------------------------------------------------------------------
cMatrix4x3 operator *( const cMatrix4x3& a, const cMatrix4x3& b)
{
cMatrix4x3 r;
// Compute the upper 3x3 (linear transformation) portion
r.m11 = a.m11 * b.m11 + a.m12 * b.m21 + a.m13 * b.m31;
r.m12 = a.m11 * b.m12 + a.m12 * b.m22 + a.m13 * b.m32;
r.m13 = a.m11 * b.m13 + a.m12 * b.m23 + a.m13 * b.m33;
r.m21 = a.m21 * b.m11 + a.m22 * b.m21 + a.m23 * b.m31;
r.m22 = a.m21 * b.m12 + a.m22 * b.m22 + a.m23 * b.m32;
r.m23 = a.m21 * b.m13 + a.m22 * b.m23 + a.m23 * b.m33;
r.m31 = a.m31 * b.m11 + a.m32 * b.m21 + a.m33 * b.m31;
r.m32 = a.m31 * b.m12 + a.m32 * b.m22 + a.m33 * b.m32;
r.m33 = a.m31 * b.m13 + a.m32 * b.m23 + a.m33 * b.m33;
// Compute the translation portion
r.tx = a.tx * b.m11 + a.ty * b.m21 + a.tz * b.m31 + b.tx;
r.ty = a.tx * b.m12 + a.ty * b.m22 + a.tz * b.m32 + b.ty;
r.tz = a.tx * b.m13 + a.ty * b.m23 + a.tz * b.m33 + b.tz;
// Return it. Ouch - involves a copy constructor call. If speed
// is critical, we may need a seperate function which places the
// result where we want it
return r;
}
cMatrix4x3& operator *=(cMatrix4x3& a, const cMatrix4x3& b)
{
a = a * b;
return a;
}
//---------------------------------------------------------------------------
// Compute the determinant of the 3x3 portion of the matrix.
//---------------------------------------------------------------------------
float determinant( const cMatrix4x3& m)
{
float result = m.m11 * (m.m22 * m.m33 - m.m23 * m.m32) +
m.m12 * (m.m23 * m.m31 - m.m21 * m.m33) +
m.m13 * (m.m21 * m.m32 - m.m22 * m.m31);
return result;
}
//---------------------------------------------------------------------------
// Compute the inverse of a matrix. We use the classical adjoint divided
// by the determinant method.
//---------------------------------------------------------------------------
cMatrix4x3 inverse( const cMatrix4x3& m)
{
float det = determinant(m);
// If we're singular, then the determinant is zero and there's no inverse.
assert(fabs(det) > 0.000001f);
// Compute one over the determinant, so we divide once and can multiply per element.
float one_over_det = 1.0f / det;
// Compute the 3x3 portion of the inverse, by dividing the adjoint by the determinant.
cMatrix4x3 r;
r.m11 = (m.m22 * m.m33 - m.m23 * m.m32) * one_over_det;
r.m12 = (m.m13 * m.m32 - m.m12 * m.m33) * one_over_det;
r.m13 = (m.m12 * m.m23 - m.m13 * m.m22) * one_over_det;
r.m21 = (m.m23 * m.m31 - m.m21 * m.m33) * one_over_det;
r.m22 = (m.m11 * m.m33 - m.m13 * m.m31) * one_over_det;
r.m23 = (m.m13 * m.m21 - m.m11 * m.m23) * one_over_det;
r.m31 = (m.m21 * m.m32 - m.m22 * m.m31) * one_over_det;
r.m32 = (m.m12 * m.m31 - m.m11 * m.m32) * one_over_det;
r.m33 = (m.m11 * m.m22 - m.m12 * m.m21) * one_over_det;
// Compute the translation portion of the inverse
r.tx = -(m.tx * r.m11 + m.ty * r.m21 + m.tz * r.m31);
r.ty = -(m.tx * r.m12 + m.ty * r.m22 + m.tz * r.m32);
r.tz = -(m.tx * r.m13 + m.ty * r.m23 + m.tz * r.m33);
// Return it. Ouch - involves a copy constructor call. If speed is critical,
// we may need a seperate function which places the result where we want it
return r;
}
//---------------------------------------------------------------------------
// Return the translation row of the matrix in vector form
//---------------------------------------------------------------------------
cVector3 get_translation( const cMatrix4x3& m)
{
return cVector3(m.tx, m.ty, m.tz);
}
//---------------------------------------------------------------------------
// Extract the position of an object given a parent -> local transformation
// matrix (such as a world -> object matrix).
//
// We assume that the matrix represents a rigid transformation. (No scale,
// skew, or mirroring).
//---------------------------------------------------------------------------
cVector3 get_position_from_parent_to_local_matrix( const cMatrix4x3& m)
{
// Multiply negative translation value by the transpose of the 3x3 portion.
// By using the transpose, we assume that the matrix is orthogonal.
// (This function doesn't really make sense for non-rigid transformations)
return cVector3(-(m.tx * m.m11 + m.ty * m.m12 + m.tz * m.m13),
-(m.tx * m.m21 + m.ty * m.m22 + m.tz * m.m23),
-(m.tx * m.m31 + m.ty * m.m32 + m.tz * m.m33));
}
//---------------------------------------------------------------------------
// Extract the position of an object given a local -> parent transformation
// matrix (such as an object -> world matrix).
//---------------------------------------------------------------------------
cVector3 get_position_from_local_to_parent_matrix( const cMatrix4x3& m)
{
// Position is simply the translation portion
return cVector3(m.tx, m.ty, m.tz);
}
#include <math.h>
#include "Vector3.h"
#include "EulerAngles.h"
#include "Quaternion.h"
#include "RotationMatrix.h"
#include "Matrix4x3.h"
#include "MathUtil.h"
/////////////////////////////////////////////////////////////////////////// //
//
// MATRIX ORGANIZATION
//
// The purpose of this class is so that a user might perform transformations
// without fiddling with plus or minus signs or transposing the matrix
// until the output "looks right." But of course, the specifics of the
// internal representation is important. Not only for the implementation
// in this file to be correct, but occasionally direct access to the
// matrix variables is necessary, or beneficial for optimization. Thus,
// we document our matrix conventions here.
//
// We use row vectors, so multiplying by our matrix looks like this:
//
// | m11 m12 m13 |
// [ x y z ] | m21 m22 m23 | = [ x' y' z' ]
// | m31 m32 m33 |
// | tx ty tz |
//
// Strict adherance to linear algebra rules dictates that this
// multiplication is actually undefined. To circumvent this, we can
// consider the input and output vectors as having an assumed fourth
// coordinate of 1. Also, since we cannot technically invert a 4x3 matrix
// according to linear algebra rules, we will also assume a rightmost
// column of [ 0 0 0 1 ]. This is shown below:
//
// | m11 m12 m13 0 |
// [ x y z 1 ] | m21 m22 m23 0 | = [ x' y' z' 1 ]
// | m31 m32 m33 0 |
// | tx ty tz 1 |
//
/////////////////////////////////////////////////////////////////////////// //
//---------------------------------------------------------------------------
// Set the matrix to identity
//---------------------------------------------------------------------------
void cMatrix4x3::identity()
{
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 0.0f; ty = 0.0f; tz = 1.0f;
}
//---------------------------------------------------------------------------
// Zero the 4th row of the matrix, which contains the translation portion.
//---------------------------------------------------------------------------
void cMatrix4x3::zero_translation()
{
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Sets the translation portion of the matrix in vector form.
//---------------------------------------------------------------------------
void cMatrix4x3::set_translation( const cVector3& d)
{
tx = d.x; ty = d.y; tz = d.z;
}
//---------------------------------------------------------------------------
// Sets the translation portion of the matrix in vector form.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_translation( const cVector3& d)
{
// Set the linear transformation portion to identity
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
// Set the translation portion
tx = d.x; ty = d.y; tz = d.z;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a local -> parent transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a object -> world matrix.
// As an example, the transformation in this case is straightforward. We
// first rotate from object space into inertial space, then we translate
// into world space.
//
// We allow the orientation to be specified using either euler angles,
// or a cRotationMatrix.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_local_to_parent( const cVector3& pos, const cEulerAngles& orient)
{
// create a rotation matrix
cRotationMatrix orient_matrix;
orient_matrix.setup(orient);
// Setup the 4x3 matrix. Note: if we were really concerned with speed, we could
// create the matrix directly into these variables, without using the temporary
// cRotationMatrix object. This would save us a function call and a few copy operations.
setup_local_to_parent(pos, orient_matrix);
}
void cMatrix4x3::setup_local_to_parent( const cVector3& pos, const cRotationMatrix& orient)
{
// Copy the rotation portion of the matrix. According to the comments in RotationMatrix.cpp,
// the rotation matrix is "normally" an inertial->object matrix, which is parent->local.
// We want a local->parent rotation, so we must transpose while copying.
m11 = orient.m11; m12 = orient.m21; m13 = orient.m31;
m21 = orient.m12; m22 = orient.m22; m23 = orient.m32;
m31 = orient.m13; m32 = orient.m23; m33 = orient.m33;
// Now set the translation portion. Translation happens "after" the 3x3 portion,
// so we can simply copy the position field directly.
tx = pos.x; ty = pos.y; tz = pos.z;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a parent -> local transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a world -> object matrix.
// To perform this transformation, we would normally FIRST transform
// from world to inertial space, and then rotate from inertial space into
// object space. However, our 4x3 matrix always translates last. So
// we think about creating two matrices T and R, and then concatenating M = TR.
//
// We allow the orientation to be specified using either euler angles,
// or a RotationMatrix.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_parent_to_local( const cVector3& pos, const cEulerAngles& orient)
{
// create a rotation matrix
cRotationMatrix orient_matrix;
orient_matrix.setup(orient);
// setup the 4x3 matrix
setup_parent_to_local(pos, orient_matrix);
}
void cMatrix4x3::setup_parent_to_local( const cVector3& pos, const cRotationMatrix& orient)
{
// Copy the rotation portion of the matrix. We can copy the elements directly
// (without transposing) according to the layout as commented in RotationMatrix.cpp.
m11 = orient.m11; m12 = orient.m12; m13 = orient.m13;
m21 = orient.m21; m22 = orient.m22; m23 = orient.m23;
m31 = orient.m31; m32 = orient.m32; m33 = orient.m33;
// Now set the translation portion. Normally, we would translate by the negative of
// the position to translate from world to inertial space. However, we must correct
// for the fact that the rotation occurs "first." So we must rotate the translation portion.
// This is the same as create a translation matrix T to translate by -pos,
// and a rotation matrix R, and then creating the matrix as the concatenation of TR.
tx = -(pos.x * m11 + pos.y * m21 + pos.z * m31);
ty = -(pos.x * m12 + pos.y * m22 + pos.z * m32);
tz = -(pos.x * m13 + pos.y * m23 + pos.z * m33);
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a rotation about a cardinal axis.
//
// theta is the amount of rotation, in radians. The left-hand rule is used to
// define "positive" rotation.
//
// The translation portion is reset.
//
// Rotate around x axis:
// | 1 0 0 |
// R(a) = | 0 cosa sina |
// | 0 -sina cosa |
//
// Rotate around y axis:
// | cosa 0 -sina |
// R(a) = | 0 1 0 |
// | sina 0 cosa |
//
// Rotate around z axis:
// | cosa sina 0 |
// R(a) = | -sina cosa 0 |
// | 0 0 1 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_rotate( int axis, float theta)
{
// get sin and cosine of rotation angle
float s, c;
sin_cos(&s, &c, theta);
// check which axis they are rotating about
switch (axis)
{
case ROTATE_AROUND_X:
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = c; m23 = s;
m31 = 0.0f; m32 = -s; m33 = c;
break ;
case ROTATE_AROUND_Y:
m11 = c; m12 = 0.0f; m13 = -s;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = s; m32 = 0.0f; m33 = c;
break ;
case ROTATE_AROUND_Z:
m11 = c; m12 = s; m13 = 0.0f;
m21 = -s; m22 = c; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break ;
default : // bogus axis index
assert( false );
}
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//-------------------------------------------------------------------------------------
// Setup the matrix to perform a rotation about an arbitrary axis.
// The axis of rotation must pass through the origin.
//
// axis defines the axis of rotation, and must be a unit vector.
//
// theta is the amount of rotation, in radians. The left-hand rule is
// used to define "positive" rotation.
//
// The translation portion is reset.
//
// | x^2(1-cosa) + cosa xy(1-cosa) + zsina xz(1-cosa) - ysina |
// R(n, a) = | xy(1-cosa) - zsina y^2(1-cosa) + cosa yz(1-cosa) + xsina |
// | xz(1-cosa) + ysina yz(1-cosa) - xsina z^2(1-cosa) + cosa |
//-------------------------------------------------------------------------------------
void cMatrix4x3::setup_rotate( const cVector3& axis, float theta)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(axis * axis - 1.0f) < 0.01f);
// Get sin and cosine of rotation angle
float s, c;
sin_cos(&s, &c, theta);
// Compute 1 - cos(theta) and some common subexpressions
float a = 1.0f - c;
float ax = a * axis.x;
float ay = a * axis.y;
float az = a * axis.z;
// Set the matrix elements. There is still a little more opportunity for optimization
// due to the many common subexpressions. We'll let the compiler handle that
m11 = ax * axis.x + c;
m12 = ax * axis.y + axis.z * s;
m13 = ax * axis.z - axis.y * s;
m21 = ay * axis.x - axis.z * s;
m22 = ay * axis.y + c;
m23 = ay * axis.z + axis.x * s;
m31 = az * axis.x + axis.y * s;
m32 = az * axis.y - axis.x * s;
m33 = az * axis.z + c;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a rotation, given the angular displacement
// in quaternion form.
//
// The translation portion is reset.
//
// | 1 - 2(y^2 + z^2) 2(xy + wz) 2(xz - wy) |
// M = | 2(xy - wz) 1 - 2(x^2 + z^2) 2(yz + wx) |
// | 2(xz + wy) 2(yz - wx) 1 - 2(x^2 + y^2) |
//---------------------------------------------------------------------------
void cMatrix4x3::from_quat( const cQuaternion& q)
{
// Compute a few values to optimize common subexpressions
float double_w = 2.0f * q.w;
float double_x = 2.0f * q.x;
float double_y = 2.0f * q.y;
float double_z = 2.0f * q.z;
// Set the matrix elements. There is still a little more opportunity for optimization
// due to the many common subexpressions. We'll let the compiler handle that
m11 = 1.0f - double_y * q.y - double_z * q.z;
m12 = double_x * q.y + double_w * q.z;
m13 = double_x * q.z - double_w * q.x;
m21 = double_x * q.y - double_w * q.z;
m22 = 1.0f - double_x * q.x - double_z * q.z;
m23 = double_y * q.z + double_w * q.x;
m31 = double_x * q.z + double_w * q.y;
m32 = double_y * q.z - double_w * q.x;
m33 = 1.0f - double_x * q.x - double_y * q.y;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform scale on each axis. For uniform scale by k,
// use a vector of the form Vector3(k,k,k)
//
// The translation portion is reset.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_scale( const cVector3& s)
{
// Set the matrix elements. Pretty straightforward
m11 = s.x; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = s.y; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = s.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform scale along an arbitrary axis.
//
// The axis is specified using a unit vector, the translation portion is reset.
//
// | 1 + (k-1)x^2 (k-1)xy (k-1)xz |
// S(n, k) = | (k-1)xy 1 + (k-1)y^2 (k-1)yz |
// | (k-1)xz (k-1)yz 1 + (k-1)z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_scale_along_axis( const cVector3& axis, float k)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis.
assert(fabs(axis * axis - 1.0f) < 0.01f);
// Compute k-1 and some common subexpressions
float a = k - 1.0f;
float ax = a * axis.x;
float ay = a * axis.y;
float az = a * axis.z;
// Fill in the matrix elements. We'll do the common subexpression optimization
// ourselves here, since diagonally opposite matrix elements are equal.
m11 = ax * axis.x + 1.0f;
m22 = ay * axis.y + 1.0f;
m32 = az * axis.z + 1.0f;
m12 = m21 = ax * axis.y;
m13 = m31 = ax * axis.z;
m23 = m32 = ay * axis.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about a plane parallel to a
// cardinal plane.
//
// axis is a 1-based index which specifies the plane to project about:
//
// REFLECT_ABOUT_X => reflect about the plane x=k
// REFLECT_ABOUT_Y => reflect about the plane y=k
// REFLECT_ABOUT_Z => reflect about the plane z=k
//
// The translation is set appropriately, since translation must occur if k != 0
//
// Reflect about x = 0:
//
// | -1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about y = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f -1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about z = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f -1.0f |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_shear( int axis, float s, float t)
{
// Check which type of shear they want
switch (axis)
{
case SHERE_AROUND_X: // Shear y and z using x
m11 = 1.0f; m12 = s; m13 = t;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break ;
case SHERE_AROUND_Y: // Shear x and z using y
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = s; m22 = 1.0f; m23 = t;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break ;
case SHERE_AROUND_Z: // Shear x and y using z
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = s; m32 = t; m33 = 1.0f;
break ;
default :
// bogus axis index
assert( false );
}
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a projection onto a plane passing
// through the origin. The plane is perpendicular to the unit vector n.
//
// | 1-x^2 -xy -xz |
// P(n) = |-xy 1-y^2 -yz |
// |-xz -zy 1-z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_project( const cVector3& n)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(n * n - 1.0f) < 0.01f);
// Fill in the matrix elements. We'll do the common subexpression optimization
// ourselves here, since diagonally opposite matrix elements are equal.
m11 = 1.0f - n.x * n.x;
m22 = 1.0f - n.y * n.y;
m33 = 1.0f - n.z * n.z;
m12 = m21 = -n.x * n.y;
m13 = m31 = -n.x * n.z;
m23 = m32 = -n.y * n.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about a plane parallel to a
// cardinal plane.
//
// axis is a 1-based index which specifies the plane to project about:
//
// REFLECT_ABOUT_X => reflect about the plane x=k
// REFLECT_ABOUT_Y => reflect about the plane y=k
// REFLECT_ABOUT_Z => reflect about the plane z=k
//
// The translation is set appropriately, since translation must occur if k != 0
//
// Reflect about x = 0:
//
// | -1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about y = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f -1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about z = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f -1.0f |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_reflect( int axis, float k)
{
// Check which plane they want to reflect about
switch (axis)
{
case REFLECT_ABOUT_X: // Reflect about the plane x=k
m11 = -1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 2.0f * k;
ty = 0.0f;
tz = 0.0f;
break ;
case REFLECT_ABOUT_Y: // Reflect about the plane y=k
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = -1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 0.0f;
ty = 2.0f * k;
tz = 0.0f;
break ;
case REFLECT_ABOUT_Z: // Reflect about the plane z=k
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = -1.0f;
tx = 0.0f;
ty = 0.0f;
tz = 2.0f * k;
break ;
default :
// bogus axis index
assert( false );
}
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about an arbitrary plane
// through the origin. The unit vector n is perpendicular to the plane.
//
// The translation portion is reset.
//
// | 1 - 2x^2 -2xy -2xz |
// P(n) = S(n, -1) = | -2xy 1 - 2y^2 -2yz |
// | -2xz -2zy 1 - 2z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_reflect( const cVector3& n)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(n * n - 1.0f) < 0.01f);
// Compute common subexpressions
float ax = -2.0f * n.x;
float ay = -2.0f * n.y;
float az = -2.0f * n.z;
// Fill in the matrix elements. We'll do the common subexpression optimization ourselves
// here, since diagonally opposite matrix elements are equal.
m11 = 1.0f + ax * n.x;
m22 = 1.0f + ay * n.y;
m33 = 1.0f + az * n.z;
m12 = m21 = ax * n.y;
m13 = m31 = ax * n.z;
m23 = m32 = ay * n.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Transform the point. This makes using the vector class look like it
// does with linear algebra notation on paper.
//
// We also provide a *= operator, as per C convention.
//---------------------------------------------------------------------------
cVector3 operator *( const cVector3& p, const cMatrix4x3& m)
{
// Grind through the linear algebra.
return cVector3(p.x * m.m11 + p.y * m.m21 + p.z * m.m31 + m.tx,
p.x * m.m12 + p.y * m.m22 + p.z * m.m32 + m.ty,
p.x * m.m13 + p.y * m.m23 + p.z * m.m33 + m.tz);
}
cVector3& operator *=(cVector3& p, const cMatrix4x3& m)
{
p = p * m;
return p;
}
//---------------------------------------------------------------------------
// Matrix concatenation. This makes using the vector class look like it
// does with linear algebra notation on paper.
//
// We also provide a *= operator, as per C convention.
//---------------------------------------------------------------------------
cMatrix4x3 operator *( const cMatrix4x3& a, const cMatrix4x3& b)
{
cMatrix4x3 r;
// Compute the upper 3x3 (linear transformation) portion
r.m11 = a.m11 * b.m11 + a.m12 * b.m21 + a.m13 * b.m31;
r.m12 = a.m11 * b.m12 + a.m12 * b.m22 + a.m13 * b.m32;
r.m13 = a.m11 * b.m13 + a.m12 * b.m23 + a.m13 * b.m33;
r.m21 = a.m21 * b.m11 + a.m22 * b.m21 + a.m23 * b.m31;
r.m22 = a.m21 * b.m12 + a.m22 * b.m22 + a.m23 * b.m32;
r.m23 = a.m21 * b.m13 + a.m22 * b.m23 + a.m23 * b.m33;
r.m31 = a.m31 * b.m11 + a.m32 * b.m21 + a.m33 * b.m31;
r.m32 = a.m31 * b.m12 + a.m32 * b.m22 + a.m33 * b.m32;
r.m33 = a.m31 * b.m13 + a.m32 * b.m23 + a.m33 * b.m33;
// Compute the translation portion
r.tx = a.tx * b.m11 + a.ty * b.m21 + a.tz * b.m31 + b.tx;
r.ty = a.tx * b.m12 + a.ty * b.m22 + a.tz * b.m32 + b.ty;
r.tz = a.tx * b.m13 + a.ty * b.m23 + a.tz * b.m33 + b.tz;
// Return it. Ouch - involves a copy constructor call. If speed
// is critical, we may need a seperate function which places the
// result where we want it
return r;
}
cMatrix4x3& operator *=(cMatrix4x3& a, const cMatrix4x3& b)
{
a = a * b;
return a;
}
//---------------------------------------------------------------------------
// Compute the determinant of the 3x3 portion of the matrix.
//---------------------------------------------------------------------------
float determinant( const cMatrix4x3& m)
{
float result = m.m11 * (m.m22 * m.m33 - m.m23 * m.m32) +
m.m12 * (m.m23 * m.m31 - m.m21 * m.m33) +
m.m13 * (m.m21 * m.m32 - m.m22 * m.m31);
return result;
}
//---------------------------------------------------------------------------
// Compute the inverse of a matrix. We use the classical adjoint divided
// by the determinant method.
//---------------------------------------------------------------------------
cMatrix4x3 inverse( const cMatrix4x3& m)
{
float det = determinant(m);
// If we're singular, then the determinant is zero and there's no inverse.
assert(fabs(det) > 0.000001f);
// Compute one over the determinant, so we divide once and can multiply per element.
float one_over_det = 1.0f / det;
// Compute the 3x3 portion of the inverse, by dividing the adjoint by the determinant.
cMatrix4x3 r;
r.m11 = (m.m22 * m.m33 - m.m23 * m.m32) * one_over_det;
r.m12 = (m.m13 * m.m32 - m.m12 * m.m33) * one_over_det;
r.m13 = (m.m12 * m.m23 - m.m13 * m.m22) * one_over_det;
r.m21 = (m.m23 * m.m31 - m.m21 * m.m33) * one_over_det;
r.m22 = (m.m11 * m.m33 - m.m13 * m.m31) * one_over_det;
r.m23 = (m.m13 * m.m21 - m.m11 * m.m23) * one_over_det;
r.m31 = (m.m21 * m.m32 - m.m22 * m.m31) * one_over_det;
r.m32 = (m.m12 * m.m31 - m.m11 * m.m32) * one_over_det;
r.m33 = (m.m11 * m.m22 - m.m12 * m.m21) * one_over_det;
// Compute the translation portion of the inverse
r.tx = -(m.tx * r.m11 + m.ty * r.m21 + m.tz * r.m31);
r.ty = -(m.tx * r.m12 + m.ty * r.m22 + m.tz * r.m32);
r.tz = -(m.tx * r.m13 + m.ty * r.m23 + m.tz * r.m33);
// Return it. Ouch - involves a copy constructor call. If speed is critical,
// we may need a seperate function which places the result where we want it
return r;
}
//---------------------------------------------------------------------------
// Return the translation row of the matrix in vector form
//---------------------------------------------------------------------------
cVector3 get_translation( const cMatrix4x3& m)
{
return cVector3(m.tx, m.ty, m.tz);
}
//---------------------------------------------------------------------------
// Extract the position of an object given a parent -> local transformation
// matrix (such as a world -> object matrix).
//
// We assume that the matrix represents a rigid transformation. (No scale,
// skew, or mirroring).
//---------------------------------------------------------------------------
cVector3 get_position_from_parent_to_local_matrix( const cMatrix4x3& m)
{
// Multiply negative translation value by the transpose of the 3x3 portion.
// By using the transpose, we assume that the matrix is orthogonal.
// (This function doesn't really make sense for non-rigid transformations
)return cVector3(-(m.tx * m.m11 + m.ty * m.m12 + m.tz * m.m13),
-(m.tx * m.m21 + m.ty * m.m22 + m.tz * m.m23),
-(m.tx * m.m31 + m.ty * m.m32 + m.tz * m.m33));
}
//---------------------------------------------------------------------------
// Extract the position of an object given a local -> parent transformation
// matrix (such as an object -> world matrix).
//---------------------------------------------------------------------------
cVector3 get_position_from_local_to_parent_matrix( const cMatrix4x3& m)
{
// Position is simply the translation portion
return cVector3(m.tx, m.ty, m.tz);
}