# lxdfigo/SimpleMath

version 0.2

1 parent d9a1f4d commit d08b393da7a7bbcebdae05ec24ca23da93ac14bc lxdfigo committed
Showing with 213 additions and 241 deletions.
1. SimpleMath.ncb
2. SimpleMath.suo
3. +5 −12 SimpleMath/main.cpp
4. +204 −226 SimpleMath/quat.h
5. +4 −3 SimpleMath/vector.h
BIN SimpleMath.ncb
Binary file not shown.
BIN SimpleMath.suo
Binary file not shown.
17 SimpleMath/main.cpp
 @@ -2,21 +2,14 @@ #include "SimpleMath.h" int main(){ - Vec4f vec(1); - Quatf quat(vec); Mat4x4d mat; + Quatf quat(1,1,0,1.57); + Vec3f vec3; + vec3.Elements[0] = 1; - printf("%f\n",mat[2][1]); + Quatf quat2 = quat * vec3 * quat.inverse(); - vec += Vec4f(); - vec /= 2.; - vec *= 3.; - - - vec = vec - Vec4f(-1); - - vec -= Vec4f(2); - printf("%f %f",vec.Elements[0],vec.Elements[3]); + printf("%f %f %f",quat2.Elements[0],quat2.Elements[1],quat2.Elements[2]); getchar(); return 0;
