immibis
/
colobot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Finished implementation of math functions

dev-ui
Piotr Dziwinski 2012-06-07 13:35:23 +02:00
parent f67a62fb28
commit 5dee2731e6
9 changed files with 636 additions and 538 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/math/all.h
View File

@ -27,5 +27,6 @@
#include "point.h" #include "point.h"
#include "vector.h" #include "vector.h"
#include "matrix.h" #include "matrix.h"
#include "geometry.h"
/* @} */ // end of group /* @} */ // end of group

8
src/math/const.h
View File

@ -29,7 +29,12 @@ namespace Math
//! Tolerance level -- minimum accepted float value //! Tolerance level -- minimum accepted float value
const float TOLERANCE = 1e-6f; 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; const float HUGE = 1.0e+38f;
//! PI //! PI
@ -50,3 +55,4 @@ namespace Math
/* @} */ // end of group /* @} */ // end of group
}; // namespace Math }; // namespace Math

11
src/math/func.h
View File

@ -1,4 +1,5 @@
// * This file is part of the COLOBOT source code // * 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) // * Copyright (C) 2012, Polish Portal of Colobot (PPC)
// * // *
// * This program is free software: you can redistribute it and/or modify // * 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; 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. //! Returns a random value between 0 and 1.
inline float Rand() inline float Rand()
{ {

600
src/math/geometry.h Normal file
View File

@ -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

216
src/math/matrix.h
View File

@ -356,198 +356,6 @@ struct Matrix
return Matrix(result); 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 }; // struct Matrix
//! Checks if two matrices are equal within given \a tolerance //! 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 The result, a 4x1 vector is then converted to 3x1 by dividing
x,y,z coords by the fourth coord (w). */ 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 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 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 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]; float w = v.x * m.m[3 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15];
if (IsZero(w)) if (IsZero(w))
@ -606,24 +418,6 @@ inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v)
return Vector(x, y, z); 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 /* @} */ // end of group
}; // namespace Math }; // namespace Math

131
src/math/point.h
View File

@ -70,7 +70,7 @@ struct Point
//! Returns the distance from (0,0) to the point (x,y) //! Returns the distance from (0,0) to the point (x,y)
inline float Length() inline float Length()
{ {
return sqrt(x*x + y*y); return sqrtf(x*x + y*y);
} }
//! Returns the inverted point //! Returns the inverted point
@ -163,134 +163,7 @@ inline void Swap(Point &a, Point &b)
//! Returns the distance between two points //! Returns the distance between two points
inline float Distance(const Point &a, const Point &b) 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)); return sqrtf((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;
} }
/* @} */ // end of group /* @} */ // end of group

28
src/math/test/matrix_test.cpp
View File

@ -331,18 +331,18 @@ int TestMultiplyVector()
const Math::Matrix mat1( const Math::Matrix mat1(
(float[4][4]) (float[4][4])
{ {
{ 0.0536517635602049, 0.1350203249258820, -1.4709867280474975, 1.4199163191255975 }, { 0.188562846910008, -0.015148651460679, 0.394512304108827, 0.906910631257135 },
{ 0.4308040094214364, 0.6860887768493787, 0.0555235810428098, 0.0245232625281863 }, { -0.297506779519667, 0.940119328178913, 0.970957796752517, 0.310559318965526 },
{ -0.9570012049134703, 1.4008557175488343, 1.0277555933198543, 1.2311131809078903 }, { -0.819770525290873, -2.316574438778879, 0.155756069319732, -0.855661405742964 },
{ 1.5609168701538376, -0.4917648784647429, 1.3748498152379420, 0.2479075063284996 } { 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 ) ) if (! Math::VectorsEqual(multiply1, expectedMultiply1, TEST_TOLERANCE ) )
{ {
fprintf(stderr, "Multiply vector 1 mismath!\n"); fprintf(stderr, "Multiply vector 1 mismath!\n");
@ -352,18 +352,18 @@ int TestMultiplyVector()
const Math::Matrix mat2( const Math::Matrix mat2(
(float[4][4]) (float[4][4])
{ {
{ 1.2078126667092564, 0.5230257362392928, -0.7623036312496848, -1.4192273892400153 }, { -0.63287117038834284, 0.55148060401816856, -0.02042395559467368, -1.50367083897656850 },
{ 0.7165407622837081, 1.3746282484390115, -0.8382279333943382, 0.8248340530209490 }, { 0.69629042156335297, 0.12982747869796774, -1.16250029235919405, 1.19084447253756909 },
{ -0.9595506321366957, -0.0169226311095793, -0.7271125620609374, -1.5540250647342428 }, { 0.44164132914357224, -0.15169304045662041, -0.00880583574621390, -0.55817802940035310 },
{ 1.2788946935793131, 0.1516426145850322, 1.2115324484930112, -0.1584402989052367 } { 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 ) ) if (! Math::VectorsEqual(multiply2, expectedMultiply2, TEST_TOLERANCE ) )
{ {
fprintf(stderr, "Multiply vector 2 mismath!\n"); fprintf(stderr, "Multiply vector 2 mismath!\n");

1
src/math/test/vector_test.cpp
View File

@ -106,6 +106,7 @@ int main()
// Functions to test // Functions to test
int (*TESTS[])() = int (*TESTS[])() =
{ {
TestLength,
TestNormalize, TestNormalize,
TestDot, TestDot,
TestCross TestCross

178
src/math/vector.h
View File

@ -75,7 +75,7 @@ struct Vector
//! Returns the vector length //! Returns the vector length
inline float Length() const inline float Length() const
{ {
return sqrt(x*x + y*y + z*z); return sqrtf(x*x + y*y + z*z);
} }
//! Normalizes the vector //! Normalizes the vector
@ -232,180 +232,12 @@ inline float Angle(const Vector &a, const Vector &b)
return a.Angle(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) inline float Distance(const Vector &a, const Vector &b)
{ {
return sqrt( (a.x-b.x)*(a.x-b.x) + return sqrtf( (a.x-b.x)*(a.x-b.x) +
(a.y-b.y)*(a.y-b.y) + (a.y-b.y)*(a.y-b.y) +
(a.z-b.z)*(a.z-b.z) ); (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;
} }
/* @} */ // end of group /* @} */ // end of group