Finished implementation of math functions

9 files changed, 636 insertions, 538 deletions

diff --git a/src/math/all.h b/src/math/all.h
index fb6f7d4..fcbcb19 100644
--- a/src/math/all.h
+++ b/src/math/all.h
@@ -27,5 +27,6 @@
 #include "point.h"
 #include "vector.h"
 #include "matrix.h"
+#include "geometry.h"
 
 /* @} */ // end of group
diff --git a/src/math/const.h b/src/math/const.h
index 4f2549e..8980287 100644
--- a/src/math/const.h
+++ b/src/math/const.h
@@ -29,7 +29,12 @@ namespace Math
   //! Tolerance level -- minimum accepted float value
   const float TOLERANCE = 1e-6f;
 
-  //! Huge number
+  //! Very small number (used in testing/returning some values)
+  const float VERY_SMALL = 1e-6f;
+  //! Very big number (used in testing/returning some values)
+  const float VERY_BIG = 1e6f;
+
+   //! Huge number
   const float HUGE = 1.0e+38f;
 
   //! PI
@@ -50,3 +55,4 @@ namespace Math
 
   /* @} */ // end of group
 }; // namespace Math
+
diff --git a/src/math/func.h b/src/math/func.h
index ef90800..79f43c1 100644
--- a/src/math/func.h
+++ b/src/math/func.h
@@ -1,4 +1,5 @@
 // * This file is part of the COLOBOT source code
+// * Copyright (C) 2001-2008, Daniel ROUX & EPSITEC SA, www.epsitec.ch
 // * Copyright (C) 2012, Polish Portal of Colobot (PPC)
 // *
 // * This program is free software: you can redistribute it and/or modify
