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;
}

Point3	mul1(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 RGB
typedef 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