D3DXMatrixDecompose Sample

//z 2015-09-28 23:59:03 L.94 '57    BG57IV3@BYH T1810273247.K.F4146951713[T38,L836,R30,V798]//z D3DXMatrixDecompose Sample 例子 tutorial#include "Math.h"#include <cassert>void debug(const Matrix &m){printf("\_11 = %10.4f, _12 = %10.4f _13 = %10.4f _14 = %10.4f\n\_21 = %10.4f, _22 = %10.4f _23 = %10.4f _24 = %10.4f\n\_31 = %10.4f, _32 = %10.4f _33 = %10.4f _34 = %10.4f\n\_41 = %10.4f, _42 = %10.4f _43 = %10.4f _44 = %10.4f\n\\n",m._11, m._12, m._13, m._14, m._21, m._22, m._23, m._24, m._31, m._32, m._33, m._34, m._41, m._42, m._43, m._44);}void debug(const Point3 &m){printf("(%6.2f %6.2f %6.2f)\n", m.x, m.y, m.z);}ostream& operator<<(ostream& s, const Vector3& v){return s << "(" << v.x << ", " << v.y << ", " << v.z << ")";}bool affine(const Matrix& m){if (fabs(m._14) > ZERO) return false;if (fabs(m._24) > ZERO) return false;if (fabs(m._34) > ZERO) return false;if (fabs(m._44-1)>ZERO) return false;return true;}float abs(const Vector3 &A){return sqrt(A.x*A.x + A.y*A.y + A.z*A.z);}Vector3 cross(const Vector3 &A, const Vector3 &B){Vector3 C;D3DXVec3Cross(&C, &A, &B);return C;}float dot(const Vector3 &A, const Vector3 &B){return D3DXVec3Dot(&A, &B);}Vector3& normalize(Vector3& A){D3DXVec3Normalize(&A, &A);//cout << "normalized: " << A << endl;return A; ///= length(A);}Vector3 project(const Vector3 &a, const Vector3 &b){return b * (dot(a,b) / dot(b,b));}Vector3 projectOnto(const Vector3 &v, const Vector3 &normal){return v - normal*dot(v, normal);}Vector3 normalized(const Vector3& A){Vector3 result;D3DXVec3Normalize(&result, &A);return result;}Vector3 mul0(const Matrix &m, const Vector3 &v){// Make sure the matrix is affine// assert(affine(m));Vector3 r;D3DXVec3TransformNormal(&r, &v, &m);return r;}Point3mul1(const Matrix &m, const Point3  &v){// Make sure the matrix is affine// assert(affine(m));Vector4 t;D3DXVec3Transform(&t, &v, &m);Vector3 r;memcpy(&r, &t, sizeof(Vector3));return r;}Matrix CreateMatrix(const Point3& position, const Matrix& orientation){Matrix r = orientation;r._41 = position.x;r._42 = position.y;r._43 = position.z;return r;}Matrix CreateMatrix(const Point3& position, float scaleX, float scaleY, float scaleZ){Matrix result(scaleX, 0, 0, 0,0, scaleY, 0, 0,0, 0, scaleZ, 0,position.x, position.y, position.z, 1);return result;}Matrix CreateMatrix(const Point3& position, float s){Matrix result(s, 0, 0, 0,0, s, 0, 0,0, 0, s, 0,position.x, position.y, position.z, 1);return result;}Matrix CreateMatrix(const Point3& position, float angle, const Vector3& axis, float scaleX, float scaleY, float scaleZ){Matrix m1;Matrix m2;Matrix result;D3DXMatrixScaling(&result,scaleX,scaleY,scaleZ);D3DXMatrixRotationAxis(&m2, &axis,angle);D3DXMatrixMultiply(&m1, &result, &m2);D3DXMatrixTranslation(&m2, position.x,position.y,position.z); return *D3DXMatrixMultiply(&result, &m1, &m2);}Matrix CreateMatrix(const Point3& position, const Vector3& up, const Vector3& fw, float uniformScale){Vector3 ap = position;Vector3 az = normalized(fw);      Vector3 ax = normalized(cross(up, az));Vector3 ay = cross(az, ax);Matrix scalingMatrix;D3DXMatrixScaling(&scalingMatrix,uniformScale,uniformScale,uniformScale);return scalingMatrix * Matrix(ax.x, ax.y, ax.z,   0,ay.x, ay.y, ay.z,   0,az.x, az.y, az.z,   0,ap.x, ap.y, ap.z,   1);}Matrix CreateMatrix(float scaleX, float scaleY, float scaleZ){Matrix result;D3DXMatrixScaling(&result,scaleX,scaleY,scaleZ);return result;}Point3 ExtractPosition(const Matrix& matrix, Point3* position){Point3 pos;pos.x = matrix._41;pos.y = matrix._42;pos.z = matrix._43;if (position) *position = pos;return pos;}void ExtractScaling(const Matrix& matrix, float& scaleX, float& scaleY, float& scaleZ){Vector3 scale;Vector3 translation;D3DXQUATERNION rotation;assert(SUCCEEDED(D3DXMatrixDecompose(&scale,&rotation,&translation, &matrix)));scaleX = scale.x;scaleY = scale.y;scaleZ = scale.z;}void ExtractRotation(const Matrix& matrix, Matrix& rotationMatrix){Vector3 scaleOrAxis;Vector3 translation;D3DXQUATERNION rotation;float angle;assert(SUCCEEDED(D3DXMatrixDecompose(&scaleOrAxis,&rotation,&translation, &matrix)));D3DXQuaternionToAxisAngle(&rotation,&scaleOrAxis, &angle);D3DXMatrixRotationAxis(&rotationMatrix, &scaleOrAxis,angle);}void ExtractAngleAxis(const Matrix& matrix, float& angle, Vector3& axis){Vector3 scale;Vector3 translation;D3DXQUATERNION rotation;assert(SUCCEEDED(D3DXMatrixDecompose(&scale,&rotation,&translation, &matrix)));D3DXQuaternionToAxisAngle(&rotation,&axis, &angle);}void Decompose(const Matrix& matrix, Point3& position, Matrix& rotation, float& scaleX, float& scaleY, float& scaleZ){Vector3 scaleOrAxis;D3DXQUATERNION quat;float angle;assert(SUCCEEDED(D3DXMatrixDecompose(&scaleOrAxis,&quat,&position, &matrix)));scaleX = scaleOrAxis.x;scaleY = scaleOrAxis.y;scaleZ = scaleOrAxis.z;D3DXQuaternionToAxisAngle(&quat,&scaleOrAxis, &angle);D3DXMatrixRotationAxis(&rotation, &scaleOrAxis,angle);}Matrix& SetTranslation(Matrix& matrix, const Vector3& translation){matrix._41 = translation.x;matrix._42 = translation.y;matrix._43 = translation.z;return matrix;}Matrix& Translate(Matrix& matrix, const Vector3& translation){matrix._41 += translation.x;matrix._42 += translation.y;matrix._43 += translation.z;return matrix;}Matrix Translated(const Matrix& matrix, const Vector3& translation){Matrix result(matrix);return Translate(result,translation);}Vector3 GetTranslation(const Matrix& matrix){return Vector3(matrix._41, matrix._42, matrix._43);}/*Matrix& Rotate(Matrix& matrix, float yaw, float pitch, float roll){Matrix m2;D3DXMatrixRotationYawPitchRoll(&m2, yaw, pitch, roll);return matrix*=m2;}Matrix Rotated(const Matrix& matrix, float yaw, float pitch, float roll){Matrix result(matrix);Rotate(result,yaw,pitch,roll);return result;}*/Matrix Inverse(const Matrix& m){Matrix r;D3DXMatrixInverse(&r, NULL, &m);return r;}Matrix Scaled(const Matrix& matrix, float x, float y, float z){Matrix result;Matrix scaling;D3DXMatrixScaling(&scaling, x,y,z);return result = scaling*matrix;}