@@ -166,16 +167,6 @@ 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/geometry.h b/src/math/geometry.h
new file mode 100644
index 0000000..2d79d8a
--- /dev/null
+++ b/src/math/geometry.h
@@ -0,0 +1,600 @@
+// * This file is part of the COLOBOT source code
+// * Copyright (C) 2001-2008, Daniel ROUX & EPSITEC SA, www.epsitec.ch
+// * Copyright (C) 2012, Polish Portal of Colobot (PPC)
+// *
+// * This program is free software: you can redistribute it and/or modify
+// * it under the terms of the GNU General Public License as published by
+// * the Free Software Foundation, either version 3 of the License, or
+// * (at your option) any later version.
+// *
+// * This program is distributed in the hope that it will be useful,
+// * but WITHOUT ANY WARRANTY; without even the implied warranty of
+// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// * GNU General Public License for more details.
+// *
+// * You should have received a copy of the GNU General Public License
+// * along with this program. If not, see  http://www.gnu.org/licenses/.
+
+/** @defgroup MathGeometryModule math/geometry.h
+   Contains math functions related to 3D geometry calculations,
+   transformations, etc.
+ */
+
+#pragma once
+
+#include "const.h"
+#include "func.h"
+#include "point.h"
+#include "vector.h"
+#include "matrix.h"
+
+#include <cmath>
+#include <cstdlib>
+
+
+// Math module namespace
+namespace Math
+{
+
+/* @{ */ // start of group
+
+
+//! 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;
+}
+
+//! Tests whether the point \a p is inside the triangle (\a a,\a b,\a c)
+inline bool IsInsideTriangle(Point a, Point b, Point c, Point p)
+{
+  float n, m;
+
+  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);
+
+  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;
+}
+
+//! Rotates a point around a center
+/** \a center center of rotation
+    \a angle angle is in radians (positive is counterclockwise (CCW) )
+    \a p the point */
+inline Point RotatePoint(const Point &center, float angle, const Point &p)
+{
+  Point a;
+  a.x = p.x-center.x;
+  a.y = p.y-center.y;
+
+  Point b;
+  b.x = a.x*cosf(angle) - a.y*sinf(angle);
+  b.y = a.x*sinf(angle) + a.y*cosf(angle);
+
+  b.x += center.x;
+  b.y += center.y;
+
+  return b;
+}
+
+//! Rotates a point around the origin (0,0)
+/** \a angle angle in radians (positive is counterclockwise (CCW) )
+    \a p the point */
+inline Point RotatePoint(float angle, const Point &p)
+{
+  float x = p.x*cosf(angle) - p.y*sinf(angle);
+  float y = p.x*sinf(angle) + p.y*cosf(angle);
+
+  return Point(x, y);
+}
+
+//! Rotates a vector (dist, 0).
+/** \a angle angle is in radians (positive is counterclockwise (CCW) )
+    \a dist distance to origin */
+inline Point RotatePoint(float angle, float dist)
+{
+  float x = dist*cosf(angle);
+  float y = dist*sinf(angle);
+
+  return Point(x, y);
+}
+
+//! TODO documentation
+inline void RotatePoint(float cx, float cy, float angle, float &px, float &py)
+{
+  float ax, ay;
+
+  px -= cx;
+  py -= cy;
+
+  ax = px*cosf(angle) - py*sinf(angle);
+  ay = px*sinf(angle) + py*cosf(angle);
+
+  px = cx+ax;
+  py = cy+ay;
+}
+
+//! Rotates a point around a center in space.
+/** \a center center of rotation
+    \a angleH,angleV rotation angles in radians (positive is counterclockwise (CCW) ) )
+    \a p the point
+    \returns the rotated point */
+inline Vector RotatePoint(const Vector &center, float angleH, float angleV, Vector p)
+{
+  p.x -= center.x;
+  p.y -= center.y;
+  p.z -= center.z;
+
+  Vector b;
+  b.x = p.x*cosf(angleH) - p.z*sinf(angleH);
+  b.y = p.z*sinf(angleV) + p.y*cosf(angleV);
+  b.z = p.x*sinf(angleH) + p.z*cosf(angleH);
+
+  return center + b;
+}
+
+//! Rotates a point around a center in space.
+/** \a center center of rotation
+    \a angleH,angleV rotation angles in radians (positive is counterclockwise (CCW) ) )
+    \a p the point
+    \returns the rotated point */
+inline Vector RotatePoint2(const Vector center, float angleH, float angleV, Vector p)
+{
+  p.x -= center.x;
+  p.y -= center.y;
+  p.z -= center.z;
+
+  Vector a;
+  a.x = p.x*cosf(angleH) - p.z*sinf(angleH);
+  a.y = p.y;
+  a.z = p.x*sinf(angleH) + p.z*cosf(angleH);
+
+  Vector b;
+  b.x = a.x;
+  b.y = a.z*sinf(angleV) + a.y*cosf(angleV);
+  b.z = a.z*cosf(angleV) - a.y*sinf(angleV);
+
+  return center + b;
+}
+
+//! 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;
+}
+
+/*inline float RotateAngle(float x, float y)
+{
+  if ( x == 0.0f && y == 0.0f )  return 0.0f;
+
+  if ( x >= 0.0f )
+  {
+    if ( y >= 0.0f )
+    {
+      if ( x > y )  return atanf(y/x);
+      else          return Math::PI*0.5f - atanf(x/y);
+    }
+    else
+    {
+      if ( x > -y )  return Math::PI*2.0f + atanf(y/x);
+      else           return Math::PI*1.5f - atanf(x/y);
+    }
+  }
+  else
+  {
+    if ( y >= 0.0f )
+    {
+      if ( -x > y )  return Math::PI*1.0f + atanf(y/x);
+      else           return Math::PI*0.5f - atanf(x/y);
+    }
+    else
+    {
+      if ( -x > -y )  return Math::PI*1.0f + atanf(y/x);
+      else            return Math::PI*1.5f - atanf(x/y);
+    }
+  }
+}*/
+
+//! Calculates the angle between two points and one center
+/** \a center the center point
+    \a p1,p2 the two points
+    \returns The angle in radians  (positive is counterclockwise (CCW) ) */
+inline float RotateAngle(const Point &center, const Point &p1, const Point &p2)
+{
+  if (PointsEqual(p1, center))
+    return 0;
+
+  if (PointsEqual(p2, center))
+    return 0;
+
+  float a1 = asinf((p1.y - center.y) / Distance(p1, center));
+  float a2 = asinf((p2.y - center.y) / Distance(p2, center));
+
+  if (p1.x < center.x) a1 = PI - a1;
+  if (p2.x < center.x) a2 = PI - a2;
+
+  float a = a2 - a1;
+  if (a < 0)
+    a += PI_MUL_2;
+
+  return a;
+}
+
+//! Loads view matrix from the given vectors
+/** \a from origin
+    \a at view direction
+    \a worldUp up vector */
+inline void LoadViewMatrix(Matrix &mat, const Vector &from, const Vector &at, const Vector &worldUp)
+{
+  // Get the z basis vector, which points straight ahead. This is the
+  // difference from the eyepoint to the lookat point.
+  Vector view = at - from;
+
+  float length = view.Length();
+  assert(! Math::IsZero(length) );
+
+  // Normalize the z basis vector
+  view /= length;
+
+  // Get the dot product, and calculate the projection of the z basis
+  // vector onto the up vector. The projection is the y basis vector.
+  float dotProduct = DotProduct(worldUp, view);
+
+  Vector up = worldUp - dotProduct * view;
+
+  // If this vector has near-zero length because the input specified a
+  // bogus up vector, let's try a default up vector
+  if ( IsZero(length = up.Length()) )
+  {
+    up = Vector(0.0f, 1.0f, 0.0f) - view.y * view;
+
+    // If we still have near-zero length, resort to a different axis.
+    if ( IsZero(length = up.Length()) )
+    {
+      up = Vector(0.0f, 0.0f, 1.0f) - view.z * view;
+
+      assert(! IsZero(up.Length()) );
+    }
+  }
+
+  // Normalize the y basis vector
+  up /= length;
+
+  // The x basis vector is found simply with the cross product of the y
+  // and z basis vectors
+  Vector right = CrossProduct(up, view);
+
+  // Start building the matrix. The first three rows contains the basis
+  // vectors used to rotate the view to point at the lookat point
+  mat.LoadIdentity();
+
+  /* (1,1) */ mat.m[0 ] = right.x;
+  /* (2,1) */ mat.m[1 ] = up.x;
+  /* (3,1) */ mat.m[2 ] = view.x;
+  /* (1,2) */ mat.m[4 ] = right.y;
+  /* (2,2) */ mat.m[5 ] = up.y;
+  /* (3,2) */ mat.m[6 ] = view.y;
+  /* (1,3) */ mat.m[8 ] = right.z;
+  /* (2,3) */ mat.m[9 ] = up.z;
+  /* (3,3) */ mat.m[10] = view.z;
+
+  // Do the translation values (rotations are still about the eyepoint)
+  /* (1,4) */ mat.m[12] = -DotProduct(from, right);
+  /* (2,4) */ mat.m[13] = -DotProduct(from, up);
+  /* (3,4) */ mat.m[14] = -DotProduct(from, view);
+}
+
+//! Loads a perspective projection matrix
+/** \a fov field of view in radians
+    \a aspect aspect ratio (width / height)
+    \a nearPlane distance to near cut plane
+    \a farPlane distance to far cut plane */
+inline void LoadProjectionMatrix(Matrix &mat, float fov = 1.570795f, float aspect = 1.0f,
+                                 float nearPlane = 1.0f, float farPlane = 1000.0f)
+{
+  assert(fabs(farPlane - nearPlane) >= 0.01f);
+  assert(fabs(sin(fov / 2)) >= 0.01f);
+
+  float w = aspect * (cosf(fov / 2) / sinf(fov / 2));
+  float h = 1.0f   * (cosf(fov / 2) / sinf(fov / 2));
+  float q = farPlane / (farPlane - nearPlane);
+
+  mat.LoadZero();
+
+  /* (1,1) */ mat.m[0 ] = w;
+  /* (2,2) */ mat.m[5 ] = h;
+  /* (3,3) */ mat.m[10] = q;
+  /* (3,4) */ mat.m[14] = 1.0f;
+  /* (4,3) */ mat.m[11] = -q * nearPlane;
+}
+
+//! Loads a translation matrix from given vector
+/** \a trans vector of translation*/
+inline void LoadTranslationMatrix(Matrix &mat, const Vector &trans)
+{
+  mat.LoadIdentity();
+  /* (1,4) */ mat.m[12] = trans.x;
+  /* (2,4) */ mat.m[13] = trans.y;
+  /* (3,4) */ mat.m[14] = trans.z;
+}
+
+//! Loads a scaling matrix fom given vector
+/** \a scale vector with scaling factors for X, Y, Z */
+inline void LoadScaleMatix(Matrix &mat, const Vector &scale)
+{
+  mat.LoadIdentity();
+  /* (1,1) */ mat.m[0 ] = scale.x;
+  /* (2,2) */ mat.m[5 ] = scale.y;
+  /* (3,3) */ mat.m[10] = scale.z;
+}
+
+//! Loads a rotation matrix along the X axis
+/** \a angle angle in radians */
+inline void LoadRotationXMatrix(Matrix &mat, float angle)
+{
+  mat.LoadIdentity();
+  /* (2,2) */ mat.m[5 ] =  cosf(angle);
+  /* (3,2) */ mat.m[6 ] =  sinf(angle);
+  /* (2,3) */ mat.m[9 ] = -sinf(angle);
+  /* (3,3) */ mat.m[10] =  cosf(angle);
+}
+
+//! Loads a rotation matrix along the Y axis
+/** \a angle angle in radians */
+inline void LoadRotationYMatrix(Matrix &mat, float angle)
+{
+  mat.LoadIdentity();
+  /* (1,1) */ mat.m[0 ] =  cosf(angle);
+  /* (3,1) */ mat.m[2 ] = -sinf(angle);
+  /* (1,3) */ mat.m[8 ] =  sinf(angle);
+  /* (3,3) */ mat.m[10] =  cosf(angle);
+}
+
+//! Loads a rotation matrix along the Z axis
+/** \a angle angle in radians */
+inline void LoadRotationZMatrix(Matrix &mat, float angle)
+{
+  mat.LoadIdentity();
+  /* (1,1) */ mat.m[0 ] =  cosf(angle);
+  /* (2,1) */ mat.m[1 ] =  sinf(angle);
+  /* (1,2) */ mat.m[4 ] = -sinf(angle);
+  /* (2,2) */ mat.m[5 ] =  cosf(angle);
+}
+
+//! Loads a rotation matrix along the given axis
+/** \a dir axis of rotation
+    \a angle angle in radians */
+inline void LoadRotationMatrix(Matrix &mat, const Vector &dir, float angle)
+{
+  float cos = cosf(angle);
+  float sin = sinf(angle);
+  Vector v = Math::Normalize(dir);
+
+  mat.LoadIdentity();
+
+  /* (1,1) */ mat.m[0 ] = (v.x * v.x) * (1.0f - cos) + cos;
+  /* (2,1) */ mat.m[1 ] = (v.x * v.y) * (1.0f - cos) - (v.z * sin);
+  /* (3,1) */ mat.m[2 ] = (v.x * v.z) * (1.0f - cos) + (v.y * sin);
+
+  /* (1,2) */ mat.m[4 ] = (v.y * v.x) * (1.0f - cos) + (v.z * sin);
+  /* (2,2) */ mat.m[5 ] = (v.y * v.y) * (1.0f - cos) + cos ;
+  /* (3,2) */ mat.m[6 ] = (v.y * v.z) * (1.0f - cos) - (v.x * sin);
+
+  /* (1,3) */ mat.m[8 ] = (v.z * v.x) * (1.0f - cos) - (v.y * sin);
+  /* (2,3) */ mat.m[9 ] = (v.z * v.y) * (1.0f - cos) + (v.x * sin);
+  /* (3,3) */ mat.m[10] = (v.z * v.z) * (1.0f - cos) + cos;
+}
+
+//! Calculates the matrix to make three rotations in the order X, Z and Y
+inline void LoadRotationXZYMatrix(Matrix &mat, const Vector &angle)
+{
+  LoadRotationXMatrix(mat, angle.x);
+
+  Matrix temp;
+  LoadRotationZMatrix(temp, angle.z);
+  mat.Multiply(temp);
+
+  LoadRotationYMatrix(temp, angle.y);
+  mat.Multiply(temp);
+}
+
+//! Calculates the matrix to make three rotations in the order Z, X and Y
+inline void LoadRotationZXYMatrix(Matrix &mat, const Vector &angle)
+{
+  LoadRotationZMatrix(mat, angle.z);
+
+  Matrix temp;
+  LoadRotationXMatrix(temp, angle.x);
+  mat.Multiply(temp);
+
+  LoadRotationYMatrix(temp, angle.y);
+  mat.Multiply(temp);
+}
+
+//! Returns the distance between projections on XZ plane of two vectors
+inline float DistanceProjected(const Vector &a, const Vector &b)
+{
+  return sqrtf( (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 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 p1 + (p2 - p1) * dist;
+}
+
+//! 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 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 ( fabs(n1.x-n2.x) > 0.1f ||
+       fabs(n1.y-n2.y) > 0.1f ||
+       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 intersection "i" right "of" the plane "abc".
+inline bool Intersect(const Vector &a, const Vector &b, const Vector &c, const Vector &d, const Vector &e, Vector &i)
+{
+  float d1 = (d.x-a.x)*((b.y-a.y)*(c.z-a.z)-(c.y-a.y)*(b.z-a.z)) -
+     (d.y-a.y)*((b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z)) +
+     (d.z-a.z)*((b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y));
+
+  float d2 = (d.x-e.x)*((b.y-a.y)*(c.z-a.z)-(c.y-a.y)*(b.z-a.z)) -
+     (d.y-e.y)*((b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z)) +
+     (d.z-e.z)*((b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y));
+
+  if (d2 == 0)
+    return false;
+
+  i.x = d.x + d1/d2*(e.x-d.x);
+  i.y = d.y + d1/d2*(e.y-d.y);
+  i.z = d.z + d1/d2*(e.z-d.z);
+
+  return true;
+}
+
+//! Calculates the intersection of the straight line passing through p (x, z)
+/** Line is parallel to the y axis, with the plane abc. Returns p.y. */
+inline bool IntersectY(const Vector &a, const Vector &b, const Vector &c, Vector &p)
+{
+  float d  = (b.x-a.x)*(c.z-a.z) - (c.x-a.x)*(b.z-a.z);
+  float d1 = (p.x-a.x)*(c.z-a.z) - (c.x-a.x)*(p.z-a.z);
+  float d2 = (b.x-a.x)*(p.z-a.z) - (p.x-a.x)*(b.z-a.z);
+
+  if (d == 0.0f)
+    return false;
+
+  p.y = a.y + d1/d*(b.y-a.y) + d2/d*(c.y-a.y);
+
+  return true;
+}
+
+//! Calculates the end point
+inline Vector LookatPoint(const Vector &eye, float angleH, float angleV, float length)
+{
+
+  Vector lookat = eye;
+  lookat.z += length;
+
+  RotatePoint(eye, angleH, angleV, lookat);
+
+  return lookat;
+}
+
+//! TODO documentation
+inline Vector Transform(const Matrix &m, const Vector &p)
+{
+  return MatrixVectorMultiply(m, p);
+}
+
+//! 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);
+}
+
+//! Calculates point of view to look at a center two angles and a distance
+inline Vector RotateView(Vector center, float angleH, float angleV, float dist)
+{
+  Matrix mat1, mat2;
+  LoadRotationZMatrix(mat1, -angleV);
+  LoadRotationYMatrix(mat2, -angleH);
+
+  Matrix mat = MultiplyMatrices(mat1, mat2);
+
+  Vector eye;
+  eye.x = 0.0f+dist;
+  eye.y = 0.0f;
+  eye.z = 0.0f;
+  eye = Transform(mat, eye);
+
+  return eye+center;
+}
+
+/* @} */ // end of group
+
+}; // namespace Math
diff --git a/src/math/matrix.h b/src/math/matrix.h
index 3b1be01..5b098d7 100644
--- a/src/math/matrix.h
+++ b/src/math/matrix.h
@@ -356,198 +356,6 @@ struct Matrix
 
     return Matrix(result);
   }
