mymath.h

人工智能

#ifndef _MYMATH
#define _MYMATH

#include <math.h>

//------------------------------------------------------------------------//
// Misc. Constants
//------------------------------------------------------------------------//

float	const	pi	= 3.14159265f;
float	const	g	= -32.174f;		// acceleration due to gravity, ft/s^2
float	const	rho = 0.0023769f;	// desity of air at sea level, slugs/ft^3
float	const	tol = 0.000000000000001f;		// float type tolerance 


//------------------------------------------------------------------------//
// Misc. Functions
//------------------------------------------------------------------------//
inline	float	DegreesToRadians(float deg);
inline	float	RadiansToDegrees(float rad);

inline	float	DegreesToRadians(float deg)
{
	return deg * pi / 180.0f;
}

inline	float	RadiansToDegrees(float rad)
{	
	return rad * 180.0f / pi;
}

//------------------------------------------------------------------------//
// Vector Class and vector functions
//------------------------------------------------------------------------//
class Vector {
public:
	float x;
	float y;
	float z;

	Vector(void);
	Vector(float xi, float yi, float zi);

	float Magnitude(void);
	void  Normalize(void);
	void  Reverse(void);

	Vector& operator+=(Vector u);	// vector addition
	Vector& operator-=(Vector u);	// vector subtraction
	Vector& operator*=(float s);	// scalar multiply
	Vector& operator/=(float s);	// scalar divide

	Vector operator-(void);

};

inline	Vector operator+(Vector u, Vector v);
inline	Vector operator-(Vector u, Vector v);
inline	Vector operator^(Vector u, Vector v);
inline	float operator*(Vector u, Vector v);
inline	Vector operator*(float s, Vector u);
inline	Vector operator*(Vector u, float s);
inline	Vector operator/(Vector u, float s);
inline	float TripleScalarProduct(Vector u, Vector v, Vector w);

inline Vector::Vector(void)
{
	x = 0;
	y = 0;
	z = 0;
}

inline Vector::Vector(float xi, float yi, float zi)
{
	x = xi;
	y = yi;
	z = zi;
}

inline	float Vector::Magnitude(void)
{
	return (float) sqrt(x*x + y*y + z*z);
}

inline	void  Vector::Normalize(void)
{
	float m = (float) sqrt(x*x + y*y + z*z);
	if(m <= tol) m = 1;
	x /= m;
	y /= m;
	z /= m;	

	if (fabs(x) < tol) x = 0.0f;
	if (fabs(y) < tol) y = 0.0f;
	if (fabs(z) < tol) z = 0.0f;
}

inline	void  Vector::Reverse(void)
{
	x = -x;
	y = -y;
	z = -z;
}

inline Vector& Vector::operator+=(Vector u)
{
	x += u.x;
	y += u.y;
	z += u.z;
	return *this;
}

inline	Vector& Vector::operator-=(Vector u)
{
	x -= u.x;
	y -= u.y;
	z -= u.z;
	return *this;
}

inline	Vector& Vector::operator*=(float s)
{
	x *= s;
	y *= s;
	z *= s;
	return *this;
}

inline	Vector& Vector::operator/=(float s)
{
	x /= s;
	y /= s;
	z /= s;
	return *this;
}

inline	Vector Vector::operator-(void)
{
	return Vector(-x, -y, -z);
}


inline	Vector operator+(Vector u, Vector v)
{
	return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
}

inline	Vector operator-(Vector u, Vector v)
{
	return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
}

// Vector cross product (u cross v)
inline	Vector operator^(Vector u, Vector v)
{
	return Vector(	u.y*v.z - u.z*v.y,
					-u.x*v.z + u.z*v.x,
					u.x*v.y - u.y*v.x );
}

// Vector dot product
inline	float operator*(Vector u, Vector v)
{
	return (u.x*v.x + u.y*v.y + u.z*v.z);
}

inline	Vector operator*(float s, Vector u)
{
	return Vector(u.x*s, u.y*s, u.z*s);
}

inline	Vector operator*(Vector u, float s)
{
	return Vector(u.x*s, u.y*s, u.z*s);
}

inline	Vector operator/(Vector u, float s)
{
	return Vector(u.x/s, u.y/s, u.z/s);
}

// triple scalar product (u dot (v cross w))
inline	float TripleScalarProduct(Vector u, Vector v, Vector w)
{
	return float(	(u.x * (v.y*w.z - v.z*w.y)) +
					(u.y * (-v.x*w.z + v.z*w.x)) +
					(u.z * (v.x*w.y - v.y*w.x)) );
	//return u*(v^w);

}



