mymath.h

liu7788414

//#include "aee.h"

#ifndef _MATH3D_H
#define _MATH3D_H

//#define USE_FLOAT
#define DEBUG_OVERFLOW
#define CRIT_VALUE 32768 //2^15
#define DANGER_VALUE 524288//2^19
#define DANGER_VALUET 4194304 //2^22

#ifdef USE_FLOAT
	#include <math.h>
	typedef float PhType;
	typedef double TriValue;
	typedef float AngDegree;
	#define PHT_EXT 0
	#define TRI_EXT 0
	#define MULT_BTW_TRI_PHT 0
	#define DEGREE_EXT 1
	TriValue DegreesToRadians(PhType deg);
	TriValue RadiansToDegrees(PhType rad);
#else
	typedef long PhType;
	typedef long TriValue;
	typedef long AngDegree;
	#define PHT_EXT 7  //2^7=128
	#define TRI_EXT 14 //2^14=16384
	#define MULT_BTW_TRI_PHT  (TRI_EXT-PHT_EXT)
	#define DEGREE_EXT 10
//	#define PhTypeRef long
	
	TriValue sin(AngDegree degree);
	TriValue sin360(AngDegree degree);
	TriValue cos(AngDegree degree);
	TriValue cos360(AngDegree degree);
	TriValue tan(AngDegree degree);
	TriValue tan360(AngDegree degree);
	AngDegree asin(TriValue value);
	AngDegree acos(TriValue value);
	AngDegree atan(TriValue value);
	AngDegree atan2(PhType y, PhType x);
	PhType sqrt(PhType value);
	PhType abs(PhType value);

//#define PRINT

#endif


class Vector
{
public:
	PhType x;
	PhType y;
	PhType z;

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

	PhType Magnitude(void);
	void  Normalize(void);
	void  Reverse(void);

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

	Vector operator-(void);

};

Vector operator+(Vector u, Vector v);
Vector operator-(Vector u, Vector v);
Vector operator^(Vector u, Vector v);
PhType operator*(Vector u, Vector v);
Vector operator*(PhType s, Vector u);
Vector operator*(Vector u, PhType s);
Vector operator/(Vector u, PhType s);
//PhType TripleScalarProduct(Vector u, Vector v, Vector w);
bool isParallel(Vector l1, Vector l2);
int calcPlumbAmong2L(Vector p1, Vector l1, Vector p2, Vector l2, Vector *p);


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

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

	PhType	det(void);
	Matrix3x3	Transpose(void);
	Matrix3x3	Inverse(void);
	Matrix3x3	InverseR(void);
	PhType		havezero(void){return ((e11==0)||(e12==0)||(e13==0)||(e21==0)||(e22==0)||(e23==0)||(e31==0)||(e32==0)||(e33==0));}

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

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

Vector MulRMV(Matrix3x3 m, Vector v);
Vector MulVRM(Vector v, Matrix3x3 m );


class Quaternion {
public:
	TriValue	n;	// number (scalar) part
	TriValue	x;	// vector part: v.x, v.y, v.z
	TriValue	y;
	TriValue	z;

	Quaternion(void);
	Quaternion(TriValue e0, TriValue e1, TriValue e2, TriValue e3);

	PhType	Magnitude(void);
	void	Normalize(void);
	Vector	GetVector(void);
//	TriValue	GetScalar(void);
	Quaternion	operator+=(Quaternion q);
	Quaternion	operator-=(Quaternion q);
	Quaternion operator*=(PhType s);
	Quaternion operator/=(PhType s);
	Quaternion	operator~(void) const { return Quaternion(n, -x, -y, -z);}
};

Quaternion operator+(Quaternion q1, Quaternion q2);
Quaternion operator-(Quaternion q1, Quaternion q2);
Quaternion operator*(Quaternion q1, Quaternion q2);
Quaternion operator*(Quaternion q, PhType s);
Quaternion operator*(PhType s, Quaternion q);
Quaternion operator*(Quaternion q, Vector v);
Quaternion operator*(Vector v, Quaternion q);
Quaternion operator/(Quaternion q, PhType s);
//PhType QGetAngle(Quaternion q);
//Vector QGetAxis(Quaternion q);
Quaternion QRotate(Quaternion q1, Quaternion q2);
Vector	QVRotate(Quaternion q, Vector v);
Quaternion	MakeQFromEulerAngles(AngDegree x, AngDegree y, AngDegree z);
Vector	MakeEulerAnglesFromQ(Quaternion q);
Matrix3x3 MakeMatrixFromQuaternion(Quaternion q);






#endif