diff options
Diffstat (limited to 'src/math')
-rw-r--r-- | src/math/const.h | 5 | ||||
-rw-r--r-- | src/math/func.h | 25 | ||||
-rw-r--r-- | src/math/matrix.h | 27 | ||||
-rw-r--r-- | src/math/point.h | 84 | ||||
-rw-r--r-- | src/math/test/matrix_test.cpp | 85 | ||||
-rw-r--r-- | src/math/vector.h | 107 |
6 files changed, 276 insertions, 57 deletions
diff --git a/src/math/const.h b/src/math/const.h index 79ad564..4f2549e 100644 --- a/src/math/const.h +++ b/src/math/const.h @@ -29,8 +29,11 @@ namespace Math //! Tolerance level -- minimum accepted float value const float TOLERANCE = 1e-6f; + //! Huge number + const float HUGE = 1.0e+38f; + //! PI - const float PI = 3.14159265358979323846f; + const float PI = 3.14159265358979323846f; //! 2 * PI const float PI_MUL_2 = 6.28318530717958623200f; //! PI / 2 diff --git a/src/math/func.h b/src/math/func.h index 8dcae99..ef90800 100644 --- a/src/math/func.h +++ b/src/math/func.h @@ -21,7 +21,6 @@ #pragma once #include "const.h" -#include "point.h" #include <cmath> #include <cstdlib> @@ -34,13 +33,13 @@ namespace Math /* @{ */ // start of group //! Compares \a a and \a b within \a tolerance -inline bool IsEqual(float a, float b, float tolerance = Math::TOLERANCE) +inline bool IsEqual(float a, float b, float tolerance = TOLERANCE) { return fabs(a - b) < tolerance; } //! Compares \a a to zero within \a tolerance -inline bool IsZero(float a, float tolerance = Math::TOLERANCE) +inline bool IsZero(float a, float tolerance = TOLERANCE) { return IsEqual(a, 0.0f, tolerance); } @@ -117,16 +116,6 @@ inline void Swap(float &a, float &b) b = c; } -//! Permutes two points -inline void Swap(Point &a, Point &b) -{ - Point c; - - c = a; - a = b; - b = c; -} - //! Returns the modulo of a floating point number /** Mod(8.1, 4) = 0.1 Mod(n, 1) = fractional part of n */ @@ -177,6 +166,16 @@ inline float Direction(float a, float g) return g-a; } +//! Returns the angle between point (x,y) and (0,0) +float RotateAngle(float x, float y) +{ + float result = std::atan2(x, y); + if (result < 0) + result = PI_MUL_2 + result; + + return result; +} + //! Returns a random value between 0 and 1. inline float Rand() { diff --git a/src/math/matrix.h b/src/math/matrix.h index f02c1cf..96fb00e 100644 --- a/src/math/matrix.h +++ b/src/math/matrix.h @@ -42,7 +42,8 @@ namespace Math The internal representation is a 16-value table in column-major order, thus: - \verbatim m[0 ] m[4 ] m[8 ] m[12] + \verbatim +m[0 ] m[4 ] m[8 ] m[12] m[1 ] m[5 ] m[9 ] m[13] m[2 ] m[6 ] m[10] m[14] m[3 ] m[7 ] m[11] m[15] \endverbatim @@ -110,14 +111,12 @@ struct Matrix //! Transposes the matrix inline void Transpose() { - Matrix temp = *this; - for (int r = 0; r < 4; ++r) - { - for (int c = 0; c < 4; ++c) - { - m[4*r+c] = temp.m[4*c+r]; - } - } + /* (2,1) <-> (1,2) */ Swap(m[1 ], m[4 ]); + /* (3,1) <-> (1,3) */ Swap(m[2 ], m[8 ]); + /* (4,1) <-> (1,4) */ Swap(m[3 ], m[12]); + /* (3,2) <-> (2,3) */ Swap(m[6 ], m[9 ]); + /* (4,2) <-> (2,4) */ Swap(m[7 ], m[13]); + /* (4,3) <-> (3,4) */ Swap(m[11], m[14]); } //! Calculates the determinant of the matrix @@ -369,7 +368,7 @@ struct Matrix Vector view = at - from; float length = view.Length(); - assert(! Math::IsZero(length) ); + assert(! IsZero(length) ); // Normalize the z basis vector view /= length; @@ -506,7 +505,7 @@ struct Matrix { float cos = cosf(angle); float sin = sinf(angle); - Vector v = Math::Normalize(dir); + Vector v = Normalize(dir); LoadIdentity(); @@ -581,7 +580,7 @@ inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v) float x = v.x * m.m[0 ] + v.y * m.m[4 ] + v.z * m.m[8 ] + m.m[12]; float y = v.x * m.m[1 ] + v.y * m.m[5 ] + v.z * m.m[9 ] + m.m[13]; float z = v.x * m.m[2 ] + v.y * m.m[6 ] + v.z * m.m[10] + m.m[14]; - float w = v.x * m.m[4 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15]; + float w = v.x * m.m[3 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15]; if (IsZero(w)) return Vector(x, y, z); @@ -594,8 +593,8 @@ inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v) } //! Checks if two matrices are equal within given \a tolerance -inline bool MatricesEqual(const Math::Matrix &m1, const Math::Matrix &m2, - float tolerance = Math::TOLERANCE) +inline bool MatricesEqual(const Matrix &m1, const Matrix &m2, + float tolerance = TOLERANCE) { for (int i = 0; i < 16; ++i) { diff --git a/src/math/point.h b/src/math/point.h index fa411d0..8b9dcba 100644 --- a/src/math/point.h +++ b/src/math/point.h @@ -20,6 +20,9 @@ #pragma once +#include "const.h" +#include "func.h" + #include <cmath> @@ -29,18 +32,9 @@ FPOINT RotatePoint(float angle, FPOINT p); FPOINT RotatePoint(float angle, float dist); void RotatePoint(float cx, float cy, float angle, float &px, float &py); - void RotatePoint(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p); - void RotatePoint2(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p); - float RotateAngle(float x, float y); float RotateAngle(FPOINT center, FPOINT p1, FPOINT p2); - float MidPoint(FPOINT a, FPOINT b, float px); - BOOL IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c, FPOINT p); - - BOOL LineFunction(FPOINT p1, FPOINT p2, float &a, float &b); - - float IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c); - + */ // Math module namespace @@ -91,12 +85,82 @@ struct Point } }; +//! Permutes two points +inline void Swap(Point &a, Point &b) +{ + Point c; + + c = a; + a = b; + b = c; +} + //! Returns the distance between two points inline float Distance(const Point &a, const Point &b) { return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y)); } +//! Returns py up on the line \a a - \a b +inline float MidPoint(const Point &a, const Point &b, float px) +{ + if (IsEqual(a.x, b.x)) + { + if (a.y < b.y) + return HUGE; + else + return -HUGE; + } + return (b.y-a.y) * (px-a.x) / (b.x-a.x) + a.y; +} + +//! Calculates the parameters a and b of the linear function passing through \a p1 and \a p2 +/** Returns \c false if the line is vertical. + \param p1,p2 points + \param a,b linear function parameters */ +inline bool LinearFunction(const Point &p1, const Point &p2, float &a, float &b) +{ + if ( IsZero(p1.x-p2.x) ) + { + a = HUGE; + b = p2.x; + return false; + } + + a = (p2.y-p1.y) / (p2.x-p1.x); + b = p2.y - p2.x*a; + + return true; +} + +//! Checks if the point is inside triangle defined by vertices \a a, \a b, \a c +inline bool IsInsideTriangle(Point a, Point b, Point c, const Point &p) +{ + if ( p.x < a.x && p.x < b.x && p.x < c.x ) return false; + if ( p.x > a.x && p.x > b.x && p.x > c.x ) return false; + if ( p.y < a.y && p.y < b.y && p.y < c.y ) return false; + if ( p.y > a.y && p.y > b.y && p.y > c.y ) return false; + + if ( a.x > b.x ) Swap(a,b); + if ( a.x > c.x ) Swap(a,c); + if ( c.x < a.x ) Swap(c,a); + if ( c.x < b.x ) Swap(c,b); + + float n, m; + + n = MidPoint(a, b, p.x); + m = MidPoint(a, c, p.x); + if ( (n>p.y || p.y>m) && (n<p.y || p.y<m) ) + return false; + + n = MidPoint(c, b, p.x); + m = MidPoint(c, a, p.x); + if ( (n>p.y || p.y>m) && (n<p.y || p.y<m) ) + return false; + + return true; +} + /* @} */ // end of group }; // namespace Math diff --git a/src/math/test/matrix_test.cpp b/src/math/test/matrix_test.cpp index 6ca92be..21e02db 100644 --- a/src/math/test/matrix_test.cpp +++ b/src/math/test/matrix_test.cpp @@ -32,6 +32,39 @@ using namespace std; const float TEST_TOLERANCE = 1e-6; +int TestTranspose() +{ + const Math::Matrix mat( + (float[4][4]) + { + { -0.07011674491203920, 1.26145596067429810, 2.09476603598066902, 0.35560176915570696 }, + { -1.34075615966224704, 1.17988499016709314, 0.00601713429241016, -0.75213676977972566 }, + { 0.59186722295223981, 0.88089224074765293, 0.70994467464257294, 0.36730385425340212 }, + { -0.95649396555068111, 0.75912182022565566, 1.34883305778387186, -1.34957997578168754 } + } + ); + + const Math::Matrix expectedTranspose( + (float[4][4]) + { + { -0.07011674491203920, -1.34075615966224704, 0.59186722295223981, -0.95649396555068111 }, + { 1.26145596067429810, 1.17988499016709314, 0.88089224074765293, 0.75912182022565566 }, + { 2.09476603598066902, 0.00601713429241016, 0.70994467464257294, 1.34883305778387186 }, + { 0.35560176915570696, -0.75213676977972566, 0.36730385425340212, -1.34957997578168754 } + } + ); + + Math::Matrix transpose = Math::Transpose(mat); + + if (! Math::MatricesEqual(transpose, expectedTranspose, TEST_TOLERANCE)) + { + fprintf(stderr, "Transpose mismatch!\n"); + return __LINE__; + } + + return 0; +} + int TestCofactor() { const Math::Matrix mat1( @@ -293,15 +326,64 @@ int TestMultiply() return 0; } +int TestMultiplyVector() +{ + const Math::Matrix mat1( + (float[4][4]) + { + { 0.0536517635602049, 0.1350203249258820, -1.4709867280474975, 1.4199163191255975 }, + { 0.4308040094214364, 0.6860887768493787, 0.0555235810428098, 0.0245232625281863 }, + { -0.9570012049134703, 1.4008557175488343, 1.0277555933198543, 1.2311131809078903 }, + { 1.5609168701538376, -0.4917648784647429, 1.3748498152379420, 0.2479075063284996 } + } + ); + + const Math::Vector vec1(0.587443623396385, 0.653347527302101, -0.434049355720428); + + const Math::Vector expectedMultiply1(8.82505163446795, 2.84325886975415, 4.61111014687784); + + Math::Vector multiply1 = Math::MatrixVectorMultiply(mat1, vec1); + if (! Math::VectorsEqual(multiply1, expectedMultiply1, TEST_TOLERANCE ) ) + { + fprintf(stderr, "Multiply vector 1 mismath!\n"); + return __LINE__; + } + + const Math::Matrix mat2( + (float[4][4]) + { + { 1.2078126667092564, 0.5230257362392928, -0.7623036312496848, -1.4192273892400153 }, + { 0.7165407622837081, 1.3746282484390115, -0.8382279333943382, 0.8248340530209490 }, + { -0.9595506321366957, -0.0169226311095793, -0.7271125620609374, -1.5540250647342428 }, + { 1.2788946935793131, 0.1516426145850322, 1.2115324484930112, -0.1584402989052367 } + } + ); + + const Math::Vector vec2(-0.7159607709627414, -0.3163937238507886, 0.0290730716146861); + + const Math::Vector expectedMultiply2(2.274144199387390, 0.135691892790685, 0.812276027335184); + + Math::Vector multiply2 = Math::MatrixVectorMultiply(mat2, vec2); + if (! Math::VectorsEqual(multiply2, expectedMultiply2, TEST_TOLERANCE ) ) + { + fprintf(stderr, "Multiply vector 2 mismath!\n"); + return __LINE__; + } + + return 0; +} + int main() { // Functions to test int (*TESTS[])() = { + TestTranspose, TestCofactor, TestDet, TestInverse, - TestMultiply + TestMultiply, + TestMultiplyVector }; const int TESTS_SIZE = sizeof(TESTS) / sizeof(*TESTS); @@ -317,3 +399,4 @@ int main() return 0; } + diff --git a/src/math/vector.h b/src/math/vector.h index 885b163..455042b 100644 --- a/src/math/vector.h +++ b/src/math/vector.h @@ -28,26 +28,16 @@ /* TODO - -float Length(const D3DVECTOR &a, const D3DVECTOR &b); -float Length2d(const D3DVECTOR &a, const D3DVECTOR &b); -D3DVECTOR Transform(const D3DMATRIX &m, D3DVECTOR p); -D3DVECTOR Projection(const D3DVECTOR &a, const D3DVECTOR &b, const D3DVECTOR &p); - -D3DVECTOR SegmentDist(const D3DVECTOR &p1, const D3DVECTOR &p2, float dist); + +void RotatePoint(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p); +void RotatePoint2(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p); BOOL Intersect(D3DVECTOR a, D3DVECTOR b, D3DVECTOR c, D3DVECTOR d, D3DVECTOR e, D3DVECTOR &i); BOOL IntersectY(D3DVECTOR a, D3DVECTOR b, D3DVECTOR c, D3DVECTOR &p); - D3DVECTOR RotateView(D3DVECTOR center, float angleH, float angleV, float dist); D3DVECTOR LookatPoint( D3DVECTOR eye, float angleH, float angleV, float length ); -void MappingObject( D3DVERTEX2* pVertices, int nb, float scale ); -void SmoothObject( D3DVERTEX2* pVertices, int nb ); - -float DistancePlanPoint(const D3DVECTOR &a, const D3DVECTOR &b, const D3DVECTOR &c, const D3DVECTOR &p); -BOOL IsSamePlane(D3DVECTOR *plan1, D3DVECTOR *plan2); */ // Math module namespace @@ -106,7 +96,7 @@ struct Vector inline void Normalize() { float l = Length(); - if (Math::IsZero(l)) + if (IsZero(l)) return; x /= l; @@ -238,12 +228,93 @@ inline Vector CrossProduct(const Vector &left, const Vector &right) return left.CrossMultiply(right); } +//! Convenience function for calculating angle (in radians) between two vectors +inline float Angle(const Vector &a, const Vector &b) +{ + return a.Angle(b); +} + //! Checks if two vectors are equal within given \a tolerance -inline bool VectorsEqual(const Vector &a, const Vector &b, float tolerance = Math::TOLERANCE) +inline bool VectorsEqual(const Vector &a, const Vector &b, float tolerance = TOLERANCE) +{ + return IsEqual(a.x, b.x, tolerance) + && IsEqual(a.y, b.y, tolerance) + && IsEqual(a.z, b.z, tolerance); +} + +//! Returns the distance between two points +inline float Distance(const Vector &a, const Vector &b) +{ + return std::sqrt( (a.x-b.x)*(a.x-b.x) + + (a.y-b.y)*(a.y-b.y) + + (a.z-b.z)*(a.z-b.z) ); +} + +//! Returns the distance between projections on XZ plane of two vectors +inline float DistanceProjected(const Vector &a, const Vector &b) +{ + return std::sqrt( (a.x-b.x)*(a.x-b.x) + + (a.z-b.z)*(a.z-b.z) ); +} + +//! Returns the normal vector to a plane +/** \param p1,p2,p3 points defining the plane */ +inline Vector NormalToPlane(const Vector &p1, const Vector &p2, const Vector &p3) +{ + Vector u = p3 - p1; + Vector v = p2 - p1; + + return Normalize(CrossProduct(u, v)); +} + +//! Returns the distance between given point and a plane +/** \param p the point + \param a,b,c points defining the plane */ +inline float DistanceToPlane(const Vector &a, const Vector &b, const Vector &c, const Vector &p) +{ + Vector n = NormalToPlane(a, b, c); + float d = -(n.x*a.x + n.y*a.y + n.z*a.z); + + return std::fabs(n.x*p.x + n.y*p.y + n.z*p.z + d); +} + +//! Checks if two planes defined by three points are the same +/** \a plane1 array of three vectors defining the first plane + \a plane2 array of three vectors defining the second plane */ +inline bool IsSamePlane(const Vector (&plane1)[3], const Vector (&plane2)[3]) +{ + Vector n1 = NormalToPlane(plane1[0], plane1[1], plane1[2]); + Vector n2 = NormalToPlane(plane2[0], plane2[1], plane2[2]); + + if ( std::fabs(n1.x-n2.x) > 0.1f || + std::fabs(n1.y-n2.y) > 0.1f || + std::fabs(n1.z-n2.z) > 0.1f ) + return false; + + float dist = DistanceToPlane(plane1[0], plane1[1], plane1[2], plane2[0]); + if ( dist > 0.1f ) + return false; + + return true; +} + +//! Calculates the projection of the point \a p on a straight line \a a to \a b. +/** \a p point to project + \a a,b two ends of the line */ +inline Vector Projection(const Vector &a, const Vector &b, const Vector &p) +{ + float k = DotProduct(b - a, p - a); + k /= DotProduct(b - a, b - a); + + return a + k*(b-a); +} + +//! Returns a point on the line \a p1 - \a p2, in \a dist distance from \a p1 +/** \a p1,p2 line start and end + \a dist scaling factor from \a p1, relative to distance between \a p1 and \a p2 */ +inline Vector SegmentPoint(const Vector &p1, const Vector &p2, float dist) { - return Math::IsEqual(a.x, b.x, tolerance) - && Math::IsEqual(a.y, b.y, tolerance) - && Math::IsEqual(a.z, b.z, tolerance); + return p1 + (p2 - p1) * dist; } /* @} */ // end of group |