430 SimpleMath/quat.h
 @@ -5,232 +5,210 @@ template class Quat{ - public: - T Elements[4]; - - inline Quat() { Elements[0]=0.0; Elements[1]=0.0; Elements[2]=0.0; Elements[3]=1.0; } - inline Quat( T x, T y, T z, T w ){ - Elements[0]=x; - Elements[1]=y; - Elements[2]=z; - Elements[3]=w; - } - - inline Quat( const Vec4f& v ) { - Elements[0]=v.Elements[0]; - Elements[1]=v.Elements[1]; - Elements[2]=v.Elements[2]; - Elements[3]=v.Elements[3]; - } - - inline Quat( const Vec4d& v ){ - Elements[0]=v.Elements[0]; - Elements[1]=v.Elements[1]; - Elements[2]=v.Elements[2]; - Elements[3]=v.Elements[3]; - } - - inline Quat( T angle, const Vec3f& axis){ - makeRotate(angle,axis); - } - inline Quat( T angle, const Vec3d& axis){ - makeRotate(angle,axis); - } - - inline Quat( T angle1, const Vec3f& axis1, - T angle2, const Vec3f& axis2, - T angle3, const Vec3f& axis3){ - makeRotate(angle1,axis1,angle2,axis2,angle3,axis3); - } - - inline Quat( T angle1, const Vec3d& axis1, - T angle2, const Vec3d& axis2, - T angle3, const Vec3d& axis3) { - makeRotate(angle1,axis1,angle2,axis2,angle3,axis3); - } - - inline Quat& operator = (const Quat& v) { Elements[0]=v.Elements[0]; Elements[1]=v.Elements[1]; Elements[2]=v.Elements[2]; Elements[3]=v.Elements[3]; return *this; } - - inline bool operator == (const Quat& v) const { return Elements[0]==v.Elements[0] && Elements[1]==v.Elements[1] && Elements[2]==v.Elements[2] && Elements[3]==v.Elements[3]; } - - inline bool operator != (const Quat& v) const { return Elements[0]!=v.Elements[0] || Elements[1]!=v.Elements[1] || Elements[2]!=v.Elements[2] || Elements[3]!=v.Elements[3]; } - - inline bool operator < (const Quat& v) const{ - if (Elements[0]v.Elements[0]) return false; - else if (Elements[1]v.Elements[1]) return false; - else if (Elements[2]v.Elements[2]) return false; - else return (Elements[3]& v ) { + Elements[0]=v.Elements[0]; + Elements[1]=v.Elements[1]; + Elements[2]=v.Elements[2]; + Elements[3]=v.Elements[3]; + } + + inline Quat( T angle, const CVector& axis){ + makeRotate(angle,axis); + } + + inline Quat& operator = (const Quat& v) { Elements[0]=v.Elements[0]; Elements[1]=v.Elements[1]; Elements[2]=v.Elements[2]; Elements[3]=v.Elements[3]; return *this; } + + inline bool operator == (const Quat& v) const { return Elements[0]==v.Elements[0] && Elements[1]==v.Elements[1] && Elements[2]==v.Elements[2] && Elements[3]==v.Elements[3]; } + + inline bool operator != (const Quat& v) const { return Elements[0]!=v.Elements[0] || Elements[1]!=v.Elements[1] || Elements[2]!=v.Elements[2] || Elements[3]!=v.Elements[3]; } + + inline bool operator < (const Quat& v) const{ + if (Elements[0]v.Elements[0]) return false; + else if (Elements[1]v.Elements[1]) return false; + else if (Elements[2]v.Elements[2]) return false; + else return (Elements[3]& rhs) const{ + return *this * Quat(rhs.Elements[0],rhs.Elements[1],rhs.Elements[2],0); + } + + inline Quat& operator*=(const Quat& rhs){ + T x = rhs.Elements[3]*Elements[0] + rhs.Elements[0]*Elements[3] + rhs.Elements[1]*Elements[2] - rhs.Elements[2]*Elements[1]; + T y = rhs.Elements[3]*Elements[1] - rhs.Elements[0]*Elements[2] + rhs.Elements[1]*Elements[3] + rhs.Elements[2]*Elements[0]; + T z = rhs.Elements[3]*Elements[2] + rhs.Elements[0]*Elements[1] - rhs.Elements[1]*Elements[0] + rhs.Elements[2]*Elements[3]; + Elements[3] = rhs.Elements[3]*Elements[3] - rhs.Elements[0]*Elements[0] - rhs.Elements[1]*Elements[1] - rhs.Elements[2]*Elements[2]; + + Elements[2] = z; + Elements[1] = y; + Elements[0] = x; + return (*this); + } + + inline Quat operator / (T rhs) const{ + T div = 1.0/rhs; + return Quat(Elements[0]*div, Elements[1]*div, Elements[2]*div, Elements[3]*div); + } + + inline Quat& operator /= (T rhs){ + T div = 1.0/rhs; + Elements[0]*=div; + Elements[1]*=div; + Elements[2]*=div; + Elements[3]*=div; + return *this; + } + + inline const Quat operator/(const Quat& denom) const { + return ( (*this) * denom.inverse() ); + } + + inline Quat& operator/=(const Quat& denom){ + (*this) = (*this) * denom.inverse(); + return (*this); + } + + inline const Quat operator + (const Quat& rhs) const{ + return Quat(Elements[0]+rhs.Elements[0], Elements[1]+rhs.Elements[1], + Elements[2]+rhs.Elements[2], Elements[3]+rhs.Elements[3]); + } + + inline Quat& operator += (const Quat& rhs) { + Elements[0] += rhs.Elements[0]; + Elements[1] += rhs.Elements[1]; + Elements[2] += rhs.Elements[2]; + Elements[3] += rhs.Elements[3]; + return *this; + } + inline const Quat operator - (const Quat& rhs) const { + return Quat(Elements[0]-rhs.Elements[0], Elements[1]-rhs.Elements[1], + Elements[2]-rhs.Elements[2], Elements[3]-rhs.Elements[3] ); + } + + inline Quat& operator -= (const Quat& rhs){ + Elements[0]-=rhs.Elements[0]; + Elements[1]-=rhs.Elements[1]; + Elements[2]-=rhs.Elements[2]; + Elements[3]-=rhs.Elements[3]; + return *this; + } + + inline const Quat operator - () const{ + return Quat (-Elements[0], -Elements[1], -Elements[2], -Elements[3]); + } + + T Dot(const Quat& rhs) const{ + return rhs.Elements[3]*Elements[3] + rhs.Elements[2]*Elements[2] + rhs.Elements[1]*Elements[1] + rhs.Elements[0]*Elements[0]; + } + + T length() const{ + return sqrt( Elements[0]*Elements[0] + Elements[1]*Elements[1] + Elements[2]*Elements[2] + Elements[3]*Elements[3]); + } + + T length2() const{ + return Elements[0]*Elements[0] + Elements[1]*Elements[1] + Elements[2]*Elements[2] + Elements[3]*Elements[3]; + } + + inline Quat conj () const { + return Quat( -Elements[0], -Elements[1], -Elements[2], Elements[3] ); + } + + inline const Quat inverse () const{ + return conj() / length2(); + } + + void makeRotate (T angle, T x, T y, T z ){ + angle /= 2.; + T sin1 = sin(angle); + T cos1 = cos(angle); + T len = sqrt(x*x + y*y + z*z); + Elements[0] = x * sin1 / len; + Elements[1] = y * sin1 / len; + Elements[2] = z * sin1 / len; + Elements[3] = cos1; + } + void makeRotate ( T angle, const CVector& vec ){ + makeRotate(angle,vec.Elements[0],vec.Elements[1],vec.Elements[2]); + } + void makeRotate( const CVector& vec1, const CVector& vec2 ){ + vec1.Normalize(); + vec2.Normalize(); + T x = vec1.Elements[1]* vec2.Elements[2] - vec1.Elements[2]*vec2.Elements[1]; + T y = vec1.Elements[2]* vec2.Elements[0] - vec1.Elements[0]*vec2.Elements[2]; + T z = vec1.Elements[0]* vec2.Elements[1] - vec1.Elements[1]*vec2.Elements[0]; + makeRotate(acos(vec1.Dot(vec2)),x,y,z); + } + + void getRotate ( T & angle, T & x, T & y, T & z ) const{ + angle = acos(Elements[3]); + T sin1 = sin(angle); + x = Elements[0] / sin1; + y = Elements[1] / sin1; + z = Elements[2] / sin1; + angle *= 2; + } + void getRotate ( T & angle, CVector& vec ) const{ + getRotate(angle,vec.Elements[0],vec.Elements[1],vec.Elements[2]); + } + + void slerp(T t, const Quat& from, const Quat& to){ + T angle = acos(from.Dot(to)); + *this = (from * sin((1-t)*angle) + to * sin(t*angle)) / sin(angle) + } }; typedef Quat Quatf;
7 SimpleMath/vector.h
 @@ -58,6 +58,7 @@ inline CVector& CVector::operator=(const CVector& Vect return *this; } + template inline bool CVector::operator < (const CVector& Vector) const { bool Result = true; @@ -162,7 +163,7 @@ inline T CVector::Length() const{ template T CVector::SquaredDistance(const CVector& Vector) const{ CVector Difference = (*this) - Vector; - return Difference.DotProduct(Difference); + return Difference.Dot(Difference); } @@ -178,13 +179,13 @@ void CVector::UniformRandomHyperSpehere(T rng){ for (int i = 0; i < DIMENSION; i++){ this->Elements[i] = 2.0f * rng - 1.0f; } - }while (this->DotProduct((*this)) > 1.0f); + }while (this->Dot((*this)) > 1.0f); } template void CVector::Normalize(){ - T Length = sqrtf(this->DotProduct(*this)); + T Length = sqrtf(this->Dot(*this)); T Scale = 1.0f / (Length + 1E-20f); for (int i = 0; i < DIMENSION; i++){