333 lines
7.9 KiB
C
333 lines
7.9 KiB
C
////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Crytek Engine Source File.
|
|
// Copyright (C), Crytek Studios, 2002.
|
|
// -------------------------------------------------------------------------
|
|
// File name: math.h
|
|
// Version: v1.00
|
|
// Created: 2/7/2002 by Timur.
|
|
// Compilers: Visual Studio.NET
|
|
// Description: Various math and geometry related functions.
|
|
// -------------------------------------------------------------------------
|
|
// History:
|
|
//
|
|
////////////////////////////////////////////////////////////////////////////
|
|
|
|
#ifndef __math_h__
|
|
#define __math_h__
|
|
|
|
#if _MSC_VER > 1000
|
|
#pragma once
|
|
#endif
|
|
|
|
#include "BBox.h"
|
|
|
|
//! Half PI
|
|
#define PI_HALF (3.1415926535897932384626433832795f / 2.0f)
|
|
|
|
//! Epsilon for vector comparasions.
|
|
#define FLOAT_EPSILON 0.0001f
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
/** Compare two vectors if they are equal.
|
|
*/
|
|
inline bool IsVectorsEqual( const Vec3 &v1,const Vec3 &v2 )
|
|
{
|
|
if (fabs(v2.x-v1.x) < FLOAT_EPSILON && fabs(v2.y-v1.y) < FLOAT_EPSILON && fabs(v2.z-v1.z) < FLOAT_EPSILON)
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
/** Compare two matrices if they are equal.
|
|
*/
|
|
inline bool IsMatrixEqual( const Matrix44 &tm1,const Matrix44 &tm2 )
|
|
{
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
if (fabs(tm1[i][0] - tm2[i][0]) > FLOAT_EPSILON ||
|
|
fabs(tm1[i][1] - tm2[i][1]) > FLOAT_EPSILON ||
|
|
fabs(tm1[i][2] - tm2[i][2]) > FLOAT_EPSILON ||
|
|
fabs(tm1[i][3] - tm2[i][3]) > FLOAT_EPSILON)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Math utilities.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
inline float PointToLineDistance2D( const Vec3 &p1,const Vec3 &p2,const Vec3 &p3 )
|
|
{
|
|
float dx = p2.x-p1.x;
|
|
float dy = p2.y-p1.y;
|
|
float u = ((p3.x-p1.x)*dx + (p3.y-p1.y)*dy) / (dx*dx + dy*dy);
|
|
if (u < 0)
|
|
return (float)sqrt( (p3.x-p1.x)*(p3.x-p1.x) + (p3.y-p1.y)*(p3.y-p1.y) );
|
|
else if (u > 1)
|
|
return (float)sqrt( (p3.x-p2.x)*(p3.x-p2.x) + (p3.y-p2.y)*(p3.y-p2.y) );
|
|
else
|
|
{
|
|
float x = p1.x + u*dx;
|
|
float y = p1.y + u*dy;
|
|
return (float)sqrt( (p3.x-x)*(p3.x-x) + (p3.y-y)*(p3.y-y) );
|
|
}
|
|
}
|
|
|
|
inline float PointToLineDistance( const Vec3 &p1,const Vec3 &p2,const Vec3 &p3 )
|
|
{
|
|
Vec3 d = p2 - p1;
|
|
float u = d.Dot(p3-p1) / GetLengthSquared(d);
|
|
if (u < 0)
|
|
return (p3 - p1).Length();
|
|
else if (u > 1)
|
|
return (p3 - p2).Length();
|
|
else
|
|
{
|
|
Vec3 p = p1 + u*d;
|
|
return (p3 - p).Length();
|
|
}
|
|
}
|
|
|
|
/** Calculate distance between point and line.
|
|
@param p1 Source line point.
|
|
@param p2 Target line point.
|
|
@param p3 Point to find intersecion with.
|
|
@param intersectPoint Intersection point on the line.
|
|
@return Distance between point and line.
|
|
*/
|
|
inline float PointToLineDistance( const Vec3 &p1,const Vec3 &p2,const Vec3 &p3,Vec3 &intersectPoint )
|
|
{
|
|
Vec3 d = p2 - p1;
|
|
float u = d.Dot(p3-p1) / GetLengthSquared(d);
|
|
if (u < 0)
|
|
{
|
|
intersectPoint = p1;
|
|
return (p3 - p1).Length();
|
|
}
|
|
else if (u > 1)
|
|
{
|
|
intersectPoint = p2;
|
|
return (p3 - p2).Length();
|
|
}
|
|
else
|
|
{
|
|
Vec3 p = p1 + u*d;
|
|
intersectPoint = p;
|
|
return (p3 - p).Length();
|
|
}
|
|
}
|
|
|
|
/**
|
|
Calculate the line segment PaPb that is the shortest route between
|
|
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
|
|
Pa = P1 + mua (P2 - P1)
|
|
Pb = P3 + mub (P4 - P3)
|
|
|
|
@param p1 Source point of first line.
|
|
@param p2 Target point of first line.
|
|
@param p3 Source point of second line.
|
|
@param p4 Target point of second line.
|
|
@return FALSE if no solution exists.
|
|
*/
|
|
inline bool LineLineIntersect( const Vec3 &p1,const Vec3 &p2,const Vec3 &p3,const Vec3 &p4,
|
|
Vec3 &pa,Vec3 &pb,float &mua,float &mub )
|
|
{
|
|
Vec3 p13,p43,p21;
|
|
float d1343,d4321,d1321,d4343,d2121;
|
|
float numer,denom;
|
|
|
|
p13.x = p1.x - p3.x;
|
|
p13.y = p1.y - p3.y;
|
|
p13.z = p1.z - p3.z;
|
|
p43.x = p4.x - p3.x;
|
|
p43.y = p4.y - p3.y;
|
|
p43.z = p4.z - p3.z;
|
|
if (fabs(p43.x) < LINE_EPS && fabs(p43.y) < LINE_EPS && fabs(p43.z) < LINE_EPS)
|
|
return false;
|
|
p21.x = p2.x - p1.x;
|
|
p21.y = p2.y - p1.y;
|
|
p21.z = p2.z - p1.z;
|
|
if (fabs(p21.x) < LINE_EPS && fabs(p21.y) < LINE_EPS && fabs(p21.z) < LINE_EPS)
|
|
return false;
|
|
|
|
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
|
|
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
|
|
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
|
|
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
|
|
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
|
|
|
|
denom = d2121 * d4343 - d4321 * d4321;
|
|
if (fabs(denom) < LINE_EPS)
|
|
return(FALSE);
|
|
numer = d1343 * d4321 - d1321 * d4343;
|
|
|
|
mua = numer / denom;
|
|
mub = (d1343 + d4321 * (mua)) / d4343;
|
|
|
|
pa.x = p1.x + mua * p21.x;
|
|
pa.y = p1.y + mua * p21.y;
|
|
pa.z = p1.z + mua * p21.z;
|
|
pb.x = p3.x + mub * p43.x;
|
|
pb.y = p3.y + mub * p43.y;
|
|
pb.z = p3.z + mub * p43.z;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*!
|
|
Calculates shortest distance between ray and a arbitary line segment.
|
|
@param raySrc Source point of ray.
|
|
@param rayTrg Target point of ray.
|
|
@param p1 First point of line segment.
|
|
@param p2 Second point of line segment.
|
|
@param intersectPoint This parameter returns nearest point on line segment to ray.
|
|
@return distance fro ray to line segment.
|
|
*/
|
|
inline float RayToLineDistance( const Vec3 &raySrc,const Vec3 &rayTrg,const Vec3 &p1,const Vec3 &p2,Vec3 &nearestPoint )
|
|
{
|
|
Vec3 intPnt;
|
|
Vec3 rayLineP1 = raySrc;
|
|
Vec3 rayLineP2 = rayTrg;
|
|
Vec3 pa,pb;
|
|
float ua,ub;
|
|
|
|
if (!LineLineIntersect( p1,p2, rayLineP1,rayLineP2, pa,pb,ua,ub ))
|
|
return FLT_MAX;
|
|
|
|
float d = 0;
|
|
if (ua < 0)
|
|
d = PointToLineDistance( rayLineP1,rayLineP2,p1,intPnt );
|
|
else if (ua > 1)
|
|
d = PointToLineDistance( rayLineP1,rayLineP2,p2,intPnt );
|
|
else
|
|
{
|
|
intPnt = rayLineP1 + ub*(rayLineP2-rayLineP1);
|
|
d = (pb-pa).Length();
|
|
}
|
|
nearestPoint = intPnt;
|
|
return d;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////
|
|
inline Matrix44 MatrixFromVector( const Vec3d &dir )
|
|
{
|
|
Matrix44 M;
|
|
// LookAt transform.
|
|
Vec3d xAxis,yAxis,zAxis;
|
|
Vec3d upVector;
|
|
|
|
yAxis = GetNormalized(dir);
|
|
|
|
if (yAxis.x == 0.0 && yAxis.y == 0)
|
|
upVector( -yAxis.z,0,0 );
|
|
else
|
|
upVector( 0,0,1.0f );
|
|
|
|
xAxis = GetNormalized((upVector.Cross(yAxis)));
|
|
zAxis = GetNormalized( -(xAxis.Cross(yAxis)) );
|
|
|
|
/*
|
|
// D3D kind of matrix.
|
|
M[0][0] = xAxis.x;
|
|
M[0][1] = yAxis.x;
|
|
M[0][2] = zAxis.x;
|
|
M[0][3] = 0;
|
|
|
|
M[1][0] = xAxis.y;
|
|
M[1][1] = yAxis.y;
|
|
M[1][2] = zAxis.y;
|
|
M[1][3] = 0;
|
|
|
|
M[2][0] = xAxis.z;
|
|
M[2][1] = yAxis.z;
|
|
M[2][2] = zAxis.z;
|
|
M[2][3] = 0;
|
|
|
|
M[3][0] = 0;
|
|
M[3][1] = 0;
|
|
M[3][2] = 0;
|
|
M[3][3] = 1;
|
|
*/
|
|
|
|
// OpenGL kind of matrix.
|
|
M[0][0] = xAxis.x;
|
|
M[1][0] = yAxis.x;
|
|
M[2][0] = zAxis.x;
|
|
M[3][0] = 0;
|
|
|
|
M[0][1] = xAxis.y;
|
|
M[1][1] = yAxis.y;
|
|
M[2][1] = zAxis.y;
|
|
M[3][1] = 0;
|
|
|
|
M[0][2] = xAxis.z;
|
|
M[1][2] = yAxis.z;
|
|
M[2][2] = zAxis.z;
|
|
M[3][2] = 0;
|
|
|
|
M[0][3] = 0;
|
|
M[1][3] = 0;
|
|
M[2][3] = 0;
|
|
M[3][3] = 1;
|
|
|
|
return M;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
inline Matrix44 MatrixFromVector( const Vec3d &dir,const Vec3d &up,float rollAngle=0 )
|
|
{
|
|
Matrix44 M;
|
|
// LookAt transform.
|
|
Vec3d xAxis,yAxis,zAxis;
|
|
Vec3d upVector = up;
|
|
|
|
yAxis = GetNormalized(-dir);
|
|
|
|
//if (zAxis.x == 0.0 && zAxis.z == 0) up.Set( -zAxis.y,0,0 ); else up.Set( 0,1.0f,0 );
|
|
|
|
xAxis = GetNormalized((upVector.Cross(yAxis)));
|
|
zAxis = GetNormalized(xAxis.Cross(yAxis));
|
|
|
|
// OpenGL kind of matrix.
|
|
M[0][0] = xAxis.x;
|
|
M[1][0] = yAxis.x;
|
|
M[2][0] = zAxis.x;
|
|
M[3][0] = 0;
|
|
|
|
M[0][1] = xAxis.y;
|
|
M[1][1] = yAxis.y;
|
|
M[2][1] = zAxis.y;
|
|
M[3][1] = 0;
|
|
|
|
M[0][2] = xAxis.z;
|
|
M[1][2] = yAxis.z;
|
|
M[2][2] = zAxis.z;
|
|
M[3][2] = 0;
|
|
|
|
M[0][3] = 0;
|
|
M[1][3] = 0;
|
|
M[2][3] = 0;
|
|
M[3][3] = 1;
|
|
|
|
if (rollAngle != 0)
|
|
{
|
|
Matrix44 RollMtx;
|
|
RollMtx.SetIdentity();
|
|
|
|
float s = sinf(rollAngle);
|
|
float c = cosf(rollAngle);
|
|
|
|
RollMtx[0][0] = c; RollMtx[2][0] = -s;;
|
|
RollMtx[0][2] = s; RollMtx[2][2] = c;
|
|
|
|
// Matrix multiply.
|
|
M = RollMtx * M;
|
|
}
|
|
|
|
return M;
|
|
}
|
|
|
|
#endif // __math_h__
|