Structs continued

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