//========= Copyright Valve Corporation, All rights reserved. ============// // // Purpose: Math functions specific to the editor. // //=============================================================================// #include "hammer_mathlib.h" #include #include // memdbgon must be the last include file in a .cpp file!!! #include // 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=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; }