#ifndef MATH_H_#define MATH_H_#include <d3dx9math.h>#include <limits>#include <cfloat>#include <iostream>using namespace std;#define PI D3DX_PI#define DEG2RAD(x) (PI*(x)/180.0)#define RAD2DEG(x) (180.0*(x)/PI)#define ZERO (1e-5)const float INF   = std::numeric_limits<float>::infinity();const float NaN   = std::numeric_limits<float>::quiet_NaN();template <typename T>bool isnan(T x) { return x != x; }typedef D3DXVECTOR3 Vector3;typedef D3DXVECTOR4 Vector4;typedef D3DXVECTOR3 Point3;typedef D3DXVECTOR4 Point4;typedef D3DXMATRIX Matrix;struct Vector2{  float u,v;};#undef RGBtypedef D3DXVECTOR3 RGB;typedef D3DXVECTOR4 RGBA;const Point3 ORIGIN = Point3(0,0,0); const Vector3 X = Vector3(1,0,0); const Vector3 Y = Vector3(0,1,0); const Vector3 Z = Vector3(0,0,1);const RGB RED   = RGB(1,0,0); const RGB GREEN = RGB(0,1,0); const RGB BLUE  = RGB(0,0,1);const RGB WHITE = RGB(1,1,1);const RGB GREY = RGB(0.5,0.5,0.5);/*const RGBA RED   = RGBA(1,0,0,1); const RGBA GREEN = RGBA(0,1,0,1); const RGBA BLUE  = RGBA(0,0,1,1);const RGBA WHITE = RGBA(1,1,1,1);*/const Matrix IDENTITY = Matrix(1,0,0,0,    0,1,0,0,   0,0,1,0,   0,0,0,1);#pragma warning( disable : 4996 4995 ) // disable deprecated warning #include <strsafe.h>void debug(const Matrix &m);void debug(const Point3 &m);ostream& operator<<(ostream& s, const Vector3& v);float abs(const Vector3 &A);Vector3 cross(const Vector3 &A, const Vector3 &B);float dot(const Vector3 &A, const Vector3 &B);/** * @brief normalize a vector in-place * * @param A *      the vector to normalize * @return a reference to the vector - for chaining */Vector3& normalize(Vector3& A);/** * @brief create a normalized version of a vector * * @param A *      the vector to normalize - will not be changed! * @return a new vector that is the normalized ve rsion of A */Vector3 normalized(const Vector3& A);inline float length(const Vector3& v){  return D3DXVec3Length(&v);}Vector3 mul0(const Matrix &m, const Vector3 &v);Point3  mul1(const Matrix &m, const Point3  &v);Vector3 project(const Vector3 &a, const Vector3 &b);Vector3 projectOnto(const Vector3 &v, const Vector3 &normal);bool affine(const Matrix& m);/** * @brief Construct a matrix from position and orientation * * @param position *the translational component of the matrix - default: origin * @param orientation *the rotational component of the matrix as a matrix - default: identity * @return the matrix TR, where T is the transation matrix and R the rotation matrix. This means R is applied first! */Matrix CreateMatrix(const Point3& position=ORIGIN, const Matrix& orientation=IDENTITY);// create tx from a frame of reference (position, up, forward)Matrix CreateMatrix(const Point3& position, const Vector3& up, const Vector3& fw, float uniformScale = 1.0f);/** * @brief Construct a matrix from position, angle, axis and scaling. * * @param position *the translational component of the matrix * @param angle *the angle by which to rotate * @param axis *the axis around which we rotate * @param scaleX *scaling factor in x direction - default: 1 * @param scaleY *scaling factor in y direction - default: 1 * @param scaleZ *scaling factor in z direction - default: 1 * @return the matrix representing the transformation of first applying the scaling,  *then the rotation and finally the translation */Matrix CreateMatrix(const Point3& position, float angle, const Vector3& axis, float scaleX=1, float scaleY=1, float scaleZ=1);Matrix CreateMatrix(const Point3& position, float scaleX, float scaleY, float scaleZ);Matrix CreateMatrix(const Point3& position, float uniformScale);Matrix CreateMatrix(float scaleX, float scaleY, float scaleZ);/** * @brief Extract only the translational component from a transformation matrix. * * @param matrix *the matrix to extract from * @param position *out: the translational component */Point3 ExtractPosition(const Matrix& matrix, Point3* position=NULL);/** * @brief Extract only the scaling component from a transformation matrix. * * @param matrix *the matrix to extract from * @param scaleX *out: the scaling factor in x direction * @param scaleY *out: the scaling factor in y direction * @param scaleZ *out: the scaling factor in z direction */void ExtractScaling(const Matrix& matrix, float& scaleX, float& scaleY, float& scaleZ);/** * @brief Extract only the rotational component as a matrix from a transformation matrix. * * @param matrix *the matrix to extract from * @param rotation *out: the rotational component as a matrix */void ExtractRotation(const Matrix& matrix, Matrix& rotation);/** * @brief Extract only the rotational component as angle/axis pair from a transformation matrix. * * @param matrix *the matrix to extract from * @param angle *out: the angle by which is rotated  * @param axis *out: the axis around which is rotated */void ExtractAngleAxis(const Matrix& matrix, float& angle, Vector3& axis);/** * @brief Decompose a transformation matrix into a position, a rotation matrix and three scale factors * * @param matrix *the matrix to decompose * @param position *out: the translational component * @param rotation *out: the rotational component as a matrix * @param scaleX *out: the scaling factor in x direction * @param scaleY *out: the scaling factor in y direction * @param scaleZ *out: the scaling factor in z direction */void Decompose(const Matrix& matrix, Point3& position, Matrix& rotation, float& scaleX, float& scaleY, float& scaleZ);Matrix& SetTranslation(Matrix& matrix, const Vector3& translation);Matrix& Translate(Matrix& matrix, const Vector3& translation);Matrix  Translated(const Matrix& matrix, const Vector3& translation);Vector3 GetTranslation(const Matrix& matrix);/*Matrix& SetRotation(Matrix& matrix, float yaw, float pitch, float roll);Matrix& Rotate(Matrix& matrix, float yaw, float pitch, float roll);Matrix  Rotated(const Matrix& matrix, float yaw, float pitch, float roll);*/Matrix  Inverse(const Matrix& m);Matrix  Scaled(const Matrix& matrix, float x, float y, float z);#endif