From 5dee2731e6fb3deb2c44e269d12c2b7db62c74cb Mon Sep 17 00:00:00 2001 From: Piotr Dziwinski Date: Thu, 7 Jun 2012 13:35:23 +0200 Subject: Finished implementation of math functions --- src/math/all.h | 1 + src/math/const.h | 8 +- src/math/func.h | 11 +- src/math/geometry.h | 600 ++++++++++++++++++++++++++++++++++++++++++ src/math/matrix.h | 216 +-------------- src/math/point.h | 131 +-------- src/math/test/matrix_test.cpp | 28 +- src/math/test/vector_test.cpp | 1 + src/math/vector.h | 178 +------------ 9 files changed, 636 insertions(+), 538 deletions(-) create mode 100644 src/math/geometry.h (limited to 'src') 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 +#include + + +// 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) && (np.y||p.y>m) && (n= 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 ¢er, 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 ¢er, 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) && (np.y || p.y>m) && (n 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 ¢er, 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 -- cgit v1.2.3-1-g7c22