-
-  //! Loads view matrix from the given vectors
-  /** \a from origin
-      \a at view direction
-      \a worldUp up vector */
-  inline void LoadView(const Vector &from, const Vector &at, const Vector &worldUp)
-  {
-    // Get the z basis vector, which points straight ahead. This is the
-    // difference from the eyepoint to the lookat point.
-    Vector view = at - from;
-
-    float length = view.Length();
-    assert(! IsZero(length) );
-
-    // Normalize the z basis vector
-    view /= length;
-
-    // Get the dot product, and calculate the projection of the z basis
-    // vector onto the up vector. The projection is the y basis vector.
-    float dotProduct = DotProduct(worldUp, view);
-
-    Vector up = worldUp - dotProduct * view;
-
-    // If this vector has near-zero length because the input specified a
-    // bogus up vector, let's try a default up vector
-    if ( IsZero(length = up.Length()) )
-    {
-      up = Vector(0.0f, 1.0f, 0.0f) - view.y * view;
-
-      // If we still have near-zero length, resort to a different axis.
-      if ( IsZero(length = up.Length()) )
-      {
-        up = Vector(0.0f, 0.0f, 1.0f) - view.z * view;
-
-        assert(! IsZero(up.Length()) );
-      }
-    }
-
-    // Normalize the y basis vector
-    up /= length;
-
-    // The x basis vector is found simply with the cross product of the y
-    // and z basis vectors
-    Vector right = CrossProduct(up, view);
-
-    // Start building the matrix. The first three rows contains the basis
-    // vectors used to rotate the view to point at the lookat point
-    LoadIdentity();
-
-    /* (1,1) */ m[0 ] = right.x;
-    /* (2,1) */ m[1 ] = up.x;
-    /* (3,1) */ m[2 ] = view.x;
-    /* (1,2) */ m[4 ] = right.y;
-    /* (2,2) */ m[5 ] = up.y;
-    /* (3,2) */ m[6 ] = view.y;
-    /* (1,3) */ m[8 ] = right.z;
-    /* (2,3) */ m[9 ] = up.z;
-    /* (3,3) */ m[10] = view.z;
-
-    // Do the translation values (rotations are still about the eyepoint)
-    /* (1,4) */ m[12] = -DotProduct(from, right);
-    /* (2,4) */ m[13] = -DotProduct(from, up);
-    /* (3,4) */ m[14] = -DotProduct(from, view);
-  }
-
-  //! Loads a perspective projection matrix
-  /** \a fov field of view in radians
-      \a aspect aspect ratio (width / height)
-      \a nearPlane distance to near cut plane
-      \a farPlane distance to far cut plane */
-  inline void LoadProjection(float fov = 1.570795f, float aspect = 1.0f,
-                             float nearPlane = 1.0f, float farPlane = 1000.0f)
-  {
-    assert(fabs(farPlane - nearPlane) >= 0.01f);
-    assert(fabs(sin(fov / 2)) >= 0.01f);
-
-    float w = aspect * (cosf(fov / 2) / sinf(fov / 2));
-    float h = 1.0f   * (cosf(fov / 2) / sinf(fov / 2));
-    float q = farPlane / (farPlane - nearPlane);
-
-    LoadZero();
-
-    /* (1,1) */ m[0 ] = w;
-    /* (2,2) */ m[5 ] = h;
-    /* (3,3) */ m[10] = q;
-    /* (3,4) */ m[14] = 1.0f;
-    /* (4,3) */ m[11] = -q * nearPlane;
-  }
-
-  //! Loads a translation matrix from given vector
-  /** \a trans vector of translation*/
-  inline void LoadTranslation(const Vector &trans)
-  {
-    LoadIdentity();
-    /* (1,4) */ m[12] = trans.x;
-    /* (2,4) */ m[13] = trans.y;
-    /* (3,4) */ m[14] = trans.z;
-  }
-
-  //! Loads a scaling matrix fom given vector
-  /** \a scale vector with scaling factors for X, Y, Z */
-  inline void LoadScale(const Vector &scale)
-  {
-    LoadIdentity();
-    /* (1,1) */ m[0 ] = scale.x;
-    /* (2,2) */ m[5 ] = scale.y;
-    /* (3,3) */ m[10] = scale.z;
-  }
-
-  //! Loads a rotation matrix along the X axis
-  /** \a angle angle in radians */
-  inline void LoadRotationX(float angle)
-  {
-    LoadIdentity();
-    /* (2,2) */ m[5 ] =  cosf(angle);
-    /* (3,2) */ m[6 ] =  sinf(angle);
-    /* (2,3) */ m[9 ] = -sinf(angle);
-    /* (3,3) */ m[10] =  cosf(angle);
-  }
-
-  //! Loads a rotation matrix along the Y axis
-  /** \a angle angle in radians */
-  inline void LoadRotationY(float angle)
-  {
-    LoadIdentity();
-    /* (1,1) */ m[0 ] =  cosf(angle);
-    /* (3,1) */ m[2 ] = -sinf(angle);
-    /* (1,3) */ m[8 ] =  sinf(angle);
-    /* (3,3) */ m[10] =  cosf(angle);
-  }
-
-  //! Loads a rotation matrix along the Z axis
-  /** \a angle angle in radians */
-  inline void LoadRotationZ(float angle)
-  {
-    LoadIdentity();
-    /* (1,1) */ m[0 ] =  cosf(angle);
-    /* (2,1) */ m[1 ] =  sinf(angle);
-    /* (1,2) */ m[4 ] = -sinf(angle);
-    /* (2,2) */ m[5 ] =  cosf(angle);
-  }
-
-  //! Loads a rotation matrix along the given axis
-  /** \a dir axis of rotation
-      \a angle angle in radians */
-  inline void LoadRotation(const Vector &dir, float angle)
-  {
-    float cos = cosf(angle);
-    float sin = sinf(angle);
-    Vector v = Normalize(dir);
-
-    LoadIdentity();
-
-    /* (1,1) */ m[0 ] = (v.x * v.x) * (1.0f - cos) + cos;
-    /* (2,1) */ m[1 ] = (v.x * v.y) * (1.0f - cos) - (v.z * sin);
-    /* (3,1) */ m[2 ] = (v.x * v.z) * (1.0f - cos) + (v.y * sin);
-
-    /* (1,2) */ m[4 ] = (v.y * v.x) * (1.0f - cos) + (v.z * sin);
-    /* (2,2) */ m[5 ] = (v.y * v.y) * (1.0f - cos) + cos ;
-    /* (3,2) */ m[6 ] = (v.y * v.z) * (1.0f - cos) - (v.x * sin);
-
-    /* (1,3) */ m[8 ] = (v.z * v.x) * (1.0f - cos) - (v.y * sin);
-    /* (2,3) */ m[9 ] = (v.z * v.y) * (1.0f - cos) + (v.x * sin);
-    /* (3,3) */ m[10] = (v.z * v.z) * (1.0f - cos) + cos;
-  }
-
-  //! Calculates the matrix to make three rotations in the order X, Z and Y
-  inline void RotateXZY(const Vector &angle)
-  {
-    this->LoadRotationX(angle.x);
-
-    Matrix temp;
-    temp.LoadRotationZ(angle.z);
-    this->Multiply(temp);
-
-    temp.LoadRotationY(angle.y);
-    this->Multiply(temp);
-  }
-
-  //! Calculates the matrix to make three rotations in the order Z, X and Y
-  inline void RotateZXY(const Vector &angle)
-  {
-    this->LoadRotationZ(angle.z);
-
-    Matrix temp;
-    temp.LoadRotationX(angle.x);
-    this->Multiply(temp);
-
-    temp.LoadRotationY(angle.y);
-    this->Multiply(temp);
-  }
-
 }; // struct Matrix
 
 //! Checks if two matrices are equal within given \a tolerance
