Finished implementation of math functions
Piotr Dziwinski
parent
f67a62fb28
commit
5dee2731e6
|
|
@ -27,5 +27,6 @@
|
|||
#include "point.h"
|
||||
#include "vector.h"
|
||||
#include "matrix.h"
|
||||
#include "geometry.h"
|
||||
|
||||
/* @} */ // end of group
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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 ¢er, 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 ¢er, 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 ¢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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
131
src/math/point.h
131
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 ¢er, 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 ¢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;
|
||||
return sqrtf((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
|
||||
}
|
||||
|
||||
/* @} */ // end of group
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -106,6 +106,7 @@ int main()
|
|||
// Functions to test
|
||||
int (*TESTS[])() =
|
||||
{
|
||||
TestLength,
|
||||
TestNormalize,
|
||||
TestDot,
|
||||
TestCross
|
||||
|
|
|
|||
|
|
@ -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 ¢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
|
||||
|
|
|
|||
Loading…
Reference in New Issue