//------------------------------------------------------------------------//
// Matrix Class and matrix functions
//------------------------------------------------------------------------//

class Matrix3x3 {
public:
	// elements eij: i -> row, j -> column
	float	e11, e12, e13, e21, e22, e23, e31, e32, e33;	

	Matrix3x3(void);
	Matrix3x3(	float r1c1, float r1c2, float r1c3, 
				float r2c1, float r2c2, float r2c3, 
				float r3c1, float r3c2, float r3c3 );

	float	det(void);
	Matrix3x3	Transpose(void);
	Matrix3x3	Inverse(void);

	Matrix3x3& operator+=(Matrix3x3 m);
	Matrix3x3& operator-=(Matrix3x3 m);
	Matrix3x3& operator*=(float s);
	Matrix3x3& operator/=(float s);
};

inline	Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
inline	Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
inline	Matrix3x3 operator/(Matrix3x3 m, float s);
inline	Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
inline	Matrix3x3 operator*(Matrix3x3 m, float s);
inline	Matrix3x3 operator*(float s, Matrix3x3 m);
inline	Vector operator*(Matrix3x3 m, Vector u);
inline	Vector operator*(Vector u, Matrix3x3 m);





inline	Matrix3x3::Matrix3x3(void)
{
	e11 = 0;
	e12 = 0;
	e13 = 0;
	e21 = 0;
	e22 = 0;
	e23 = 0;
	e31 = 0;
	e32 = 0;
	e33 = 0;
}

inline	Matrix3x3::Matrix3x3(	float r1c1, float r1c2, float r1c3, 
								float r2c1, float r2c2, float r2c3, 
								float r3c1, float r3c2, float r3c3 )
{
	e11 = r1c1;
	e12 = r1c2;
	e13 = r1c3;
	e21 = r2c1;
	e22 = r2c2;
	e23 = r2c3;
	e31 = r3c1;
	e32 = r3c2;
	e33 = r3c3;
}

inline	float	Matrix3x3::det(void)
{
	return	e11*e22*e33 - 
			e11*e32*e23 + 
			e21*e32*e13 - 
			e21*e12*e33 + 
			e31*e12*e23 - 
			e31*e22*e13;	
}

inline	Matrix3x3	Matrix3x3::Transpose(void)
{
	return Matrix3x3(e11,e21,e31,e12,e22,e32,e13,e23,e33);
}

inline	Matrix3x3	Matrix3x3::Inverse(void)
{
	float	d = e11*e22*e33 - 
				e11*e32*e23 + 
				e21*e32*e13 - 
				e21*e12*e33 + 
				e31*e12*e23 - 
				e31*e22*e13;

	if (d == 0) d = 1;

	return	Matrix3x3(	(e22*e33-e23*e32)/d,
						-(e12*e33-e13*e32)/d,
						(e12*e23-e13*e22)/d,
						-(e21*e33-e23*e31)/d,
						(e11*e33-e13*e31)/d,
						-(e11*e23-e13*e21)/d,
						(e21*e32-e22*e31)/d,
						-(e11*e32-e12*e31)/d,
						(e11*e22-e12*e21)/d );	
}

inline	Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
{
	e11 += m.e11;
	e12 += m.e12;
	e13 += m.e13;
	e21 += m.e21;
	e22 += m.e22;
	e23 += m.e23;
	e31 += m.e31;
	e32 += m.e32;
	e33 += m.e33;
	return *this;
}

inline	Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
{
	e11 -= m.e11;
	e12 -= m.e12;
	e13 -= m.e13;
	e21 -= m.e21;
	e22 -= m.e22;
	e23 -= m.e23;
	e31 -= m.e31;
	e32 -= m.e32;
	e33 -= m.e33;
	return *this;
}

inline	Matrix3x3& Matrix3x3::operator*=(float s)
{
	e11 *= s;
	e12 *= s;
	e13 *= s;
	e21 *= s;
	e22 *= s;
	e23 *= s;
	e31 *= s;
	e32 *= s;
	e33 *= s;
	return *this;
}

inline	Matrix3x3& Matrix3x3::operator/=(float s)
{
	e11 /= s;
	e12 /= s;
	e13 /= s;
	e21 /= s;
	e22 /= s;
	e23 /= s;
	e31 /= s;
	e32 /= s;
	e33 /= s;
	return *this;
}

inline	Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
{
	return	Matrix3x3(	m1.e11+m2.e11,
						m1.e12+m2.e12,
						m1.e13+m2.e13,
						m1.e21+m2.e21,
						m1.e22+m2.e22,