@@ -589,11 +397,15 @@ inline Matrix MultiplyMatrices(const Matrix &left, const Matrix &right)
 
     The result, a 4x1 vector is then converted to 3x1 by dividing
     x,y,z coords by the fourth coord (w). */
-inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v)
+inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v, bool wDivide = false)
 {
   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];
+
+  if (!wDivide)
+    return Vector(x, y, z);
+
   float w = v.x * m.m[3 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15];
 
   if (IsZero(w))
@@ -606,24 +418,6 @@ inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v)
   return Vector(x, y, z);
 }
 
-//! Calculation point of view to look at a center two angles and a distance
-inline Vector RotateView(const Vector &center, float angleH, float angleV, float dist)
-{
-  Matrix mat1, mat2, mat;
-
-  mat1.LoadRotationZ(-angleV);
-  mat2.LoadRotationY(-angleH);
-  mat = MultiplyMatrices(mat1, mat2);
-
-  Vector eye;
-  eye.x = 0.0f+dist;
-  eye.y = 0.0f;
-  eye.z = 0.0f;
-  eye = MatrixVectorMultiply(mat, eye);
-
-  return eye + center;
-}
-
 /* @} */ // end of group
 
 }; // namespace Math
diff --git a/src/math/point.h b/src/math/point.h
index 7000138..fdb3051 100644
--- a/src/math/point.h
+++ b/src/math/point.h
@@ -70,7 +70,7 @@ struct Point
   //! Returns the distance from (0,0) to the point (x,y)
   inline float Length()
   {
-    return sqrt(x*x + y*y);
+    return sqrtf(x*x + y*y);
   }
 
     //! Returns the inverted point
