664 lines
12 KiB
C++
664 lines
12 KiB
C++
#include "stdafx.h"
|
|
|
|
extern "C"
|
|
{
|
|
float pi()
|
|
{
|
|
__asm fldpi
|
|
}
|
|
|
|
void memclear(void* dest, size_t _size)
|
|
{
|
|
__asm
|
|
{
|
|
xor eax, eax
|
|
mov edi, dest
|
|
mov ecx, _size
|
|
rep stosb
|
|
}
|
|
}
|
|
|
|
void* memcpy(void* dest, const void* source, size_t _size)
|
|
{
|
|
__asm
|
|
{
|
|
mov esi, source
|
|
mov edi, dest
|
|
mov ecx, _size
|
|
rep movsb
|
|
}
|
|
}
|
|
|
|
size_t strlen(const char* string)
|
|
{
|
|
__asm
|
|
{
|
|
xor ecx, ecx
|
|
mov edi, string
|
|
not ecx
|
|
xor al, al
|
|
cld
|
|
repne scasb
|
|
not ecx
|
|
lea eax, [ecx-1]
|
|
}
|
|
}
|
|
|
|
float pow(float x, float y)
|
|
{
|
|
float r;
|
|
__asm
|
|
{
|
|
fld y
|
|
fld x
|
|
fyl2x
|
|
fld1
|
|
fld st(1)
|
|
fprem
|
|
f2xm1
|
|
faddp st(1),st
|
|
fscale
|
|
fxch st(1)
|
|
fstp st(0)
|
|
fstp r
|
|
}
|
|
return r;
|
|
}
|
|
|
|
float sqrt(float x)
|
|
{
|
|
__asm fld x
|
|
__asm fsqrt
|
|
}
|
|
|
|
float sin(float x)
|
|
{
|
|
__asm fld x
|
|
__asm fsin
|
|
}
|
|
|
|
float cos(float x)
|
|
{
|
|
__asm fld x
|
|
__asm fcos
|
|
}
|
|
|
|
float fabs(float x)
|
|
{
|
|
__asm fld x
|
|
__asm fabs
|
|
}
|
|
|
|
float tan(float f)
|
|
{
|
|
return sin(f) / cos(f);
|
|
}
|
|
|
|
float smoothstep(float x, float a, float b)
|
|
{
|
|
if (x<a) return 0.0f;
|
|
if (x>b) return 1.0f;
|
|
x = (x-a)/(b-a);
|
|
return x*x*(3.0f-2.0f*x);
|
|
}
|
|
|
|
float exp2(float f)
|
|
{
|
|
_asm fld dword ptr [f]
|
|
_asm fld1
|
|
_asm fld st(1)
|
|
_asm fprem
|
|
_asm f2xm1
|
|
_asm faddp st(1), st
|
|
_asm fscale
|
|
_asm fstp st(1)
|
|
_asm fstp dword ptr [f]
|
|
return f;
|
|
}
|
|
|
|
unsigned long ftol(float x)
|
|
{
|
|
int t;
|
|
_asm fld x
|
|
_asm fistp t
|
|
return t;
|
|
}
|
|
|
|
long _ftol2_sse(float x)
|
|
{
|
|
*((char*)0) = 0; // If it breaks here, it's because you have a cast to (int) somewhere. Use ftol for float-to-int casts.
|
|
return ftol(x);
|
|
}
|
|
|
|
float min(float a, float b)
|
|
{
|
|
if (a < b) return a;
|
|
return b;
|
|
}
|
|
|
|
float max(float a, float b)
|
|
{
|
|
if (a > b) return a;
|
|
return b;
|
|
}
|
|
|
|
int _fltused = 1;
|
|
}
|
|
|
|
vec4::vec4():x(0),y(0),z(0),w(0){};
|
|
vec4::vec4(const vec4& arg):x(arg.x),y(arg.y),z(arg.z),w(arg.w){};
|
|
vec4::vec4(float X, float Y, float Z, float W):x(X),y(Y),z(Z),w(W){};
|
|
vec4::vec4(float a):x(a),y(a),z(a),w(0){};
|
|
vec4::vec4(const float* arg){memcpy(m, arg, 16);};
|
|
vec4::vec4(const __m128& arg){_mm_store_ps(m, arg);};
|
|
|
|
int vec4::ToInt32() const
|
|
{
|
|
return (((ftol(255 * x) << 8) + ftol(255 * y)) << 8) + ftol(255 * z);
|
|
}
|
|
|
|
vec4::operator __m128() const
|
|
{
|
|
return _mm_load_ps(m);
|
|
}
|
|
|
|
float& vec4::operator[](int i)
|
|
{
|
|
return m[i];
|
|
}
|
|
|
|
vec4 vec4::operator + (const vec4& arg) const
|
|
{
|
|
return _mm_add_ps(_mm_load_ps(m), _mm_load_ps(arg.m));
|
|
}
|
|
|
|
vec4 vec4::operator + (float arg) const
|
|
{
|
|
return *this + _mm_set_ps1(arg);
|
|
}
|
|
|
|
void vec4::operator += (const vec4& arg)
|
|
{
|
|
_mm_store_ps(m, _mm_add_ps(*this, arg));
|
|
}
|
|
|
|
void vec4::operator += (float arg)
|
|
{
|
|
*this += _mm_set_ps1(arg);
|
|
}
|
|
|
|
vec4 vec4::operator - (const vec4& arg) const
|
|
{
|
|
return _mm_sub_ps(*this, arg);
|
|
}
|
|
|
|
vec4 vec4::operator - (float arg) const
|
|
{
|
|
return *this - _mm_set_ps1(arg);
|
|
}
|
|
|
|
void vec4::operator -= (const vec4& arg)
|
|
{
|
|
_mm_store_ps(m, _mm_sub_ps(*this, arg));
|
|
}
|
|
|
|
void vec4::operator -= (float arg)
|
|
{
|
|
*this -= _mm_set_ps1(arg);
|
|
}
|
|
|
|
vec4 vec4::operator * (const vec4& arg) const
|
|
{
|
|
return _mm_mul_ps(*this, arg);
|
|
}
|
|
|
|
vec4 vec4::operator * (float arg) const
|
|
{
|
|
return *this * _mm_set_ps1(arg);
|
|
}
|
|
|
|
void vec4::operator *= (const vec4& arg)
|
|
{
|
|
_mm_store_ps(m, _mm_mul_ps(*this, arg));
|
|
}
|
|
|
|
void vec4::operator *= (float arg)
|
|
{
|
|
*this *= _mm_set_ps1(arg);
|
|
}
|
|
|
|
vec4 vec4::operator / (float arg) const
|
|
{
|
|
return *this * _mm_set_ps1(1.0f / arg);
|
|
}
|
|
|
|
void vec4::operator /= (float arg)
|
|
{
|
|
*this *= _mm_set_ps1(1.0f / arg);
|
|
}
|
|
|
|
vec4 vec4::operator -() const
|
|
{
|
|
return *this * -1.0f;
|
|
}
|
|
|
|
__inline vec4 align(vec4& arg)
|
|
{
|
|
return arg;
|
|
};
|
|
|
|
float dot(const vec4& arg1, const vec4& arg2)
|
|
{
|
|
__m128 temp = _mm_mul_ps(arg1, arg2);
|
|
temp = _mm_hadd_ps(temp, temp);
|
|
temp = _mm_hadd_ps(temp, temp);
|
|
return temp.m128_f32[0];
|
|
}
|
|
|
|
float length(vec4& arg)
|
|
{
|
|
return sqrt(dot(arg, arg));
|
|
}
|
|
|
|
vec4 normalize(vec4 const& arg)
|
|
{
|
|
return arg * _mm_rsqrt_ss(_mm_set_ps1(dot(arg,arg))).m128_f32[0];
|
|
}
|
|
|
|
vec4 mix(vec4& arg1, vec4& arg2, float t)
|
|
{
|
|
return arg1 * (1.0f - t) + arg2 * t;
|
|
}
|
|
|
|
vec4 clamp(vec4& arg, float lo, float hi)
|
|
{
|
|
return _mm_max_ps(_mm_min_ps(arg, _mm_set_ps1(hi)), _mm_set_ps1(lo));
|
|
}
|
|
|
|
float clamp(float arg, float lo, float hi)
|
|
{
|
|
return max(min(arg, hi), lo);
|
|
}
|
|
|
|
vec4 reflect(vec4& arg1, vec4& arg2)
|
|
{
|
|
return arg1 - arg2 * 2.0f * dot(arg2, arg1);
|
|
}
|
|
|
|
vec4 cross(vec4& va, vec4& vb)
|
|
{
|
|
__m128 a = va;
|
|
__m128 b = vb;
|
|
__m128 ea = _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 0, 2, 1));
|
|
__m128 eb = _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 1, 0, 2));
|
|
__m128 xa = _mm_mul_ps(ea, eb);
|
|
a = _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 1, 0, 2));
|
|
b = _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 0, 2, 1));
|
|
__m128 xb = _mm_mul_ps(a, b);
|
|
return _mm_sub_ps(xa, xb);
|
|
}
|
|
|
|
Quaternion::Quaternion()
|
|
: x(0)
|
|
, y(0)
|
|
, z(0)
|
|
, w(1)
|
|
{
|
|
};
|
|
|
|
Quaternion::Quaternion(const Quaternion& arg)
|
|
: x(arg.x)
|
|
, y(arg.y)
|
|
, z(arg.z)
|
|
, w(arg.w)
|
|
{
|
|
};
|
|
|
|
Quaternion::Quaternion(const float* M)
|
|
{
|
|
memcpy(m, M, 4 * sizeof(float));
|
|
};
|
|
|
|
Quaternion::operator __m128() const
|
|
{
|
|
return _mm_load_ps(m);
|
|
}
|
|
|
|
Quaternion::Quaternion(float X, float Y, float Z, float W, bool ab_Rotation)
|
|
: x(X)
|
|
, y(Y)
|
|
, z(Z)
|
|
, w(W)
|
|
{
|
|
if (ab_Rotation)
|
|
{
|
|
W *= pi();
|
|
W /= 180.0;
|
|
w = 0.0f;
|
|
normalize();
|
|
_mm_store_ps(m, _mm_mul_ps(*this, _mm_set_ps1(sin(W / 2.0f))));
|
|
w = cos(W / 2.0f);
|
|
}
|
|
};
|
|
|
|
Quaternion::Quaternion(const __m128& arg)
|
|
{
|
|
_mm_store_ps(m, arg);
|
|
};
|
|
|
|
Quaternion::operator float*()
|
|
{
|
|
return m;
|
|
}
|
|
|
|
Quaternion Quaternion::operator * (const Quaternion& arg) const
|
|
{
|
|
return _mm_set_ps(
|
|
x * -arg.x - y * arg.y - z * arg.z * w * arg.w,
|
|
x * -arg.y + y * arg.x + z * arg.w - w * arg.z,
|
|
x * -arg.z - y * arg.w + z * arg.x + w * arg.y,
|
|
x * -arg.w + y * arg.z - z * arg.y + w * arg.x);
|
|
}
|
|
|
|
Quaternion Quaternion::operator -() const
|
|
{
|
|
return _mm_set_ps(-x, -y, -z, -w);
|
|
}
|
|
|
|
void Quaternion::operator *= (const Quaternion& arg)
|
|
{
|
|
_mm_store_ps(m, *this * arg);
|
|
}
|
|
|
|
void Quaternion::operator += (const Quaternion& arg)
|
|
{
|
|
_mm_store_ps(m, _mm_add_ps(*this, arg));
|
|
}
|
|
|
|
void Quaternion::rotate (const Quaternion& arg)
|
|
{
|
|
_mm_store_ps(m, -arg * *this * arg);
|
|
}
|
|
|
|
float Quaternion::lengthSquared() const
|
|
{
|
|
__m128 temp = _mm_mul_ps(*this, *this);
|
|
temp = _mm_hadd_ps(temp, temp);
|
|
temp = _mm_hadd_ps(temp, temp);
|
|
return temp.m128_f32[0];
|
|
}
|
|
|
|
float Quaternion::length() const
|
|
{
|
|
return sqrt(lengthSquared());
|
|
}
|
|
|
|
void Quaternion::normalize()
|
|
{
|
|
_mm_store_ps(m, _mm_mul_ps(*this, _mm_set_ps1(_mm_rsqrt_ss(_mm_set_ps1(lengthSquared())).m128_f32[0])));
|
|
}
|
|
|
|
Matrix::Matrix()
|
|
: r1(1, 0, 0, 0)
|
|
, r2(0, 1, 0, 0)
|
|
, r3(0, 0, 1, 0)
|
|
, r4(0, 0, 0, 1)
|
|
{
|
|
};
|
|
|
|
Matrix::Matrix(const Matrix& arg)
|
|
: r1(arg.r1)
|
|
, r2(arg.r2)
|
|
, r3(arg.r3)
|
|
, r4(arg.r4)
|
|
{
|
|
};
|
|
|
|
Matrix::Matrix(const vec4& R1, const vec4& R2, const vec4& R3, const vec4& R4)
|
|
: r1(R1)
|
|
, r2(R2)
|
|
, r3(R3)
|
|
, r4(R4)
|
|
{
|
|
};
|
|
|
|
Matrix::Matrix(const float* M)
|
|
: r1(&M[0])
|
|
, r2(&M[4])
|
|
, r3(&M[8])
|
|
, r4(&M[12])
|
|
{
|
|
};
|
|
|
|
Matrix::Matrix(const Quaternion& aQuaternion)
|
|
{
|
|
Quaternion lQuaterion(aQuaternion);
|
|
float lf_Length2 = lQuaterion.lengthSquared();
|
|
if (lf_Length2 != 1.0 && lf_Length2 != 0.0)
|
|
{
|
|
lQuaterion.normalize();
|
|
}
|
|
|
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
|
|
|
x2 = lQuaterion.m[0] + lQuaterion.m[0];
|
|
y2 = lQuaterion.m[1] + lQuaterion.m[1];
|
|
z2 = lQuaterion.m[2] + lQuaterion.m[2];
|
|
|
|
xx = lQuaterion.m[0] * x2;
|
|
xy = lQuaterion.m[0] * y2;
|
|
xz = lQuaterion.m[0] * z2;
|
|
|
|
yy = lQuaterion.m[1] * y2;
|
|
yz = lQuaterion.m[1] * z2;
|
|
zz = lQuaterion.m[2] * z2;
|
|
|
|
wx = lQuaterion.m[3] * x2;
|
|
wy = lQuaterion.m[3] * y2;
|
|
wz = lQuaterion.m[3] * z2;
|
|
|
|
r1 = vec4(1.0f - (yy + zz), xy - wz, xz + wy, 0.0f);
|
|
r2 = vec4(xy + wz, 1.0f - (xx + zz), yz - wx, 0.0f);
|
|
r3 = vec4(xz - wy, yz + wx, 1.0f - (xx + yy), 0.0f);
|
|
r4 = vec4(0.0f, 0.0f, 0.0f, 1.0f);
|
|
};
|
|
|
|
float& Matrix::operator[](int i)
|
|
{
|
|
return m[i];
|
|
}
|
|
|
|
vec4 Matrix::operator * (const vec4& arg) const
|
|
{
|
|
return vec4(dot(r1, arg), dot(r2, arg), dot(r3, arg), dot(r4, arg));
|
|
}
|
|
|
|
Matrix Matrix::operator * (const Matrix& arg) const
|
|
{
|
|
Matrix temp;
|
|
for (int i = 0; i < 4; ++i) // zeile
|
|
{
|
|
for (int j = 0; j < 4; ++j) // spalte
|
|
{
|
|
float value = 0;
|
|
for (int k = 0; k < 4; ++k)
|
|
{
|
|
value += m[i*4+k] * arg.m[k*4+j];
|
|
}
|
|
temp.m[i*4+j] = value;
|
|
}
|
|
}
|
|
return temp;
|
|
}
|
|
|
|
void Matrix::operator *= (const Matrix& arg)
|
|
{
|
|
Matrix lTemp = *this;
|
|
r1 = lTemp * arg.r1;
|
|
r2 = lTemp * arg.r2;
|
|
r3 = lTemp * arg.r3;
|
|
r4 = lTemp * arg.r4;
|
|
}
|
|
|
|
Matrix Matrix::rotateX(float af_Rad)
|
|
{
|
|
float s = sin(af_Rad);
|
|
float c = cos(af_Rad);
|
|
return Matrix(
|
|
vec4(1,0,0,0),
|
|
vec4(0,c,-s,0),
|
|
vec4(0,s,c,0),
|
|
vec4(0,0,0,1));
|
|
}
|
|
|
|
Matrix Matrix::rotateY(float af_Rad)
|
|
{
|
|
float s = sin(af_Rad);
|
|
float c = cos(af_Rad);
|
|
return Matrix(
|
|
vec4(c,0,s,0),
|
|
vec4(0,1,0,0),
|
|
vec4(-s,0,c,0),
|
|
vec4(0,0,0,1));
|
|
}
|
|
|
|
Matrix Matrix::rotateZ(float af_Rad)
|
|
{
|
|
float s = sin(af_Rad);
|
|
float c = cos(af_Rad);
|
|
return Matrix(
|
|
vec4(c,-s,0,0),
|
|
vec4(s,c,0,0),
|
|
vec4(0,0,1,0),
|
|
vec4(0,0,0,1));
|
|
}
|
|
|
|
Matrix Matrix::scale(float s)
|
|
{
|
|
return Matrix(
|
|
vec4(s,0,0,0),
|
|
vec4(0,s,0,0),
|
|
vec4(0,0,s,0),
|
|
vec4(0,0,0,1));
|
|
}
|
|
|
|
Matrix Matrix::scale(const vec4& s)
|
|
{
|
|
return Matrix(
|
|
vec4(s.x,0,0,0),
|
|
vec4(0,s.y,0,0),
|
|
vec4(0,0,s.z,0),
|
|
vec4(0,0,0,1));
|
|
}
|
|
|
|
Matrix Matrix::translate(const vec4& aTranslation)
|
|
{
|
|
return Matrix(
|
|
vec4(1,0,0,aTranslation.x),
|
|
vec4(0,1,0,aTranslation.y),
|
|
vec4(0,0,1,aTranslation.z),
|
|
vec4(0,0,0,1));
|
|
}
|
|
|
|
void Matrix::transpose()
|
|
{
|
|
Matrix lTemp(
|
|
vec4(r1.x, r2.x, r3.x, r4.x),
|
|
vec4(r1.y, r2.y, r3.y, r4.y),
|
|
vec4(r1.z, r2.z, r3.z, r4.z),
|
|
vec4(r1.w, r2.w, r3.w, r4.w));
|
|
r1 = lTemp.r1;
|
|
r2 = lTemp.r2;
|
|
r3 = lTemp.r3;
|
|
r4 = lTemp.r4;
|
|
}
|
|
|
|
float Matrix::Determinante2x2(float a1, float a2, float b1, float b2)
|
|
{
|
|
return a1 * b2 - b1 * a2;
|
|
}
|
|
|
|
float Matrix::Determinante3x3( float a1, float a2, float a3,
|
|
float b1, float b2, float b3,
|
|
float c1, float c2, float c3)
|
|
{
|
|
return a1 * Determinante2x2( b2, b3, c2, c3 ) - b1 * Determinante2x2( a2, a3, c2, c3 ) + c1 * Determinante2x2( a2, a3, b2, b3 );
|
|
}
|
|
|
|
void Matrix::inverse()
|
|
{
|
|
Matrix lResult;
|
|
lResult.m[0] = Determinante3x3( m[5], m[9], m[13], m[6], m[10], m[14], m[7], m[11], m[15] );
|
|
lResult.m[4] = -Determinante3x3( m[4], m[8], m[12], m[6], m[10], m[14], m[7], m[11], m[15] );
|
|
lResult.m[8] = Determinante3x3( m[4], m[8], m[12], m[5], m[9], m[13], m[7], m[11], m[15] );
|
|
lResult.m[12] = -Determinante3x3( m[4], m[8], m[12], m[5], m[9], m[13], m[6], m[10], m[14] );
|
|
|
|
lResult.m[1] = -Determinante3x3( m[1], m[9], m[13], m[2], m[10], m[14], m[3], m[11], m[15] );
|
|
lResult.m[5] = Determinante3x3( m[0], m[8], m[12], m[2], m[10], m[14], m[3], m[11], m[15] );
|
|
lResult.m[9] = -Determinante3x3( m[0], m[8], m[12], m[1], m[9], m[13], m[3], m[11], m[15] );
|
|
lResult.m[13] = Determinante3x3( m[0], m[8], m[12], m[1], m[9], m[13], m[2], m[10], m[14] );
|
|
|
|
lResult.m[2] = Determinante3x3( m[1], m[5], m[13], m[2], m[6], m[14], m[3], m[7], m[15] );
|
|
lResult.m[6] = -Determinante3x3( m[0], m[4], m[12], m[2], m[6], m[14], m[3], m[7], m[15] );
|
|
lResult.m[10] = Determinante3x3( m[0], m[4], m[12], m[1], m[5], m[13], m[3], m[7], m[15] );
|
|
lResult.m[14] = -Determinante3x3( m[0], m[4], m[12], m[1], m[5], m[13], m[2], m[6], m[14] );
|
|
|
|
lResult.m[3] = -Determinante3x3( m[1], m[5], m[9], m[2], m[6], m[10], m[3], m[7], m[11] );
|
|
lResult.m[7] = Determinante3x3( m[0], m[4], m[8], m[2], m[6], m[10], m[3], m[7], m[11] );
|
|
lResult.m[11] = -Determinante3x3( m[0], m[4], m[8], m[1], m[5], m[9], m[3], m[7], m[11] );
|
|
lResult.m[15] = Determinante3x3( m[0], m[4], m[8], m[1], m[5], m[9], m[2], m[6], m[10] );
|
|
|
|
float lf_Det = (m[0] * lResult.m[0]) + (m[1] * lResult.m[4]) + (m[2] * lResult.m[8]) + (m[3] * lResult.m[12]);
|
|
if (lf_Det == 0.0f)
|
|
return; // matrix is singular
|
|
|
|
lf_Det = 1.0f / lf_Det;
|
|
|
|
*this = lResult;
|
|
|
|
m[0] *= lf_Det;
|
|
m[4] *= lf_Det;
|
|
m[8] *= lf_Det;
|
|
m[12] *= lf_Det;
|
|
|
|
m[1] *= lf_Det;
|
|
m[5] *= lf_Det;
|
|
m[9] *= lf_Det;
|
|
m[13] *= lf_Det;
|
|
|
|
m[2] *= lf_Det;
|
|
m[6] *= lf_Det;
|
|
m[10] *= lf_Det;
|
|
m[14] *= lf_Det;
|
|
|
|
m[3] *= lf_Det;
|
|
m[7] *= lf_Det;
|
|
m[11] *= lf_Det;
|
|
m[15] *= lf_Det;
|
|
}
|
|
|
|
Matrix Matrix::Perspective(float fov, float aspectRatio, float zNear, float zFar)
|
|
{
|
|
float f = 1.0f / tan(fov / 2.0f);
|
|
float m = zNear - zFar;
|
|
Matrix M(
|
|
vec4(f / aspectRatio, 0, 0, 0),
|
|
vec4(0, f, 0, 0),
|
|
vec4(0, 0, (zFar + zNear) / m, (2 * zFar * zNear) / m),
|
|
vec4(0, 0, -1, 0));
|
|
return M;
|
|
}
|
|
|
|
Matrix Matrix::LookAt(vec4& eye, vec4& center, vec4& up)
|
|
{
|
|
vec4 f = normalize(center - eye);
|
|
vec4 u = normalize(up);
|
|
vec4 s = cross(f, u);
|
|
u = cross(s, f);
|
|
s[3] = u[3] = f[3] = 0;
|
|
Matrix M(s, u, -f, vec4(0, 0, 0, 1));
|
|
//M.transpose();
|
|
M *= Matrix::translate(-eye);
|
|
return M;
|
|
}
|