 |
Vector / Matrix / Quaternion Math Library
Submitted by |
The development process of my new 3D engine produced some code part that may be useful for other coders: The main math directory, containing a 2D-, 3D- and 4D vector implementation, a 4x4 matrix class and of course a complete quaternion package. There's also a implementation for fast Square Root computation and assembled SinCos and SinCosVec functions.
During the development process I changed the math system several times. First of all I began with some big classes (CVector3, CMatrix4x4, ...) that contained the functions direct in the .cpp-file. It had two bad effects: Unuseability ("Construct a entire class for just computing a dot product????") and Slowness (20 cpu clocks for a __thiscall and 10 for the computation....). Then I changed to a semi-class structure: Some seperate functions in the .cpp-file with __fastcall call convention and only a few in the class. But this was a big mistake: You'd always to look up wheter the function is in the class or directly accessible..... Not good.
Now the math system is at a state I like it. All smaller functions are now macros in the .h-file (the DotProduct for example) and the others are normal functions in the .cpp file. The class only exists in the .h file any longer and wraps all functions via ForceInline.
As a result you can chose now for your own the system you want to use: The macros provide you with high speed (no function overhead), the cpp-functions give you full access from every point of the program and the class, if you want to use it (I'm currently not) gives you always the big view over all the functions.
Important note: The math library is designed for speed, not for readability or other purposes. It does not contain any comments or documentation and also some of the function doesn't seem to make any sense cause of precomputation (for example precomputed matrix X-, Y- and Z-rotation). But if you just search a implementation of working (I tested nearly every function, but surely there are some bugs left for you :-) ) stuff as reference thats it. So you should be a little bit familar with the mathematical stuff.
Copyright note: Everyone can use the library for free (commercial or not) when he notices my work. The QuatToMatrix function (quaternion module) is closely taken from the Genesis 3D engine ( www.genesis3d.com I think) - good work to the coder who made it.
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/FastSqrt.cpp] - (717 bytes)
// FastSqrt.cpp
//
#include "FastSqrt.h"
#include <math.h>
typedef union FastSqrtUnion FastSqrtUnion, *PFastSqrtUnion, &RFastSqrtUnion;
union FastSqrtUnion
{
Float float_val;
UInt32 uint_val;
};
UInt32 FastSqrtTable[0x10000];
Void BuildSqrtTable()
{
Int i;
FastSqrtUnion squareroot;
for (i = 0; i <= 0x7FFF; i++)
{
squareroot.uint_val = (i << 8) | (0x7F << 23);
squareroot.float_val = (Float) sqrt(squareroot.float_val);
FastSqrtTable[i + 0x8000] = (squareroot.uint_val & 0x7FFFFF);
squareroot.uint_val = (i << 8) | (0x80 << 23);
squareroot.float_val = (Float) sqrt(squareroot.float_val);
FastSqrtTable[i] = (squareroot.uint_val & 0x7FFFFF);
}
} |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/FastSqrt.h] - (442 bytes)
// FastSqrt.h
//
#ifndef _3D_FASTSQRT_H
#define _3D_FASTSQRT_H
#include "Types.h"
#define FSqrt(x, squareroot) { extern UInt32 FastSqrtTable[0x10000]; *((UInt *) &(squareroot)) = (FastSqrtTable[(*(Int *) &((Float) x) >> 8) & 0xFFFF] ^ ((((*(Int *) &((Float) x) - 0x3F800000) >> 1) + 0x3F800000) & 0x7F800000)); }
ForceInline Float FastSqrt(Float x)
{
FSqrt(x, x);
return x;
}
Void BuildSqrtTable();
#endif |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/FastTrigonometry.cpp] - (68 bytes)
// FastTrigonometry.cpp
//
#include "FastTrigonometry.h"
|
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/FastTrigonometry.h] - (1,195 bytes)
// FastTrigonometry.h
//
#ifndef _3D_FASTTRIGONOMETRY_H
#define _3D_FASTTRIGONOMETRY_H
#include "Types.h"
#include "Math\Vector\Vector 3.h"
ForceInline Float __fastcall SinCos(Float x, PFloat cosval)
{
x; cosval; // Just for this shit "unreferenced formal parameter" warning
__asm
{
fld dword ptr [x]
fsincos
fstp dword ptr [cosval]
}
}
ForceInline Void __fastcall SinCosVec(PVector3 vec, PVector3 sinvalues, PVector3 cosvalues)
{
vec; sinvalues; cosvalues; // Just for this shit "unreferenced formal parameter" warning
__asm
{
mov eax, vec
mov ecx, sinvalues
mov edx, cosvalues
fld dword ptr [eax]
fsincos
fstp dword ptr [ecx]
fstp dword ptr [edx]
fld dword ptr [eax+4]
fsincos
fstp dword ptr [ecx+4]
fstp dword ptr [edx+4]
fld dword ptr [eax+8]
fsincos
fstp dword ptr [ecx+8]
fstp dword ptr [edx+8]
}
}
ForceInline Float __fastcall CoTan(Float x)
{
x; // Just for this shit "unreferenced formal parameter" warning
__asm
{
fld dword ptr [x]
fptan
fdivrp st(1), st
}
}
#endif |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Matrix/Matrix 4x4.cpp] - (15,613 bytes)
// Matrix 4x4.cpp
//
#include "Matrix 4x4.h"
#include "Math\FastSqrt.h"
#include "Math\FastTrigonometry.h"
#include <math.h>
const Matrix4x4 IdentityMatrix = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f };
#define MATRIX_ORTHONORMAL_TOLERANCE 0.001
#define MATRIX_ORTHOGONAL_TOLERANCE 0.001
Bool MatrixIsValid(RMatrix4x4 matrix)
{
if ((matrix.f11 * matrix.f11) < 0.0f)
return FALSE;
if ((matrix.f21 * matrix.f21) < 0.0f)
return FALSE;
if ((matrix.f31 * matrix.f31) < 0.0f)
return FALSE;
if ((matrix.f41 * matrix.f41) < 0.0f)
return FALSE;
if ((matrix.f12 * matrix.f12) < 0.0f)
return FALSE;
if ((matrix.f22 * matrix.f22) < 0.0f)
return FALSE;
if ((matrix.f32 * matrix.f32) < 0.0f)
return FALSE;
if ((matrix.f42 * matrix.f42) < 0.0f)
return FALSE;
if ((matrix.f13 * matrix.f13) < 0.0f)
return FALSE;
if ((matrix.f23 * matrix.f23) < 0.0f)
return FALSE;
if ((matrix.f33 * matrix.f33) < 0.0f)
return FALSE;
if ((matrix.f43 * matrix.f43) < 0.0f)
return FALSE;
if ((matrix.f14 * matrix.f14) < 0.0f)
return FALSE;
if ((matrix.f24 * matrix.f24) < 0.0f)
return FALSE;
if ((matrix.f34 * matrix.f34) < 0.0f)
return FALSE;
if ((matrix.f44 * matrix.f44) < 0.0f)
return FALSE;
return TRUE;
}
Bool MatrixIsOrthonormal(RMatrix4x4 matrix)
{
Vector3 normal12;
Vector3 col3;
normal12.m_X = (matrix.f21 * matrix.f32) - (matrix.f31 * matrix.f22);
normal12.m_Y = (matrix.f31 * matrix.f12) - (matrix.f11 * matrix.f32);
normal12.m_Z = (matrix.f11 * matrix.f22) - (matrix.f21 * matrix.f12);
col3.m_X = matrix.f13;
col3.m_Y = matrix.f23;
col3.m_Z = matrix.f33;
if (!Vec3Compare(normal12, col3, (Float) MATRIX_ORTHONORMAL_TOLERANCE))
{
Vec3Invert(col3);
if (!Vec3Compare(normal12, col3, (Float) MATRIX_ORTHONORMAL_TOLERANCE))
{
return FALSE;
}
}
return TRUE;
}
Bool MatrixIsOrthogonal(RMatrix4x4 matrix)
{
Vector3 normal12;
Vector3 col3;
normal12.m_X = (matrix.f21 * matrix.f32) - (matrix.f31 * matrix.f22);
normal12.m_Y = (matrix.f31 * matrix.f12) - (matrix.f11 * matrix.f32);
normal12.m_Z = (matrix.f11 * matrix.f22) - (matrix.f21 * matrix.f12);
col3.m_X = matrix.f13;
col3.m_Y = matrix.f23;
col3.m_Z = matrix.f33;
Vec3Normalize(normal12);
Vec3Normalize(col3);
if (!Vec3Compare(normal12, col3, (Float) MATRIX_ORTHONORMAL_TOLERANCE))
{
Vec3Invert(col3);
if (!Vec3Compare(normal12, col3, (Float) MATRIX_ORTHONORMAL_TOLERANCE))
{
return FALSE;
}
}
return TRUE;
}
Void MatrixOrthonormalize(RMatrix4x4 matrix)
{
Float fLength = FastSqrt((matrix.f11 * matrix.f11) + (matrix.f21 * matrix.f21) + (matrix.f31 * matrix.f31));
if (fLength > 0.0f)
{
fLength = 1.0f / fLength;
matrix.f11 *= fLength;
matrix.f21 *= fLength;
matrix.f31 *= fLength;
}
fLength = FastSqrt((matrix.f12 * matrix.f12) + (matrix.f22 * matrix.f22) + (matrix.f32 * matrix.f32));
if (fLength > 0.0f)
{
fLength = 1.0f / fLength;
matrix.f12 *= fLength;
matrix.f22 *= fLength;
matrix.f32 *= fLength;
}
matrix.f13 = (matrix.f21 * matrix.f32) - (matrix.f31 * matrix.f22);
matrix.f23 = (matrix.f31 * matrix.f12) - (matrix.f11 * matrix.f32);
matrix.f33 = (matrix.f11 * matrix.f22) - (matrix.f21 * matrix.f12);
}
Void MatrixSetXRotation(RMatrix4x4 matrix, Float m_XAngle)
{
Float sinx, cosx;
sinx = SinCos(m_XAngle, &cosx);
matrix.f11 = matrix.f44 = 1.0f;
matrix.f22 = cosx;
matrix.f23 = -sinx;
matrix.f32 = sinx;
matrix.f33 = cosx;
matrix.f12 = matrix.f13 = matrix.f14 = matrix.f21 = matrix.f24 = matrix.f31 = matrix.f34 = matrix.f41 = matrix.f42 = matrix.f43 = 0.0f;
}
Void MatrixSetYRotation(RMatrix4x4 matrix, Float m_YAngle)
{
Float sy, cy;
sy = SinCos(m_YAngle, &cy);
matrix.f22 = matrix.f44 = 1.0f;
matrix.f11 = cy;
matrix.f13 = sy;
matrix.f31 = -sy;
matrix.f33 = cy;
matrix.f12 = matrix.f14 = matrix.f21 = matrix.f23 = matrix.f24 = matrix.f32 = matrix.f34 = matrix.f41 = matrix.f42 = matrix.f43 = 0.0f;
}
Void MatrixSetZRotation(RMatrix4x4 matrix, Float m_ZAngle)
{
Float sz, cz;
sz = SinCos(m_ZAngle, &cz);
matrix.f33 = matrix.f44 = 1.0f;
matrix.f11 = cz;
matrix.f12 = -sz;
matrix.f21 = sz;
matrix.f22 = cz;
matrix.f13 = matrix.f14 = matrix.f23 = matrix.f24 = matrix.f31 = matrix.f32 = matrix.f34 = matrix.f41 = matrix.f42 = matrix.f43 = 0.0f;
}
Void MatrixSetXYZRotation(RMatrix4x4 matrix, Float m_XAngle, Float m_YAngle, Float m_ZAngle)
{
Float sx, cxx;
Float sy, cy;
Float sz, cz;
sx = SinCos(m_XAngle, &cxx);
sy = SinCos(m_YAngle, &cy);
sz = SinCos(m_ZAngle, &cz);
matrix.f11 = cy * cz;
matrix.f12 = -cy * sz;
matrix.f13 = sy;
matrix.f21 = cz * sx * sy + cxx * sz;
matrix.f22 = cxx * cz - sx * sy * sz;
matrix.f23 = -cy * sx;
matrix.f31 = -cxx * cz * sy + sx * sz;
matrix.f32 = cz * sx + cxx * sy + sz;
matrix.f33 = cxx * cy;
matrix.f44 = 1.0f;
matrix.f14 = matrix.f24 = matrix.f34 = matrix.f41 = matrix.f42 = matrix.f43 = 0.0f;
}
Void MatrixSetRotationScalingTranslation(RMatrix4x4 matrix, RVector3 vRotation, RVector3 vScaling, RVector3 vTranslation)
{
Vector3 s, c;
SinCosVec(&vRotation, &s, &c);
matrix.f11 = vScaling.m_X * ( c.m_Y * c.m_Z);
matrix.f12 = vScaling.m_Y * (-c.m_Y * s.m_Z);
matrix.f13 = vScaling.m_Z * ( s.m_Y);
matrix.f14 = vTranslation.m_X;
matrix.f21 = vScaling.m_X * ( c.m_Z * s.m_X * s.m_Y + c.m_X * s.m_Z);
matrix.f22 = vScaling.m_Y * ( c.m_X * c.m_Z - s.m_X * s.m_Y * s.m_Z);
matrix.f23 = vScaling.m_Z * (-c.m_Y * s.m_X);
matrix.f24 = vTranslation.m_Y;
matrix.f31 = vScaling.m_X * (-c.m_X * c.m_Z * s.m_Y + s.m_X * s.m_Z);
matrix.f32 = vScaling.m_Y * ( c.m_Z * s.m_X + c.m_X * s.m_Y + s.m_Z);
matrix.f33 = vScaling.m_Z * ( c.m_X * c.m_Y);
matrix.f34 = vTranslation.m_Z;
matrix.f41 = matrix.f42 = matrix.f43 = 0.0f;
matrix.f44 = 1.0f;
}
Bool MatrixSetViewMatrix(RMatrix4x4 matrix, RVector3 vFrom, RVector3 vView, RVector3 vWorldUp)
{
Vector3 vUp, vRight;
Float fDotProduct = (vWorldUp.m_X * vView.m_X) + (vWorldUp.m_Y * vView.m_Y) + (vWorldUp.m_Z * vView.m_Z);
vUp.m_X = vWorldUp.m_X - (fDotProduct * vView.m_X);
vUp.m_Y = vWorldUp.m_Y - (fDotProduct * vView.m_Y);
vUp.m_Z = vWorldUp.m_Z - (fDotProduct * vView.m_Z);
Float fLength = FastSqrt((vUp.m_X * vUp.m_X) + (vUp.m_Y * vUp.m_Y) + (vUp.m_Z * vUp.m_Z));
if (1E-6f > fLength)
{
vUp.m_X = 0.0f - (vView.m_Y * vView.m_X);
vUp.m_Y = 1.0f - (vView.m_Y * vView.m_Y);
vUp.m_Z = 0.0f - (vView.m_Y * vView.m_Z);
fLength = FastSqrt((vUp.m_X * vUp.m_X) + (vUp.m_Y * vUp.m_Y) + (vUp.m_Z * vUp.m_Z));
if (1E-6f > fLength)
{
vUp.m_X = 0.0f - (vView.m_Z * vView.m_X);
vUp.m_Y = 0.0f - (vView.m_Z * vView.m_Y);
vUp.m_Z = 1.0f - (vView.m_Z * vView.m_Z);
fLength = FastSqrt((vUp.m_X * vUp.m_X) + (vUp.m_Y * vUp.m_Y) + (vUp.m_Z * vUp.m_Z));
if (1E-6f > fLength)
{
return FALSE;
}
}
}
fLength = 1.0f / fLength;
vUp.m_X *= fLength;
vUp.m_Y *= fLength;
vUp.m_Z *= fLength;
vRight.m_X = vUp.m_Y * vView.m_Z - vUp.m_Z * vView.m_Y;
vRight.m_Y = vUp.m_Z * vView.m_X - vUp.m_X * vView.m_Z;
vRight.m_Z = vUp.m_X * vView.m_Y - vUp.m_Y * vView.m_X;
matrix.f11 = vRight.m_X;
matrix.f12 = vRight.m_Y;
matrix.f13 = vRight.m_Z;
matrix.f14 = - ((vFrom.m_X * vRight.m_X) + (vFrom.m_Y * vRight.m_Y) + (vFrom.m_Z * vRight.m_Z));
matrix.f21 = vUp.m_X;
matrix.f22 = vUp.m_Y;
matrix.f23 = vUp.m_Z;
matrix.f24 = - ((vFrom.m_X * vUp.m_X) + (vFrom.m_Y * vUp.m_Y) + (vFrom.m_Z * vUp.m_Z));
matrix.f31 = vView.m_X;
matrix.f32 = vView.m_Y;
matrix.f33 = vView.m_Z;
matrix.f34 = - ((vFrom.m_X * vView.m_X) + (vFrom.m_Y * vView.m_Y) + (vFrom.m_Z * vView.m_Z));
matrix.f41 = matrix.f42 = matrix.f43 = 0.0f;
matrix.f44 = 1.0f;
return TRUE;
}
Void MatrixSetOrthogonalMatrix(RMatrix4x4 matrix, Float fLeft, Float fRight, Float fBottom, Float fTop, Float fNear, Float fFar)
{
matrix.f11 = 2.0f / (fRight - fLeft);
matrix.f12 = 0.0f;
matrix.f13 = 0.0f;
matrix.f14 = (fLeft + fRight) / (fLeft - fRight);
matrix.f21 = 0.0f;
matrix.f22 = 2.0f / (fTop - fBottom);
matrix.f23 = 0.0f;
matrix.f24 = (fBottom + fTop) / (fBottom - fTop);
matrix.f31 = 0.0f;
matrix.f32 = 0.0f;
matrix.f33 = 2.0f / (fNear - fFar);
matrix.f34 = (fNear + fFar) / (fNear - fFar);
matrix.f14 = matrix.f24 = matrix.f34 = 0.0f;
matrix.f44 = 1.0f;
}
Void MatrixSetOrthogonal2DMatrix(RMatrix4x4 matrix, Float fLeft, Float fRight, Float fBottom, Float fTop)
{
matrix.f11 = 2.0f / (fRight - fLeft);
matrix.f12 = 0.0f;
matrix.f13 = 0.0f;
matrix.f14 = (fLeft + fRight) / (fLeft - fRight);
matrix.f21 = 0.0f;
matrix.f22 = 2.0f / (fTop - fBottom);
matrix.f23 = 0.0f;
matrix.f24 = (fBottom + fTop) / (fBottom - fTop);
matrix.f31 = 0.0f;
matrix.f32 = 0.0f;
matrix.f33 = -1.0f;
matrix.f34 = 0.0f;
matrix.f41 = matrix.f42 = matrix.f43 = 0.0f;
matrix.f44 = 1.0f;
}
Void MatrixSetFrustumMatrix(RMatrix4x4 matrix, Float fLeft, Float fRight, Float fBottom, Float fTop, Float fNear, Float fFar)
{
matrix.f11 = (2.0f * fNear) / (fRight - fLeft);
matrix.f12 = 0.0f;
matrix.f13 = (fRight + fLeft) / (fRight - fLeft);
matrix.f14 = 0.0f;
matrix.f21 = 0.0f;
matrix.f22 = (2.0f * fNear) / (fTop - fBottom);
matrix.f23 = (fTop + fBottom) / (fTop - fBottom);
matrix.f24 = 0.0f;
matrix.f31 = 0.0f;
matrix.f32 = 0.0f;
matrix.f33 = (fFar + fNear) / (fNear - fFar);
matrix.f34 = (2.0f * fFar * fNear) / (fNear - fFar);
matrix.f41 = 0.0f;
matrix.f42 = 0.0f;
matrix.f43 = -1.0f;
matrix.f44 = 0.0f;
}
Void MatrixSetPerspectiveMatrix(RMatrix4x4 matrix, Float fFov, Float fAspect, Float fNear, Float fFar)
{
fFov = CoTan(fFov * 0.5f);
matrix.f11 = fNear / (fFov * fAspect);
matrix.f12 = 0.0f;
matrix.f13 = 0.0f;
matrix.f14 = 0.0f;
matrix.f21 = 0.0f;
matrix.f22 = fNear / fFov;
matrix.f23 = 0.0f;
matrix.f24 = 0.0f;
matrix.f31 = 0.0f;
matrix.f32 = 0.0f;
matrix.f33 = (fFar + fNear) / (fNear - fFar);
matrix.f34 = (2.0f * fFar * fNear) / (fNear - fFar);
matrix.f41 = 0.0f;
matrix.f42 = 0.0f;
matrix.f43 = -1.0f;
matrix.f44 = 0.0f;
}
Void MatrixAdd(RMatrix4x4 m1, RMatrix4x4 m2, RMatrix4x4 mdest)
{
mdest.f11 = m1.f11 + m2.f11;
mdest.f12 = m1.f12 + m2.f12;
mdest.f13 = m1.f13 + m2.f13;
mdest.f14 = m1.f14 + m2.f14;
mdest.f21 = m1.f21 + m2.f21;
mdest.f22 = m1.f22 + m2.f22;
mdest.f23 = m1.f23 + m2.f23;
mdest.f24 = m1.f24 + m2.f24;
mdest.f31 = m1.f31 + m2.f31;
mdest.f32 = m1.f32 + m2.f32;
mdest.f33 = m1.f33 + m2.f33;
mdest.f34 = m1.f34 + m2.f34;
mdest.f41 = m1.f41 + m2.f41;
mdest.f42 = m1.f42 + m2.f42;
mdest.f43 = m1.f43 + m2.f43;
mdest.f44 = m1.f44 + m2.f44;
}
Void MatrixMultiply(RMatrix4x4 m1, RMatrix4x4 m2, RMatrix4x4 mdest)
{
Matrix4x4 tmp;
tmp.f11 = (m1.f11 * m2.f11) + (m1.f12 * m2.f21) + (m1.f13 * m2.f31) + (m1.f14 * m2.f41);
tmp.f12 = (m1.f11 * m2.f12) + (m1.f12 * m2.f22) + (m1.f13 * m2.f32) + (m1.f14 * m2.f42);
tmp.f13 = (m1.f11 * m2.f13) + (m1.f12 * m2.f23) + (m1.f13 * m2.f33) + (m1.f14 * m2.f43);
tmp.f14 = (m1.f11 * m2.f14) + (m1.f12 * m2.f24) + (m1.f13 * m2.f34) + (m1.f14 * m2.f44);
tmp.f21 = (m1.f21 * m2.f11) + (m1.f22 * m2.f21) + (m1.f23 * m2.f31) + (m1.f24 * m2.f41);
tmp.f22 = (m1.f21 * m2.f12) + (m1.f22 * m2.f22) + (m1.f23 * m2.f32) + (m1.f24 * m2.f42);
tmp.f23 = (m1.f21 * m2.f13) + (m1.f22 * m2.f23) + (m1.f23 * m2.f33) + (m1.f24 * m2.f43);
tmp.f24 = (m1.f21 * m2.f14) + (m1.f22 * m2.f24) + (m1.f23 * m2.f34) + (m1.f24 * m2.f44);
tmp.f31 = (m1.f31 * m2.f11) + (m1.f32 * m2.f21) + (m1.f33 * m2.f31) + (m1.f34 * m2.f41);
tmp.f32 = (m1.f31 * m2.f12) + (m1.f32 * m2.f22) + (m1.f33 * m2.f32) + (m1.f34 * m2.f42);
tmp.f33 = (m1.f31 * m2.f13) + (m1.f32 * m2.f23) + (m1.f33 * m2.f33) + (m1.f34 * m2.f43);
tmp.f34 = (m1.f31 * m2.f14) + (m1.f32 * m2.f24) + (m1.f33 * m2.f34) + (m1.f34 * m2.f44);
tmp.f11 = (m1.f41 * m2.f11) + (m1.f42 * m2.f21) + (m1.f43 * m2.f31) + (m1.f44 * m2.f41);
tmp.f12 = (m1.f41 * m2.f12) + (m1.f42 * m2.f22) + (m1.f43 * m2.f32) + (m1.f44 * m2.f42);
tmp.f13 = (m1.f41 * m2.f13) + (m1.f42 * m2.f23) + (m1.f43 * m2.f33) + (m1.f44 * m2.f43);
tmp.f14 = (m1.f41 * m2.f14) + (m1.f42 * m2.f24) + (m1.f43 * m2.f34) + (m1.f44 * m2.f44);
mdest = tmp;
}
Void MatrixRotateX(RMatrix4x4 matrix, Float m_XAngle)
{
Float sx, cx;
sx = SinCos(m_XAngle, &cx);
matrix.f12 = (matrix.f12 * cx) + (matrix.f13 * sx);
matrix.f22 = (matrix.f22 * cx) + (matrix.f23 * sx);
matrix.f32 = (matrix.f32 * cx) + (matrix.f33 * sx);
matrix.f42 = (matrix.f42 * cx) + (matrix.f43 * sx);
matrix.f13 = (matrix.f13 * cx) - (matrix.f12 * sx);
matrix.f23 = (matrix.f23 * cx) - (matrix.f22 * sx);
matrix.f33 = (matrix.f33 * cx) - (matrix.f32 * sx);
matrix.f43 = (matrix.f43 * cx) - (matrix.f42 * sx);
}
Void MatrixRotateY(RMatrix4x4 matrix, Float m_YAngle)
{
Float sy, cy;
sy = SinCos(m_YAngle, &cy);
matrix.f11 = (matrix.f11 * cy) - (matrix.f13 * sy);
matrix.f21 = (matrix.f21 * cy) - (matrix.f23 * sy);
matrix.f31 = (matrix.f31 * cy) - (matrix.f33 * sy);
matrix.f41 = (matrix.f41 * cy) - (matrix.f43 * sy);
matrix.f13 = (matrix.f13 * cy) + (matrix.f11 * sy);
matrix.f23 = (matrix.f23 * cy) + (matrix.f21 * sy);
matrix.f33 = (matrix.f33 * cy) + (matrix.f31 * sy);
matrix.f43 = (matrix.f43 * cy) + (matrix.f41 * sy);
}
Void MatrixRotateZ(RMatrix4x4 matrix, Float m_ZAngle)
{
Float sz, cz;
sz = SinCos(m_ZAngle, &cz);
matrix.f11 = (matrix.f11 * cz) + (matrix.f12 * sz);
matrix.f21 = (matrix.f21 * cz) + (matrix.f22 * sz);
matrix.f31 = (matrix.f31 * cz) + (matrix.f32 * sz);
matrix.f41 = (matrix.f41 * cz) + (matrix.f42 * sz);
matrix.f12 = (matrix.f12 * cz) - (matrix.f11 * sz);
matrix.f22 = (matrix.f22 * cz) - (matrix.f21 * sz);
matrix.f32 = (matrix.f32 * cz) - (matrix.f31 * sz);
matrix.f42 = (matrix.f42 * cz) - (matrix.f41 * sz);
}
Void MatrixRotateXYZ(RMatrix4x4 matrix, Float m_XAngle, Float m_YAngle, Float m_ZAngle)
{
Matrix4x4 mrot;
MatrixSetXYZRotation(mrot, m_XAngle, m_YAngle, m_ZAngle);
MatrixMultiply(matrix, mrot, matrix);
}
Void MatrixGetXYZRotationAngles(RMatrix4x4 matrix, RVector3 vAngles)
{
vAngles.m_Y = asinf(matrix.f13);
if (vAngles.m_Y < ((Float) CONST_PI_OVER_2))
{
if (vAngles.m_Y > ((Float) -CONST_PI_OVER_2))
{
vAngles.m_X = atan2f(-matrix.f23, matrix.f33);
vAngles.m_Z = atan2f(-matrix.f12, matrix.f11);
}
else
{
vAngles.m_X = -atan2f(matrix.f21, matrix.f22);
vAngles.m_Z = 0.0f;
}
}
else
{
vAngles.m_X = atan2f(matrix.f21, matrix.f22);
vAngles.m_Z = 0.0f;
}
}
Void MatrixRotateVectorArray(RMatrix4x4 matrix, PVector3 vIn, PVector3 vOut, UInt uiNumVecs)
{
while (uiNumVecs--)
{
MatrixRotateVector(matrix, *vIn, *vOut);
vIn++;
vOut++;
}
}
Void MatrixTransformVectorArray(RMatrix4x4 matrix, PVector3 vIn, PVector3 vOut, UInt uiNumVecs)
{
while (uiNumVecs--)
{
MatrixTransformVector(matrix, *vIn, *vOut);
vIn++;
vOut++;
}
}
Void MatrixProjectVectorArray(RMatrix4x4 matrix, PVector3 vIn, PVector3 vOut, UInt uiNumVecs)
{
while (uiNumVecs--)
{
MatrixProjectVector(matrix, *vIn, *vOut);
vIn++;
vOut++;
}
} |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Matrix/Matrix 4x4.h] - (11,101 bytes)
// Matrix 4x4.h
//
#ifndef _3D_MATRIX4X4_H
#define _3D_MATRIX4X4_H
#include "Types.h"
#include "Math\Vector\Vector 3.h"
typedef struct Matrix4x4 Matrix4x4, *PMatrix4x4, &RMatrix4x4;
struct Matrix4x4
{
Float f11, f21, f31, f41;
Float f12, f22, f32, f42;
Float f13, f23, f33, f43;
Float f14, f24, f34, f44;
};
// Matrix order:
//
// _11 _12 _13 _14
// _21 _22 _23 _24
// _31 _32 _33 _34
// _41 _42 _43 _44
extern const Matrix4x4 IdentityMatrix;
#define MatrixSetIdentity(m) { (m) = IdentityMatrix; }
#define MatrixSetScaling(m, x, y, z) { (m).f11 = (x); (m).f22 = (y); (m).f33 = (z); (m).f12 = (m).f13 = (m).f14 = (m).f21 = (m).f23 = (m).f24 = (m).f31 = (m).f32 = (m).f34 = (m).f41 = (m).f42 = (m).f43 = 0.0f; (m).f44 = 1.0f; }
#define MatrixSetScalingVec(m, vec) { MatrixSetScaling(m, (vec).m_X, (vec).m_Y, (vec).m_Z); }
#define MatrixSetTranslation(m, x, y, z) { (m).f14 = (x); (m).f24 = (y); (m).f34 = (z); (m).f11 = (m).f22 = (m).f33 = (m).f44 = 1.0f; (m).f12 = (m).f13 = (m).f21 = (m).f23 = (m).f31 = (m).f32 = (m).f41 = (m).f42 = (m).f43 = 0.0f; }
#define MatrixSetTranslationVec(m, vec) { MatrixSetTranslation(m, (vec).m_X, (vec).m_Y, (vec).m_Z); }
#define MatrixSetLeftUpIn(m, left, up, in) { (m).f11 = -(left).m_X; (m).f12 = -(left).m_Y; (m).f13 = -(left).m_Z; (m).f21 = (up).m_X; (m).f22 = (up).m_Y; (m).f23 = (up).m_Z; (m).f31 = -(in).m_X; (m).f32 = -(in).m_Y; (m).f33 = -(in).m_Z; (m).f14 = (m).f24 = (m).f34 = (m).f41 = (m).f42 = (m).f43 = 0.0f; (m).f44 = 1.0f; }
#define MatrixGetLeftVec(m, vecleft) { (vecleft).m_X = -(m).f11; (vecleft).m_Y = -(m).f21; (vecleft).m_Z = -(m).f31; }
#define MatrixGetUpVec(m, vecup) { (vecup).m_X = (m).f12; (vecup).m_Y = (m).f22; (vecup).m_Z = (m).f32; }
#define MatrixGetInVec(m, vecin) { (vecin).m_X = -(m).f13; (vecin).m_Y = -(m).f23; (vecin).m_Z = -(m).f33; }
#define MatrixGetLeftUpIn(m, left, up, in) { MatrixGetLeftVec(m, left); MatrixGetUpVec(m, up); MatrixGetInVec(m, in); }
#define MatrixGetTranspose(m, mtrans) { (mtrans).f11 = (m).f11; (mtrans).f12 = (m).f21; (mtrans).f13 = (m).f31; (mtrans).f14 = -(m).f14; (mtrans).f21 = (m).f12; (mtrans).f22 = (m).f22; (mtrans).f23 = (m).f32; (mtrans).f24 = -(m).f24; (mtrans).f31 = (m).f13; (mtrans).f32 = (m).f23; (mtrans).f33 = (m).f33; (mtrans).f34 = -(m).f34; (mtrans).f41 = (m).f41; (mtrans).f42 = (m).f42; (mtrans).f43 = (m).f43; (mtrans).f44 = -(m).f44; }
#define MatrixTranslate(m, x, y, z) { (m).f14 += (x); (m).f24 += (y); (m).f34 += (z); }
#define MatrixTranslateVec(m, vec) { MatrixTranslate(m, (vec).m_X, (vec).m_Y, (vec).m_Z); }
#define MatrixScale(m, x, y, z) { (m).f11 *= (x); (m).f12 *= (x); (m).f13 *= (x); (m).f14 *= (x); (m).f21 *= (y); (m).f22 *= (y); (m).f23 *= (y); (m).f24 *= (y); (m).f31 *= (z); (m).f32 *= (z); (m).f33 *= (z); (m).f34 *= (z); }
#define MatrixScaleVec(m, vec) { MatrixScale(m, (vec).m_X, (vec).m_Y, (vec).m_Z); }
#define MatrixRotateVector(m, vecin, vecout) { (vecout).m_X = ((vecin).m_X * (m).f11) + ((vecin).m_Y * (m).f12) + ((vecin).m_Z * (m).f13); (vecout).m_Y = ((vecin).m_X * (m).f21) + ((vecin).m_Y * (m).f22) + ((vecin).m_Z * (m).f23); (vecout).m_Z = ((vecin).m_X * (m).f31) + ((vecin).m_Y * (m).f32) + ((vecin).m_Z * (m).f33); }
#define MatrixTransformVector(m, vecin, vecout) { (vecout).m_X = ((vecin).m_X * (m).f11) + ((vecin).m_Y * (m).f12) + ((vecin).m_Z * (m).f13) + (m).f14; (vecout).m_Y = ((vecin).m_X * (m).f21) + ((vecin).m_Y * (m).f22) + ((vecin).m_Z * (m).f23) + (m).f24; (vecout).m_Z = ((vecin).m_X * (m).f31) + ((vecin).m_Y * (m).f32) + ((vecin).m_Z * (m).f33) + (m).f34; }
#define MatrixProjectVector(m, vecin, vecout) { Float rhw = 1.0f / (((vecin).m_X * (m).f41) + ((vecin).m_Y * (m).f42) + ((vecin).m_Z * (m).f43) + (m).f44); (vecout).m_X = (((vecin).m_X * (m).f11) + ((vecin).m_Y * (m).f12) + ((vecin).m_Z * (m).f13) + (m).f14) * rhw; (vecout).m_Y = (((vecin).m_X * (m).f21) + ((vecin).m_Y * (m).f22) + ((vecin).m_Z * (m).f23) + (m).f24) * rhw; (vecout).m_Z = (((vecin).m_X * (m).f31) + ((vecin).m_Y * (m).f32) + ((vecin).m_Z * (m).f33) + (m).f34) * rhw; }
#define MatrixCopy(m, mdst) { (mdst) = (m); }
#define MatrixSwap(a, b) { Matrix4x4 tmp = (a); (a) = (b); (b) = tmp; }
Bool MatrixIsValid(RMatrix4x4 matrix);
Bool MatrixIsOrthonormal(RMatrix4x4 matrix);
Bool MatrixIsOrthogonal(RMatrix4x4 matrix);
Void MatrixOrthonormalize(RMatrix4x4 matrix);
Void MatrixSetXRotation(RMatrix4x4 matrix, Float m_XAngle);
Void MatrixSetYRotation(RMatrix4x4 matrix, Float m_YAngle);
Void MatrixSetZRotation(RMatrix4x4 matrix, Float m_ZAngle);
Void MatrixSetXYZRotation(RMatrix4x4 matrix, Float m_XAngle, Float m_YAngle, Float m_ZAngle);
Void MatrixSetRotationScalingTranslation(RMatrix4x4 matrix, RVector3 vRotation, RVector3 vScaling, RVector3 vTranslation);
Bool MatrixSetViewMatrix(RMatrix4x4 matrix, RVector3 vFrom, RVector3 vView, RVector3 vWorldUp);
Void MatrixSetOrthogonalMatrix(RMatrix4x4 matrix, Float fLeft, Float fRight, Float fBottom, Float fTop, Float fNear, Float fFar);
Void MatrixSetOrthogonal2DMatrix(RMatrix4x4 matrix, Float fLeft, Float fRight, Float fBottom, Float fTop);
Void MatrixSetFrustumMatrix(RMatrix4x4 matrix, Float fLeft, Float fRight, Float fBottom, Float fTop, Float fNear, Float fFar);
Void MatrixSetPerspectiveMatrix(RMatrix4x4 matrix, Float fFov, Float fAspect, Float fNear, Float fFar);
Void MatrixAdd(RMatrix4x4 m1, RMatrix4x4 m2, RMatrix4x4 mdest);
Void MatrixMultiply(RMatrix4x4 m1, RMatrix4x4 m2, RMatrix4x4 mdest);
Void MatrixRotateX(RMatrix4x4 matrix, Float m_XAngle);
Void MatrixRotateY(RMatrix4x4 matrix, Float m_YAngle);
Void MatrixRotateZ(RMatrix4x4 matrix, Float m_ZAngle);
Void MatrixRotateXYZ(RMatrix4x4 matrix, Float m_XAngle, Float m_YAngle, Float m_ZAngle);
Void MatrixGetXYZRotationAngles(RMatrix4x4 matrix, RVector3 vAngles);
Void MatrixRotateVectorArray(RMatrix4x4 matrix, PVector3 vIn, PVector3 vOut, UInt uiNumVecs);
Void MatrixTransformVectorArray(RMatrix4x4 matrix, PVector3 vIn, PVector3 vOut, UInt uiNumVecs);
Void MatrixProjectVectorArray(RMatrix4x4 matrix, PVector3 vIn, PVector3 vOut, UInt uiNumVecs);
class CMatrix4x4 : public Matrix4x4
{
public:
ForceInline CMatrix4x4()
{ SetIdentity(); }
public:
ForceInline Void SetIdentity()
{ MatrixSetIdentity(*(PMatrix4x4) this); }
ForceInline Void SetScaling(Float x, Float y, Float z)
{ MatrixSetScaling(*this, x, y, z); }
ForceInline Void SetScaling(RVector3 vec)
{ MatrixSetScalingVec(*this, vec); }
ForceInline Void SetTranslation(Float x, Float y, Float z)
{ MatrixSetTranslation(*this, x, y, z); }
ForceInline Void SetTranslation(RVector3 vec)
{ MatrixSetTranslationVec(*this, vec); }
ForceInline Void SetLeftUpIn(RVector3 left, RVector3 up, RVector3 in)
{ MatrixSetLeftUpIn(*this, left, up, in); }
ForceInline Void GetLeftUpIn(RVector3 left, RVector3 up, RVector3 in)
{ MatrixGetLeftUpIn(*this, left, up, in); }
ForceInline Void GetLeftVec(RVector3 left)
{ MatrixGetLeftVec(*this, left); }
ForceInline Void GetUpVec(RVector3 up)
{ MatrixGetUpVec(*this, up); }
ForceInline Void GetInVec(RVector3 in)
{ MatrixGetInVec(*this, in); }
ForceInline Void GetTranspose(RMatrix4x4 mdst)
{ MatrixGetTranspose(*this, mdst); }
ForceInline Void Translate(Float x, Float y, Float z)
{ MatrixTranslate(*this, x, y, z); }
ForceInline Void Translate(RVector3 vec)
{ MatrixTranslateVec(*this, vec); }
ForceInline Void Scale(Float x, Float y, Float z)
{ MatrixScale(*this, x, y, z); }
ForceInline Void Scale(RVector3 vec)
{ MatrixScaleVec(*this, vec); }
ForceInline Void RotateVector(RVector3 vecin, RVector3 vecout)
{ MatrixRotateVector(*this, vecin, vecout); }
ForceInline Void TransformVector(RVector3 vecin, RVector3 vecout)
{ MatrixTransformVector(*this, vecin, vecout); }
ForceInline Void ProjectVector(RVector3 vecin, RVector3 vecout)
{ MatrixProjectVector(*this, vecin, vecout); }
ForceInline Void Copy(RMatrix4x4 mdst)
{ MatrixCopy(*this, mdst); }
ForceInline Void Swap(RMatrix4x4 m2)
{ MatrixSwap(*(PMatrix4x4) this, m2); }
ForceInline Bool IsValid()
{ return (MatrixIsValid(*this)); }
ForceInline Bool IsOrthonormal()
{ return (MatrixIsOrthonormal(*this)); }
ForceInline Bool IsOrthogonal()
{ return (MatrixIsOrthogonal(*this)); }
ForceInline Void Orthonormalize()
{ MatrixOrthonormalize(*this); }
ForceInline Void SetXRotation(Float x)
{ MatrixSetXRotation(*this, x); }
ForceInline Void SetYRotation(Float y)
{ MatrixSetYRotation(*this, y); }
ForceInline Void SetZRotation(Float z)
{ MatrixSetZRotation(*this, z); }
ForceInline Void SetRotationScalingTranslation(RVector3 rotation, RVector3 scaling, RVector3 translation)
{ MatrixSetRotationScalingTranslation(*this, rotation, scaling, translation); }
ForceInline Void SetViewMatrix(RVector3 from, RVector3 view, RVector3 worldup)
{ MatrixSetViewMatrix(*this, from, view, worldup); }
ForceInline Void SetOrthogonalMatrix(Float left, Float right, Float bottom, Float top, Float znear, Float zfar)
{ MatrixSetOrthogonalMatrix(*this, left, right, bottom, top, znear, zfar); }
ForceInline Void SetOrthogonal2DMatrix(Float left, Float right, Float bottom, Float top)
{ MatrixSetOrthogonal2DMatrix(*this, left, right, bottom, top); }
ForceInline Void SetFrustumMatrix(Float left, Float right, Float bottom, Float top, Float znear, Float zfar)
{ MatrixSetFrustumMatrix(*this, left, right, bottom, top, znear, zfar); }
ForceInline Void SetPerspectiveMatrix(Float fov, Float aspect, Float znear, Float zfar)
{ MatrixSetPerspectiveMatrix(*this, fov, aspect, znear, zfar); }
ForceInline Void Add(RMatrix4x4 m2, RMatrix4x4 mdst)
{ MatrixAdd(*this, m2, mdst); }
ForceInline Void Multiply(RMatrix4x4 m2, RMatrix4x4 mdst)
{ MatrixMultiply(*this, m2, mdst); }
ForceInline Void RotateX(Float x)
{ MatrixRotateX(*this, x); }
ForceInline Void RotateY(Float y)
{ MatrixRotateY(*this, y); }
ForceInline Void RotateZ(Float z)
{ MatrixRotateZ(*this, z); }
ForceInline Void RotateXYZ(Float x, Float y, Float z)
{ MatrixRotateXYZ(*this, x, y, z); }
ForceInline Void RotateXYZ(RVector3 vec)
{ MatrixRotateXYZ(*this, vec.m_X, vec.m_Y, vec.m_Z); }
ForceInline Void GetXYZRotationAngles(RVector3 angles)
{ MatrixGetXYZRotationAngles(*this, angles); }
ForceInline Void RotateVectorArray(PVector3 vecin, PVector3 vecout, UInt count)
{ MatrixRotateVectorArray(*this, vecin, vecout, count); }
ForceInline Void TransformVectorArray(PVector3 vecin, PVector3 vecout, UInt count)
{ MatrixTransformVectorArray(*this, vecin, vecout, count); }
ForceInline Void ProjectVectorArray(PVector3 vecin, PVector3 vecout, UInt count)
{ MatrixProjectVectorArray(*this, vecin, vecout, count); }
public:
ForceInline operator PMatrix4x4()
{ return ((PMatrix4x4) this); }
ForceInline operator += (RMatrix4x4 right)
{ Add(right, *this); }
ForceInline operator *= (RMatrix4x4 right)
{ Multiply(right, *this); }
};
#endif |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Quaternion/Quaternion.cpp] - (10,346 bytes)
// Quaternion.cpp
//
#include "Quaternion.h"
#include "Math\FastSqrt.h"
#include "Math\FastTrigonometry.h"
#include <math.h>
#include <assert.h>
#define QUAT_UNIT_TOLERANCE 0.001
#define QUAT_QZERO_TOLERANCE 0.00001
#define QUAT_TRACE_QZERO_TOLERANCE 0.1
#define QUAT_AA_QZERO_TOLERANCE 0.0001
#define QUAT_ZERO_EPSILON 0.0001
#define QUAT_EPSILON 0.0001
Bool QuatIsValid(RQuaternion quat)
{
if ((quat.m_W * quat.m_W) < 0.0f)
return FALSE;
if ((quat.m_X * quat.m_X) < 0.0f)
return FALSE;
if ((quat.m_Y * quat.m_Y) < 0.0f)
return FALSE;
if ((quat.m_Z * quat.m_Z) < 0.0f)
return FALSE;
return TRUE;
}
Bool QuatIsUnit(RQuaternion quat)
{
if (fabs(QuatLengthSquared(quat) - 1.0f) < (QUAT_UNIT_TOLERANCE * QUAT_UNIT_TOLERANCE))
return TRUE;
return FALSE;
}
Float QuatNormalize(RQuaternion quat)
{
Float length = FastSqrt(QuatLengthSquared(quat));
if (fabs(length) < QUAT_QZERO_TOLERANCE)
return 0.0f;
Float oneoverlength = 1.0f / length;
QuatScale(quat, oneoverlength);
return length;
}
Bool QuatCompare(RQuaternion q1, RQuaternion q2, Float fTolerance)
{
if (fabs(q1.m_X - q2.m_X) > fTolerance)
return FALSE;
if (fabs(q1.m_Y - q2.m_Y) > fTolerance)
return FALSE;
if (fabs(q1.m_Z - q2.m_Z) > fTolerance)
return FALSE;
if (fabs(q1.m_W - q2.m_W) > fTolerance)
return FALSE;
return TRUE;
}
Void QuatSetFromAxisAngle(RQuaternion quat, RVector3 axis, Float theta)
{
theta = SinCos(theta * 0.5f, &quat.m_W);
quat.m_X = axis.m_X * theta;
quat.m_Y = axis.m_Y * theta;
quat.m_Z = axis.m_Z * theta;
}
Bool QuatGetAxisAngle(RQuaternion quat, RVector3 axis, PFloat theta)
{
Float OneOverSinTheta;
Float HalfTheta = acosf(quat.m_W);
if (HalfTheta > QUAT_QZERO_TOLERANCE)
{
OneOverSinTheta = 1.0f / sinf(HalfTheta);
axis.m_X = OneOverSinTheta * quat.m_X;
axis.m_Y = OneOverSinTheta * quat.m_Y;
axis.m_Z = OneOverSinTheta * quat.m_Z;
*theta = 2.0f * HalfTheta;
return TRUE;
}
else
{
Vec3Clear(axis);
*theta = 0.0f;
return FALSE;
}
}
Void QuatFromMatrix(RMatrix4x4 matrix, RQuaternion quat)
{
Float trace, s;
trace = matrix.f11 + matrix.f22 + matrix.f33;
if (trace > 0.0f)
{
s = FastSqrt(trace + 1.0f);
quat.m_W = s * 0.5f;
s = 0.5f / s;
quat.m_X = (matrix.f32 - matrix.f23) * s;
quat.m_Y = (matrix.f13 - matrix.f31) * s;
quat.m_Z = (matrix.f21 - matrix.f12) * s;
}
else
{
Int biggest;
enum {A,E,I};
if (matrix.f11 > matrix.f22)
{
if (matrix.f33 > matrix.f11)
biggest = I;
else
biggest = A;
}
else
{
if (matrix.f33 > matrix.f11)
biggest = I;
else
biggest = E;
}
switch (biggest)
{
case A:
s = FastSqrt(matrix.f11 - (matrix.f22 + matrix.f33) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_X = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f32 - matrix.f23) * s;
quat.m_Y = (matrix.f12 + matrix.f21) * s;
quat.m_Z = (matrix.f13 + matrix.f31) * s;
break;
}
// I
s = FastSqrt(matrix.f33 - (matrix.f11 + matrix.f22) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_Z = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f21 - matrix.f12) * s;
quat.m_X = (matrix.f31 + matrix.f13) * s;
quat.m_Y = (matrix.f32 + matrix.f23) * s;
break;
}
// E
s = FastSqrt(matrix.f22 - (matrix.f33 + matrix.f11) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_Y = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f13 - matrix.f31) * s;
quat.m_Z = (matrix.f23 + matrix.f32) * s;
quat.m_X = (matrix.f21 + matrix.f12) * s;
break;
}
break;
case E:
s = FastSqrt(matrix.f22 - (matrix.f33 + matrix.f11) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_Y = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f13 - matrix.f31) * s;
quat.m_Z = (matrix.f23 + matrix.f32) * s;
quat.m_X = (matrix.f21 + matrix.f12) * s;
break;
}
// I
s = FastSqrt(matrix.f33 - (matrix.f11 + matrix.f22) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_Z = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f21 - matrix.f12) * s;
quat.m_X = (matrix.f31 + matrix.f13) * s;
quat.m_Y = (matrix.f32 + matrix.f23) * s;
break;
}
// A
s = FastSqrt(matrix.f11 - (matrix.f22 + matrix.f33) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_X = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f32 - matrix.f23) * s;
quat.m_Y = (matrix.f12 + matrix.f21) * s;
quat.m_Z = (matrix.f13 + matrix.f31) * s;
break;
}
break;
case I:
s = FastSqrt(matrix.f33 - (matrix.f11 + matrix.f22) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_Z = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f21 - matrix.f12) * s;
quat.m_X = (matrix.f31 + matrix.f13) * s;
quat.m_Y = (matrix.f32 + matrix.f23) * s;
break;
}
// A
s = FastSqrt(matrix.f11 - (matrix.f22 + matrix.f33) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_X = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f32 - matrix.f23) * s;
quat.m_Y = (matrix.f12 + matrix.f21) * s;
quat.m_Z = (matrix.f13 + matrix.f31) * s;
break;
}
// E
s = FastSqrt(matrix.f22 - (matrix.f33 + matrix.f11) + 1.0f);
if (s > QUAT_TRACE_QZERO_TOLERANCE)
{
quat.m_Y = s * 0.5f;
s = 0.5f / s;
quat.m_W = (matrix.f13 - matrix.f31) * s;
quat.m_Z = (matrix.f23 + matrix.f32) * s;
quat.m_X = (matrix.f21 + matrix.f12) * s;
break;
}
break;
default:
assert(0);
}
}
}
Void QuatToMatrix(RQuaternion quat, RMatrix4x4 matrix)
{
Float X2, Y2, Z2;
Float XX2, YY2, ZZ2;
Float XY2, XZ2, XW2;
Float YZ2, YW2, ZW2;
X2 = 2.0f * quat.m_X;
XX2 = X2 * quat.m_X;
XY2 = X2 * quat.m_Y;
XZ2 = X2 * quat.m_Z;
XW2 = X2 * quat.m_W;
Y2 = 2.0f * quat.m_Y;
YY2 = Y2 * quat.m_Y;
YZ2 = Y2 * quat.m_Z;
YW2 = Y2 * quat.m_W;
Z2 = 2.0f * quat.m_Z;
ZZ2 = Z2 * quat.m_Z;
ZW2 = Z2 * quat.m_W;
matrix.f11 = 1.0f - YY2 - ZZ2;
matrix.f12 = XY2 - ZW2;
matrix.f13 = XZ2 + YW2;
matrix.f21 = XY2 + ZW2;
matrix.f22 = 1.0f - XX2 - ZZ2;
matrix.f23 = YZ2 - XW2;
matrix.f31 = XZ2 - YW2;
matrix.f32 = YZ2 + XW2;
matrix.f33 = 1.0f - XX2 - YY2;
matrix.f14 = matrix.f24 = matrix.f34 = matrix.f14 = matrix.f42 = matrix.f43 = 0.0f;
matrix.f44 = 1.0f;
}
Void QuatSlerp(RQuaternion q1, RQuaternion q2, Float t, RQuaternion qdst)
{
Float omega, cosom, sinom, Scale1, Scale2;
Quaternion qtmp;
cosom = QuatMagnitude(q1, q2);
if (cosom < 0)
{
cosom = -cosom;
QuatNegCopy(q2, qtmp);
}
else
{
qtmp = q2;
}
if ((1.0f - cosom) > QUAT_EPSILON)
{
omega = acosf(cosom);
sinom = sinf(omega);
Scale1 = sinf((1.0f - t) * omega) / sinom;
Scale2 = sinf(t * omega) / sinom;
}
else
{
Scale1 = 1.0f - t;
Scale2 = t;
}
QuatDualScaleAdd(q1, Scale1, qtmp, Scale2, qdst);
}
Void QuatSlerpNotShortest(RQuaternion q1, RQuaternion q2, Float t, RQuaternion qdst)
{
Float omega, cosom, sinom, Scale1, Scale2;
cosom = QuatMagnitude(q1, q2);
if ((1.0f + cosom) > QUAT_EPSILON)
{
if ((1.0f - cosom) > QUAT_EPSILON)
{
omega = acosf(cosom);
sinom = sinf(omega);
if (sinom < QUAT_EPSILON)
{
Scale1 = 1.0f - t;
Scale2 = t;
}
else
{
Scale1 = sinf((1.0f - t) * omega) / sinom;
Scale2 = sinf(t * omega) / sinom;
}
}
else
{
Scale1 = 1.0f - t;
Scale2 = t;
}
QuatDualScaleAdd(q1, Scale1, q2, Scale2, qdst);
}
else
{
qdst.m_X = -q1.m_Y;
qdst.m_Y = q1.m_X;
qdst.m_Z = -q1.m_W;
qdst.m_W = q1.m_Z;
Scale1 = sinf((1.0f - t) * (Float) CONST_PI_OVER_2);
Scale2 = sinf(t * (Float) CONST_PI_OVER_2);
QuatDualScaleAdd(q1, Scale1, qdst, Scale2, qdst);
}
}
Void QuatMultiply(RQuaternion q1, RQuaternion q2, RQuaternion qdst)
{
Quaternion qtmp;
qtmp.m_W = ((q1.m_W * q2.m_W) - (q1.m_X * q2.m_X) - (q1.m_Y * q2.m_Y) - (q1.m_Z * q2.m_Z));
qtmp.m_X = ((q1.m_W * q2.m_X) + (q1.m_X * q2.m_W) + (q1.m_Y * q2.m_Z) - (q1.m_Z * q2.m_Y));
qtmp.m_Y = ((q1.m_W * q2.m_Y) - (q1.m_X * q2.m_Z) + (q1.m_Y * q2.m_W) + (q1.m_Z * q2.m_X));
qtmp.m_Z = ((q1.m_W * q2.m_Z) + (q1.m_X * q2.m_Y) - (q1.m_Y * q2.m_X) + (q1.m_Z * q2.m_W));
qdst = qtmp;
}
Void QuatRotateVector(RQuaternion quat, RVector3 vecin, RVector3 vecout)
{
Quaternion qtmp;
qtmp.m_W = (-(quat.m_X * vecin.m_X) - (quat.m_Y * vecin.m_Y) - (quat.m_Z * vecin.m_Z));
qtmp.m_X = ( (quat.m_W * vecin.m_X) + (quat.m_Y * vecin.m_Z) - (quat.m_Z * vecin.m_Y));
qtmp.m_Y = ( (quat.m_W * vecin.m_Y) - (quat.m_X * vecin.m_Z) + (quat.m_Z * vecin.m_X));
qtmp.m_Z = ( (quat.m_W * vecin.m_Z) + (quat.m_X * vecin.m_Y) - (quat.m_Y * vecin.m_X));
vecout.m_X = ((qtmp.m_Z * vecin.m_Y) - (qtmp.m_W * vecin.m_X) - (qtmp.m_Y * vecin.m_Z));
vecout.m_Y = ((qtmp.m_X * vecin.m_Z) - (qtmp.m_W * vecin.m_Y) - (qtmp.m_Z * vecin.m_X));
vecout.m_Z = ((qtmp.m_Y * vecin.m_X) - (qtmp.m_W * vecin.m_Z) - (qtmp.m_X * vecin.m_Y));
}
Void QuatLn(RQuaternion quat, RQuaternion qdst)
{
Quaternion qtmp;
if (quat.m_W < 0.0f)
{
QuatNegCopy(quat, qtmp);
}
else
{
qtmp = quat;
}
Float theta = acosf(qtmp.m_W);
if (theta < QUAT_ZERO_EPSILON)
{
qdst.m_W = 0.0f;
qdst.m_X = qtmp.m_X;
qdst.m_Y = qtmp.m_Y;
qdst.m_Z = qtmp.m_Z;
}
else
{
theta /= sinf(theta);
qdst.m_W = 0.0f;
qdst.m_X = theta * qtmp.m_X;
qdst.m_Y = theta * qtmp.m_Y;
qdst.m_Z = theta * qtmp.m_Z;
}
}
Void QuatExp(RQuaternion quat, RQuaternion qdst)
{
Float theta = FastSqrt(QuatLengthSquared(quat));
Float SinThetaOverTheta;
if (theta > QUAT_ZERO_EPSILON)
{
SinThetaOverTheta = SinCos(theta, &qdst.m_W) / theta;
}
else
{
qdst.m_W = cosf(theta);
SinThetaOverTheta = 1.0f;
}
qdst.m_X = SinThetaOverTheta * quat.m_X;
qdst.m_Y = SinThetaOverTheta * quat.m_Y;
qdst.m_Z = SinThetaOverTheta * quat.m_Z;
} |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Quaternion/Quaternion.h] - (5,931 bytes)
// Quaternion.h
//
#ifndef _3D_QUATERNION_H
#define _3D_QUATERNION_H
#include "Types.h"
#include "Math\Vector\Vector 3.h"
#include "Math\Matrix\Matrix 4x4.h"
typedef struct Quaternion Quaternion, *PQuaternion, &RQuaternion;
struct Quaternion
{
Float m_W;
Float m_X;
Float m_Y;
Float m_Z;
};
#define QuatClear(quat) { (quat).m_X = (quat).m_Y = (quat).m_Z = 0.0f; (quat).m_W = 1.0f; }
#define QuatSet(quat, w, x, y, z) { (quat).m_X = (x); (quat).m_Y = (y); (quat).m_Z = (z); (quat).m_W = (w); }
#define QuatSetVec(quat, w, vec) { QuatSet(quat, (vec).m_X, (vec).m_Y, (vec).m_Z, w); }
#define QuatMagnitude(q1, q2) ( ((q1).m_X * (q2).m_X) + ((q1).m_Y * (q2).m_Y) + ((q1).m_Z * (q2).m_Z) + ((q1).m_W * (q2).m_W))
#define QuatLengthSquared(quat) ( QuatMagnitude(quat, quat))
#define QuatNegCopy(q, qinv) { (qinv).m_X = -(q).m_X; (qinv).m_Y = -(q).m_Y; (qinv).m_Z = -(q).m_Z; (qinv).m_W = -(q).m_W; }
#define QuatNeg(quat) { QuatNegCopy(quat, quat); }
#define QuatDualScaleAdd(q1, q1s, q2, q2s, qdst) { (qdst).m_X = ((q1).m_X * (q1s)) + ((q2).m_X * (q2s)); (qdst).m_Y = ((q1).m_Y * (q1s)) + ((q2).m_Y * (q2s)); (qdst).m_Z = ((q1).m_Z * (q1s)) + ((q2).m_Z * (q2s)); (qdst).m_W = ((q1).m_W * (q1s)) + ((q2).m_W * (q2s)); }
#define QuatScale(quat, scale) { (quat).m_X *= scale; (quat).m_Y *= scale; (quat).m_Z *= scale; (quat).m_W *= scale; }
#define QuatInverseCopy(q, qdst) { (q).m_X = -(qdst).m_X; (q).m_Y = -(qdst).m_Y; (q).m_Z = -(qdst).m_Z; }
#define QuatInverse(quat) { QuatInverseCopy(quat, quat); }
#define QuatAdd(q1, q2, qdst) { (qdst).m_X = (q1).m_X + (q2).m_X; (qdst).m_Y = (q1).m_Y + (q2).m_Y; (qdst).m_Z = (q1).m_Z + (q2).m_Z; (qdst).m_W = (q1).m_W + (q2).m_W; }
#define QuatSub(q1, q2, qdst) { (qdst).m_X = (q1).m_X - (q2).m_X; (qdst).m_Y = (q1).m_Y - (q2).m_Y; (qdst).m_Z = (q1).m_Z - (q2).m_Z; (qdst).m_W = (q1).m_W - (q2).m_W; }
#define QuatCopy(quat, qdst) { (qdst) = (quat); }
#define QuatSwap(a, b) { Quaternion tmp = (a); (a) = (b); (b) = tmp; }
Bool QuatIsValid(RQuaternion quat);
Bool QuatIsUnit(RQuaternion quat);
Float QuatNormalize(RQuaternion quat);
Bool QuatCompare(RQuaternion q1, RQuaternion q2, Float fTolerance);
Void QuatSetFromAxisAngle(RQuaternion quat, RVector3 axis, Float theta);
Bool QuatGetAxisAngle(RQuaternion quat, RVector3 axis, PFloat theta);
Void QuatFromMatrix(RMatrix4x4 matrix, RQuaternion quat);
Void QuatToMatrix(RQuaternion quat, RMatrix4x4 matrix);
Void QuatSlerp(RQuaternion q1, RQuaternion q2, Float t, RQuaternion qdst);
Void QuatSlerpNotShortest(RQuaternion q1, RQuaternion q2, Float t, RQuaternion qdst);
Void QuatMultiply(RQuaternion q1, RQuaternion q2, RQuaternion qdst);
Void QuatRotateVector(RQuaternion quat, RVector3 vecin, RVector3 vecout);
Void QuatLn(RQuaternion quat, RQuaternion qdst);
Void QuatExp(RQuaternion quat, RQuaternion qdst);
class CQuaternion : public Quaternion
{
public:
ForceInline CQuaternion()
{ QuatClear(*this); }
ForceInline CQuaternion(Float w, Float x, Float y, Float z)
{ QuatSet(*this, x, y, z, w); }
ForceInline CQuaternion(Float w, RVector3 vec)
{ QuatSetVec(*this, w, vec); }
public:
ForceInline Void Clear()
{ QuatClear(*this); }
ForceInline Void Set(Float w, Float x, Float y, Float z)
{ QuatSet(*this, w, x, y, z); }
ForceInline Void SetVec(Float w, RVector3 vec)
{ QuatSetVec(*this, w, vec); }
ForceInline Float Magnitude(RQuaternion q2)
{ return (QuatMagnitude(*this, q2)); }
ForceInline Float LengthSquared()
{ return (QuatLengthSquared(*this)); }
ForceInline Void NegCopy(RQuaternion qdst)
{ QuatNegCopy(*this, qdst); }
ForceInline Void Neg()
{ QuatNeg(*this); }
ForceInline Void DualScaleAdd(Float q1scale, RQuaternion q2, Float q2scale, RQuaternion qdst)
{ QuatDualScaleAdd(*this, q1scale, q2, q2scale, qdst); }
ForceInline Void Scale(Float scale)
{ QuatScale(*this, scale); }
ForceInline Void InverseCopy(RQuaternion qdst)
{ QuatInverseCopy(*this, qdst); }
ForceInline Void Inverse()
{ QuatInverse(*this); }
ForceInline Void Add(RQuaternion q2, RQuaternion qdst)
{ QuatAdd(*this, q2, qdst); }
ForceInline Void Sub(RQuaternion q2, RQuaternion qdst)
{ QuatSub(*this, q2, qdst); }
ForceInline Void Copy(RQuaternion qdst)
{ QuatCopy(*this, qdst); }
ForceInline Void Swap(RQuaternion b)
{ QuatSwap(*(PQuaternion) this, b); }
ForceInline Bool IsValid()
{ return (QuatIsValid(*this)); }
ForceInline Bool IsUnit()
{ return (QuatIsUnit(*this)); }
ForceInline Float Normalize()
{ return (QuatNormalize(*this)); }
ForceInline Bool Compare(RQuaternion q2, Float tolerance)
{ return (QuatCompare(*this, q2, tolerance)); }
ForceInline Void SetFromAxisAngle(RVector3 axis, Float theta)
{ QuatSetFromAxisAngle(*this, axis, theta); }
ForceInline Void GetAxisAngle(RVector3 axis, PFloat theta)
{ QuatGetAxisAngle(*this, axis, theta); }
ForceInline Void FromMatrix(RMatrix4x4 matrix)
{ QuatFromMatrix(matrix, *this); }
ForceInline Void ToMatrix(RMatrix4x4 mdst)
{ QuatToMatrix(*this, mdst); }
ForceInline Void Slerp(RQuaternion q2, Float t, RQuaternion qdst)
{ QuatSlerp(*this, q2, t, qdst); }
ForceInline Void SlerpNotShortest(RQuaternion q2, Float t, RQuaternion qdst)
{ QuatSlerpNotShortest(*this, q2, t, qdst); }
ForceInline Void Multiply(RQuaternion q2, RQuaternion qdst)
{ QuatMultiply(*this, q2, qdst); }
ForceInline Void RotateVector(RVector3 vecin, RVector3 vecout)
{ QuatRotateVector(*this, vecin, vecout); }
ForceInline Void Ln(RQuaternion qdst)
{ QuatLn(*this, qdst); }
ForceInline Void Exp(RQuaternion qdst)
{ QuatExp(*this, qdst); }
public:
ForceInline operator PQuaternion ()
{ return ((PQuaternion) this); }
ForceInline operator *= (RQuaternion right)
{ Multiply(right, *this); }
};
#endif |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Vector/Vector 2.cpp] - (1,177 bytes)
// Vector 3.cpp
//
#include "Vector 2.h"
#include "Math\FastSqrt.h"
#include <math.h>
#define VECTOR_COMPARE_EPSILON 0.0005
Bool VecIsValid(RVector2 vec)
{
if ((vec.m_X * vec.m_X) < 0.0f)
return FALSE;
if ((vec.m_Y * vec.m_Y) < 0.0f)
return FALSE;
return TRUE;
}
Bool VecIsNormalized(RVector2 vec)
{
if (fabs(Vec2LengthSquared(vec) - 1.0f) < (VECTOR_COMPARE_EPSILON * VECTOR_COMPARE_EPSILON))
return TRUE;
return FALSE;
}
Float VecNormalize(RVector2 vec)
{
Float fLength = FastSqrt(Vec2LengthSquared(vec));
if (fLength == 0.0f)
return 0.0f;
Float fOneOverLength = 1.0f / fLength;
vec.m_X *= fOneOverLength;
vec.m_Y *= fOneOverLength;
return fLength;
}
Float VecLength(RVector2 vec)
{
return (FastSqrt(Vec2LengthSquared(vec)));
}
Float VecDistanceBetween(RVector2 v1, RVector2 v2)
{
Vector2 tmp;
Vec2Sub(v1, v2, tmp);
return (FastSqrt(Vec2LengthSquared(tmp)));
}
Bool VecCompare(RVector2 v1, RVector2 v2, Float fTolerance)
{
if (fabs(v1.m_X - v2.m_X) > fTolerance)
return FALSE;
if (fabs(v1.m_Y - v2.m_Y) > fTolerance)
return FALSE;
return TRUE;
} |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Vector/Vector 2.h] - (3,649 bytes)
// Vector 3.h
//
#ifndef _3D_VECTOR2D_H
#define _3D_VECTOR2D_H
#include "Types.h"
typedef struct Vector2 Vector2, *PVector2, &RVector2;
struct Vector2
{
Float m_X;
Float m_Y;
};
#define Vec2Set(vec, x, y) { (vec).m_X = (x); (vec).m_Y = (y); }
#define Vec2Clear(a) { (a).m_X = (a).m_Y = 0.0f; }
#define Vec2DotProduct(a, b) (((a).m_X * (b).m_X) + ((a).m_Y * (b).m_Y))
#define Vec2LengthSquared(a) (Vec2DotProduct((a), (a)))
#define Vec2Add(a, b, res) { (res).m_X = (a).m_X + (b).m_X; (res).m_Y = (a).m_Y + (b).m_Y; }
#define Vec2Sub(a, b, res) { (res).m_X = (a).m_X - (b).m_X; (res).m_Y = (a).m_Y - (b).m_Y; }
#define Vec2Invert(a) { (a).m_X = -(a).m_X; (a).m_Y = -(a).m_Y; }
#define Vec2Scale(a, factor) { (a).m_X *= (factor); (a).m_Y *= (factor); }
#define Vec2Copy(src, dst) { (dst) = (src); }
#define Vec2Swap(a, b) { Vector2 tmp = (a); (a) = (b); (b) = tmp; }
#define Vec2DistBetweenSquared(a, b, dist) { Vector2 tmp; Vec2Sub((a), (b), tmp); dist = Vec2LengthSquared(tmp); }
#define Vec2AddScaled(a, b, bscale, dst) { (dst).m_X = (a).m_X + ((b).m_X * (bscale)); (dst).m_Y = (a).m_Y + ((b).m_Y * (bscale)); }
#define Vec2SubScaled(a, b, bscale, dst) { (dst).m_X = (a).m_X - ((b).m_X * (bscale)); (dst).m_Y = (a).m_Y - ((b).m_Y * (bscale)); }
Bool Vec2IsValid(RVector2 vec);
Bool Vec2IsNormalized(RVector2 vec);
Float Vec2Normalize(RVector2 vec);
Float Vec2Length(RVector2 vec);
Float Vec2DistanceBetween(RVector2 v1, RVector2 v2);
Bool Vec2Compare(RVector2 v1, RVector2 v2, Float fTolerance);
class CVector2 : public Vector2
{
public:
ForceInline CVector2()
{ Vec2Clear(*this); }
ForceInline CVector2(Float x, Float y)
{ Vec2Set(*this, x, y); }
public:
ForceInline Void Set(Float x, Float y)
{ Vec2Set(*this, x, y); }
ForceInline Void Clear()
{ Vec2Clear(*this); }
ForceInline Float DotProduct(RVector2 v2)
{ return (Vec2DotProduct(*this, v2)); }
ForceInline Float LengthSquared()
{ return (Vec2LengthSquared(*this)); }
ForceInline Void Add(RVector2 v2, RVector2 vdst)
{ Vec2Add(*this, v2, vdst); }
ForceInline Void Sub(RVector2 v2, RVector2 vdst)
{ Vec2Sub(*this, v2, vdst); }
ForceInline Void Invert()
{ Vec2Invert(*this); }
ForceInline Void Scale(Float factor)
{ Vec2Scale(*this, factor); }
ForceInline Void Copy(RVector2 vdst)
{ Vec2Copy(*this, vdst); }
ForceInline Void Swap(RVector2 v2)
{ Vec2Swap(*(PVector2) this, v2); }
ForceInline Float DistBetweenSquared(RVector2 v2)
{ Float distsq; Vec2DistBetweenSquared(*this, v2, distsq); return distsq; }
ForceInline Void AddScaled(RVector2 v2, Float v2scale, RVector2 vdst)
{ Vec2AddScaled(*this, v2, v2scale, vdst); }
ForceInline Void SubScaled(RVector2 v2, Float v2scale, RVector2 vdst)
{ Vec2SubScaled(*this, v2, v2scale, vdst); }
ForceInline Bool IsValid()
{ return (Vec2IsValid(*this)); }
ForceInline Bool IsNormalized()
{ return (Vec2IsNormalized(*this)); }
ForceInline Float Normalize()
{ return (Vec2Normalize(*this)); }
ForceInline Float Length()
{ return (Vec2Length(*this)); }
ForceInline Float DistanceBetween(RVector2 v2)
{ return (Vec2DistanceBetween(*this, v2)); }
ForceInline Bool Compare(RVector2 v2, Float tolerance)
{ return (Vec2Compare(*this, v2, tolerance)); }
public:
ForceInline operator PVector2 ()
{ return ((PVector2) this); }
ForceInline operator += (RVector2 right)
{ Add(right, *this); }
ForceInline operator -= (RVector2 right)
{ Sub(right, *this); }
ForceInline operator *= (Float right)
{ Scale(right); }
};
#endif |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Vector/Vector 3.cpp] - (1,316 bytes)
// Vector 3.cpp
//
#include "Vector 3.h"
#include "Math\FastSqrt.h"
#include <math.h>
#define VECTOR_COMPARE_EPSILON 0.0005
Bool VecIsValid(RVector3 vec)
{
if ((vec.m_X * vec.m_X) < 0.0f)
return FALSE;
if ((vec.m_Y * vec.m_Y) < 0.0f)
return FALSE;
if ((vec.m_Z * vec.m_Z) < 0.0f)
return FALSE;
return TRUE;
}
Bool VecIsNormalized(RVector3 vec)
{
if (fabs(Vec3LengthSquared(vec) - 1.0f) < (VECTOR_COMPARE_EPSILON * VECTOR_COMPARE_EPSILON))
return TRUE;
return FALSE;
}
Float VecNormalize(RVector3 vec)
{
Float fLength = FastSqrt(Vec3LengthSquared(vec));
if (fLength == 0.0f)
return 0.0f;
Float fOneOverLength = 1.0f / fLength;
vec.m_X *= fOneOverLength;
vec.m_Y *= fOneOverLength;
vec.m_Z *= fOneOverLength;
return fLength;
}
Float VecLength(RVector3 vec)
{
return (FastSqrt(Vec3LengthSquared(vec)));
}
Float VecDistanceBetween(RVector3 v1, RVector3 v2)
{
Vector3 tmp;
Vec3Sub(v1, v2, tmp);
return (FastSqrt(Vec3LengthSquared(tmp)));
}
Bool VecCompare(RVector3 v1, RVector3 v2, Float fTolerance)
{
if (fabs(v1.m_X - v2.m_X) > fTolerance)
return FALSE;
if (fabs(v1.m_Y - v2.m_Y) > fTolerance)
return FALSE;
if (fabs(v1.m_Z - v2.m_Z) > fTolerance)
return FALSE;
return TRUE;
} |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Vector/Vector 3.h] - (4,490 bytes)
// Vector 3.h
//
#ifndef _3D_VECTOR3D_H
#define _3D_VECTOR3D_H
#include "Types.h"
typedef struct Vector3 Vector3, *PVector3, &RVector3;
struct Vector3
{
Float m_X;
Float m_Y;
Float m_Z;
};
#define Vec3Set(vec, x, y, z) { (vec).m_X = (x); (vec).m_Y = (y); (vec).m_Z = (z); }
#define Vec3Clear(a) { (a).m_X = (a).m_Y = (a).m_Z = 0.0f; }
#define Vec3DotProduct(a, b) (((a).m_X * (b).m_X) + ((a).m_Y * (b).m_Y) + ((a).m_Z * (b).m_Z))
#define Vec3LengthSquared(a) (Vec3DotProduct((a), (a)))
#define Vec3Add(a, b, res) { (res).m_X = (a).m_X + (b).m_X; (res).m_Y = (a).m_Y + (b).m_Y; (res).m_Z = (a).m_Z + (b).m_Z; }
#define Vec3Sub(a, b, res) { (res).m_X = (a).m_X - (b).m_X; (res).m_Y = (a).m_Y - (b).m_Y; (res).m_Z = (a).m_Z - (b).m_Z; }
#define Vec3Invert(a) { (a).m_X = -(a).m_X; (a).m_Y = -(a).m_Y; (a).m_Z = -(a).m_Z; }
#define Vec3Scale(a, factor) { (a).m_X *= (factor); (a).m_Y *= (factor); (a).m_Z *= (factor); }
#define Vec3Copy(src, dst) { (dst) = (src); }
#define Vec3CrossProduct(a, b, dst) { (dst).m_X = ((a).m_Y * (b).m_Z) - ((a).m_Z * (b).m_Y); (dst).m_Y = ((a).m_Z * (b).m_X) - ((a).m_X * (b).m_Z); (dst).m_Z = ((a).m_X * (b).m_Y) - ((a).m_Y * (b).m_X); }
#define Vec3Normal(a, b, c, dst) { Vector3 vdif1, vdif2; Vec3Sub(b, a, vdif1); Vec3Sub(c, a, vdif2); Vec3CrossProduct(vdif1, vdif2, dst); }
#define Vec3Swap(a, b) { Vector3 tmp = (a); (a) = (b); (b) = tmp; }
#define Vec3DistBetweenSquared(a, b, dist) { Vector3 tmp; Vec3Sub((a), (b), tmp); dist = Vec3LengthSquared(tmp); }
#define Vec3AddScaled(a, b, bscale, dst) { (dst).m_X = (a).m_X + ((b).m_X * (bscale)); (dst).m_Y = (a).m_Y + ((b).m_Y * (bscale)); (dst).m_Z = (a).m_Z + ((b).m_Z * (bscale)); }
#define Vec3SubScaled(a, b, bscale, dst) { (dst).m_X = (a).m_X - ((b).m_X * (bscale)); (dst).m_Y = (a).m_Y - ((b).m_Y * (bscale)); (dst).m_Z = (a).m_Z - ((b).m_Z * (bscale)); }
Bool Vec3IsValid(RVector3 vec);
Bool Vec3IsNormalized(RVector3 vec);
Float Vec3Normalize(RVector3 vec);
Float Vec3Length(RVector3 vec);
Float Vec3DistanceBetween(RVector3 v1, RVector3 v2);
Bool Vec3Compare(RVector3 v1, RVector3 v2, Float fTolerance);
class CVector3 : public Vector3
{
public:
ForceInline CVector3()
{ Vec3Clear(*this); }
ForceInline CVector3(Float x, Float y, Float z)
{ Vec3Set(*this, x, y, z); }
public:
ForceInline Void Set(Float x, Float y, Float z)
{ Vec3Set(*this, x, y, z); }
ForceInline Void Clear()
{ Vec3Clear(*this); }
ForceInline Float DotProduct(RVector3 v2)
{ return (Vec3DotProduct(*this, v2)); }
ForceInline Float LengthSquared()
{ return (Vec3LengthSquared(*this)); }
ForceInline Void Add(RVector3 v2, RVector3 vdst)
{ Vec3Add(*this, v2, vdst); }
ForceInline Void Sub(RVector3 v2, RVector3 vdst)
{ Vec3Sub(*this, v2, vdst); }
ForceInline Void Invert()
{ Vec3Invert(*this); }
ForceInline Void Scale(Float factor)
{ Vec3Scale(*this, factor); }
ForceInline Void Copy(RVector3 vdst)
{ Vec3Copy(*this, vdst); }
ForceInline Void CrossProduct(RVector3 v2, RVector3 vdst)
{ Vec3CrossProduct(*this, v2, vdst); }
ForceInline Void Normal(RVector3 v2, RVector3 v3, RVector3 vdst)
{ Vec3Normal(*this, v2, v3, vdst); }
ForceInline Void Swap(RVector3 v2)
{ Vec3Swap(*(PVector3) this, v2); }
ForceInline Float DistBetweenSquared(RVector3 v2)
{ Float distsq; Vec3DistBetweenSquared(*this, v2, distsq); return distsq; }
ForceInline Void AddScaled(RVector3 v2, Float v2scale, RVector3 vdst)
{ Vec3AddScaled(*this, v2, v2scale, vdst); }
ForceInline Void SubScaled(RVector3 v2, Float v2scale, RVector3 vdst)
{ Vec3SubScaled(*this, v2, v2scale, vdst); }
ForceInline Bool IsValid()
{ return (Vec3IsValid(*this)); }
ForceInline Bool IsNormalized()
{ return (Vec3IsNormalized(*this)); }
ForceInline Float Normalize()
{ return (Vec3Normalize(*this)); }
ForceInline Float Length()
{ return (Vec3Length(*this)); }
ForceInline Float DistanceBetween(RVector3 v2)
{ return (Vec3DistanceBetween(*this, v2)); }
ForceInline Bool Compare(RVector3 v2, Float tolerance)
{ return (Vec3Compare(*this, v2, tolerance)); }
public:
ForceInline operator PVector3 ()
{ return ((PVector3) this); }
ForceInline operator += (RVector3 right)
{ Add(right, *this); }
ForceInline operator -= (RVector3 right)
{ Sub(right, *this); }
ForceInline operator *= (Float right)
{ Scale(right); }
};
#endif |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Vector/Vector 4.cpp] - (1,455 bytes)
// Vector 4.cpp
//
#include "Vector 4.h"
#include "Math\FastSqrt.h"
#include <math.h>
#define VECTOR_COMPARE_EPSILON 0.0005
Bool VecIsValid(RVector4 vec)
{
if ((vec.m_X * vec.m_X) < 0.0f)
return FALSE;
if ((vec.m_Y * vec.m_Y) < 0.0f)
return FALSE;
if ((vec.m_Z * vec.m_Z) < 0.0f)
return FALSE;
if ((vec.m_W * vec.m_W) < 0.0f)
return FALSE;
return TRUE;
}
Bool VecIsNormalized(RVector4 vec)
{
if (fabs(Vec4LengthSquared(vec) - 1.0f) < (VECTOR_COMPARE_EPSILON * VECTOR_COMPARE_EPSILON))
return TRUE;
return FALSE;
}
Float VecNormalize(RVector4 vec)
{
Float fLength = FastSqrt(Vec4LengthSquared(vec));
if (fLength == 0.0f)
return 0.0f;
Float fOneOverLength = 1.0f / fLength;
vec.m_X *= fOneOverLength;
vec.m_Y *= fOneOverLength;
vec.m_Z *= fOneOverLength;
vec.m_W *= fOneOverLength;
return fLength;
}
Float VecLength(RVector4 vec)
{
return (FastSqrt(Vec4LengthSquared(vec)));
}
Float VecDistanceBetween(RVector4 v1, RVector4 v2)
{
Vector4 tmp;
Vec4Sub(v1, v2, tmp);
return (FastSqrt(Vec4LengthSquared(tmp)));
}
Bool VecCompare(RVector4 v1, RVector4 v2, Float fTolerance)
{
if (fabs(v1.m_X - v2.m_X) > fTolerance)
return FALSE;
if (fabs(v1.m_Y - v2.m_Y) > fTolerance)
return FALSE;
if (fabs(v1.m_Z - v2.m_Z) > fTolerance)
return FALSE;
if (fabs(v1.m_W - v2.m_W) > fTolerance)
return FALSE;
return TRUE;
} |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Math/Vector/Vector 4.h] - (4,207 bytes)
// Vector 4.h
//
#ifndef _3D_VECTOR4D_H
#define _3D_VECTOR4D_H
#include "Types.h"
typedef struct Vector4 Vector4, *PVector4, &RVector4;
struct Vector4
{
Float m_X;
Float m_Y;
Float m_Z;
Float m_W;
};
#define Vec4Set(vec, x, y, z, w) { (vec).m_X = (x); (vec).m_Y = (y); (vec).m_Z = (z); (vec).m_W = (w); }
#define Vec4Clear(a) { (a).m_X = (a).m_Y = (a).m_Z = (a).m_W = 0.0f; }
#define Vec4DotProduct(a, b) (((a).m_X * (b).m_X) + ((a).m_Y * (b).m_Y) + ((a).m_Z * (b).m_Z) + ((a).m_W * (a).m_W))
#define Vec4LengthSquared(a) (Vec4DotProduct((a), (a)))
#define Vec4Add(a, b, res) { (res).m_X = (a).m_X + (b).m_X; (res).m_Y = (a).m_Y + (b).m_Y; (res).m_Z = (a).m_Z + (b).m_Z; (res).m_W = (a).m_W + (b).m_W; }
#define Vec4Sub(a, b, res) { (res).m_X = (a).m_X - (b).m_X; (res).m_Y = (a).m_Y - (b).m_Y; (res).m_Z = (a).m_Z - (b).m_Z; (res).m_W = (a).m_W - (b).m_W; }
#define Vec4Invert(a) { (a).m_X = -(a).m_X; (a).m_Y = -(a).m_Y; (a).m_Z = -(a).m_Z; (a).m_W = -(a).m_W; }
#define Vec4Scale(a, factor) { (a).m_X *= (factor); (a).m_Y *= (factor); (a).m_Z *= (factor); (a).m_W *= (factor); }
#define Vec4Copy(src, dst) { (dst) = (src); }
#define Vec4Swap(a, b) { Vector4 tmp = (a); (a) = (b); (b) = tmp; }
#define Vec4DistBetweenSquared(a, b, dist) { Vector4 tmp; Vec4Sub((a), (b), tmp); dist = Vec4LengthSquared(tmp); }
#define Vec4AddScaled(a, b, bscale, dst) { (dst).m_X = (a).m_X + ((b).m_X * (bscale)); (dst).m_Y = (a).m_Y + ((b).m_Y * (bscale)); (dst).m_Z = (a).m_Z + ((b).m_Z * (bscale)); (dst).m_W = (a).m_W + ((b).m_W * (bscale)); }
#define Vec4SubScaled(a, b, bscale, dst) { (dst).m_X = (a).m_X - ((b).m_X * (bscale)); (dst).m_Y = (a).m_Y - ((b).m_Y * (bscale)); (dst).m_Z = (a).m_Z - ((b).m_Z * (bscale)); (dst).m_W = (a).m_W - ((b).m_W * (bscale)); }
Bool Vec4IsValid(RVector4 vec);
Bool Vec4IsNormalized(RVector4 vec);
Float Vec4Normalize(RVector4 vec);
Float Vec4Length(RVector4 vec);
Float Vec4DistanceBetween(RVector4 v1, RVector4 v2);
Bool Vec4Compare(RVector4 v1, RVector4 v2, Float fTolerance);
class CVector4 : public Vector4
{
public:
ForceInline CVector4()
{ Vec4Clear(*this); }
ForceInline CVector4(Float x, Float y, Float z, Float w)
{ Vec4Set(*this, x, y, z, w); }
public:
ForceInline Void Set(Float x, Float y, Float z, Float w)
{ Vec4Set(*this, x, y, z, w); }
ForceInline Void Clear()
{ Vec4Clear(*this); }
ForceInline Float DotProduct(RVector4 v2)
{ return (Vec4DotProduct(*this, v2)); }
ForceInline Float LengthSquared()
{ return (Vec4LengthSquared(*this)); }
ForceInline Void Add(RVector4 v2, RVector4 vdst)
{ Vec4Add(*this, v2, vdst); }
ForceInline Void Sub(RVector4 v2, RVector4 vdst)
{ Vec4Sub(*this, v2, vdst); }
ForceInline Void Invert()
{ Vec4Invert(*this); }
ForceInline Void Scale(Float factor)
{ Vec4Scale(*this, factor); }
ForceInline Void Copy(RVector4 vdst)
{ Vec4Copy(*this, vdst); }
ForceInline Void Swap(RVector4 v2)
{ Vec4Swap(*(PVector4) this, v2); }
ForceInline Float DistBetweenSquared(RVector4 v2)
{ Float distsq; Vec4DistBetweenSquared(*this, v2, distsq); return distsq; }
ForceInline Void AddScaled(RVector4 v2, Float v2scale, RVector4 vdst)
{ Vec4AddScaled(*this, v2, v2scale, vdst); }
ForceInline Void SubScaled(RVector4 v2, Float v2scale, RVector4 vdst)
{ Vec4SubScaled(*this, v2, v2scale, vdst); }
ForceInline Bool IsValid()
{ return (Vec4IsValid(*this)); }
ForceInline Bool IsNormalized()
{ return (Vec4IsNormalized(*this)); }
ForceInline Float Normalize()
{ return (Vec4Normalize(*this)); }
ForceInline Float Length()
{ return (Vec4Length(*this)); }
ForceInline Float DistanceBetween(RVector4 v2)
{ return (Vec4DistanceBetween(*this, v2)); }
ForceInline Bool Compare(RVector4 v2, Float tolerance)
{ return (Vec4Compare(*this, v2, tolerance)); }
public:
ForceInline operator PVector4 ()
{ return ((PVector4) this); }
ForceInline operator += (RVector4 right)
{ Add(right, *this); }
ForceInline operator -= (RVector4 right)
{ Sub(right, *this); }
ForceInline operator *= (Float right)
{ Scale(right); }
};
#endif |
|
Currently browsing [vecmatquat.zip] (17,015 bytes) - [Types.h] - (3,332 bytes)
// Types.h
//
#ifndef _3D_TYPES_H
#define _3D_TYPES_H
//#include "..\packs\mcom\MCOM.h"
// Definitions:
///////////////
// Integer types:
typedef signed long int Int, *PInt; // must be 32 bits at minimum
typedef signed char Int8, *PInt8;
typedef signed short int Int16, *PInt16;
typedef signed long int Int32, *PInt32;
typedef signed __int64 Int64, *PInt64;
// Unsigned integer types:
typedef unsigned long int UInt, *PUInt; // must be 32 bits at minimum
typedef unsigned char UInt8, *PUInt8;
typedef unsigned short int UInt16, *PUInt16;
typedef unsigned long int UInt32, *PUInt32;
typedef unsigned __int64 UInt64, *PUInt64;
// Floating point types:
typedef float Float, *PFloat;
typedef double Double, *PDouble;
// String types:
typedef char Char, *PChar;
typedef char * Str, *PStr;
typedef const char * CStr, *PCStr;
// Misc types:
typedef int File, *PFile;
typedef Int Bool, *PBool;
typedef void Void, *PVoid;
typedef void * Ptr, *PPtr;
typedef const void * CPtr, *PCPtr;
typedef signed long int Result, *PResult;
typedef unsigned char Byte, *PByte; // must be exactly 1 byte
typedef unsigned char SmallBool, *PSmallBool;
typedef unsigned int Enum, *PEnum;
typedef unsigned int Bitfield, *PBitfield;
#define ForceInline __forceinline
#define Inline __inline
#ifndef NULL
#define NULL (0)
#endif
#ifndef FALSE
#define FALSE (0)
#endif
#ifndef TRUE
#define TRUE (1)
#endif
// Standard mathematic and physic constants:
#define CONST_PI_OVER_4 0.7853981633974483096156608458198757210492923498437764552437361480769541015715522496570087063355292669
#define CONST_PI_OVER_2 1.570796326794896619231321691639751442098584699687552910487472296153908203143104499314017412671058533
#define CONST_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117067
#define CONST_2_PI 6.233185307179586476925286766559005768394338798750211641949889184615632812572417997256069650684234135
#define CONST_E 2.718281828459045235360287471352662497757247093699959574966967627724076630353547594571382178525166427
#define CONST_LN_10 2.302585092994045684017991454684364207601101488628772976033327900967572609677352480235997205089598298
#define CONST_LOG_E 0.4342944819032518276511289189166050822943970058036665661144537831658646492088707747292249493384317483
#define CONST_C 299792456.2 // meter per second
#define CONST_U 1.6605519E-27 // kilograms
#define CONST_ELECTRON_MASS 9.1095E-31 // kilograms
#define CONST_NEUTRON_MASS 1.6749E-27 // kilograms
#define CONST_PROTON_MASS 1.6726E-27 // kilograms
// OS platform:
//#define ENGINE_WIN32_IMPLEMENTATION
#endif |
|
The zip file viewer built into the Developer Toolbox made use
of the zlib library, as well as the zlibdll source additions.
|