@@ -163,134 +163,7 @@ inline void Swap(Point &a, Point &b)
 //! 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;
-}
-
-//! Rotates a point around a center
-/** \a center center of rotation
-    \a angle angle is in radians (positive is counterclockwise (CCW) )
-    \a p the point */
-inline Point RotatePoint(const Point &center, float angle, const Point &p)
-{
-  Point a;
-  a.x = p.x-center.x;
-  a.y = p.y-center.y;
-
-  Point b;
-  b.x = a.x*cosf(angle) - a.y*sinf(angle);
-  b.y = a.x*sinf(angle) + a.y*cosf(angle);
-
-  b.x += center.x;
-  b.y += center.y;
-
-  return b;
-}
-
-//! Rotates a point around the origin (0,0)
-/** \a angle angle in radians (positive is counterclockwise (CCW) )
-    \a p the point */
-inline Point RotatePoint(float angle, const Point &p)
-{
-  float x = p.x*cosf(angle) - p.y*sinf(angle);
-  float y = p.x*sinf(angle) + p.y*cosf(angle);
-
-  return Point(x, y);
-}
-
-//! Rotates a vector (dist, 0).
-/** \a angle angle is in radians (positive is counterclockwise (CCW) )
-    \a dist distance to origin */
-inline Point RotatePoint(float angle, float dist)
-{
-  float x = dist*cosf(angle);
-  float y = dist*sinf(angle);
-
-  return Point(x, y);
-}
-
-//! Calculates the angle between two points and one center
-/** \a center the center point
-    \a p1,p2 the two points
-    \returns The angle in radians  (positive is counterclockwise (CCW) ) */
-inline float RotateAngle(const Point &center, const Point &p1, const Point &p2)
-{
-  if (PointsEqual(p1, center))
-    return 0;
-
-  if (PointsEqual(p2, center))
-    return 0;
-
-  float a1 = asinf((p1.y - center.y) / Distance(p1, center));
-  float a2 = asinf((p2.y - center.y) / Distance(p2, center));
-
-  if (p1.x < center.x) a1 = PI - a1;
-  if (p2.x < center.x) a2 = PI - a2;
-
-  float a = a2 - a1;
-  if (a < 0)
-    a += PI_MUL_2;
-
-  return a;
+  return sqrtf((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
 }
 
 /* @} */ // end of group
diff --git a/src/math/test/matrix_test.cpp b/src/math/test/matrix_test.cpp
index 21e02db..95f1ed7 100644
--- a/src/math/test/matrix_test.cpp
+++ b/src/math/test/matrix_test.cpp
@@ -331,18 +331,18 @@ 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 }
+      {  0.188562846910008, -0.015148651460679,  0.394512304108827,  0.906910631257135 },
+      { -0.297506779519667,  0.940119328178913,  0.970957796752517,  0.310559318965526 },
+      { -0.819770525290873, -2.316574438778879,  0.155756069319732, -0.855661405742964 },
+      {  0.000000000000000,  0.000000000000000,  0.000000000000000,  1.000000000000000 }
     }
   );
 
