Modified source engine (2017) developed by valve and leaked in 2020. Not for commercial purporses
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

560 lines
12 KiB

5 years ago
//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose: Math functions specific to the editor.
//
//=============================================================================//
#include "hammer_mathlib.h"
#include <string.h>
#include <Windows.h>
// memdbgon must be the last include file in a .cpp file!!!
#include <tier0/memdbgon.h>
// provide implementation for mathlib Sys_Error()
extern void Error(char* fmt, ...);
extern "C" void Sys_Error( char *error, ... )
{
Error( "%s", error );
}
float V_rint(float f)
{
if (f > 0.0f) {
return (float) floor(f + 0.5f);
} else if (f < 0.0f) {
return (float) ceil(f - 0.5f);
} else
return 0.0f;
}
static int s_BoxFaces[6][3] =
{
{ 0, 4, 2 },
{ 4, 5, 6 },
{ 5, 1, 7 },
{ 1, 0, 3 },
{ 2, 6, 3 },
{ 5, 4, 1 },
};
void polyMake( float x1, float y1, float x2, float y2, int npoints, float start_ang, Vector *pmPoints )
{
int point;
double angle = start_ang, angle_delta = 360.0 / (double) npoints;
double xrad = (x2-x1) / 2, yrad = (y2-y1) / 2;
// make centerpoint for polygon:
float xCenter = x1 + xrad;
float yCenter = y1 + yrad;
for( point = 0; point < npoints; point++, angle += angle_delta )
{
if( angle > 360 )
angle -= 360;
pmPoints[point][0] = V_rint(xCenter + (sin(DEG2RAD(angle)) * (float)xrad));
pmPoints[point][1] = V_rint(yCenter + (cos(DEG2RAD(angle)) * (float)yrad));
}
pmPoints[point][0] = pmPoints[0][0];
pmPoints[point][1] = pmPoints[0][1];
}
float fixang(float a)
{
if(a < 0.0)
return a+360.0;
if(a > 359.9)
return a-360.0;
return a;
}
float lineangle(float x1, float y1, float x2, float y2)
{
float x, y;
float rvl;
x = x2 - x1;
y = y2 - y1;
if(!x && !y)
return 0.0;
rvl = RAD2DEG(atan2( y, x ));
return (rvl);
}
//-----------------------------------------------------------------------------
// Purpose: Builds the matrix for a counterclockwise rotation about an arbitrary axis.
//
// | ax2 + (1 - ax2)cosQ axay(1 - cosQ) - azsinQ azax(1 - cosQ) + aysinQ |
// Ra(Q) = | axay(1 - cosQ) + azsinQ ay2 + (1 - ay2)cosQ ayaz(1 - cosQ) - axsinQ |
// | azax(1 - cosQ) - aysinQ ayaz(1 - cosQ) + axsinQ az2 + (1 - az2)cosQ |
//
// Input : Matrix -
// Axis -
// fAngle -
//-----------------------------------------------------------------------------
void AxisAngleMatrix(VMatrix& Matrix, const Vector& Axis, float fAngle)
{
float fRadians;
float fAxisXSquared;
float fAxisYSquared;
float fAxisZSquared;
float fSin;
float fCos;
fRadians = fAngle * M_PI / 180.0;
fSin = sin(fRadians);
fCos = cos(fRadians);
fAxisXSquared = Axis[0] * Axis[0];
fAxisYSquared = Axis[1] * Axis[1];
fAxisZSquared = Axis[2] * Axis[2];
// Column 0:
Matrix[0][0] = fAxisXSquared + (1 - fAxisXSquared) * fCos;
Matrix[1][0] = Axis[0] * Axis[1] * (1 - fCos) + Axis[2] * fSin;
Matrix[2][0] = Axis[2] * Axis[0] * (1 - fCos) - Axis[1] * fSin;
Matrix[3][0] = 0;
// Column 1:
Matrix[0][1] = Axis[0] * Axis[1] * (1 - fCos) - Axis[2] * fSin;
Matrix[1][1] = fAxisYSquared + (1 - fAxisYSquared) * fCos;
Matrix[2][1] = Axis[1] * Axis[2] * (1 - fCos) + Axis[0] * fSin;
Matrix[3][1] = 0;
// Column 2:
Matrix[0][2] = Axis[2] * Axis[0] * (1 - fCos) + Axis[1] * fSin;
Matrix[1][2] = Axis[1] * Axis[2] * (1 - fCos) - Axis[0] * fSin;
Matrix[2][2] = fAxisZSquared + (1 - fAxisZSquared) * fCos;
Matrix[3][2] = 0;
// Column 3:
Matrix[0][3] = 0;
Matrix[1][3] = 0;
Matrix[2][3] = 0;
Matrix[3][3] = 1;
}
void RotateAroundAxis(VMatrix& Matrix, float fDegrees, int nAxis)
{
int a,b;
if ( fDegrees == 0 )
return;
if ( nAxis == 0 )
{
a=1; b=2;
}
else if ( nAxis == 1)
{
a=0;b=2;
}
else
{
a=0; b=1;
}
float fRadians = DEG2RAD(fDegrees);
float fSin = (float)sin(fRadians);
float fCos = (float)cos(fRadians);
if ( nAxis == 1 )
fSin = -fSin;
float Temp0a = Matrix[0][a] * fCos + Matrix[0][b] * fSin;
float Temp1a = Matrix[1][a] * fCos + Matrix[1][b] * fSin;
float Temp2a = Matrix[2][a] * fCos + Matrix[2][b] * fSin;
float Temp3a = Matrix[3][a] * fCos + Matrix[3][b] * fSin;
if ( nAxis == 1 )
fSin = -fSin;
float Temp0b = Matrix[0][a] * -fSin + Matrix[0][b] * fCos;
float Temp1b = Matrix[1][a] * -fSin + Matrix[1][b] * fCos;
float Temp2b = Matrix[2][a] * -fSin + Matrix[2][b] * fCos;
float Temp3b = Matrix[3][a] * -fSin + Matrix[3][b] * fCos;
Matrix[0][a] = Temp0a;
Matrix[1][a] = Temp1a;
Matrix[2][a] = Temp2a;
Matrix[3][a] = Temp3a;
Matrix[0][b] = Temp0b;
Matrix[1][b] = Temp1b;
Matrix[2][b] = Temp2b;
Matrix[3][b] = Temp3b;
}
//-----------------------------------------------------------------------------
// Purpose:
// Input : pt1 -
// pt2 -
// x1 -
// y1 -
// x2 -
// y2 -
//-----------------------------------------------------------------------------
bool IsLineInside(const Vector2D &pt1, const Vector2D &pt2, int x1, int y1, int x2, int y2)
{
int lx1 = pt1.x;
int ly1 = pt1.y;
int lx2 = pt2.x;
int ly2 = pt2.y;
int i;
// is the line totally on one side of the box?
if( (lx2 > x2 && lx1 > x2) ||
(lx2 < x1 && lx1 < x1) ||
(ly2 > y2 && ly1 > y2) ||
(ly2 < y1 && ly1 < y1) )
return false;
if( lx1 >= x1 && lx1 <= x2 && ly1 >= y1 && ly1 <= y2 )
return true; // the first point is inside the box
if( lx2 >= x1 && lx2 <= x2 && ly2 >= y1 && ly2 <= y2 )
return true; // the second point is inside the box
if( (ly1 > y1) != (ly2 > y1) )
{
i = lx1 + (int) ( (long) (y1 - ly1) * (long) (lx2 - lx1) / (long) (ly2 - ly1));
if( i >= x1 && i <= x2 )
return true; // the line crosses the y1 side (left)
}
if( (ly1 > y2) != (ly2 > y2))
{
i = lx1 + (int) ( (long) (y2 - ly1) * (long) (lx2 - lx1) / (long) (ly2 - ly1));
if( i >= x1 && i <= x2 )
return true; // the line crosses the y2 side (right)
}
if( (lx1 > x1) != (lx2 > x1))
{
i = ly1 + (int) ( (long) (x1 - lx1) * (long) (ly2 - ly1) / (long) (lx2 - lx1));
if( i >= y1 && i <= y2 )
return true; // the line crosses the x1 side (down)
}
if( (lx1 > x2) != (lx2 > x2))
{
i = ly1 + (int) ( (long) (x2 - lx1) * (long) (ly2 - ly1) / (long) (lx2 - lx1));
if( i >= y1 && i <= y2 )
return true; // the line crosses the x2 side (up)
}
// The line does not intersect the box.
return false;
}
bool IsPointInside(const Vector2D &pt, const Vector2D &mins, const Vector2D &maxs )
{
return ( pt.x >= mins.x ) && ( pt.y >= mins.y ) && ( pt.x <= maxs.x ) && ( pt.y <= maxs.y );
}
// Is box 1 inside box 2?
bool IsBoxInside( const Vector2D &min1, const Vector2D &max1, const Vector2D &min2, const Vector2D &max2 )
{
if ( ( min1.x < min2.x ) || ( max1.x > max2.x ) )
return false;
if ( ( min1.y < min2.y ) || ( max1.y > max2.y ) )
return false;
return true;
}
bool IsBoxIntersecting( const Vector2D &min1, const Vector2D &max1, const Vector2D &min2, const Vector2D &max2 )
{
if ( ( min1.x >= max2.x ) || ( max1.x <= min2.x ) )
return false;
if ( ( min1.y >= max2.y ) || ( max1.y <= min2.y ) )
return false;
return true;
}
void NormalizeBox( Vector &mins, Vector &maxs )
{
for (int i=0; i<3; i++ )
{
if ( mins[i] > maxs[i])
{
V_swap( mins[i], maxs[i] );
}
}
}
void NormalizeBox( Vector2D &mins, Vector2D &maxs )
{
if ( mins.x > maxs.x )
{
V_swap( mins.x, maxs.x );
}
if ( mins.y > maxs.y )
{
V_swap( mins.y, maxs.y );
}
}
bool IsValidBox( Vector &mins, Vector &maxs )
{
return ( mins.x <= maxs.x ) && ( mins.y <= maxs.y ) && ( mins.z <= maxs.z );
}
bool IsValidBox( const Vector2D &mins, const Vector2D &maxs )
{
return ( mins.x <= maxs.x ) && ( mins.y <= maxs.y );
}
void LimitBox( Vector &mins, Vector &maxs, float limit )
{
for ( int i=0; i<3;i++)
{
if ( mins[i] < -limit )
mins[i] = -limit;
if ( maxs[i] > limit )
maxs[i] = limit;
}
}
void GetAxisFromFace( int nFace, Vector& vHorz, Vector &vVert, Vector &vThrd )
{
Assert( nFace >= 0 && nFace < 6);
Vector points[8];
PointsFromBox( Vector(0,0,0), Vector(1,1,1), points );
Vector p1 = points[s_BoxFaces[nFace][0]];
Vector p2 = points[s_BoxFaces[nFace][1]];
Vector p3 = points[s_BoxFaces[nFace][2]];
// compose equation
vHorz = p2 - p1;
vVert = p3 - p1;
vThrd = CrossProduct( vHorz, vVert );
}
//
// Generate the corner points of a box:
// 3---7
// /| /|
// / | / |
// 2---6 |
// | 1|--5
// | / | /
// |/ |/
// 0---4
void PointsFromBox( const Vector &mins, const Vector &maxs, Vector *points )
{
points[0][0] = mins[0];
points[0][1] = mins[1];
points[0][2] = mins[2];
points[1][0] = mins[0];
points[1][1] = mins[1];
points[1][2] = maxs[2];
points[2][0] = mins[0];
points[2][1] = maxs[1];
points[2][2] = mins[2];
points[3][0] = mins[0];
points[3][1] = maxs[1];
points[3][2] = maxs[2];
points[4][0] = maxs[0];
points[4][1] = mins[1];
points[4][2] = mins[2];
points[5][0] = maxs[0];
points[5][1] = mins[1];
points[5][2] = maxs[2];
points[6][0] = maxs[0];
points[6][1] = maxs[1];
points[6][2] = mins[2];
points[7][0] = maxs[0];
points[7][1] = maxs[1];
points[7][2] = maxs[2];
}
float IntersectionLineAABBox( const Vector& mins, const Vector& maxs, const Vector& vStart, const Vector& vEnd, int &nFace )
{
Vector vz = vEnd - vStart;
// quick distance check first
Vector vCenter = (mins+maxs)/2;
Vector vTmp = maxs-vCenter;
float radius = DotProduct(vTmp,vTmp);
vTmp = CrossProduct(vz,(vStart-vCenter));
float dist = DotProduct( vTmp,vTmp ) / DotProduct( vz,vz );
nFace = -1;
if ( dist > radius )
{
return -1;
}
// ok, now check against all 6 faces
Vector points[8];
PointsFromBox( mins, maxs, points );
vz = -vz;
float fDistance = 999999;
for ( int i=0; i<6; i++ )
{
// get points of face
Vector p1 = points[s_BoxFaces[i][0]];
Vector p2 = points[s_BoxFaces[i][1]];
Vector p3 = points[s_BoxFaces[i][2]];
// compose equation
Vector v0 = vStart - p1;
Vector vx = p2 - p1;
Vector vy = p3 - p1;
Vector vOut;
// solve equation v0 = x*v1 + y*v2 + z*v3
if ( !SolveLinearEquation( v0, vx, vy, vz, vOut) )
continue;
if ( vOut.z < 0 || vOut.z > 1 )
continue;
if ( vOut.x < 0 || vOut.x > 1 )
continue;
if ( vOut.y < 0 || vOut.y > 1 )
continue;
if ( vOut.z < fDistance )
{
nFace = i;
fDistance = vOut.z;
}
}
if ( nFace >= 0 )
{
return fDistance*VectorLength(vz);
}
else
{
return -1;
}
}
void RoundVector( Vector2D &v )
{
v.x = (int)(v.x+0.5f);
v.y = (int)(v.y+0.5f);
}
void PointsRevertOrder( Vector *pPoints, int nPoints)
{
Vector *tmpPoints = (Vector*)_alloca( sizeof(Vector)*nPoints );
memcpy( tmpPoints, pPoints, sizeof(Vector)*nPoints );
for ( int i = 0; i<nPoints; i++)
{
pPoints[i] = tmpPoints[nPoints-i-1];
}
}
const Vector &GetNormalFromFace( int nFace )
{
// ok, now check against all 6 faces
Vector points[8];
Assert( nFace>=0 && nFace<6 );
PointsFromBox( Vector(0,0,0), Vector(1,1,1), points );
return GetNormalFromPoints( points[s_BoxFaces[nFace][0]], points[s_BoxFaces[nFace][1]],points[s_BoxFaces[nFace][2]] );
}
const Vector &GetNormalFromPoints( const Vector &p0, const Vector &p1, const Vector &p2 )
{
static Vector vNormal;
Vector v1 = p0 - p1;
Vector v2 = p2 - p1;
CrossProduct(v1, v2, vNormal);
VectorNormalize(vNormal);
return vNormal;
}
// solve equation v0 = x*v1 + y*v2 + z*v3
bool SolveLinearEquation( const Vector& v0, const Vector& v1, const Vector& v2, const Vector& v3, Vector& vOut)
{
VMatrix matrix, inverse;
matrix.Init(
v1.x, v1.y, v1.z, 0,
v2.x, v2.y, v2.z, 0,
v3.x, v3.y, v3.z, 0,
0.0f, 0.0f, 0.0f, 1
);
if( !matrix.InverseGeneral(inverse) )
return false;
vOut = inverse.VMul3x3Transpose( v0 );
return true;
}
bool BuildAxesFromNormal( const Vector &vNormal, Vector &vHorz, Vector &vVert )
{
vHorz.Init();
vVert.Init();
// find the major axis
float bestMin = 99999;
int bestAxis = -1;
for (int i=0 ; i<3; i++)
{
float a = fabs(vNormal[i]);
if (a < bestMin)
{
bestAxis = i;
bestMin = a;
}
}
if (bestAxis==-1)
return false;
vHorz[bestAxis] = 1;
CrossProduct( vNormal,vHorz,vVert);
CrossProduct( vNormal,vVert,vHorz);
VectorNormalize( vHorz );
VectorNormalize( vVert );
return true;
}