From 449cc186d5b8117d74ba22d6173497d00939f5f1 Mon Sep 17 00:00:00 2001 From: Piotr Dziwinski Date: Sat, 28 Apr 2012 17:53:17 +0200 Subject: Source files split into modules --- src/math/README.txt | 3 + src/math/d3dmath.cpp | 343 +++++++++++++++++ src/math/d3dmath.h | 97 +++++ src/math/math3d.cpp | 1035 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/math/math3d.h | 106 ++++++ 5 files changed, 1584 insertions(+) create mode 100644 src/math/README.txt create mode 100644 src/math/d3dmath.cpp create mode 100644 src/math/d3dmath.h create mode 100644 src/math/math3d.cpp create mode 100644 src/math/math3d.h (limited to 'src/math') diff --git a/src/math/README.txt b/src/math/README.txt new file mode 100644 index 0000000..1a5ce93 --- /dev/null +++ b/src/math/README.txt @@ -0,0 +1,3 @@ +src/math + +Contains common mathematical structures and functions. diff --git a/src/math/d3dmath.cpp b/src/math/d3dmath.cpp new file mode 100644 index 0000000..2686215 --- /dev/null +++ b/src/math/d3dmath.cpp @@ -0,0 +1,343 @@ +// * This file is part of the COLOBOT source code +// * Copyright (C) 2001-2008, Daniel ROUX & EPSITEC SA, www.epsitec.ch +// * +// * 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/. + +//----------------------------------------------------------------------------- +// File: D3DMath.cpp +// +// Desc: Shortcut macros and functions for using DX objects +// +// Copyright (c) 1997-1999 Microsoft Corporation. All rights reserved +//----------------------------------------------------------------------------- +#define D3D_OVERLOADS +#define STRICT +#include +#include +#include "d3dmath.h" + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_MatrixMultiply() +// Desc: Does the matrix operation: [Q] = [A] * [B]. Note that the order of +// this operation was changed from the previous version of the DXSDK. +//----------------------------------------------------------------------------- +VOID D3DMath_MatrixMultiply( D3DMATRIX& q, D3DMATRIX& a, D3DMATRIX& b ) +{ + FLOAT* pA = (FLOAT*)&a; + FLOAT* pB = (FLOAT*)&b; + FLOAT pM[16]; + + ZeroMemory( pM, sizeof(D3DMATRIX) ); + + for( WORD i=0; i<4; i++ ) + for( WORD j=0; j<4; j++ ) + for( WORD k=0; k<4; k++ ) + pM[4*i+j] += pA[4*i+k] * pB[4*k+j]; + + memcpy( &q, pM, sizeof(D3DMATRIX) ); +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_MatrixInvert() +// Desc: Does the matrix operation: [Q] = inv[A]. Note: this function only +// works for matrices with [0 0 0 1] for the 4th column. +//----------------------------------------------------------------------------- +HRESULT D3DMath_MatrixInvert( D3DMATRIX& q, D3DMATRIX& a ) +{ + if( fabs(a._44 - 1.0f) > .001f) + return E_INVALIDARG; + if( fabs(a._14) > .001f || fabs(a._24) > .001f || fabs(a._34) > .001f ) + return E_INVALIDARG; + + FLOAT fDetInv = 1.0f / ( a._11 * ( a._22 * a._33 - a._23 * a._32 ) - + a._12 * ( a._21 * a._33 - a._23 * a._31 ) + + a._13 * ( a._21 * a._32 - a._22 * a._31 ) ); + + q._11 = fDetInv * ( a._22 * a._33 - a._23 * a._32 ); + q._12 = -fDetInv * ( a._12 * a._33 - a._13 * a._32 ); + q._13 = fDetInv * ( a._12 * a._23 - a._13 * a._22 ); + q._14 = 0.0f; + + q._21 = -fDetInv * ( a._21 * a._33 - a._23 * a._31 ); + q._22 = fDetInv * ( a._11 * a._33 - a._13 * a._31 ); + q._23 = -fDetInv * ( a._11 * a._23 - a._13 * a._21 ); + q._24 = 0.0f; + + q._31 = fDetInv * ( a._21 * a._32 - a._22 * a._31 ); + q._32 = -fDetInv * ( a._11 * a._32 - a._12 * a._31 ); + q._33 = fDetInv * ( a._11 * a._22 - a._12 * a._21 ); + q._34 = 0.0f; + + q._41 = -( a._41 * q._11 + a._42 * q._21 + a._43 * q._31 ); + q._42 = -( a._41 * q._12 + a._42 * q._22 + a._43 * q._32 ); + q._43 = -( a._41 * q._13 + a._42 * q._23 + a._43 * q._33 ); + q._44 = 1.0f; + + return S_OK; +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_VectorMatrixMultiply() +// Desc: Multiplies a vector by a matrix +//----------------------------------------------------------------------------- +HRESULT D3DMath_VectorMatrixMultiply( D3DVECTOR& vDest, D3DVECTOR& vSrc, + D3DMATRIX& mat) +{ + FLOAT x = vSrc.x*mat._11 + vSrc.y*mat._21 + vSrc.z* mat._31 + mat._41; + FLOAT y = vSrc.x*mat._12 + vSrc.y*mat._22 + vSrc.z* mat._32 + mat._42; + FLOAT z = vSrc.x*mat._13 + vSrc.y*mat._23 + vSrc.z* mat._33 + mat._43; + FLOAT w = vSrc.x*mat._14 + vSrc.y*mat._24 + vSrc.z* mat._34 + mat._44; + + if( fabs( w ) < g_EPSILON ) + return E_INVALIDARG; + + vDest.x = x/w; + vDest.y = y/w; + vDest.z = z/w; + + return S_OK; +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_VertexMatrixMultiply() +// Desc: Multiplies a vertex by a matrix +//----------------------------------------------------------------------------- +HRESULT D3DMath_VertexMatrixMultiply( D3DVERTEX& vDest, D3DVERTEX& vSrc, + D3DMATRIX& mat ) +{ + HRESULT hr; + D3DVECTOR* pSrcVec = (D3DVECTOR*)&vSrc.x; + D3DVECTOR* pDestVec = (D3DVECTOR*)&vDest.x; + + if( SUCCEEDED( hr = D3DMath_VectorMatrixMultiply( *pDestVec, *pSrcVec, + mat ) ) ) + { + pSrcVec = (D3DVECTOR*)&vSrc.nx; + pDestVec = (D3DVECTOR*)&vDest.nx; + hr = D3DMath_VectorMatrixMultiply( *pDestVec, *pSrcVec, mat ); + } + return hr; +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_QuaternionFromRotation() +// Desc: Converts a normalized axis and angle to a unit quaternion. +//----------------------------------------------------------------------------- +VOID D3DMath_QuaternionFromRotation( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w, + D3DVECTOR& v, FLOAT fTheta ) +{ + x = sinf( fTheta/2.0f ) * v.x; + y = sinf( fTheta/2.0f ) * v.y; + z = sinf( fTheta/2.0f ) * v.z; + w = cosf( fTheta/2.0f ); +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_RotationFromQuaternion() +// Desc: Converts a normalized axis and angle to a unit quaternion. +//----------------------------------------------------------------------------- +VOID D3DMath_RotationFromQuaternion( D3DVECTOR& v, FLOAT& fTheta, + FLOAT x, FLOAT y, FLOAT z, FLOAT w ) + +{ + fTheta = acosf(w) * 2.0f; + v.x = x / sinf( fTheta/2.0f ); + v.y = y / sinf( fTheta/2.0f ); + v.z = z / sinf( fTheta/2.0f ); +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_QuaternionFromAngles() +// Desc: Converts euler angles to a unit quaternion. +//----------------------------------------------------------------------------- +VOID D3DMath_QuaternionFromAngles( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w, + FLOAT fYaw, FLOAT fPitch, FLOAT fRoll ) + +{ + FLOAT fSinYaw = sinf( fYaw/2.0f ); + FLOAT fSinPitch = sinf( fPitch/2.0f ); + FLOAT fSinRoll = sinf( fRoll/2.0f ); + FLOAT fCosYaw = cosf( fYaw/2.0f ); + FLOAT fCosPitch = cosf( fPitch/2.0f ); + FLOAT fCosRoll = cosf( fRoll/2.0f ); + + x = fSinRoll * fCosPitch * fCosYaw - fCosRoll * fSinPitch * fSinYaw; + y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw; + z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw; + w = fCosRoll * fCosPitch * fCosYaw + fSinRoll * fSinPitch * fSinYaw; +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_MatrixFromQuaternion() +// Desc: Converts a unit quaternion into a rotation matrix. +//----------------------------------------------------------------------------- +VOID D3DMath_MatrixFromQuaternion( D3DMATRIX& mat, FLOAT x, FLOAT y, FLOAT z, + FLOAT w ) +{ + FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z; + FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z; + FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z; + + mat._11 = 1 - 2 * ( yy + zz ); + mat._12 = 2 * ( xy - wz ); + mat._13 = 2 * ( xz + wy ); + + mat._21 = 2 * ( xy + wz ); + mat._22 = 1 - 2 * ( xx + zz ); + mat._23 = 2 * ( yz - wx ); + + mat._31 = 2 * ( xz - wy ); + mat._32 = 2 * ( yz + wx ); + mat._33 = 1 - 2 * ( xx + yy ); + + mat._14 = mat._24 = mat._34 = 0.0f; + mat._41 = mat._42 = mat._43 = 0.0f; + mat._44 = 1.0f; +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_QuaternionFromMatrix() +// Desc: Converts a rotation matrix into a unit quaternion. +//----------------------------------------------------------------------------- +VOID D3DMath_QuaternionFromMatrix( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w, + D3DMATRIX& mat ) +{ + if( mat._11 + mat._22 + mat._33 > 0.0f ) + { + FLOAT s = sqrtf( mat._11 + mat._22 + mat._33 + mat._44 ); + + x = (mat._23-mat._32) / (2*s); + y = (mat._31-mat._13) / (2*s); + z = (mat._12-mat._21) / (2*s); + w = 0.5f * s; + } + else + { + + + } + FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z; + FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z; + FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z; + + mat._11 = 1 - 2 * ( yy + zz ); + mat._12 = 2 * ( xy - wz ); + mat._13 = 2 * ( xz + wy ); + + mat._21 = 2 * ( xy + wz ); + mat._22 = 1 - 2 * ( xx + zz ); + mat._23 = 2 * ( yz - wx ); + + mat._31 = 2 * ( xz - wy ); + mat._32 = 2 * ( yz + wx ); + mat._33 = 1 - 2 * ( xx + yy ); + + mat._14 = mat._24 = mat._34 = 0.0f; + mat._41 = mat._42 = mat._43 = 0.0f; + mat._44 = 1.0f; +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_QuaternionMultiply() +// Desc: Mulitples two quaternions together as in {Q} = {A} * {B}. +//----------------------------------------------------------------------------- +VOID D3DMath_QuaternionMultiply( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw, + FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw, + FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw ) +{ + FLOAT Dx = Ax*Bw + Ay*Bz - Az*By + Aw*Bx; + FLOAT Dy = -Ax*Bz + Ay*Bw + Az*Bx + Aw*By; + FLOAT Dz = Ax*By - Ay*Bx + Az*Bw + Aw*Bz; + FLOAT Dw = -Ax*Bx - Ay*By - Az*Bz + Aw*Bw; + + Qx = Dx; Qy = Dy; Qz = Dz; Qw = Dw; +} + + + + +//----------------------------------------------------------------------------- +// Name: D3DMath_SlerpQuaternions() +// Desc: Compute a quaternion which is the spherical linear interpolation +// between two other quaternions by dvFraction. +//----------------------------------------------------------------------------- +VOID D3DMath_QuaternionSlerp( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw, + FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw, + FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw, + FLOAT fAlpha ) +{ + // Compute dot product (equal to cosine of the angle between quaternions) + FLOAT fCosTheta = Ax*Bx + Ay*By + Az*Bz + Aw*Bw; + + // Check angle to see if quaternions are in opposite hemispheres + if( fCosTheta < 0.0f ) + { + // If so, flip one of the quaterions + fCosTheta = -fCosTheta; + Bx = -Bx; By = -By; Bz = -Bz; Bw = -Bw; + } + + // Set factors to do linear interpolation, as a special case where the + // quaternions are close together. + FLOAT fBeta = 1.0f - fAlpha; + + // If the quaternions aren't close, proceed with spherical interpolation + if( 1.0f - fCosTheta > 0.001f ) + { + FLOAT fTheta = acosf( fCosTheta ); + + fBeta = sinf( fTheta*fBeta ) / sinf( fTheta); + fAlpha = sinf( fTheta*fAlpha ) / sinf( fTheta); + } + + // Do the interpolation + Qx = fBeta*Ax + fAlpha*Bx; + Qy = fBeta*Ay + fAlpha*By; + Qz = fBeta*Az + fAlpha*Bz; + Qw = fBeta*Aw + fAlpha*Bw; +} + + + + diff --git a/src/math/d3dmath.h b/src/math/d3dmath.h new file mode 100644 index 0000000..d28707d --- /dev/null +++ b/src/math/d3dmath.h @@ -0,0 +1,97 @@ +// * This file is part of the COLOBOT source code +// * Copyright (C) 2001-2008, Daniel ROUX & EPSITEC SA, www.epsitec.ch +// * +// * 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/. + +//----------------------------------------------------------------------------- +// File: D3DMath.h +// +// Desc: Math functions and shortcuts for Direct3D programming. +// +// Copyright (c) 1997-1999 Microsoft Corporation. All rights reserved +//----------------------------------------------------------------------------- +#ifndef D3DMATH_H +#define D3DMATH_H +#include +#include + + +//----------------------------------------------------------------------------- +// Useful Math constants +//----------------------------------------------------------------------------- +const FLOAT g_PI = 3.14159265358979323846f; // Pi +const FLOAT g_2_PI = 6.28318530717958623200f; // 2 * Pi +const FLOAT g_PI_DIV_2 = 1.57079632679489655800f; // Pi / 2 +const FLOAT g_PI_DIV_4 = 0.78539816339744827900f; // Pi / 4 +const FLOAT g_INV_PI = 0.31830988618379069122f; // 1 / Pi +const FLOAT g_DEGTORAD = 0.01745329251994329547f; // Degrees to Radians +const FLOAT g_RADTODEG = 57.29577951308232286465f; // Radians to Degrees +const FLOAT g_HUGE = 1.0e+38f; // Huge number for FLOAT +const FLOAT g_EPSILON = 1.0e-5f; // Tolerance for FLOATs + + + + +//----------------------------------------------------------------------------- +// Fuzzy compares (within tolerance) +//----------------------------------------------------------------------------- +inline BOOL D3DMath_IsZero( FLOAT a, FLOAT fTol = g_EPSILON ) +{ return ( a <= 0.0f ) ? ( a >= -fTol ) : ( a <= fTol ); } + + + + +//----------------------------------------------------------------------------- +// Matrix functions +//----------------------------------------------------------------------------- +VOID D3DMath_MatrixMultiply( D3DMATRIX& q, D3DMATRIX& a, D3DMATRIX& b ); +HRESULT D3DMath_MatrixInvert( D3DMATRIX& q, D3DMATRIX& a ); + + + + +//----------------------------------------------------------------------------- +// Vector functions +//----------------------------------------------------------------------------- +HRESULT D3DMath_VectorMatrixMultiply( D3DVECTOR& vDest, D3DVECTOR& vSrc, + D3DMATRIX& mat); +HRESULT D3DMath_VertexMatrixMultiply( D3DVERTEX& vDest, D3DVERTEX& vSrc, + D3DMATRIX& mat ); + + + + +//----------------------------------------------------------------------------- +// Quaternion functions +//----------------------------------------------------------------------------- +VOID D3DMath_QuaternionFromRotation( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w, + D3DVECTOR& v, FLOAT fTheta ); +VOID D3DMath_RotationFromQuaternion( D3DVECTOR& v, FLOAT& fTheta, + FLOAT x, FLOAT y, FLOAT z, FLOAT w ); +VOID D3DMath_QuaternionFromAngles( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w, + FLOAT fYaw, FLOAT fPitch, FLOAT fRoll ); +VOID D3DMath_MatrixFromQuaternion( D3DMATRIX& mat, FLOAT x, FLOAT y, FLOAT z, + FLOAT w ); +VOID D3DMath_QuaternionFromMatrix( FLOAT &x, FLOAT &y, FLOAT &z, FLOAT &w, + D3DMATRIX& mat ); +VOID D3DMath_QuaternionMultiply( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw, + FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw, + FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw ); +VOID D3DMath_QuaternionSlerp( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw, + FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw, + FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw, + FLOAT fAlpha ); + + +#endif // D3DMATH_H diff --git a/src/math/math3d.cpp b/src/math/math3d.cpp new file mode 100644 index 0000000..9aa5f89 --- /dev/null +++ b/src/math/math3d.cpp @@ -0,0 +1,1035 @@ +// * This file is part of the COLOBOT source code +// * Copyright (C) 2001-2008, Daniel ROUX & EPSITEC SA, www.epsitec.ch +// * +// * 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/. + +// math3d.cpp + +#define STRICT +#define D3D_OVERLOADS + +#include +#include +#include + +#include "struct.h" +#include "d3dengine.h" +#include "d3dmath.h" +#include "d3dutil.h" +#include "math3d.h" + + + +// Returns TRUE if two numbers are nearly equal. + +BOOL IsEqual(float a, float b) +{ + return Abs(a-b) < CHOUIA; +} + + +// Returns the minimum value. + +float Min(float a, float b) +{ + if ( a <= b ) return a; + else return b; +} + +float Min(float a, float b, float c) +{ + return Min( Min(a,b), c ); +} + +float Min(float a, float b, float c, float d) +{ + return Min( Min(a,b), Min(c,d) ); +} + +float Min(float a, float b, float c, float d, float e) +{ + return Min( Min(a,b), Min(c,d), e ); +} + + +// Returns the maximum value. + +float Max(float a, float b) +{ + if ( a >= b ) return a; + else return b; +} + +float Max(float a, float b, float c) +{ + return Max( Max(a,b), c ); +} + +float Max(float a, float b, float c, float d) +{ + return Max( Max(a,b), Max(c,d) ); +} + +float Max(float a, float b, float c, float d, float e) +{ + return Max( Max(a,b), Max(c,d), e ); +} + + +// Returns the normalized value (0 .. 1). + +float Norm(float a) +{ + if ( a < 0.0f ) return 0.0f; + if ( a > 1.0f ) return 1.0f; + return a; +} + + +// Returns the absolute value of a number. + +float Abs(float a) +{ + return (float)fabs(a); +} + + +// Swaps two integers. + +void Swap(int &a, int &b) +{ + int c; + + c = a; + a = b; + b = c; +} + +// Swaps two real numbers. + +void Swap(float &a, float &b) +{ + float c; + + c = a; + a = b; + b = c; +} + +// Permutes two points. + +void Swap(FPOINT &a, FPOINT &b) +{ + FPOINT c; + + c = a; + a = b; + b = c; +} + +// Returns the modulo of a floating point number. +// Mod(8.1, 4) = 0.1 +// Mod(n, 1) = fractional part of n + +float Mod(float a, float m) +{ + return a - ((int)(a/m))*m; +} + +// Returns a normalized angle, that is in other words between 0 and 2 * PI. + +float NormAngle(float angle) +{ + angle = Mod(angle, PI*2.0f); + if ( angle < 0.0f ) + { + return PI*2.0f + angle; + } + else + { + return angle; + } +} + +// Test if a angle is between two terminals. + +BOOL TestAngle(float angle, float min, float max) +{ + angle = NormAngle(angle); + min = NormAngle(min); + max = NormAngle(max); + + if ( min > max ) + { + return ( angle <= max || angle >= min ); + } + else + { + return ( angle >= min && angle <= max ); + } +} + + +// Calculates the angle to rotate the angle a to the angle g. +// A positive angle is counterclockwise (CCW). + +float Direction(float a, float g) +{ + a = NormAngle(a); + g = NormAngle(g); + + if ( a < g ) + { + if ( a+PI*2.0f-g < g-a ) a += PI*2.0f; + } + else + { + if ( g+PI*2.0f-a < a-g ) g += PI*2.0f; + } + return (g-a); +} + + +// Rotates a point around a center. +// The angle is in radians. +// A positive angle is counterclockwise (CCW). + +FPOINT RotatePoint(FPOINT center, float angle, FPOINT p) +{ + FPOINT a, b; + + a.x = p.x-center.x; + a.y = p.y-center.y; + + 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. +// The angle is in radians. +// A positive angle is counterclockwise (CCW). + +FPOINT RotatePoint(float angle, FPOINT p) +{ + FPOINT a; + + a.x = p.x*cosf(angle) - p.y*sinf(angle); + a.y = p.x*sinf(angle) + p.y*cosf(angle); + + return a; +} + +// Rotates a vector (dist, 0). +// The angle is in radians. +// A positive angle is counterclockwise (CCW). + +FPOINT RotatePoint(float angle, float dist) +{ + FPOINT a; + + a.x = dist*cosf(angle); + a.y = dist*sinf(angle); + + return a; +} + +// Calculates the angle of a right triangle. +// The angle is counterclockwise (CCW), between 0 and 2 * PI. +// For an angle clockwise (CW), just go ahead. +// +// ^ +// | +// y o----o +// | / | +// |/)a | +// ----o----o--> +// | x +// | + +float RotateAngle(float x, float y) +{ +#if 1 + 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 PI*0.5f - atanf(x/y); + } + else + { + if ( x > -y ) return PI*2.0f + atanf(y/x); + else return PI*1.5f - atanf(x/y); + } + } + else + { + if ( y >= 0.0f ) + { + if ( -x > y ) return PI*1.0f + atanf(y/x); + else return PI*0.5f - atanf(x/y); + } + else + { + if ( -x > -y ) return PI*1.0f + atanf(y/x); + else return PI*1.5f - atanf(x/y); + } + } +#else + float angle; + + if ( x == 0.0f ) + { + if ( y > 0.0f ) + { + return 90.0f*PI/180.0f; + } + else + { + return 270.0f*PI/180.0f; + } + } + else + { + angle = atanf(y/x); + if ( x < 0.0f ) + { + angle += PI; + } + return angle; + } +#endif +} + +// Calculates the angle between two points and one center. +// The angle is in radians. +// A positive angle is counterclockwise (CCW). + +float RotateAngle(FPOINT center, FPOINT p1, FPOINT p2) +{ + float a1, a2, a; + + if ( p1.x == center.x && + p1.y == center.y ) return 0; + + if ( p2.x == center.x && + p2.y == center.y ) return 0; + + a1 = asinf((p1.y-center.y)/Length(p1,center)); + a2 = asinf((p2.y-center.y)/Length(p2,center)); + + if ( p1.x < center.x ) a1 = PI-a1; + if ( p2.x < center.x ) a2 = PI-a2; + + a = a2-a1; + if ( a < 0 ) a += PI*2; + return a; +} + +// Returns py up on the line ab. + +float MidPoint(FPOINT a, FPOINT b, float px) +{ + if ( Abs(a.x-b.x) < CHOUIA ) + { + if ( a.y < b.y ) return BEAUCOUP; + else return -BEAUCOUP; + } + return (b.y-a.y)*(px-a.x)/(b.x-a.x)+a.y; +} + +// Advance "dist" along the segment p1-p2. + +D3DVECTOR SegmentDist(const D3DVECTOR &p1, const D3DVECTOR &p2, float dist) +{ + return p1+Normalize(p2-p1)*dist; +} + +// Check if a point is inside a triangle. + +BOOL IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c, FPOINT 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) && (np.y||p.y>m) && (n= 100 ) break; + } + } + + sum.x = 0; + sum.y = 0; + sum.z = 0; + for ( j=0 ; j 0.1f || + Abs(n1.y-n2.y) > 0.1f || + Abs(n1.z-n2.z) > 0.1f ) return FALSE; + + dist = DistancePlanPoint(plan1[0], plan1[1], plan1[2], plan2[0]); + if ( dist > 0.1f ) return FALSE; + + return TRUE; +} + + +// Calculates the matrix to make three rotations in the X, Y and Z +// >>>>>> OPTIMIZING!!! + +void MatRotateXZY(D3DMATRIX &mat, D3DVECTOR angle) +{ + D3DMATRIX temp; + + D3DUtil_SetRotateXMatrix(temp, angle.x); + D3DUtil_SetRotateZMatrix(mat, angle.z); + D3DMath_MatrixMultiply(mat, mat, temp); + D3DUtil_SetRotateYMatrix(temp, angle.y); + D3DMath_MatrixMultiply(mat, mat, temp); // X-Z-Y +} + +// Calculates the matrix to make three rotations in the order Z, X and Y. +// >>>>>> OPTIMIZING!!! + +void MatRotateZXY(D3DMATRIX &mat, D3DVECTOR angle) +{ + D3DMATRIX temp; + + D3DUtil_SetRotateZMatrix(temp, angle.z); + D3DUtil_SetRotateXMatrix(mat, angle.x); + D3DMath_MatrixMultiply(mat, mat, temp); + D3DUtil_SetRotateYMatrix(temp, angle.y); + D3DMath_MatrixMultiply(mat, mat, temp); // Z-X-Y +} + + +// Returns a random value between 0 and 1. + +float Rand() +{ + return (float)rand()/RAND_MAX; +} + + +// Managing the dead zone of a joystick. + +// in: -1 0 1 +// --|-------|----o----|-------|--> +// <----> +// dead +// out: -1 0 0 1 + +float Neutral(float value, float dead) +{ + if ( Abs(value) <= dead ) + { + return 0.0f; + } + else + { + if ( value > 0.0f ) return (value-dead)/(1.0f-dead); + else return (value+dead)/(1.0f-dead); + } +} + + +// Calculates a value (radians) proportional between a and b (degrees). + +float Prop(int a, int b, float p) +{ + float aa, bb; + + aa = (float)a*PI/180.0f; + bb = (float)b*PI/180.0f; + + return aa+p*(bb-aa); +} + +// Gently advanced a desired value from its current value. +// Over time, the greater the progression is rapid. + +float Smooth(float actual, float hope, float time) +{ + float futur; + + futur = actual + (hope-actual)*time; + + if ( hope > actual ) + { + if ( futur > hope ) futur = hope; + } + if ( hope < actual ) + { + if ( futur < hope ) futur = hope; + } + + return futur; +} + + +// Bounces any movement. + +// out +// | +// 1+------o-------o--- +// | o | o o | | bounce +// | o | o---|--- +// | o | | +// | o | | +// -o------|-------+----> progress +// 0| | 1 +// |<---->|middle + +float Bounce(float progress, float middle, float bounce) +{ + if ( progress < middle ) + { + progress = progress/middle; // 0..1 + return 0.5f+sinf(progress*PI-PI/2.0f)/2.0f; + } + else + { + progress = (progress-middle)/(1.0f-middle); // 0..1 + return (1.0f-bounce/2.0f)+sinf((0.5f+progress*2.0f)*PI)*(bounce/2.0f); + } +} + + +// Returns the color corresponding D3DCOLOR. + +D3DCOLOR RetColor(float intensity) +{ + D3DCOLOR color; + + if ( intensity <= 0.0f ) return 0x00000000; + if ( intensity >= 1.0f ) return 0xffffffff; + + color = (int)(intensity*255.0f)<<24; + color |= (int)(intensity*255.0f)<<16; + color |= (int)(intensity*255.0f)<<8; + color |= (int)(intensity*255.0f); + + return color; +} + +// Returns the color corresponding D3DCOLOR. + +D3DCOLOR RetColor(D3DCOLORVALUE intensity) +{ + D3DCOLOR color; + + color = (int)(intensity.a*255.0f)<<24; + color |= (int)(intensity.r*255.0f)<<16; + color |= (int)(intensity.g*255.0f)<<8; + color |= (int)(intensity.b*255.0f); + + return color; +} + +// Returns the color corresponding D3DCOLORVALUE. + +D3DCOLORVALUE RetColor(D3DCOLOR intensity) +{ + D3DCOLORVALUE color; + + color.r = (float)((intensity>>16)&0xff)/256.0f; + color.g = (float)((intensity>>8 )&0xff)/256.0f; + color.b = (float)((intensity>>0 )&0xff)/256.0f; + color.a = (float)((intensity>>24)&0xff)/256.0f; + + return color; +} + + +// RGB to HSV conversion. + +void RGB2HSV(D3DCOLORVALUE src, ColorHSV &dest) +{ + float min, max, delta; + + min = Min(src.r, src.g, src.b); + max = Max(src.r, src.g, src.b); + + dest.v = max; // intensity + + if ( max == 0.0f ) + { + dest.s = 0.0f; // saturation + dest.h = 0.0f; // undefined color! + } + else + { + delta = max-min; + dest.s = delta/max; // saturation + + if ( src.r == max ) // between yellow & magenta + { + dest.h = (src.g-src.b)/delta; + } + else if ( src.g == max ) // between cyan & yellow + { + dest.h = 2.0f+(src.b-src.r)/delta; + } + else // between magenta & cyan + { + dest.h = 4.0f+(src.r-src.g)/delta; + } + + dest.h *= 60.0f; // in degrees + if ( dest.h < 0.0f ) dest.h += 360.0f; + dest.h /= 360.0f; // 0..1 + } +} + +// HSV to RGB conversion. + +void HSV2RGB(ColorHSV src, D3DCOLORVALUE &dest) +{ + int i; + float f,v,p,q,t; + + src.h = Norm(src.h)*360.0f; + src.s = Norm(src.s); + src.v = Norm(src.v); + + if ( src.s == 0.0f ) // zero saturation? + { + dest.r = src.v; + dest.g = src.v; + dest.b = src.v; // gray + } + else + { + if ( src.h == 360.0f ) src.h = 0.0f; + src.h /= 60.0f; + i = (int)src.h; // integer part (0 .. 5) + f = src.h-i; // fractional part + + v = src.v; + p = src.v*(1.0f-src.s); + q = src.v*(1.0f-(src.s*f)); + t = src.v*(1.0f-(src.s*(1.0f-f))); + + switch (i) + { + case 0: dest.r=v; dest.g=t; dest.b=p; break; + case 1: dest.r=q; dest.g=v; dest.b=p; break; + case 2: dest.r=p; dest.g=v; dest.b=t; break; + case 3: dest.r=p; dest.g=q; dest.b=v; break; + case 4: dest.r=t; dest.g=p; dest.b=v; break; + case 5: dest.r=v; dest.g=p; dest.b=q; break; + } + } +} + diff --git a/src/math/math3d.h b/src/math/math3d.h new file mode 100644 index 0000000..ce7eee3 --- /dev/null +++ b/src/math/math3d.h @@ -0,0 +1,106 @@ +// * This file is part of the COLOBOT source code +// * Copyright (C) 2001-2008, Daniel ROUX & EPSITEC SA, www.epsitec.ch +// * +// * 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/. + +// math3d.h + +#ifndef _MATH3D_H_ +#define _MATH3D_H_ + + +#define STRICT +#define D3D_OVERLOADS +#include + + +#define PI 3.14159265358979323846f +#define CHOUIA 1e-6f +#define BEAUCOUP 1e6f + + + +BOOL IsEqual(float a, float b); + +float Min(float a, float b); +float Min(float a, float b, float c); +float Min(float a, float b, float c, float d); +float Min(float a, float b, float c, float d, float e); + +float Max(float a, float b); +float Max(float a, float b, float c); +float Max(float a, float b, float c, float d); +float Max(float a, float b, float c, float d, float e); + +float Norm(float a); +float Abs(float a); + +void Swap(int &a, int &b); +void Swap(float &a, float &b); +void Swap(FPOINT &a, FPOINT &b); + +float Mod(float a, float m); +float NormAngle(float angle); +BOOL TestAngle(float angle, float min, float max); + +float Direction(float a, float g); +FPOINT RotatePoint(FPOINT center, float angle, FPOINT p); +FPOINT RotatePoint(float angle, FPOINT p); +FPOINT RotatePoint(float angle, float dist); +float RotateAngle(float x, float y); +float RotateAngle(FPOINT center, FPOINT p1, FPOINT p2); +float MidPoint(FPOINT a, FPOINT b, float px); +D3DVECTOR SegmentDist(const D3DVECTOR &p1, const D3DVECTOR &p2, float dist); +BOOL IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c, FPOINT p); +BOOL Intersect(D3DVECTOR a, D3DVECTOR b, D3DVECTOR c, D3DVECTOR d, D3DVECTOR e, D3DVECTOR &i); +BOOL IntersectY(D3DVECTOR a, D3DVECTOR b, D3DVECTOR c, D3DVECTOR &p); +void RotatePoint(float cx, float cy, float angle, float &px, float &py); +void RotatePoint(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p); +void RotatePoint2(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p); +D3DVECTOR RotateView(D3DVECTOR center, float angleH, float angleV, float dist); +D3DVECTOR LookatPoint( D3DVECTOR eye, float angleH, float angleV, float length ); +float Length(FPOINT a, FPOINT b); +float Length(float x, float y); +float Length(const D3DVECTOR &u); +float Length(const D3DVECTOR &a, const D3DVECTOR &b); +float Length2d(const D3DVECTOR &a, const D3DVECTOR &b); +float Angle( D3DVECTOR u, D3DVECTOR v ); +D3DVECTOR Cross( D3DVECTOR u, D3DVECTOR v ); +D3DVECTOR ComputeNormal( D3DVECTOR p1, D3DVECTOR p2, D3DVECTOR p3 ); +D3DVECTOR Transform(const D3DMATRIX &m, D3DVECTOR p); +D3DVECTOR Projection(const D3DVECTOR &a, const D3DVECTOR &b, const D3DVECTOR &p); + +void MappingObject( D3DVERTEX2* pVertices, int nb, float scale ); +void SmoothObject( D3DVERTEX2* pVertices, int nb ); +BOOL LineFunction(FPOINT p1, FPOINT p2, float &a, float &b); +float DistancePlanPoint(const D3DVECTOR &a, const D3DVECTOR &b, const D3DVECTOR &c, const D3DVECTOR &p); +BOOL IsSamePlane(D3DVECTOR *plan1, D3DVECTOR *plan2); +void MatRotateXZY(D3DMATRIX &mat, D3DVECTOR angle); +void MatRotateZXY(D3DMATRIX &mat, D3DVECTOR angle); + +float Rand(); +float Neutral(float value, float dead); + +float Prop(int a, int b, float p); +float Smooth(float actual, float hope, float time); +float Bounce(float progress, float middle=0.3f, float bounce=0.4f); + +D3DCOLOR RetColor(float intensity); +D3DCOLOR RetColor(D3DCOLORVALUE intensity); +D3DCOLORVALUE RetColor(D3DCOLOR intensity); + +void RGB2HSV(D3DCOLORVALUE src, ColorHSV &dest); +void HSV2RGB(ColorHSV src, D3DCOLORVALUE &dest); + +#endif //_MATH3D_H_ -- cgit v1.2.3-1-g7c22