-  const Math::Vector vec1(0.587443623396385, 0.653347527302101, -0.434049355720428);
+  const Math::Vector vec1(-0.824708565156661, -1.598287748103842, -0.422498044734181);
 
-  const Math::Vector expectedMultiply1(8.82505163446795, 2.84325886975415, 4.61111014687784);
+  const Math::Vector expectedMultiply1(0.608932463260470, -1.356893266403749, 3.457156276255142);
 
-  Math::Vector multiply1 = Math::MatrixVectorMultiply(mat1, vec1);
+  Math::Vector multiply1 = Math::MatrixVectorMultiply(mat1, vec1, false);
   if (! Math::VectorsEqual(multiply1, expectedMultiply1, TEST_TOLERANCE ) )
   {
     fprintf(stderr, "Multiply vector 1 mismath!\n");
@@ -352,18 +352,18 @@ int TestMultiplyVector()
   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 }
+      { -0.63287117038834284,  0.55148060401816856, -0.02042395559467368, -1.50367083897656850 },
+      {  0.69629042156335297,  0.12982747869796774, -1.16250029235919405,  1.19084447253756909 },
+      {  0.44164132914357224, -0.15169304045662041, -0.00880583574621390, -0.55817802940035310 },
+      {  0.95680476533530789, -1.51912346889253125, -0.74209769406615944, -0.20938988867903682 }
     }
   );
 
