diff options
Diffstat (limited to 'src/math/d3dmath.cpp')
| -rw-r--r-- | src/math/d3dmath.cpp | 343 |
1 files changed, 343 insertions, 0 deletions
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 <math.h>
+#include <stdio.h>
+#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;
+}
+
+
+
+
|