-  const Math::Vector vec2(-0.7159607709627414, -0.3163937238507886, 0.0290730716146861);
+  const Math::Vector vec2(0.330987381051962, 1.494375516393466, 1.483422335561857);
 
-  const Math::Vector expectedMultiply2(2.274144199387390, 0.135691892790685, 0.812276027335184);
+  const Math::Vector expectedMultiply2(0.2816820577317669, 0.0334468811767428, 0.1996974284970455);
 
-  Math::Vector multiply2 = Math::MatrixVectorMultiply(mat2, vec2);
+  Math::Vector multiply2 = Math::MatrixVectorMultiply(mat2, vec2, true);
   if (! Math::VectorsEqual(multiply2, expectedMultiply2, TEST_TOLERANCE ) )
   {
     fprintf(stderr, "Multiply vector 2 mismath!\n");
diff --git a/src/math/test/vector_test.cpp b/src/math/test/vector_test.cpp
index 2f4dd09..d2bf231 100644
--- a/src/math/test/vector_test.cpp
+++ b/src/math/test/vector_test.cpp
@@ -106,6 +106,7 @@ int main()
   // Functions to test
   int (*TESTS[])() =
   {
+    TestLength,
     TestNormalize,
     TestDot,
     TestCross
diff --git a/src/math/vector.h b/src/math/vector.h
index 48db4c2..fd9aa52 100644
--- a/src/math/vector.h
+++ b/src/math/vector.h
@@ -75,7 +75,7 @@ struct Vector
   //! Returns the vector length
   inline float Length() const
   {
-    return sqrt(x*x + y*y + z*z);
+    return sqrtf(x*x + y*y + z*z);
   }
 
   //! Normalizes the vector
@@ -232,180 +232,12 @@ inline float Angle(const Vector &a, const Vector &b)
   return a.Angle(b);
 }
 
-//! Returns the distance between two points
+//! Returns the distance between the ends of two vectors
 inline float Distance(const Vector &a, const Vector &b)
 {
-  return 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 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 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 ( fabs(n1.x-n2.x) > 0.1f ||
-       fabs(n1.y-n2.y) > 0.1f ||
-       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 p1 + (p2 - p1) * dist;
-}
-
-//! Rotates a point around a center in space.
-/** \a center center of rotation
-    \a angleH,angleV rotation angles in radians (positive is counterclockwise (CCW) ) )
-    \a p the point
-    \returns the rotated point */
-inline Vector RotatePoint(const Vector &center, float angleH, float angleV, Vector p)
-{
-  Vector a, b;
-
-  p.x -= center.x;
-  p.y -= center.y;
-  p.z -= center.z;
-
-  b.x = p.x*cosf(angleH) - p.z*sinf(angleH);
-  b.y = p.z*sinf(angleV) + p.y*cosf(angleV);
-  b.z = p.x*sinf(angleH) + p.z*cosf(angleH);
-
-  float x = center.x+b.x;
-  float y = center.y+b.y;
-  float z = center.z+b.z;
-
-  return Vector(x, y, z);
-}
-
-//! Rotates a point around a center in space.
-/** \a center center of rotation
-    \a angleH,angleV rotation angles in radians (positive is counterclockwise (CCW) ) )
-    \a p the point
-    \returns the rotated point */
-inline Vector RotatePoint2(const Vector center, float angleH, float angleV, Vector p)
-{
-  Vector a, b;
-
-  p.x -= center.x;
-  p.y -= center.y;
-  p.z -= center.z;
-
-  a.x = p.x*cosf(angleH) - p.z*sinf(angleH);
-  a.y = p.y;
-  a.z = p.x*sinf(angleH) + p.z*cosf(angleH);
-
-  b.x = a.x;
-  b.y = a.z*sinf(angleV) + a.y*cosf(angleV);
-  b.z = a.z*cosf(angleV) - a.y*sinf(angleV);
-
-  float x = center.x+b.x;
-  float y = center.y+b.y;
-  float z = center.z+b.z;
-
-  return Vector(x, y, z);
-}
-
-//! Calculates the intersection "i" right "of" the plane "abc".
-inline bool Intersect(const Vector &a, const Vector &b, const Vector &c, const Vector &d, const Vector &e, Vector &i)
-{
-  float d1 = (d.x-a.x)*((b.y-a.y)*(c.z-a.z)-(c.y-a.y)*(b.z-a.z)) -
-     (d.y-a.y)*((b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z)) +
-     (d.z-a.z)*((b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y));
-
-  float d2 = (d.x-e.x)*((b.y-a.y)*(c.z-a.z)-(c.y-a.y)*(b.z-a.z)) -
-     (d.y-e.y)*((b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z)) +
-     (d.z-e.z)*((b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y));
-
-  if (d2 == 0)
-    return false;
-
-  i.x = d.x + d1/d2*(e.x-d.x);
-  i.y = d.y + d1/d2*(e.y-d.y);
-  i.z = d.z + d1/d2*(e.z-d.z);
-
-  return true;
-}
-
-//! Calculates the intersection of the straight line passing through p (x, z)
-/** Line is parallel to the y axis, with the plane abc. Returns p.y. */
-inline bool IntersectY(const Vector &a, const Vector &b, const Vector &c, Vector &p)
-{
-  float d  = (b.x-a.x)*(c.z-a.z) - (c.x-a.x)*(b.z-a.z);
-  float d1 = (p.x-a.x)*(c.z-a.z) - (c.x-a.x)*(p.z-a.z);
-  float d2 = (b.x-a.x)*(p.z-a.z) - (p.x-a.x)*(b.z-a.z);
-
-  if (d == 0.0f)
-    return false;
-
-  p.y = a.y + d1/d*(b.y-a.y) + d2/d*(c.y-a.y);
-
-  return true;
-}
-
-//! Calculates the end point
-inline Vector LookatPoint(const Vector &eye, float angleH, float angleV, float length)
-{
-
-  Vector lookat = eye;
-  lookat.z += length;
-
-  RotatePoint(eye, angleH, angleV, lookat);
-
-  return lookat;
+  return sqrtf( (a.x-b.x)*(a.x-b.x) +
+                (a.y-b.y)*(a.y-b.y) +
+                (a.z-b.z)*(a.z-b.z) );
 }
 
 /* @} */ // end of group