diff --git a/src/5threed/Image.h b/src/5threed/Image.h index 15b800e..cbf08e0 100644 --- a/src/5threed/Image.h +++ b/src/5threed/Image.h @@ -40,7 +40,7 @@ public: //检查文件类型 FREE_IMAGE_FORMAT fifmt = FreeImage_GetFileType(fileName, 0); if (fifmt == FIF_UNKNOWN) { - die("FreeImage_GetFileType"); + //die("FreeImage_GetFileType"); } //获取 diff --git a/src/5threed/Raster.cc b/src/5threed/Raster.cc index 6e71a73..e562b5b 100644 --- a/src/5threed/Raster.cc +++ b/src/5threed/Raster.cc @@ -43,6 +43,8 @@ Raster::Raster(int width, int height) { _defaultUvPointer.data = _defaultUvArray; _matModel = matrix4(1); + _matView = matrix4(1); + _matProj = matrix4(1); } Raster::~Raster() { @@ -186,7 +188,7 @@ void Raster::drawSpan(const Span &span, Image* image) { - setPixel(i, span.y, dstColor); + setPixel(i, span.y, blendColor); } } @@ -321,62 +323,73 @@ void Raster::drawArrays(DRAWMODE pri, int start, int count) { char* cData = (char*) colorPointerdesc.data; char* uvData = (char*) uvPointerdesc.data; + _matProjView = _matProj * _matView; + matrix4 matPVT = _matProjView.transpose(); + + _frust.loadFrustum(matPVT); + for (int i = start; i < start + count; i += 3) { float* pp = (float*) posData; - float4 mp0(pp[0], pp[1], pp[2], 1); - mp0 = _matModel * mp0; + float3 mp0(pp[0], pp[1], pp[2]); + mp0 = mp0 * _matModel; posData += _positionPointer.stride; pp = (float*) posData; - float4 mp1(pp[0], pp[1], pp[2], 1); - mp1 = _matModel * mp1; + float3 mp1(pp[0], pp[1], pp[2]); + mp1 = mp1 * _matModel; posData += _positionPointer.stride; pp = (float*) posData; - float4 mp2(pp[0], pp[1], pp[2], 1); - mp2 = _matModel * mp2; - - int2 p0(mp0.x, mp0.y); - int2 p1(mp1.x, mp1.y); - int2 p2(mp2.x, mp2.y); - - posData += _positionPointer.stride; - - Rgba* pc = (Rgba*) cData; - Rgba c0(*pc); - cData += _colorPointer.stride; - pc = (Rgba*) cData; - Rgba c1(*pc); - cData += _colorPointer.stride; - pc = (Rgba*) cData; - Rgba c2(*pc); - cData += _colorPointer.stride; - - float* puv = (float*) uvData; - float2 uv0(puv[0], puv[1]); - uvData += _uvPointer.stride; - puv = (float*) uvData; - float2 uv1(puv[0], puv[1]); - uvData += _uvPointer.stride; - puv = (float*) uvData; - float2 uv2(puv[0], puv[1]); - uvData += _uvPointer.stride; - - Ege eges[3] = { - Ege(p0.x, p0.y, p1.x, p1.y, c0, c1, uv0, uv1), - Ege(p1.x, p1.y, p2.x, p2.y, c1, c2, uv1, uv2), - Ege(p2.x, p2.y, p0.x, p0.y, c2, c0, uv2, uv0), - }; - - - drawTriangle(eges, _texture); - - if (_colorPointer.data == 0) { - cData = (char*) colorPointerdesc.data; - } - - if (_uvPointer.data == 0) { - uvData = (char*) uvPointerdesc.data; + float3 mp2(pp[0], pp[1], pp[2]); + mp2 = mp2 * _matModel; + + if (_frust.pointInFrustum(mp0) || _frust.pointInFrustum(mp1) || _frust.pointInFrustum(mp2)) { + mp0 = piplineTransform(mp0); + mp1 = piplineTransform(mp1); + mp2 = piplineTransform(mp2); + + int2 p0(mp0.x, mp0.y); + int2 p1(mp1.x, mp1.y); + int2 p2(mp2.x, mp2.y); + + posData += _positionPointer.stride; + + Rgba* pc = (Rgba*) cData; + Rgba c0(*pc); + cData += _colorPointer.stride; + pc = (Rgba*) cData; + Rgba c1(*pc); + cData += _colorPointer.stride; + pc = (Rgba*) cData; + Rgba c2(*pc); + cData += _colorPointer.stride; + + float* puv = (float*) uvData; + float2 uv0(puv[0], puv[1]); + uvData += _uvPointer.stride; + puv = (float*) uvData; + float2 uv1(puv[0], puv[1]); + uvData += _uvPointer.stride; + puv = (float*) uvData; + float2 uv2(puv[0], puv[1]); + uvData += _uvPointer.stride; + + Ege eges[3] = { + Ege(p0.x, p0.y, p1.x, p1.y, c0, c1, uv0, uv1), + Ege(p1.x, p1.y, p2.x, p2.y, c1, c2, uv1, uv2), + Ege(p2.x, p2.y, p0.x, p0.y, c2, c0, uv2, uv0), + }; + + + drawTriangle(eges, _texture); + + if (_colorPointer.data == 0) { + cData = (char*) colorPointerdesc.data; + } + + if (_uvPointer.data == 0) { + uvData = (char*) uvPointerdesc.data; + } } } @@ -390,6 +403,58 @@ void Raster::loadIdentity() { _matModel = matrix4(1); } +void Raster::loadViewMatrix(const matrix4& mat) { + _matView = mat; +} + +void Raster::loadViewIdentity() { + _matView = matrix4(1); +} + +void Raster::loadProjMatrix(const matrix4& mat) { + _matProj = mat; +} + +void Raster::loadProjIdentity() { + _matProj = matrix4(1); +} + +void Raster::setPerspective(float fovy, float aspect, float zNear, float zFar) { + _matProj = perspective(fovy, aspect, zNear, zFar); +} + +void Raster::lookat(const float3& eye, const float3& center, const float3& up) { + _matView = lookAt(eye, center, up); +} + +void Raster::setViewPort(int x, int y, int w, int h) { + _viewPort.x = w; + _viewPort.y = h; +} + +float3 Raster::piplineTransform(float3 pos) { + float4 world(pos.x, pos.y, pos.z, 1); + + float4 screen = (_matProj * _matView * _matModel) * world; + + if (screen.w == 0.0f) { + return false; + } + + screen.x /= screen.w; + screen.y /= screen.w; + screen.z /= screen.w; + + screen.x = screen.x * 0.5f + 0.5f; + screen.y = screen.y * 0.5f + 0.5f; + screen.z = screen.z * 0.5f + 0.5f; + + screen.x = screen.x * _viewPort.x; + screen.y = screen.y * _viewPort.y; + + return float3(screen.x, screen.y, screen.z); +} + void Raster::bindTexture(Image* image) { _texture = image; } diff --git a/src/5threed/Raster.h b/src/5threed/Raster.h index e4be465..441a120 100644 --- a/src/5threed/Raster.h +++ b/src/5threed/Raster.h @@ -141,6 +141,14 @@ public: void bindTexture(Image* image); void loadMatrix(const matrix4& mat); void loadIdentity(); + void loadViewMatrix(const matrix4& mat); + void loadViewIdentity(); + void loadProjMatrix(const matrix4& mat); + void loadProjIdentity(); + void setPerspective(float fovy, float aspect, float zNear, float zFar); + void lookat(float3 const &eye, float3 const ¢er, float3 const &up); + void setViewPort(int x, int y, int w, int h); + float3 piplineTransform(float3 pos); int size(); void clean(); bool setPixel(int x, int y, Rgba color); @@ -151,8 +159,13 @@ private: int _height; Rgba _color; Image* _texture; - + matrix4 _matModel; + matrix4 _matView; + matrix4 _matProj; + matrix4 _matProjView; + float2 _viewPort; + Frustum _frust; DataElementDes _positionPointer; DataElementDes _colorPointer; diff --git a/src/5threed/common.h b/src/5threed/common.h index bf33036..07994db 100644 --- a/src/5threed/common.h +++ b/src/5threed/common.h @@ -17,23 +17,42 @@ #include #include #include -#include "Rgba.h" -#include "struct/tvec2.h" -#include "struct/tvec3.h" -#include "struct/tmat3x3.h" -#include "struct/tvec4.h" -#include "struct/tmat4x4.h" +#include +#include +#include +#include +#include +#include +#include +#include #define die(m) do { perror(m); exit(EXIT_FAILURE); } while(0) #define def2rad(theta) (0.01745329251994329 * (theta)) //每个角度所对应的弧度 +#define PI 3.14159265358979323 +#define TWO_PI 6.28318530717958647 +#define HALF_PI 1.57079632679489661 +#define DEG2RAD(theta) (0.01745329251994329 * (theta)) +#define RAD2DEG 57.2957795130823208 +#define LOG2 0.69314718055994529 +#define WGS_84_RADIUS_EQUATOR 6378137.0 +#define WGS_84_RADIUS_POLAR 6356752.3142 +#define MIN(a,b) ((a) < (b) ? (a) : (b)) +#define MAX(a,b) ((a) > (b) ? (a) : (b)) +#ifndef FLT_MAX +#define FLT_MAX 3.402823466e+38F +#endif + +#ifndef FLT_MIN +#define FLT_MIN 1.175494351e-38F +#endif + +#define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16)) -typedef tvec2 float2; -typedef tvec2 int2; -typedef tvec3 float3; -typedef tvec4 float4; typedef unsigned char byte; -typedef tmat3x3 matrix3; -typedef tmat4x4 matrix4; +typedef long long int64; +typedef unsigned short ushort; +typedef unsigned int uint; +typedef unsigned long ulong; template inline T tmin(T a, T b) { return a < b ? a : b; @@ -43,31 +62,5376 @@ template inline T tmax(T a, T b) { return a > b ? a : b; } -inline Rgba colorLerp(const Rgba& color1, const Rgba& color2, float step) { - Rgba ret; +union LargeInt { + + struct __LARGE_INT { + unsigned int LowPart; + unsigned int HighPart; + } _largeInt; + int64 int64Data; +}; + +inline float unitRandom() { + return float(rand()) / float( RAND_MAX); +} + +//----------------------------------------------------------------------- + +inline float rangeRandom(float fLow, float fHigh) { + return (fHigh - fLow)*unitRandom() + fLow; +} + +/** + * ����64λ���� + */ +inline int64 makeInt64(unsigned low, unsigned hi) { + LargeInt intdata; + intdata._largeInt.HighPart = low; + intdata._largeInt.LowPart = hi; + return intdata.int64Data; +} + +template +struct tvec2 { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 type; + + value_type x; + value_type y; + + value_type & operator[](size_type i) { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const { + assert(i < this->length()); + return (&x)[i]; + } + + tvec2() : + x(value_type(0)), + y(value_type(0)) { + } + + tvec2(tvec2 const & v) : + x(v.x), + y(v.y) { + } + + tvec2(value_type const & s) : + x(s), + y(s) { + } + + tvec2(value_type const & s1, value_type const & s2) : + x(s1), + y(s2) { + } + + template + tvec2(U const & x) : + x(value_type(x)), + y(value_type(x)) { + } + + template + tvec2(U const & a, V b) : + x(value_type(a)), + y(value_type(b)) { + } + + template + tvec2(tvec2 const & v) : + x(value_type(v.x)), + y(value_type(v.y)) { + } + + tvec2 & operator=(tvec2 const & v) { + this->x = v.x; + this->y = v.y; + return *this; + } + + template + tvec2 & operator=(tvec2 const & v) { + this->x = T(v.x); + this->y = T(v.y); + return *this; + } + + template + tvec2 & operator+=(U const & s) { + this->x += T(s); + this->y += T(s); + return *this; + } + + template + tvec2 & operator+=(tvec2 const & v) { + this->x += T(v.x); + this->y += T(v.y); + return *this; + } + + template + tvec2 & operator-=(U const & s) { + this->x -= T(s); + this->y -= T(s); + return *this; + } + + template + tvec2 & operator-=(tvec2 const & v) { + this->x -= T(v.x); + this->y -= T(v.y); + return *this; + } + + template + tvec2 & operator*=(U s) { + this->x *= T(s); + this->y *= T(s); + return *this; + } + + template + tvec2 & operator*=(tvec2 const & v) { + this->x *= T(v.x); + this->y *= T(v.y); + return *this; + } + + template + tvec2 & operator/=(U s) { + this->x /= T(s); + this->y /= T(s); + return *this; + } + + template + tvec2 & operator/=(tvec2 const & v) { + this->x /= T(v.x); + this->y /= T(v.y); + return *this; + } + + tvec2 & operator++() { + ++this->x; + ++this->y; + return *this; + } + + tvec2 & operator--() { + --this->x; + --this->y; + return *this; + } + + void makeCeil(tvec2 cmp) { + if (cmp.x > x) x = cmp.x; + if (cmp.y > y) y = cmp.y; + } + + void makeFloor(tvec2 cmp) { + if (cmp.x < x) x = cmp.x; + if (cmp.y < y) y = cmp.y; + } +}; + +template +tvec2 rotate(tvec2 const & v, T angle) { + tvec2 res; + T const c(cos(DEG2RAD(angle))); + T const s(sin(DEG2RAD(angle))); + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; +} + +template +bool operator==(tvec2 const & v1, tvec2 const & v2) { + return (v1.x == v2.x) && (v1.y == v2.y); +} + +template +bool operator!=(tvec2 const & v1, tvec2 const & v2) { + return (v1.x != v2.x) || (v1.y != v2.y); +} + +template +tvec2 operator+(tvec2 const & v, T const & s) { + return tvec2( + v.x + T(s), + v.y + T(s)); +} + +template +tvec2 operator+(T const & s, tvec2 const & v) { + return tvec2( + T(s) + v.x, + T(s) + v.y); +} + +template +tvec2 operator+(tvec2 const & v1, tvec2 const & v2) { + return tvec2( + v1.x + T(v2.x), + v1.y + T(v2.y)); +} + +template +tvec2 operator-(tvec2 const & v, T const & s) { + return tvec2( + v.x - T(s), + v.y - T(s)); +} + +template +tvec2 operator-(T const & s, tvec2 const & v) { + return tvec2( + T(s) - v.x, + T(s) - v.y); +} + +template +tvec2 operator-(tvec2 const & v1, tvec2 const & v2) { + return tvec2( + v1.x - T(v2.x), + v1.y - T(v2.y)); +} + +template +tvec2 operator*(tvec2 const & v, T const & s) { + return tvec2( + v.x * T(s), + v.y * T(s)); +} + +template +tvec2 operator*(T const & s, tvec2 const & v) { + return tvec2( + T(s) * v.x, + T(s) * v.y); +} + +template +tvec2 operator*(tvec2 const & v1, tvec2 const & v2) { + return tvec2( + v1.x * T(v2.x), + v1.y * T(v2.y)); +} + +template +tvec2 operator/(tvec2 const & v, T const & s) { + return tvec2( + v.x / T(s), + v.y / T(s)); +} + +template +tvec2 operator/(T const & s, tvec2 const & v) { + return tvec2( + T(s) / v.x, + T(s) / v.y); +} + +template +tvec2 operator/(tvec2 const & v1, tvec2 const & v2) { + return tvec2( + v1.x / T(v2.x), + v1.y / T(v2.y) + ); +} + +template +tvec2 operator-(tvec2 const & v) { + return tvec2 ( + -v.x, + -v.y + ); +} + +template +tvec2 operator++(tvec2 const & v, int) { + return tvec2( + v.x + T(1), + v.y + T(1) + ); +} + +template +tvec2 operator--(tvec2 const & v, int) { + return tvec2( + v.x - T(1), + v.y - T(1) + ); +} + +template +struct tvec3 { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 type; + + value_type x; + value_type y; + value_type z; + + size_type length() const { + return 3; + } + + value_type & operator[](size_type i) { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const { + assert(i < this->length()); + return (&x)[i]; + } + + inline tvec3() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)) { + } + + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) { + } + + inline tvec3(value_type s) : + x(s), + y(s), + z(s) { + } + + inline tvec3(value_type s0, value_type s1, value_type s2) : + x(s0), + y(s1), + z(s2) { + } + + template + tvec3(U s) : + x(value_type(s)), + y(value_type(s)), + z(value_type(s)) { + } + + template + tvec3(A x, B y, C z) : + x(value_type(x)), + y(value_type(y)), + z(value_type(z)) { + } + + template + tvec3(tvec2 const& v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(s)) { + } + + template + tvec3(A s, tvec2 const& v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)) { + } + + template + tvec3(tvec3 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)) { + } + + tvec3& operator=(tvec3 const & v) { + this->x = v.x; + this->y = v.y; + this->z = v.z; + return *this; + } + + template + tvec3& operator=(tvec3 const & v) { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + + template + tvec3 & operator+=(U const & s) { + this->x += T(s); + this->y += T(s); + this->z += T(s); + return *this; + } + + template + tvec3 & operator+=(tvec3 const & v) { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + return *this; + } + + template + tvec3 & operator-=(U const & s) { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + return *this; + } + + template + tvec3 & operator-=(tvec3 const & v) { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + return *this; + } + + template + tvec3 & operator*=(U const & s) { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } + + template + tvec3 & operator*=(tvec3 const & v) { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + return *this; + } + + template + tvec3 & operator/=(U const & s) { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + return *this; + } + + template + tvec3 & operator/=(tvec3 const & v) { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + return *this; + } + + tvec3 & operator++() { + ++this->x; + ++this->y; + ++this->z; + return *this; + } + + tvec3 & operator--() { + --this->x; + --this->y; + --this->z; + return *this; + } + + void makeFloor(const tvec3& cmp) { + if (cmp.x < x) x = cmp.x; + if (cmp.y < y) y = cmp.y; + if (cmp.z < z) z = cmp.z; + } + + void makeCeil(const tvec3& cmp) { + if (cmp.x > x) x = cmp.x; + if (cmp.y > y) y = cmp.y; + if (cmp.z > z) z = cmp.z; + } + + T lengthf() const { + return (T) sqrtf(x * x + y * y + z * z); + } + +}; + +template +bool operator>(const tvec3& left, const tvec3& right) { + return left.x > right.x && left.y > right.y && left.z > right.z; +} + +template +bool operator<(const tvec3& left, const tvec3& right) { + return left.x < right.x && left.y < right.y && left.z < right.z; +} + +template +tvec3 rotateX(const tvec3& v, T angle) { + tvec3 res(v); + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; +} + +template +tvec3 rotateY(tvec3 const & v, T angle) { + tvec3 res = v; + + T c = cos(T(DEG2RAD(angle))); + T s = sin(T(DEG2RAD(angle))); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; +} + +template +tvec3 rotateZ(tvec3 const & v, T angle) { + + tvec3 res = v; + + T c = cos(DEG2RAD(angle)); + + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; +} + +/** + * ���������ļн� + * ������������ A,B + * A��B = |A|*|B|*cos(@) + * cos(@) = A��B/|A|*|B| + * @ = acos(@) + */ +template +T angleBetweenVector(const tvec3& a, const tvec3& b) { +#define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z)) + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag(a) * Mag(b); + T angle = acos(dotProduct / vectorsMagnitude); + T result = angle * T(RAD2DEG); + if (_isnan(result)) { + return T(0); + } else { + return result; + } +} + +template +inline bool _isnan(T t) { + return t == t; +} + +template +T angleBetweenVector(const tvec2& a, const tvec2& b) { +#define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y)) + + T dotProduct = dot(a, b); + T vectorsMagnitude = Mag2D(a) * Mag2D(b); + T angle = acos(dotProduct / vectorsMagnitude); + T result = angle * T(RAD2DEG); + if (_isnan(result)) { + return T(0); + } else { + return result; + } +} + +template +static T clamp(T val, T minval, T maxval) { + assert(minval < maxval && "Invalid clamp range"); + return MAX(MIN(val, maxval), minval); +} + +template +inline T acosEx(T val) { + if (T(-1.0f) < val) { + if (val < 1.0f) + return T(acos(val)); + else + return T(0); + } else { + return T(PI); + } +} + +template +inline T angleBetween(const tvec3& a, const tvec3& b) { + T lenProduct = a.lengthf() * b.lengthf(); - ret._r = (unsigned char) color1._r + step * (color2._r - color1._r); - ret._g = (unsigned char) color1._g + step * (color2._g - color1._g); - ret._b = (unsigned char) color1._b + step * (color2._b - color1._b); - ret._a = (unsigned char) color1._a + step * (color2._a - color1._a); + // Divide by zero check + if (lenProduct < 1e-6f) + lenProduct = 1e-6f; - return ret; + float f = dot(a, b) / lenProduct; + + f = clamp(f, T(-1.0), T(1.0)); + return acosEx(f); + +} + +/** + * ���ڶ������ + * ������ڶ�����У��򣬵���ߵļн�֮�� == 360 + */ +template +bool insidePolyon(const tvec3& point, const tvec3 polygon[], size_t count) { + tvec3 vA, vB; + T angle = T(0.0); + for (size_t i = 0; i < count; ++i) { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + angle += angleBetweenVector(vA, vB); + } + if (abs(angle - 360) >= 0.5f) { + return true; + } + return false; +} + +template +bool insidePolyon(const tvec2& point, const tvec2 polygon[], size_t count) { + T angle = T(0.0); + tvec2 vA, vB; + for (size_t i = 0; i < count; ++i) { + vA = polygon[i] - point; + vB = polygon[(i + 1) % count] - point; + tvec3 a(vA.x, vA.y, 0); + tvec3 b(vB.x, vB.y, 0); + angle += angleBetweenVector(a, b); + } + if (abs(angle - 360) >= 0.5f) { + return true; + } + return false; +} + +template +bool pointinTriangle(tvec3 A, tvec3 B, tvec3 C, tvec3 P) { + tvec3 v0 = C - A; + tvec3 v1 = B - A; + tvec3 v2 = P - A; + + float dot00 = dot(v0, v0); + float dot01 = dot(v0, v1); + float dot02 = dot(v0, v2); + float dot11 = dot(v1, v1); + float dot12 = dot(v1, v2); + + float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01); + + float u = (dot11 * dot02 - dot01 * dot12) * inverDeno; + if (u < 0 || u > 1) // if u out of range, return directly + { + return false; + } + + float v = (dot00 * dot12 - dot01 * dot02) * inverDeno; + if (v < 0 || v > 1) // if v out of range, return directly + { + return false; + } + + return u + v <= 1; } -inline float2 uvLerp(const float2& uv1, const float2& uv2, float step) { +template +bool pointinTriangle(tvec2 A, tvec2 B, tvec2 C, tvec2 P) { + return pointinTriangle( + tvec3(A.x, A.y, 0), + tvec3(B.x, B.y, 0), + tvec3(C.x, C.y, 0), + tvec3(P.x, P.y, 0)); +} + +/** + * �������������ཻ + */ +template +bool intersectTriangle( + const tvec3& orig, + const tvec3& dir, + tvec3& v0, + tvec3& v1, + tvec3& v2, + T* t, + T* u, + T* v + ) { + // Find vectors for two edges sharing vert0 + tvec3 edge1 = v1 - v0; + tvec3 edge2 = v2 - v0; + + // Begin calculating determinant - also used to calculate U parameter + tvec3 pvec; + pvec = cross(dir, edge2); - if (step < 0 || step > 1) { - printf("step : %f\n", step); - die("step must more than zero and less than 1"); + // If determinant is near zero, ray lies in plane of triangle + T det = dot(edge1, pvec); + + tvec3 tvec; + if (det > 0) { + tvec = orig - v0; + } else { + tvec = v0 - orig; + det = -det; } + if (det < 0.0001f) + return false; + // Calculate U parameter and test bounds + *u = dot(tvec, pvec); + if (*u < 0.0f || *u > det) + return false; + + // Prepare to test V parameter + tvec3 qvec; + qvec = cross(tvec, edge1); + + // Calculate V parameter and test bounds + *v = dot(dir, qvec); + if (*v < T(0.0f) || *u + *v > det) + return false; + + *t = dot(edge2, qvec); + T fInvDet = T(1.0) / det; + *t *= fInvDet; + *u *= fInvDet; + *v *= fInvDet; + + return true; +} + +/** + * ������������� + */ +template T calcTriangleArea(const tvec3& pt1, const tvec3& pt2, const tvec3& pt3) { + tvec3 e1 = pt2 - pt1; + tvec3 e2 = pt3 - pt1; + tvec3 e3 = cross(e1, e2); + return length(e3) * T(0.5); +} + +template +bool operator==(tvec3 const & v1, tvec3 const & v2) { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z); +} + +template +bool operator!=(tvec3 const & v1, tvec3 const & v2) { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z); +} + +template +tvec3 operator+(tvec3 const & v, T const & s) { + return tvec3( + v.x + T(s), + v.y + T(s), + v.z + T(s)); +} + +template +tvec3 operator+(T const & s, tvec3 const & v) { + return tvec3( + T(s) + v.x, + T(s) + v.y, + T(s) + v.z); +} + +template +tvec3 operator+(tvec3 const & v1, tvec3 const & v2) { + return tvec3( + v1.x + T(v2.x), + v1.y + T(v2.y), + v1.z + T(v2.z)); +} + +template +tvec3 operator-(tvec3 const & v, T const & s) { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); +} + +template +tvec3 operator-(T const & s, tvec3 const & v) { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); +} + +template +tvec3 operator-(tvec3 const & v1, tvec3 const & v2) { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); +} + +template +tvec3 operator*(tvec3 const & v, T const & s) { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); +} + +template +tvec3 operator*(T const & s, tvec3 const & v) { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); +} + +template +tvec3 operator*(tvec3 const & v1, tvec3 const & v2) { + return tvec3( + v1.x * T(v2.x), + v1.y * T(v2.y), + v1.z * T(v2.z)); +} + +template +tvec3 operator/(tvec3 const & v, T const & s) { + return tvec3( + v.x / T(s), + v.y / T(s), + v.z / T(s)); +} + +template +tvec3 operator/(T const & s, tvec3 const & v) { + return tvec3( + T(s) / v.x, + T(s) / v.y, + T(s) / v.z); +} + +template +tvec3 operator/(tvec3 const & v1, tvec3 const & v2) { + return tvec3( + v1.x / T(v2.x), + v1.y / T(v2.y), + v1.z / T(v2.z)); +} - float2 ret; +template +tvec3 operator-(tvec3 const & v) { + return tvec3( + -v.x, + -v.y, + -v.z); +} - ret.x = (float) uv1.x + (uv2.x - uv1.x) * step; - ret.y = (float) uv1.y + (uv2.y - uv1.y) * step; +template +tvec3 operator++(tvec3 const & v, int) { + return tvec3( + v.x + T(1), + v.y + T(1), + v.z + T(1)); +} - return ret; +template +tvec3 operator--(tvec3 const & v, int) { + return tvec3( + v.x - T(1), + v.y - T(1), + v.z - T(1)); } +template +struct tvec4 { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 type; + + + + value_type x, y, z, w; + + size_type length() const { + return 4; + } + + value_type & operator[](size_type i) { + assert(i < this->length()); + return (&x)[i]; + } + + value_type const & operator[](size_type i) const { + assert(i < this->length()); + return (&x)[i]; + } + + tvec4() : + x(value_type(0)), + y(value_type(0)), + z(value_type(0)), + w(value_type(0)) { + } + + tvec4(tvec3 const& v, T s) : + x(v.x), + y(v.y), + z(v.z), + w(s) { + } + + tvec4(T s) : + x(s), + y(s), + z(s), + w(s) { + } + + tvec4(tvec4 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(v.w) { + } + + template + tvec4(tvec3 const & v, B s) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(s)) { + } + + template + tvec4(A s, tvec3 const & v) : + x(value_type(s)), + y(value_type(v.x)), + z(value_type(v.y)), + w(value_type(v.z)) { + } + + template + tvec4(tvec4 const & v) : + x(value_type(v.x)), + y(value_type(v.y)), + z(value_type(v.z)), + w(value_type(v.w)) { + } + + tvec4 + ( + value_type s1, + value_type s2, + value_type s3, + value_type s4 + ) : + x(s1), + y(s2), + z(s3), + w(s4) { + } + + tvec4 & operator=(tvec4 const & v) { + this->x = v.x; + this->y = v.y; + this->z = v.z; + this->w = v.w; + return *this; + } + + template + tvec4 & operator=(tvec4 const & v) { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + this->w = T(v.w); + return *this; + } + + template + tvec4 & operator+=(U const & s) { + this->x += T(s); + this->y += T(s); + this->z += T(s); + this->w += T(s); + return *this; + } + + template + tvec4 & operator+=(tvec4 const & v) { + this->x += T(v.x); + this->y += T(v.y); + this->z += T(v.z); + this->w += T(v.w); + return *this; + } + + template + tvec4 & operator-=(U const & s) { + this->x -= T(s); + this->y -= T(s); + this->z -= T(s); + this->w -= T(s); + return *this; + } + + template + tvec4 & operator-=(tvec4 const & v) { + this->x -= T(v.x); + this->y -= T(v.y); + this->z -= T(v.z); + this->w -= T(v.w); + return *this; + } + + template + tvec4 & operator*=(U const & s) { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + this->w *= T(s); + return *this; + } + + template + tvec4 & operator*=(tvec4 const & v) { + this->x *= T(v.x); + this->y *= T(v.y); + this->z *= T(v.z); + this->w *= T(v.w); + return *this; + } + + template + tvec4 & operator/=(U const & s) { + this->x /= T(s); + this->y /= T(s); + this->z /= T(s); + this->w /= T(s); + return *this; + } + + template + tvec4 & operator/=(tvec4 const & v) { + this->x /= T(v.x); + this->y /= T(v.y); + this->z /= T(v.z); + this->w /= T(v.w); + return *this; + } + + tvec4 & operator++() { + ++this->x; + ++this->y; + ++this->z; + ++this->w; + return *this; + } + + tvec4 & operator--() { + --this->x; + --this->y; + --this->z; + --this->w; + return *this; + } +}; + +template +tvec4 rotateX(const tvec4& v, T angle) { + tvec4 res(v); + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.y = v.y * c - v.z * s; + res.z = v.y * s + v.z * c; + return res; +} + +template +tvec4 rotateY(tvec4 const & v, T angle) { + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c + v.z * s; + res.z = -v.x * s + v.z * c; + return res; +} + +template +tvec4 rotateZ(tvec4 const & v, T angle) { + + tvec4 res = v; + + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + + res.x = v.x * c - v.y * s; + res.y = v.x * s + v.y * c; + return res; +} + +template +tvec4 operator+(tvec4 const & v, T const & s) { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); +} + +template +tvec4 operator+(T const & s, tvec4 const & v) { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); +} + +template +tvec4 operator+(tvec4 const & v1, tvec4 const & v2) { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); +} + +template +tvec4 operator-(tvec4 const & v, T const & s) { + return tvec4( + v.x - s, + v.y - s, + v.z - s, + v.w - s); +} + +template +tvec4 operator-(T const & s, tvec4 const & v) { + return tvec4( + s - v.x, + s - v.y, + s - v.z, + s - v.w); +} + +template +tvec4 operator- +( + tvec4 const & v1, + tvec4 const & v2 + ) { + return tvec4( + v1.x - v2.x, + v1.y - v2.y, + v1.z - v2.z, + v1.w - v2.w); +} + +template +tvec4 operator*(tvec4 const & v, T const & s) { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); +} + +template +tvec4 operator*(T const & s, tvec4 const & v) { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); +} + +template +tvec4 operator*(tvec4 const & v1, tvec4 const & v2) { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); +} + +template +tvec4 operator/(tvec4 const & v, T const & s) { + return tvec4( + v.x / s, + v.y / s, + v.z / s, + v.w / s); +} + +template +tvec4 operator/(T const & s, tvec4 const & v) { + return tvec4( + s / v.x, + s / v.y, + s / v.z, + s / v.w); +} + +template +tvec4 operator/(tvec4 const & v1, tvec4 const & v2) { + return tvec4( + v1.x / v2.x, + v1.y / v2.y, + v1.z / v2.z, + v1.w / v2.w); +} + +template +tvec4 operator-(tvec4 const & v) { + return tvec4( + -v.x, + -v.y, + -v.z, + -v.w); +} + +template +bool operator== +( + tvec4 const & v1, + tvec4 const & v2 + ) { + return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w); +} + +template +bool operator!=(tvec4 const & v1, tvec4 const & v2) { + return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w); +} + +template +class trect { +public: + + trect(T left = 0, T top = 0, T right = 0, T bottom = 0) { + _left = left; + _top = top; + _right = right; + _bottom = bottom; + } + + void fromCenter(T x, T y, T size) { + _left = x - size * T(0.5f); + _top = y - size * T(0.5f); + _right = x + size * T(0.5f); + _bottom = y + size * T(0.5f); + } + + void fromCenter(T x, T y, T sizeX, T sizeY) { + _left = x - sizeX * T(0.5f); + _top = y - sizeY * T(0.5f); + _right = x + sizeX * T(0.5f); + _bottom = y + sizeY * T(0.5f); + } + + bool ptInRect(T x, T y) { + return x >= _left && x <= _right && y >= _top && y <= _bottom; + } + + tvec2center() const { + return tvec2((_left + _right) * T(0.5f), (_bottom + _top) * T(0.5f)); + } + + tvec2halSize() const { + return tvec2((_right - _left) * T(0.5f), (_bottom - _top) * T(0.5f)); + } +public: + T _left; + T _top; + T _right; + T _bottom; +}; + +template +struct tmat2x2 { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec2 col_type; + typedef tvec2 row_type; + typedef tmat2x2 type; + typedef tmat2x2 transpose_type; + + +public: + + tmat2x2 _inverse() const { + value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1]; + + tmat2x2 Inverse( + +this->value[1][1] / Determinant, + -this->value[0][1] / Determinant, + -this->value[1][0] / Determinant, + +this->value[0][0] / Determinant); + return Inverse; + } + +private: + col_type value[2]; + +public: + + size_type length() const { + return 2; + } + + static size_type col_size() { + return 2; + } + + static size_type row_size() { + return 2; + } + + col_type &operator[](size_type i) { + assert(i < this->length()); + return this->value[i]; + } + + col_type const &operator[](size_type i) const { + assert(i < this->length()); + return this->value[i]; + } + + tmat2x2() { + this->value[0] = col_type(1, 0); + this->value[1] = col_type(0, 1); + } + + tmat2x2(tmat2x2 const & m) { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + } + + tmat2x2(value_type s) { + value_type const Zero(0); + this->value[0] = col_type(s, Zero); + this->value[1] = col_type(Zero, s); + } + + tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1) { + this->value[0] = col_type(x0, y0); + this->value[1] = col_type(x1, y1); + } + + tmat2x2(col_type const & v0, col_type const & v1) { + this->value[0] = v0; + this->value[1] = v1; + } + + template + tmat2x2(U s) { + value_type const Zero(0); + this->value[0] = tvec2(value_type(s), Zero); + this->value[1] = tvec2(Zero, value_type(s)); + } + + template + tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2) { + this->value[0] = col_type(value_type(x1), value_type(y1)); + this->value[1] = col_type(value_type(x2), value_type(y2)); + } + + template + tmat2x2 + ( + tvec2 const & v1, + tvec2 const & v2 + ) { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + } + + template + tmat2x2(tmat2x2 const & m) { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + } + + tmat2x2& operator=(tmat2x2 const & m) { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator= + ( + tmat2x2 const & m + ) { + this->value[0] = m[0]; + this->value[1] = m[1]; + return *this; + } + + template + tmat2x2& operator+=(U const & s) { + this->value[0] += s; + this->value[1] += s; + return *this; + } + + template + tmat2x2& operator+= + ( + tmat2x2 const & m + ) { + this->value[0] += m[0]; + this->value[1] += m[1]; + return *this; + } + + template + tmat2x2& operator-=(U const & s) { + this->value[0] -= s; + this->value[1] -= s; + return *this; + } + + template + tmat2x2& operator-=(tmat2x2 const & m) { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + return *this; + } + + template + tmat2x2& operator*=(U const & s) { + this->value[0] *= s; + this->value[1] *= s; + return *this; + } + + template + tmat2x2& operator*=(tmat2x2 const & m) { + return (*this = *this * m); + } + + template + tmat2x2& operator/=(U const & s) { + this->value[0] /= s; + this->value[1] /= s; + return *this; + } + + template + tmat2x2& operator/=(tmat2x2 const & m) { + return (*this = *this / m); + } + + tmat2x2& operator++() { + ++this->value[0]; + ++this->value[1]; + return *this; + } + + tmat2x2& operator--() { + --this->value[0]; + --this->value[1]; + return *this; + }; +}; + +template +tmat2x2 rotate(T angle) { + T c = cos(DEG2RAD(angle)); + T s = sin(DEG2RAD(angle)); + return tmat2x2(c, s, -s, c); +} + +template +tmat2x2 operator+(tmat2x2 const & m, T const & s) { + return tmat2x2(m[0] + s, m[1] + s); +} + +template +tmat2x2 operator+(T const & s, tmat2x2 const & m) { + return tmat2x2(m[0] + s, m[1] + s); +} + +template +tmat2x2 operator+(tmat2x2 const & m1, tmat2x2 const & m2) { + return tmat2x2(m1[0] + m2[0], m1[1] + m2[1]); +} + +template +tmat2x2 operator-(tmat2x2 const & m, T const & s) { + return tmat2x2(m[0] - s, m[1] - s); +} + +template +tmat2x2 operator-(T const & s, tmat2x2 const & m) { + return tmat2x2(s - m[0], s - m[1]); +} + +template +tmat2x2 operator-(tmat2x2 const & m1, tmat2x2 const & m2) { + return tmat2x2(m1[0] - m2[0], m1[1] - m2[1]); +} + +template +tmat2x2 operator*(tmat2x2 const & m, T const & s) { + return tmat2x2(m[0] * s, m[1] * s); +} + +template +tmat2x2 operator*(T const & s, tmat2x2 const & m) { + return tmat2x2(m[0] * s, m[1] * s); +} + +template +tvec2 operator*(tmat2x2 const & m, tvec2 const & v) { + return tvec2( + m[0][0] * v.x + m[1][0] * v.y, + m[0][1] * v.x + m[1][1] * v.y); +} + +template +tvec2 operator*(tvec2 const & v, tmat2x2 const & m) { + return tvec2( + v.x * m[0][0] + v.y * m[0][1], + v.x * m[1][0] + v.y * m[1][1]); +} + +template +tmat2x2 operator*(tmat2x2 const & m1, tmat2x2 const & m2) { + return tmat2x2( + m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1], + m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1], + m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1], + m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]); +} + +template +struct tmat3x3 { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec3 col_type; + typedef tvec3 row_type; + typedef tmat3x3 type; + typedef tmat3x3 transpose_type; + + +private: + // Data + col_type value[3]; + +public: + + size_type length() const { + return 3; + } + + size_type col_size() { + return 3; + } + + size_type row_size() { + return 3; + } + + tmat3x3() { + value_type const Zero(0); + value_type const One(1); + this->value[0] = col_type(One, Zero, Zero); + this->value[1] = col_type(Zero, One, Zero); + this->value[2] = col_type(Zero, Zero, One); + } + + tmat3x3 + ( + tmat3x3 const & m + ) { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + } + + tmat3x3(value_type const & s) { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero); + this->value[2] = col_type(Zero, Zero, s); + } + + tmat3x3 + ( + value_type const & x0, value_type const & y0, value_type const & z0, + value_type const & x1, value_type const & y1, value_type const & z1, + value_type const & x2, value_type const & y2, value_type const & z2 + ) { + this->value[0] = col_type(x0, y0, z0); + this->value[1] = col_type(x1, y1, z1); + this->value[2] = col_type(x2, y2, z2); + } + + tmat3x3 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2 + ) { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + } + + template + tmat3x3(U const & s) { + value_type const Zero(0); + this->value[0] = tvec3(value_type(s), Zero, Zero); + this->value[1] = tvec3(Zero, value_type(s), Zero); + this->value[2] = tvec3(Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, + typename X2, typename Y2, typename Z2, + typename X3, typename Y3, typename Z3> + tmat3x3 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, + X2 const & x2, Y2 const & y2, Z2 const & z2, + X3 const & x3, Y3 const & y3, Z3 const & z3 + ) { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3)); + } + + template + tmat3x3 + ( + tvec3 const & v1, + tvec3 const & v2, + tvec3 const & v3 + ) { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + } + + template + tmat3x3(tmat3x3 const & m) { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + } + + tmat3x3 _inverse() const { + T S00 = value[0][0]; + T S01 = value[0][1]; + T S02 = value[0][2]; + + T S10 = value[1][0]; + T S11 = value[1][1]; + T S12 = value[1][2]; + + T S20 = value[2][0]; + T S21 = value[2][1]; + T S22 = value[2][2]; + + tmat3x3 Inverse( + S11 * S22 - S21 * S12, + S12 * S20 - S22 * S10, + S10 * S21 - S20 * S11, + S02 * S21 - S01 * S22, + S00 * S22 - S02 * S20, + S01 * S20 - S00 * S21, + S12 * S01 - S11 * S02, + S10 * S02 - S12 * S00, + S11 * S00 - S10 * S01); + + T Determinant = S00 * (S11 * S22 - S21 * S12) + - S10 * (S01 * S22 - S21 * S02) + + S20 * (S01 * S12 - S11 * S02); + + Inverse /= Determinant; + return Inverse; + } + + col_type & operator[](size_type i) { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const { + assert(i < this->length()); + return this->value[i]; + } + + tmat3x3 & operator=(tmat3x3 const & m) { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator=(tmat3x3 const & m) { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + return *this; + } + + template + tmat3x3 & operator+=(U const & s) { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + return *this; + } + + template + tmat3x3 & operator+=(tmat3x3 const & m) { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + return *this; + } + + template + tmat3x3 & operator-=(U const & s) { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + return *this; + } + + template + tmat3x3 & operator-=(tmat3x3 const & m) { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + return *this; + } + + template + tmat3x3 & operator*=(U const & s) { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + return *this; + } + + template + tmat3x3 & operator*=(tmat3x3 const & m) { + return (*this = *this * m); + } + + template + tmat3x3 & operator/=(U const & s) { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + return *this; + } + + template + tmat3x3 & operator/=(tmat3x3 const & m) { + return (*this = *this / m); + } + + tmat3x3 & operator++() { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + return *this; + } + + tmat3x3 & operator--() { + --this->value[0]; + --this->value[1]; + --this->value[2]; + return *this; + } + + tvec3 operator*(const tvec3 &v) const { + + return tvec3( + value[0][0] * v[0] + value[1][0] * v[1] + value[2][0] * v[2] + , value[0][1] * v[0] + value[1][1] * v[1] + value[2][1] * v[2] + , value[0][2] * v[0] + value[1][2] * v[1] + value[2][2] * v[2] + ); + } + + tvec2 operator*(const tvec2 &v) const { + return tvec2( + value[0][0] * v[0] + value[1][0] * v[1] + value[2][0] + , value[0][1] * v[0] + value[1][1] * v[1] + value[2][1] + ); + } + + void scale(T x, T y) { + this->value[0] = col_type(value_type(x), value_type(0), value_type(0)); + this->value[1] = col_type(value_type(0), value_type(y), value_type(0)); + this->value[2] = col_type(value_type(0), value_type(0), value_type(1)); + } + + void rotate(T angle) { + T rad = DEG2RAD(angle); + T c = cos(rad); + T s = sin(rad); + this->value[0] = col_type(value_type(c), value_type(-s), value_type(0)); + this->value[1] = col_type(value_type(s), value_type(c), value_type(0)); + this->value[2] = col_type(value_type(0), value_type(0), value_type(1)); + } + + void translate(T x, T y) { + this->value[0] = col_type(value_type(1), value_type(0), value_type(0)); + this->value[1] = col_type(value_type(0), value_type(1), value_type(0)); + this->value[2] = col_type(value_type(x), value_type(y), value_type(1)); + } +}; + +template +tmat3x3 operator+(tmat3x3 const & m, T const & s) { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); +} + +template +tmat3x3 operator+(T const & s, tmat3x3 const & m) { + return tmat3x3( + m[0] + s, + m[1] + s, + m[2] + s); +} + +template +tmat3x3 operator+(tmat3x3 const & m1, tmat3x3 const & m2) { + return tmat3x3( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2]); +} + +template +tmat3x3 operator-(tmat3x3 const & m, T const & s) { + return tmat3x3( + m[0] - s, + m[1] - s, + m[2] - s); +} + +template +tmat3x3 operator-(T const & s, tmat3x3 const & m) { + return tmat3x3( + s - m[0], + s - m[1], + s - m[2]); +} + +template +tmat3x3 operator-(tmat3x3 const & m1, tmat3x3 const & m2) { + return tmat3x3( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2]); +} + +template +tmat3x3 operator*(tmat3x3 const & m, T const & s) { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); +} + +template +tmat3x3 operator*(T const & s, tmat3x3 const & m) { + return tmat3x3( + m[0] * s, + m[1] * s, + m[2] * s); +} + +template +tvec3 operator*(tmat3x3 const & m, tvec3 const & v) { + return tvec3( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z); +} + +template +tvec3 operator*(tvec3 const & v, tmat3x3 const & m) { + return tvec3( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z); +} + +template +tmat3x3 operator*(tmat3x3 const & m1, tmat3x3 const & m2) { + T const srcA00 = m1[0][0]; + T const srcA01 = m1[0][1]; + T const srcA02 = m1[0][2]; + T const srcA10 = m1[1][0]; + T const srcA11 = m1[1][1]; + T const srcA12 = m1[1][2]; + T const srcA20 = m1[2][0]; + T const srcA21 = m1[2][1]; + T const srcA22 = m1[2][2]; + + T const srcB00 = m2[0][0]; + T const srcB01 = m2[0][1]; + T const srcB02 = m2[0][2]; + T const srcB10 = m2[1][0]; + T const srcB11 = m2[1][1]; + T const srcB12 = m2[1][2]; + T const srcB20 = m2[2][0]; + T const srcB21 = m2[2][1]; + T const srcB22 = m2[2][2]; + + tmat3x3 res; + res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02; + res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02; + res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02; + res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12; + res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12; + res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12; + res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22; + res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22; + res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22; + return res; +} + +template +tmat3x3 operator/(tmat3x3 const & m, T const & s) { + return tmat3x3( + m[0] / s, + m[1] / s, + m[2] / s); +} + +template +tmat3x3 operator/(T const & s, tmat3x3 const & m) { + return tmat3x3( + s / m[0], + s / m[1], + s / m[2] + ); +} + +template +tvec3 operator/(tmat3x3 const & m, tvec3 const & v) { + return m._inverse() * v; +} + +template +tvec3 operator/(tvec3 const & v, tmat3x3 const & m) { + return v * m._inverse(); +} + +template +tmat3x3 operator/(tmat3x3 const & m1, tmat3x3 const & m2) { + return m1 * m2._inverse(); +} + +template +tmat3x3 const operator-(tmat3x3 const & m) { + return tmat3x3( + -m[0], + -m[1], + -m[2]); +} + +template +tmat3x3 const operator++(tmat3x3 const & m, int) { + return tmat3x3( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1)); +} + +template +tmat3x3 const operator--(tmat3x3 const & m, int) { + return tmat3x3( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1)); +} + +template +bool operator==(tmat3x3 const & m1, tmat3x3 const & m2) { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]); +} + +template +bool operator!=(tmat3x3 const & m1, tmat3x3 const & m2) { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]); +} + +template +struct tmat4x4 { + typedef T value_type; + typedef std::size_t size_type; + typedef tvec4 col_type; + typedef tvec4 row_type; + typedef tmat4x4 type; + typedef tmat4x4 transpose_type; + + +public: + + tmat4x4 inverse() const { + value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3]; + value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3]; + value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2]; + value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3]; + value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2]; + value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1]; + value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3]; + value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2]; + value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3]; + value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2]; + value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3]; + value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1]; + value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3]; + value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3]; + value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2]; + value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3]; + value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2]; + value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1]; + + tmat4x4 res( + +this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02, + -this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04, + +this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05, + -this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05, + + -this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02, + +this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04, + -this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05, + +this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05, + + +this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08, + -this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10, + +this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12, + -this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12, + + -this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15, + +this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17, + -this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18, + +this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18); + + value_type determinant = + +this->value[0][0] * res[0][0] + + this->value[0][1] * res[1][0] + + this->value[0][2] * res[2][0] + + this->value[0][3] * res[3][0]; + + res /= determinant; + return res; + } + + +private: + col_type value[4]; +public: + + size_type length() const { + return 4; + } + + size_type col_size() { + return 4; + } + + size_type row_size() { + return 4; + } + + void identify() { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + } + + col_type & operator[](size_type i) { + assert(i < this->length()); + return this->value[i]; + } + + col_type const & operator[](size_type i) const { + assert(i < this->length()); + return this->value[i]; + } + + tmat4x4(tmat4x4 const & m) { + this->value[0] = m.value[0]; + this->value[1] = m.value[1]; + this->value[2] = m.value[2]; + this->value[3] = m.value[3]; + } + + tmat4x4(tmat3x3 const & m) { + this->value[0] = col_type(m[0], value_type(0)); + this->value[1] = col_type(m[1], value_type(0)); + this->value[2] = col_type(m[2], value_type(0)); + this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1)); + } + + tmat4x4() { + } + + tmat4x4(value_type s) { + value_type const Zero(0); + this->value[0] = col_type(s, Zero, Zero, Zero); + this->value[1] = col_type(Zero, s, Zero, Zero); + this->value[2] = col_type(Zero, Zero, s, Zero); + this->value[3] = col_type(Zero, Zero, Zero, s); + } + + tmat4x4 + ( + value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0, + value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1, + value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2, + value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3 + ) { + this->value[0] = col_type(x0, y0, z0, w0); + this->value[1] = col_type(x1, y1, z1, w1); + this->value[2] = col_type(x2, y2, z2, w2); + this->value[3] = col_type(x3, y3, z3, w3); + } + + tmat4x4 + ( + col_type const & v0, + col_type const & v1, + col_type const & v2, + col_type const & v3 + ) { + this->value[0] = v0; + this->value[1] = v1; + this->value[2] = v2; + this->value[3] = v3; + } + + template + tmat4x4(tmat4x4 const & m) { + this->value[0] = col_type(m[0]); + this->value[1] = col_type(m[1]); + this->value[2] = col_type(m[2]); + this->value[3] = col_type(m[3]); + } + + template + tmat4x4(U const & s) { + value_type const Zero(0); + this->value[0] = tvec4(value_type(s), Zero, Zero, Zero); + this->value[1] = tvec4(Zero, value_type(s), Zero, Zero); + this->value[2] = tvec4(Zero, Zero, value_type(s), Zero); + this->value[3] = tvec4(Zero, Zero, Zero, value_type(s)); + } + + template < + typename X1, typename Y1, typename Z1, typename W1, + typename X2, typename Y2, typename Z2, typename W2, + typename X3, typename Y3, typename Z3, typename W3, + typename X4, typename Y4, typename Z4, typename W4> + tmat4x4 + ( + X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1, + X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2, + X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3, + X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4 + ) { + this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1)); + this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2)); + this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3)); + this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4)); + } + + template + tmat4x4 + ( + tvec4 const & v1, + tvec4 const & v2, + tvec4 const & v3, + tvec4 const & v4 + ) { + this->value[0] = col_type(v1); + this->value[1] = col_type(v2); + this->value[2] = col_type(v3); + this->value[3] = col_type(v4); + } + + T const * data() const { + return &this->value[0][0]; + } + + tmat4x4& operator=(tmat4x4 const & m) { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator=(tmat4x4 const & m) { + this->value[0] = m[0]; + this->value[1] = m[1]; + this->value[2] = m[2]; + this->value[3] = m[3]; + return *this; + } + + template + tmat4x4& operator+=(U const & s) { + this->value[0] += s; + this->value[1] += s; + this->value[2] += s; + this->value[3] += s; + return *this; + } + + template + tmat4x4& operator+=(tmat4x4 const & m) { + this->value[0] += m[0]; + this->value[1] += m[1]; + this->value[2] += m[2]; + this->value[3] += m[3]; + return *this; + } + + template + tmat4x4 & operator-=(U const & s) { + this->value[0] -= s; + this->value[1] -= s; + this->value[2] -= s; + this->value[3] -= s; + return *this; + } + + template + tmat4x4 & operator-=(tmat4x4 const & m) { + this->value[0] -= m[0]; + this->value[1] -= m[1]; + this->value[2] -= m[2]; + this->value[3] -= m[3]; + return *this; + } + + template + tmat4x4 & operator*=(U const & s) { + this->value[0] *= s; + this->value[1] *= s; + this->value[2] *= s; + this->value[3] *= s; + return *this; + } + + template + tmat4x4 & operator*=(tmat4x4 const & m) { + return (*this = *this * m); + } + + template + tmat4x4 & operator/=(U const & s) { + this->value[0] /= s; + this->value[1] /= s; + this->value[2] /= s; + this->value[3] /= s; + return *this; + } + + template + tmat4x4 & operator/=(tmat4x4 const & m) { + return (*this = *this / m); + } + + tmat4x4 & operator++() { + ++this->value[0]; + ++this->value[1]; + ++this->value[2]; + ++this->value[3]; + return *this; + } + + tmat4x4 & operator--() { + --this->value[0]; + --this->value[1]; + --this->value[2]; + --this->value[3]; + return *this; + } + + tmat4x4& translate(value_type x, value_type y, value_type z) { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(x, y, z, 1); + return *this; + } + + template + tmat4x4& translate(U x, U y, U z) { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(x), T(y), T(z), 1); + return *this; + } + + tmat4x4& translate(tvec3 const& pos) { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(pos.x, pos.y, pos.z, 1); + return *this; + } + + template + tmat4x4& translate(tvec3 const& pos) { + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(T(pos.x), T(pos.y), T(pos.z), 1); + return *this; + } + + tmat4x4& rotate(value_type angle, tvec3 const & v) { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + this->value[0][0] = c + temp[0] * axis[0]; + this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + this->value[0][3] = 0; + + this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + this->value[1][1] = c + temp[1] * axis[1]; + this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + this->value[1][3] = 0; + + this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + this->value[2][2] = c + temp[2] * axis[2]; + this->value[2][3] = 0; + + this->value[3][0] = 0; + this->value[3][1] = 0; + this->value[3][2] = 0; + this->value[3][3] = 1; + return *this; + } + + tmat4x4& rotateX(value_type angle) { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0, -s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& rotateX(U angle) { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(1, 0, 0, 0); + this->value[1] = col_type(0, c, s, 0); + this->value[2] = col_type(0, -s, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& rotateY(value_type angle) { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0, -s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + + template + tmat4x4& rotateY(U angle) { + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, 0, -s, 0); + this->value[1] = col_type(0, 1, 0, 0); + this->value[2] = col_type(s, 0, c, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + + } + + tmat4x4& rotateZ(value_type angle) { + T a = T(DEG2RAD(angle)); + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + template + tmat4x4& rotateZ(U angle) { + T a = DEG2RAD(angle); + + T c = cos(a); + T s = sin(a); + + this->value[0] = col_type(c, s, 0, 0); + this->value[1] = col_type(-s, c, 0, 0); + this->value[2] = col_type(0, 0, 1, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateXY(T angleX, T angleY) { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type(cosY, -sinX * sinY, cosX * sinY, 0); + this->value[1] = col_type(0, cosX, sinX, 0); + this->value[2] = col_type(-sinY, -sinX * cosY, cosX * cosY, 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 rotateYX(T angleX, T angleY) { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + + this->value[0] = col_type(cosY, 0, sinY, 0); + this->value[1] = col_type(-sinX * sinY, cosX, sinX * cosY, 0); + this->value[2] = col_type(-cosX * sinY, -sinX, cosX * cosY, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4 rotateYXZ(T yaw, T pitch, T roll) { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 Result; + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + this->value[0][1] = tmp_sb * tmp_cp; + this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + this->value[0][3] = T(0); + this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + this->value[1][1] = tmp_cb * tmp_cp; + this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + this->value[1][3] = T(0); + this->value[2][0] = tmp_sh * tmp_cp; + this->value[2][1] = -tmp_sp; + this->value[2][2] = tmp_ch * tmp_cp; + this->value[2][3] = T(0); + this->value[3][0] = T(0); + this->value[3][1] = T(0); + this->value[3][2] = T(0); + this->value[3][3] = T(1); + + return *this; + } + + tmat4x4& scale(tvec3 const& s) { + this->value[0] = col_type(s[0], 0, 0, 0); + this->value[1] = col_type(0, s[1], 0, 0); + this->value[2] = col_type(0, 0, s[2], 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + tmat4x4& scale(value_type x, value_type y, value_type z) { + this->value[0] = col_type(x, 0, 0, 0); + this->value[1] = col_type(0, y, 0, 0); + this->value[2] = col_type(0, 0, z, 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x, U y, U z) { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + + return *this; + } + + template + tmat4x4& scale(U x, V y, W z) { + this->value[0] = col_type(value_type(x), 0, 0, 0); + this->value[1] = col_type(0, value_type(y), 0, 0); + this->value[2] = col_type(0, 0, value_type(z), 0); + this->value[3] = col_type(0, 0, 0, 1); + return *this; + } + + tmat4x4 transpose() const { + return tmat4x4( + this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0], + this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1], + this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2], + this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3] + ); + } + + tmat4x4 extractMatrixRotation() const { + return tmat4x4( + this->value[0][0], this->value[0][1], this->value[0][2], 0.0, + this->value[1][0], this->value[1][1], this->value[1][2], 0.0, + this->value[2][0], this->value[2][1], this->value[2][2], 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + } +}; + +template +tmat4x4 rotateX(T angleX) { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + + return tmat4x4( + T(1), T(0), T(0), T(0), + T(0), cosX, sinX, T(0), + T(0), -sinX, cosX, T(0), + T(0), T(0), T(0), T(1)); +} + +template +tmat4x4 rotateY(T angleY) { + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + T(0), T(1), T(0), T(0), + -sinY, T(0), cosY, T(0), + T(0), T(0), T(0), T(1)); +} + +template +tmat4x4 rotateZ(T angleZ) { + T cosZ = (T) cos(DEG2RAD(angleZ)); + T sinZ = (T) sin(DEG2RAD(angleZ)); + + return tmat4x4( + cosZ, sinZ, T(0), T(0), + -sinZ, cosZ, T(0), T(0), + T(0), T(0), T(1), T(0), + T(0), T(0), T(0), T(1)); +} + +template +tmat4x4 rotateXY(T angleX, T angleY) { + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, -sinX * sinY, cosX * sinY, T(0), + T(0), cosX, sinX, T(0), + -sinY, -sinX * cosY, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); +} + +template +tmat4x4 rotateYX(T angleY, T angleX) { + + T cosX = cos(DEG2RAD(angleX)); + T sinX = sin(DEG2RAD(angleX)); + T cosY = cos(DEG2RAD(angleY)); + T sinY = sin(DEG2RAD(angleY)); + + return tmat4x4( + cosY, T(0), sinY, T(0), + -sinX * sinY, cosX, sinX * cosY, T(0), + -cosX * sinY, -sinX, cosX * cosY, T(0), + T(0), T(0), T(0), T(1)); +} + +template +tmat4x4 rotateXZ(T angleX, T angleZ) { + return rotateX(angleX) * rotateZ(angleZ); +} + +template +tmat4x4 rotateZX(T angleX, T angleZ) { + return rotateZ(angleZ) * rotateX(angleX); +} + +template +tmat4x4 rotateYXZ(T yaw, T pitch, T roll) { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; +} + +template +tmat4x4 yawPitchRoll(T yaw, T pitch, T roll) { + T tmp_ch = cos(DEG2RAD(yaw)); + T tmp_sh = sin(DEG2RAD(yaw)); + T tmp_cp = cos(DEG2RAD(pitch)); + T tmp_sp = sin(DEG2RAD(pitch)); + T tmp_cb = cos(DEG2RAD(roll)); + T tmp_sb = sin(DEG2RAD(roll)); + + + tmat4x4 res; + res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb; + res[0][1] = tmp_sb * tmp_cp; + res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb; + res[0][3] = T(0); + res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb; + res[1][1] = tmp_cb * tmp_cp; + res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb; + res[1][3] = T(0); + res[2][0] = tmp_sh * tmp_cp; + res[2][1] = -tmp_sp; + res[2][2] = tmp_ch * tmp_cp; + res[2][3] = T(0); + res[3][0] = T(0); + res[3][1] = T(0); + res[3][2] = T(0); + res[3][3] = T(1); + return res; +} + +template +void axisAngle +( + tmat4x4 const & mat, + tvec3 & axis, + T & angle + ) { + T epsilon = (T) 0.01; + T epsilon2 = (T) 0.1; + + if ((fabs(mat[1][0] - mat[0][1]) < epsilon) && + (fabs(mat[2][0] - mat[0][2]) < epsilon) && + (fabs(mat[2][1] - mat[1][2]) < epsilon)) { + if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) && + (fabs(mat[2][0] + mat[0][2]) < epsilon2) && + (fabs(mat[2][1] + mat[1][2]) < epsilon2) && + (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T) 3.0) < epsilon2)) { + angle = (T) 0.0; + axis.x = (T) 1.0; + axis.y = (T) 0.0; + axis.z = (T) 0.0; + return; + } + angle = T(3.1415926535897932384626433832795); + T xx = (mat[0][0] + (T) 1.0) / (T) 2.0; + T yy = (mat[1][1] + (T) 1.0) / (T) 2.0; + T zz = (mat[2][2] + (T) 1.0) / (T) 2.0; + T xy = (mat[1][0] + mat[0][1]) / (T) 4.0; + T xz = (mat[2][0] + mat[0][2]) / (T) 4.0; + T yz = (mat[2][1] + mat[1][2]) / (T) 4.0; + if ((xx > yy) && (xx > zz)) { + if (xx < epsilon) { + axis.x = (T) 0.0; + axis.y = (T) 0.7071; + axis.z = (T) 0.7071; + } else { + axis.x = sqrt(xx); + axis.y = xy / axis.x; + axis.z = xz / axis.x; + } + } else if (yy > zz) { + if (yy < epsilon) { + axis.x = (T) 0.7071; + axis.y = (T) 0.0; + axis.z = (T) 0.7071; + } else { + axis.y = sqrt(yy); + axis.x = xy / axis.y; + axis.z = yz / axis.y; + } + } else { + if (zz < epsilon) { + axis.x = (T) 0.7071; + axis.y = (T) 0.7071; + axis.z = (T) 0.0; + } else { + axis.z = sqrt(zz); + axis.x = xz / axis.z; + axis.y = yz / axis.z; + } + } + return; + } + T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); + if (abs(s) < T(0.001)) + s = (T) 1.0; + angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T) 1.0) / (T) 2.0); + axis.x = (mat[1][2] - mat[2][1]) / s; + axis.y = (mat[2][0] - mat[0][2]) / s; + axis.z = (mat[0][1] - mat[1][0]) / s; +} + +template +tmat4x4 axisAngleMatrix(tvec3 const & axis, T const angle) { + T c = cos(angle); + T s = sin(angle); + T t = T(1) - c; + tvec3 n = normalize(axis); + + return tmat4x4( + t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), + t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), + t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), + T(0), T(0), T(0), T(1) + ); +} + +template +tmat4x4 interpolate +( + tmat4x4 const & m1, + tmat4x4 const & m2, + T const delta + ) { + tmat4x4 m1rot = m1.extractMatrixRotation(); + tmat4x4 dltRotation = m2 * m1rot.transpose(); + tvec3 dltAxis; + T dltAngle; + axisAngle(dltRotation, dltAxis, dltAngle); + tmat4x4 out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; + out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); + out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); + out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); + return out; +} + +template +tvec3 operator*(tvec3 const& v, tmat4x4 const& mat) { + return tvec3 + ( + v.x * mat[0][0] + v.y * mat[1][0] + v.z * mat[2][0] + 1 * mat[3][0], + v.x * mat[0][1] + v.y * mat[1][1] + v.z * mat[2][1] + 1 * mat[3][1], + v.x * mat[0][2] + v.y * mat[1][2] + v.z * mat[2][2] + 1 * mat[3][2] + ); +} + +template +tmat4x4 operator+(tmat4x4 const & m, typename tmat4x4::value_type s) { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); +} + +template +tmat4x4 operator+(typename tmat4x4::value_type s, tmat4x4 const & m) { + return tmat4x4( + m[0] + s, + m[1] + s, + m[2] + s, + m[3] + s); +} + +template +tmat4x4 operator+(tmat4x4 const & m1, tmat4x4 const & m2) { + return tmat4x4( + m1[0] + m2[0], + m1[1] + m2[1], + m1[2] + m2[2], + m1[3] + m2[3]); +} + +template +tmat4x4 operator-(tmat4x4 const & m, typename tmat4x4::value_type s) { + return tmat4x4( + m[0] - s, + m[1] - s, + m[2] - s, + m[3] - s); +} + +template +tmat4x4 operator-(typename tmat4x4::value_type s, tmat4x4 const & m) { + return tmat4x4( + s - m[0], + s - m[1], + s - m[2], + s - m[3]); +} + +template +tmat4x4 operator-(tmat4x4 const & m1, tmat4x4 const & m2) { + return tmat4x4( + m1[0] - m2[0], + m1[1] - m2[1], + m1[2] - m2[2], + m1[3] - m2[3]); +} + +template +tmat4x4 operator*(tmat4x4 const & m, typename tmat4x4::value_type s) { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); +} + +template +tmat4x4 operator*(typename tmat4x4::value_type s, tmat4x4 const & m) { + return tmat4x4( + m[0] * s, + m[1] * s, + m[2] * s, + m[3] * s); +} + +template +typename tmat4x4::col_type operator*(tmat4x4 const & m, typename tmat4x4::row_type const & v) { + return typename tmat4x4::col_type( + m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w, + m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w, + m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w, + m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w); +} + +template +typename tmat4x4::row_type operator*(typename tmat4x4::col_type const & v, tmat4x4 const & m) { + return typename tmat4x4::row_type( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w, + m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w); +} + +template +tmat4x4 operator*(tmat4x4 const & m1, tmat4x4 const & m2) { + typename tmat4x4::col_type const srcA0 = m1[0]; + typename tmat4x4::col_type const srcA1 = m1[1]; + typename tmat4x4::col_type const srcA2 = m1[2]; + typename tmat4x4::col_type const srcA3 = m1[3]; + + typename tmat4x4::col_type const srcB0 = m2[0]; + typename tmat4x4::col_type const srcB1 = m2[1]; + typename tmat4x4::col_type const srcB2 = m2[2]; + typename tmat4x4::col_type const srcB3 = m2[3]; + + tmat4x4 res; + res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3]; + res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3]; + res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3]; + res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3]; + return res; +} + +template +tmat4x4 operator/(tmat4x4 const & m, typename tmat4x4::value_type s) { + return tmat4x4( + m[0] / s, + m[1] / s, + m[2] / s, + m[3] / s); +} + +template +tmat4x4 operator/(typename tmat4x4::value_type s, tmat4x4 const & m) { + return tmat4x4( + s / m[0], + s / m[1], + s / m[2], + s / m[3]); +} + +template +typename tmat4x4::col_type operator/(tmat4x4 const & m, typename tmat4x4::row_type const & v) { + return m.inverse() * v; +} + +template +typename tmat4x4::row_type operator/(typename tmat4x4::col_type const & v, tmat4x4 const & m) { + return v * m.inverse(); +} + +template +tmat4x4 operator/(tmat4x4 const & m1, tmat4x4 const & m2) { + return m1 * m2.inverse(); +} + +template +tmat4x4 const operator-(tmat4x4 const & m) { + return tmat4x4( + -m[0], + -m[1], + -m[2], + -m[3]); +} + +template +tmat4x4 const operator++(tmat4x4 const & m, int) { + return tmat4x4( + m[0] + T(1), + m[1] + T(1), + m[2] + T(1), + m[3] + T(1)); +} + +template +tmat4x4 const operator--(tmat4x4 const & m, int) { + return tmat4x4( + m[0] - T(1), + m[1] - T(1), + m[2] - T(1), + m[3] - T(1)); +} + +template +bool operator==(tmat4x4 const & m1, tmat4x4 const & m2) { + return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]); +} + +template +bool operator!=(tmat4x4 const & m1, tmat4x4 const & m2) { + return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]); +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +//! ������ +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// + + + +////////////////////////////////////////////////////////////////////////////////////////////////////////// + +template +typename tvec2::value_type length(tvec2 const & v) { + typename tvec2::value_type sqr = v.x * v.x + v.y * v.y; + return sqrt(sqr); +} + +template +typename tvec3::value_type length(tvec3 const & v) { + typename tvec3::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z; + return sqrt(sqr); +} + +template +typename tvec4::value_type length(tvec4 const & v) { + typename tvec4::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; + return sqrt(sqr); +} + +template +typename tvec2::value_type distance(tvec2 const & p0, tvec2 const & p1) { + return length(p1 - p0); +} + +template +typename tvec3::value_type distance(tvec3 const & p0, tvec3 const & p1) { + return length(p1 - p0); +} + +template +typename tvec4::value_type distance(tvec4 const & p0, tvec4 const & p1) { + return length(p1 - p0); +} + +template +typename tvec2::value_type dot(tvec2 const & x, tvec2 const & y) { + return x.x * y.x + x.y * y.y; +} + +template +typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) { + return x.x * y.x + x.y * y.y + x.z * y.z; +} + +template +typename tvec4::value_type dot(tvec4 const & x, tvec4 const & y) { + return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w; +} + +template +tvec3 cross(tvec3 const & x, tvec3 const & y) { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); +} + +template +T inversesqrt(T x) { + return T(1) / sqrt(x); +} + +template +tvec2 normalize(tvec2 const & x) { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); +} + +template +tvec3 normalize(tvec3 const & x) { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); +} + +template +tvec4 normalize(tvec4 const & x) { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); +} + + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// + +template +struct tquat { + typedef T value_type; + typedef std::size_t size_type; + +public: + value_type x; + value_type y; + value_type z; + value_type w; + + size_type length() const { + return 4; + } + + tquat() : + x(0), + y(0), + z(0), + w(1) { + } + + explicit tquat(value_type s, tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z), + w(s) { + } + + explicit tquat(tvec3 const & v, value_type s) : + x(v.x), + y(v.y), + z(v.z), + w(s) { + } + + explicit tquat(value_type w, value_type x, value_type y, value_type z) : + x(x), + y(y), + z(z), + w(w) { + } + + explicit tquat(tvec3 const & eulerAngle) { + tvec3 c = cos(eulerAngle * value_type(0.5)); + tvec3 s = sin(eulerAngle * value_type(0.5)); + + this->w = c.x * c.y * c.z + s.x * s.y * s.z; + this->x = s.x * c.y * c.z - c.x * s.y * s.z; + this->y = c.x * s.y * c.z + s.x * c.y * s.z; + this->z = c.x * c.y * s.z - s.x * s.y * c.z; + } + + explicit tquat(tmat3x3 const & m) { + *this = quat_cast(m); + } + + explicit tquat(tmat4x4 const & m) { + *this = quat_cast(m); + } + + value_type & operator[](int i) { + return (&x)[i]; + } + + value_type const & operator[](int i) const { + return (&x)[i]; + } + + tquat & operator*=(value_type s) { + this->w *= s; + this->x *= s; + this->y *= s; + this->z *= s; + return *this; + } + + tquat & operator=(const tquat& right) { + this->w = right.w; + this->x = right.x; + this->y = right.y; + this->z = right.z; + return *this; + } + + tquat & operator/=(value_type s) { + this->w /= s; + this->x /= s; + this->y /= s; + this->z /= s; + return *this; + } +}; + +template< typename T> +tmat4x4 makeTransform(tvec3 const & position, tvec3 const& scale, const tquat& orientation) { + tmat3x3 rot3x3 = mat3_cast(orientation); + + return tmat4x4 + ( + scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0, + scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0, + scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0, + position.x, position.y, position.z, 1 + ); +} + +template +T dot(tquat const & q1, tquat const & q2) { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; +} + +template +tquat cross(tquat const & q1, tquat const & q2) { + return tquat( + q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, + q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y, + q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z, + q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x); +} + +template +T length(tquat const & q) { + return sqrt(dot(q, q)); +} + +template +genType mix(genType x, genType y, genType a) { + return x + a * (y - x); +} + +template +T epsilon() { + return std::numeric_limits::epsilon(); +} + +template +tquat conjugate(tquat const & q) { + return tquat(q.w, -q.x, -q.y, -q.z); +} + +template +tquat inverse(tquat const & q) { + return conjugate(q) / dot(q, q); +} + +template +bool operator==(tquat const & q1, tquat const & q2) { + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); +} + +template +bool operator!=(tquat const & q1, tquat const & q2) { + return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w); +} + +template +tquat operator-(tquat const & q) { + return tquat(-q.w, -q.x, -q.y, -q.z); +} + +template +tquat operator+(tquat const & q, tquat const & p) { + return tquat( + q.w + p.w, + q.x + p.x, + q.y + p.y, + q.z + p.z + ); +} + +template +tquat operator*(tquat const & q, tquat const & p) { + return tquat( + q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z, + q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y, + q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z, + q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x + ); +} + +template +tvec3 operator*(tquat const & q, tvec3 const & v) { + typename tquat::value_type two(2); + + tvec3 uv; + tvec3 uuv; + tvec3 quatVector(q.x, q.y, q.z); + uv = cross(quatVector, v); + uuv = cross(quatVector, uv); + uv *= two * q.w; + uuv *= two; + return v + uv + uuv; +} + +template +tvec3 operator*(tvec3 const & v, tquat const & q) { + return inverse(q) * v; +} + +template +tquat operator*(tquat const & q, typename tquat::value_type s) { + return tquat(q.w * s, q.x * s, q.y * s, q.z * s); +} + +template +tquat operator*(typename tquat::value_type s, tquat const & q) { + return q * s; +} + +template +tquat operator/(tquat const & q, typename tquat::value_type s) { + return tquat(q.w / s, q.x / s, q.y / s, q.z / s); +} + +template +tquat mix(tquat const & x, tquat const & y, T const & a) { + T cosTheta = dot(x, y); + if (cosTheta > T(1) - epsilon()) { + return tquat( + mix(x.w, y.w, a), + mix(x.x, y.x, a), + mix(x.y, y.y, a), + mix(x.z, y.z, a) + ); + } else { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle); + } +} + +template +tquat lerp(tquat const & x, tquat const & y, T a) { + assert(a >= T(0)); + assert(a <= T(1)); + return x * (T(1) - a) + (y * a); +} + +template +tquat slerp(tquat const & x, tquat const & y, T a) { + tquat z = y; + + T cosTheta = dot(x, y); + + if (cosTheta < T(0)) { + z = -y; + cosTheta = -cosTheta; + } + if (cosTheta > T(1) - epsilon()) { + return tquat + ( + mix(x.w, z.w, a), + mix(x.x, z.x, a), + mix(x.y, z.y, a), + mix(x.z, z.z, a) + ); + } else { + // Essential Mathematics, page 467 + T angle = acos(cosTheta); + return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle); + } +} + +template +tquat rotate +( + typename tquat::value_type angle, + tvec3 const & axis + ) { + tvec3 Tmp = axis; + + typename tquat::value_type len = length(Tmp); + if (abs(len - T(1)) > T(0.001f)) { + T oneOverLen = T(1) / len; + Tmp.x *= oneOverLen; + Tmp.y *= oneOverLen; + Tmp.z *= oneOverLen; + } + typename tquat::value_type const AngleRad = (T) DEG2RAD(angle); + typename tquat::value_type const Sin = (T) sin(AngleRad * T(0.5)); + return tquat((T) cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin); +} + +template +valType roll(tquat const & q) { + return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG); +} + +template +valType pitch(tquat const & q) { + return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * valType(RAD2DEG); +} + +template +valType yaw(tquat const & q) { + return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG); +} + +template +tvec3 eulerAngles(tquat const & x) { + return tvec3(pitch(x), yaw(x), roll(x)); +} + +template +tmat3x3 mat3_cast(const tquat& q) { + + return tmat3x3 + ( + 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y, + 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x, + 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y + ); +} + +template +tmat4x4 mat4_cast(tquat const & q) { + return tmat4x4(mat3_cast(q)); +} + +template +tquat quat_cast(tmat3x3 const & m) { + typename tquat::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2]; + typename tquat::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2]; + typename tquat::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1]; + typename tquat::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2]; + + int biggestIndex = 0; + typename tquat::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if (fourXSquaredMinus1 > fourBiggestSquaredMinus1) { + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } + if (fourYSquaredMinus1 > fourBiggestSquaredMinus1) { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; + } + if (fourZSquaredMinus1 > fourBiggestSquaredMinus1) { + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; + } + + typename tquat::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5); + typename tquat::value_type mult = T(0.25) / biggestVal; + + tquat res; + switch (biggestIndex) { + case 0: + res.w = biggestVal; + res.x = (m[1][2] - m[2][1]) * mult; + res.y = (m[2][0] - m[0][2]) * mult; + res.z = (m[0][1] - m[1][0]) * mult; + break; + case 1: + res.w = (m[1][2] - m[2][1]) * mult; + res.x = biggestVal; + res.y = (m[0][1] + m[1][0]) * mult; + res.z = (m[2][0] + m[0][2]) * mult; + break; + case 2: + res.w = (m[2][0] - m[0][2]) * mult; + res.x = (m[0][1] + m[1][0]) * mult; + res.y = biggestVal; + res.z = (m[1][2] + m[2][1]) * mult; + break; + case 3: + res.w = (m[0][1] - m[1][0]) * mult; + res.x = (m[2][0] + m[0][2]) * mult; + res.y = (m[1][2] + m[2][1]) * mult; + res.z = biggestVal; + break; + + default: + assert(false); + break; + } + return res; +} + +template +tquat quat_cast(tmat4x4 const & m4) { + return quat_cast(tmat3x3(m4[0][0], m4[0][1], m4[0][2], + m4[1][0], m4[1][1], m4[1][2], + m4[2][0], m4[2][1], m4[2][2])); +} + +template +T angle(tquat const & x) { + return acos(x.w) * T(2) * T(RAD2DEG); +} + +template +tvec3 axis(tquat const & x) { + T tmp1 = T(1) - x.w * x.w; + if (tmp1 <= T(0)) { + return tvec3(0, 0, 1); + } + T tmp2 = T(1) / sqrt(tmp1); + + return tvec3(x.x * tmp2, x.y * tmp2, x.z * tmp2); +} + +template +tquat angleAxis(valType angle, tvec3 const & axis) { + tquat result; + + valType a = (valType) (valType(DEG2RAD(angle))); + valType s = sin(a * valType(0.5)); + + result.w = cos(a * valType(0.5)); + result.x = axis.x * s; + result.y = axis.y * s; + result.z = axis.z * s; + return result; +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +template +tmat4x4 translate(tmat4x4 const & m, tvec3 const & v) { + tmat4x4 res(m); + res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3]; + return res; +} + +template +tmat4x4 rotate +( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) { + + T a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + + tvec3 axis = normalize(v); + + tvec3 temp = (T(1) - c) * axis; + + tmat4x4 res; + res[0][0] = c + temp[0] * axis[0]; + res[0][1] = 0 + temp[0] * axis[1] + s * axis[2]; + res[0][2] = 0 + temp[0] * axis[2] - s * axis[1]; + + res[1][0] = 0 + temp[1] * axis[0] - s * axis[2]; + res[1][1] = c + temp[1] * axis[1]; + res[1][2] = 0 + temp[1] * axis[2] + s * axis[0]; + + res[2][0] = 0 + temp[2] * axis[0] + s * axis[1]; + res[2][1] = 0 + temp[2] * axis[1] - s * axis[0]; + res[2][2] = c + temp[2] * axis[2]; + + tmat4x4 rot; + + rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2]; + rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2]; + rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2]; + rot[3] = m[3]; + return rot; +} + +template +tmat4x4 scale(tmat4x4 const & m, tvec3 const & v) { + tmat4x4 res; + res[0] = m[0] * v[0]; + res[1] = m[1] * v[1]; + res[2] = m[2] * v[2]; + res[3] = m[3]; + return res; +} + +template +tmat4x4 rotate_slow +( + tmat4x4 const & m, + T const & angle, + tvec3 const & v + ) { + + T const a = DEG2RAD(angle); + T c = cos(a); + T s = sin(a); + tmat4x4 res; + + tvec3 axis = normalize(v); + + res[0][0] = c + (1 - c) * axis.x * axis.x; + res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z; + res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y; + res[0][3] = 0; + + res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z; + res[1][1] = c + (1 - c) * axis.y * axis.y; + res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x; + res[1][3] = 0; + + res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y; + res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x; + res[2][2] = c + (1 - c) * axis.z * axis.z; + res[2][3] = 0; + + res[3] = tvec4(0, 0, 0, 1); + return m * res; +} + +template +tmat4x4 scale_slow(tmat4x4 const & m, tvec3 const & v) { + tmat4x4 res(T(1)); + res[0][0] = v.x; + res[1][1] = v.y; + res[2][2] = v.z; + + return m * res; +} + +template +tmat4x4 ortho +( + valType left, + valType right, + valType bottom, + valType top, + valType zNear, + valType zFar + ) { + tmat4x4 res(1); + res[0][0] = valType(2) / (right - left); + res[1][1] = valType(2) / (top - bottom); + res[2][2] = -valType(2) / (zFar - zNear); + res[3][0] = -(right + left) / (right - left); + res[3][1] = -(top + bottom) / (top - bottom); + res[3][2] = -(zFar + zNear) / (zFar - zNear); + return res; +} + +template +tmat4x4 frustum +( + valType left, + valType right, + valType bottom, + valType top, + valType nearVal, + valType farVal + ) { + tmat4x4 res(0); + res[0][0] = (valType(2) * nearVal) / (right - left); + res[1][1] = (valType(2) * nearVal) / (top - bottom); + res[2][0] = (right + left) / (right - left); + res[2][1] = (top + bottom) / (top - bottom); + res[2][2] = -(farVal + nearVal) / (farVal - nearVal); + res[2][3] = valType(-1); + res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal); + return res; +} + +template +tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = -(zFar + zNear) / (zFar - zNear); + res[2][3] = -valType(1); + res[3][2] = -(valType(2) * zFar * zNear) / (zFar - zNear); + return res; +} + +template +tvec3 project +( + tvec3 const & obj, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) { + tvec4 tmp = tvec4(obj.x, obj.y, obj.z, T(1)); + tmp = model * tmp; + tmp = proj * tmp; + + tmp /= tmp.w; + tmp = tmp * T(0.5) + T(0.5); + tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]); + tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]); + + return tvec3(tmp.x, tmp.y, tmp.z); +} + +template +tvec3 unProject +( + tvec3 const & win, + tmat4x4 const & model, + tmat4x4 const & proj, + tvec4 const & viewport + ) { + tmat4x4 inverses = (proj * model).inverse(); + + tvec4 tmp = tvec4(win.x, win.y, win.z, T(1)); + tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]); + tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]); + tmp = tmp * T(2) - T(1); + + tvec4 obj = inverses * tmp; + obj /= obj.w; + + return tvec3(obj.x, obj.y, obj.z); +} + +template +tmat4x4 pickMatrix +( + tvec2 const & center, + tvec2 const & delta, + tvec4 const & viewport + ) { + assert(delta.x > T(0) && delta.y > T(0)); + tmat4x4 res(1.0f); + + if (!(delta.x > T(0) && delta.y > T(0))) { + return res; + } + + tvec3 Temp + ( + (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x, + (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y, + T(0) + ); + res = translate(res, Temp); + return scale(res, tvec3(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1))); +} + +template +tmat4x4 lookAt +( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; +} + +template +class AxisAlignedBox2D { +public: + + enum Extent { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; +public: + tvec2 _minimum; + tvec2 _maximum; + Extent _extent; +public: + + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox2D() { + _minimum = tvec2(T(-0.5), T(-0.5)); + _maximum = tvec2(T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + + AxisAlignedBox2D(const AxisAlignedBox2D & rkBox) { + setExtents(rkBox._minimum, rkBox._maximum); + _extent = rkBox._extent; + } + + AxisAlignedBox2D(const tvec2& min, const tvec2& max) { + setExtents(min, max); + } + + AxisAlignedBox2D( + T mx, T my, + T Mx, T My + ) { + setExtents(tvec2(mx, my), tvec2(Mx, My)); + } + + AxisAlignedBox2D& operator=(const AxisAlignedBox2D& right) { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox2D() { + } + + /** + * Gets the minimum corner of the box. + */ + const tvec2& getMinimum(void) const { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec2& getMinimum(void) { + return _minimum; + } + + void setMinimum(const tvec2& vec) { + _minimum = vec; + } + + void setMinimum(T x, T y) { + _minimum = tvec2(x, y); + } + + /** + * Gets the maximum corner of the box. + */ + const tvec2& getMaximum(void) const { + return _maximum; + } + + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec2& getMaximum(void) { + return _maximum; + } + + /** + * Sets the maximum corner of the box. + */ + void setMaximum(const tvec2& vec) { + _maximum = vec; + } + + void setMaximum(T x, T y) { + _maximum.x = x; + _maximum.y = y; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents(const tvec2& min, const tvec2& max) { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, + T Mx, T My + ) { + _minimum.x = mx; + _minimum.y = my; + + _maximum.x = Mx; + _maximum.y = My; + _extent = EXTENT_FINITE; + } + + inline bool intersects(const AxisAlignedBox2D& b2) const { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + } + + inline AxisAlignedBox2D intersection(const AxisAlignedBox2D& b2) const { + tvec2 intMin = _minimum; + tvec2 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y) { + return AxisAlignedBox2D(intMin, intMax); + } + + return AxisAlignedBox2D(); + } + + inline void setNull() { + _extent = EXTENT_NULL; + } + + inline bool isNull(void) const { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const { + return (_extent == EXTENT_FINITE); + } + + inline void setInfinite() { + _extent = EXTENT_INFINITE; + } + + inline bool isInfinite(void) const { + return (_extent == EXTENT_INFINITE); + } + + inline bool intersects(const tvec2& v) const { + return ( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y); + } + + inline tvec2 getCenter(void) const { + return tvec2( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f)); + } + + /** + * Gets the size of the box + */ + inline tvec2 getSize(void) const { + return _maximum - _minimum; + } + + inline tvec2 getHalfSize(void) const { + return (_maximum - _minimum) * T(0.5); + } + + inline bool contains(const tvec2& v) const { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y; + } + + inline bool contains(const AxisAlignedBox2D& other) const { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y; + } + + inline bool operator==(const AxisAlignedBox2D& right) const { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + + inline bool operator!=(const AxisAlignedBox2D& right) const { + return !(*this == right); + } + + inline void merge(tvec2 point) { + if (_minimum.x > point.x) { + _minimum.x = point.x; + } + + if (_minimum.y > point.y) { + _minimum.y = point.y; + } + if (_maximum.x < point.x) { + _maximum.x = point.x; + } + if (_maximum.y < point.y) { + _maximum.y = point.y; + } + } + + inline void merge(AxisAlignedBox2D other) { + _maximum.makeCeil(other._maximum); + _minimum.makeFloor(other._minimum); + } + +}; + +template +class AxisAlignedBox { +public: + + enum Extent { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; +public: + tvec3 _minimum; + tvec3 _maximum; + Extent _extent; +public: + + /* + 1-----2 + /| /| + / | / | + 5-----4 | + | 0--|--3 + | / | / + |/ |/ + 6-----7 + */ + typedef enum { + FAR_LEFT_BOTTOM = 0, + FAR_LEFT_TOP = 1, + FAR_RIGHT_TOP = 2, + FAR_RIGHT_BOTTOM = 3, + NEAR_RIGHT_BOTTOM = 7, + NEAR_LEFT_BOTTOM = 6, + NEAR_LEFT_TOP = 5, + NEAR_RIGHT_TOP = 4 + } CornerEnum; + + AxisAlignedBox() { + _minimum = tvec3(T(-0.5), T(-0.5), T(-0.5)); + _maximum = tvec3(T(0.5), T(0.5), T(0.5)); + _extent = EXTENT_NULL; + } + + AxisAlignedBox(const AxisAlignedBox & rkBox) { + setExtents(rkBox._minimum, rkBox._maximum); + _extent = rkBox._extent; + } + + AxisAlignedBox(const tvec3& min, const tvec3& max) { + setExtents(min, max); + } + + AxisAlignedBox( + T mx, T my, T mz, + T Mx, T My, T Mz + ) { + setExtents(mx, my, mz, Mx, My, Mz); + } + + AxisAlignedBox& operator=(const AxisAlignedBox& right) { + setExtents(right._minimum, right._maximum); + return *this; + } + + ~AxisAlignedBox() { + } + + /** + * Gets the minimum corner of the box. + */ + const tvec3& getMinimum(void) const { + return _minimum; + } + + /** + * Gets a modifiable version of the minimum + * corner of the box. + */ + tvec3& getMinimum(void) { + return _minimum; + } + + void setMinimum(const tvec3& mins) { + _minimum = mins; + } + + void setMinimum(T x, T y, T z) { + _minimum = tvec3(x, y, z); + } + + /** + * Gets the maximum corner of the box. + */ + const tvec3& getMaximum(void) const { + return _maximum; + } + + /** + * Gets a modifiable version of the maximum + * corner of the box. + */ + tvec3& getMaximum(void) { + return _maximum; + } + + /** + * Sets the maximum corner of the box. + */ + void setMaximum(const tvec3& vec) { + _maximum = vec; + } + + void setMaximum(T x, T y, T z) { + _maximum.x = x; + _maximum.y = y; + _maximum.z = z; + } + + /** + * Changes one of the components of the maximum corner of the box + * used to resize only one dimension of the box + */ + void setMaximumX(T x) { + _maximum.x = x; + } + + void setMaximumY(T y) { + _maximum.y = y; + } + + void setMaximumZ(T z) { + _maximum.z = z; + } + + /** + * Sets both minimum and maximum extents at once. + */ + void setExtents(const tvec3& min, const tvec3& max) { + _minimum = min; + _maximum = max; + _extent = EXTENT_FINITE; + } + + void setExtents( + T mx, T my, T mz, + T Mx, T My, T Mz) { + _minimum.x = mx; + _minimum.y = my; + _minimum.z = mz; + + _maximum.x = Mx; + _maximum.y = My; + _maximum.z = Mz; + _extent = EXTENT_FINITE; + + } + + /** Returns a pointer to an array of 8 corner points, useful for + collision vs. non-aligned objects. + @remarks + If the order of these corners is important, they are as + follows: The 4 points of the minimum Z face (note that + because Ogre uses right-handed coordinates, the minimum Z is + at the 'back' of the box) starting with the minimum point of + all, then anticlockwise around this face (if you are looking + onto the face from outside the box). Then the 4 points of the + maximum Z face, starting with maximum point of all, then + anticlockwise around this face (looking onto the face from + outside the box). Like this: + 
+    1-----2
+    /|    /|
+    / |   / |
+    5-----4  |
+    |  0--|--3
+    | /   | /
+    |/    |/
+    6-----7
+
+ @remarks as this implementation uses a static member, make sure to use your own copy ! + */ + void getAllCorners(tvec3 mpCorners[8]) const { + mpCorners[0] = _minimum; + mpCorners[1].x = _minimum.x; + mpCorners[1].y = _maximum.y; + mpCorners[1].z = _minimum.z; + mpCorners[2].x = _maximum.x; + mpCorners[2].y = _maximum.y; + mpCorners[2].z = _minimum.z; + mpCorners[3].x = _maximum.x; + mpCorners[3].y = _minimum.y; + mpCorners[3].z = _minimum.z; + + mpCorners[4] = _maximum; + mpCorners[5].x = _minimum.x; + mpCorners[5].y = _maximum.y; + mpCorners[5].z = _maximum.z; + mpCorners[6].x = _minimum.x; + mpCorners[6].y = _minimum.y; + mpCorners[6].z = _maximum.z; + mpCorners[7].x = _maximum.x; + mpCorners[7].y = _minimum.y; + mpCorners[7].z = _maximum.z; + } + + /** + * gets the position of one of the corners + */ + tvec3 getCorner(CornerEnum cornerToGet) const { + switch (cornerToGet) { + case FAR_LEFT_BOTTOM: + return _minimum; + case FAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_TOP: + return tvec3(_maximum.x, _maximum.y, _minimum.z); + case FAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _minimum.z); + case NEAR_RIGHT_BOTTOM: + return tvec3(_maximum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_BOTTOM: + return tvec3(_minimum.x, _minimum.y, _maximum.z); + case NEAR_LEFT_TOP: + return tvec3(_minimum.x, _maximum.y, _maximum.z); + case NEAR_RIGHT_TOP: + return _maximum; + default: + return tvec3(); + } + } + + /** + * Merges the passed in box into the current box. The result is the + * box which encompasses both. + */ + void merge(const AxisAlignedBox& right) { + + if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE)) { + return; + } else if (right._extent == EXTENT_INFINITE) { + _extent = EXTENT_INFINITE; + } else if (_extent == EXTENT_NULL) { + setExtents(right._minimum, right._maximum); + } else { + //! merge + tvec3 min = _minimum; + tvec3 max = _maximum; + max.makeCeil(right._maximum); + min.makeFloor(right._minimum); + setExtents(min, max); + } + } + + /** + * Extends the box to encompass the specified point (if needed). + */ + void merge(const tvec3& point) { + switch (_extent) { + case EXTENT_NULL: // if null, use this point + setExtents(point, point); + return; + + case EXTENT_FINITE: + _maximum.makeCeil(point); + _minimum.makeFloor(point); + return; + + case EXTENT_INFINITE: + return; + } + } + + void transform(const tmat4x4& matrix) { + tvec3 oldMin; + tvec3 oldMax; + tvec3 currentCorner; + + oldMin = _minimum; + oldMax = _maximum; + + + // We sequentially compute the corners in the following order : + // 0, 6, 5, 1, 2, 4 ,7 , 3 + // This sequence allows us to only change one member at a time to get at all corners. + + // For each one, we transform it using the matrix + // Which gives the resulting point and merge the resulting point. + + currentCorner = oldMin; + tvec3 vVert = currentCorner * matrix; + setExtents(vVert, vVert); + + // First corner + // min min min + currentCorner = oldMin; + merge(currentCorner * matrix); + + // min,min,max + currentCorner.z = oldMax.z; + merge(currentCorner * matrix); + + // min max max + currentCorner.y = oldMax.y; + merge(currentCorner * matrix); + + // min max min + currentCorner.z = oldMin.z; + merge(currentCorner * matrix); + + // max max min + currentCorner.x = oldMax.x; + merge(currentCorner * matrix); + + // max max max + currentCorner.z = oldMax.z; + merge(currentCorner * matrix); + + // max min max + currentCorner.y = oldMin.y; + merge(currentCorner * matrix); + + // max min min + currentCorner.z = oldMin.z; + merge(currentCorner * matrix); + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersects(const AxisAlignedBox& b2) const { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + if (_maximum.z < b2._minimum.z) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + if (_minimum.z > b2._maximum.z) + return false; + return true; + + } + + /** + * Returns whether or not this box intersects another. + */ + bool intersectsNoZ(const AxisAlignedBox& b2) const { + if (_maximum.x < b2._minimum.x) + return false; + if (_maximum.y < b2._minimum.y) + return false; + + if (_minimum.x > b2._maximum.x) + return false; + if (_minimum.y > b2._maximum.y) + return false; + return true; + + } + + AxisAlignedBox intersection(const AxisAlignedBox& b2) const { + tvec3 intMin = _minimum; + tvec3 intMax = _maximum; + + intMin.makeCeil(b2.getMinimum()); + intMax.makeFloor(b2.getMaximum()); + + if (intMin.x < intMax.x && + intMin.y < intMax.y && + intMin.z < intMax.z) { + return AxisAlignedBox(intMin, intMax); + } + + return AxisAlignedBox(); + } + + void setNull() { + _extent = EXTENT_NULL; + } + + bool isNull(void) const { + return (_extent == EXTENT_NULL); + } + + bool isFinite(void) const { + return (_extent == EXTENT_FINITE); + } + + void setInfinite() { + _extent = EXTENT_INFINITE; + } + + bool isInfinite(void) const { + return (_extent == EXTENT_INFINITE); + } + + void scale(const tvec3& s) { + tvec3 min = _minimum * s; + tvec3 max = _maximum * s; + setExtents(min, max); + } + + bool intersects(const tvec3& v) const { + return ( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y && + v.z >= _minimum.z && v.z <= _maximum.z); + } + + bool intersects(const tvec2& v) const { + return ( v.x >= _minimum.x && v.x <= _maximum.x && + v.y >= _minimum.y && v.y <= _maximum.y); + } + + tvec3 getCenter(void) const { + return tvec3( + (_maximum.x + _minimum.x) * T(0.5f), + (_maximum.y + _minimum.y) * T(0.5f), + (_maximum.z + _minimum.z) * T(0.5f) + ); + } + + /** + * Gets the size of the box + */ + tvec3 getSize(void) const { + return _maximum - _minimum; + } + + tvec3 getHalfSize(void) const { + return (_maximum - _minimum) * T(0.5); + } + + bool contains(const tvec3& v) const { + return _minimum.x <= v.x && v.x <= _maximum.x && + _minimum.y <= v.y && v.y <= _maximum.y && + _minimum.z <= v.z && v.z <= _maximum.z; + } + + bool contains(const AxisAlignedBox& other) const { + return this->_minimum.x <= other._minimum.x && + this->_minimum.y <= other._minimum.y && + this->_minimum.z <= other._minimum.z && + other._maximum.x <= this->_maximum.x && + other._maximum.y <= this->_maximum.y && + other._maximum.z <= this->_maximum.z; + } + + bool operator==(const AxisAlignedBox& right) const { + return this->_minimum == right._minimum && + this->_maximum == right._maximum; + } + + bool operator!=(const AxisAlignedBox& right) const { + return !(*this == right); + } +}; + +template +class tspline { +public: + + tspline() { + mCoeffs[0][0] = 2; + mCoeffs[0][1] = -2; + mCoeffs[0][2] = 1; + mCoeffs[0][3] = 1; + mCoeffs[1][0] = -3; + mCoeffs[1][1] = 3; + mCoeffs[1][2] = -2; + mCoeffs[1][3] = -1; + mCoeffs[2][0] = 0; + mCoeffs[2][1] = 0; + mCoeffs[2][2] = 1; + mCoeffs[2][3] = 0; + mCoeffs[3][0] = 1; + mCoeffs[3][1] = 0; + mCoeffs[3][2] = 0; + mCoeffs[3][3] = 0; + + mCoeffs = mCoeffs.transpose(); + mAutoCalc = true; + } + + ~tspline() { + }; + + void addPoint(const tvec3& p) { + mPoints.push_back(p); + if (mAutoCalc) { + recalcTangents(); + } + } + + const tvec3& getPoint(size_t index) const { + assert(index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + tvec3& getPoint(size_t index) { + assert(index < mPoints.size() && "Point index is out of bounds!!"); + + return mPoints[index]; + } + + /** + * ��ȡ������� + */ + size_t getNumPoints(void) const { + return mPoints.size(); + } + + /** + * ������еĵ����� + */ + void clear(void) { + mPoints.clear(); + mTangents.clear(); + } + + /** + * ���µ����� + */ + void updatePoint(size_t index, const tvec3& value) { + assert(index < mPoints.size() && "Point index is out of bounds!!"); + + mPoints[index] = value; + if (mAutoCalc) { + recalcTangents(); + } + } + + /** + * ��ֵ��ȡ������ + */ + tvec3 interpolate(T time) const { + T fSeg = time * (mPoints.size() - 1); + unsigned segIdx = (unsigned) fSeg; + // Apportion t + time = fSeg - segIdx; + + return interpolate(segIdx, time); + } + + /** + * ����������ֵ + */ + tvec3 interpolate(size_t fromIndex, T t) const { + // Bounds check + assert(fromIndex < mPoints.size() && "fromIndex out of bounds"); + + if ((fromIndex + 1) == mPoints.size()) { + // Duff request, cannot blend to nothing + // Just return source + return mPoints[fromIndex]; + } + // Fast special cases + if (t == 0.0f) { + return mPoints[fromIndex]; + } else if (t == 1.0f) { + return mPoints[fromIndex + 1]; + } + + // float interpolation + // Form a vector of powers of t + T t2, t3; + t2 = t * t; + t3 = t2 * t; + tvec4 powers(t3, t2, t, 1); + + const tvec3& point1 = mPoints[fromIndex]; + const tvec3& point2 = mPoints[fromIndex + 1]; + const tvec3& tan1 = mTangents[fromIndex]; + const tvec3& tan2 = mTangents[fromIndex + 1]; + tmat4x4 pt; + + pt[0][0] = point1.x; + pt[0][1] = point1.y; + pt[0][2] = point1.z; + pt[0][3] = 1.0f; + pt[1][0] = point2.x; + pt[1][1] = point2.y; + pt[1][2] = point2.z; + pt[1][3] = 1.0f; + pt[2][0] = tan1.x; + pt[2][1] = tan1.y; + pt[2][2] = tan1.z; + pt[2][3] = 1.0f; + pt[3][0] = tan2.x; + pt[3][1] = tan2.y; + pt[3][2] = tan2.z; + pt[3][3] = 1.0f; + + pt = pt.transpose(); + + tvec4 ret = powers * mCoeffs * pt; + + return tvec3(ret.x, ret.y, ret.z); + } + + /** + * �Զ������� + */ + void setAutoCalculate(bool autoCalc) { + mAutoCalc = autoCalc; + } + + /** + * �������� + */ + void recalcTangents(void) { + size_t i, numPoints; + bool isClosed; + + numPoints = mPoints.size(); + if (numPoints < 2) { + return; + } + if (mPoints[0] == mPoints[numPoints - 1]) { + isClosed = true; + } else { + isClosed = false; + } + + mTangents.resize(numPoints); + + + for (i = 0; i < numPoints; ++i) { + if (i == 0) { + // Special case start + if (isClosed) { + // Use numPoints-2 since numPoints-1 is the last point and == [0] + mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints - 2]); + } else { + mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]); + } + } else if (i == numPoints - 1) { + if (isClosed) { + mTangents[i] = mTangents[0]; + } else { + mTangents[i] = 0.5f * (mPoints[i] - mPoints[i - 1]); + } + } else { + mTangents[i] = 0.5f * (mPoints[i + 1] - mPoints[i - 1]); + } + } + } + +public: + bool mAutoCalc; + std::vector< tvec3 > mPoints; + std::vector< tvec3 > mTangents; + tmat4x4 mCoeffs; +}; + +template < typename T > +class tellipsoidModel { +public: + + tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR), T radiusPolar = T(WGS_84_RADIUS_POLAR)) { + _radiusEquator = radiusEquator; + _radiusPolar = radiusPolar; + T flattening = (_radiusEquator - _radiusPolar) / _radiusEquator; + _eccentricitySquared = T(2) * flattening - flattening*flattening; + } + + ~tellipsoidModel(void) { + } + + void convertLatLongHeightToXYZ( + T latitude, + T longitude, + T height, + T& X, + T& Y, + T& Z + ) const { + // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif + T sin_latitude = sin(latitude); + T cos_latitude = cos(latitude); + T N = _radiusEquator / sqrt(1.0 - _eccentricitySquared * sin_latitude * sin_latitude); + X = (N + height) * cos_latitude * cos(longitude); + Y = (N + height) * cos_latitude * sin(longitude); + Z = (N * (1 - _eccentricitySquared) + height) * sin_latitude; + } + + void convertXYZToLatLongHeight( + T X, + T Y, + T Z, + T& latitude, + T& longitude, + T& height + ) const { + // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif + T p = (T) sqrt(X * X + Y * Y); + T theta = (T) atan2(Z*_radiusEquator, (p * _radiusPolar)); + T eDashSquared = (_radiusEquator * _radiusEquator - _radiusPolar * _radiusPolar) / (_radiusPolar * _radiusPolar); + + T sin_theta = (T) sin(theta); + T cos_theta = (T) cos(theta); + + latitude = (T) atan((Z + eDashSquared * _radiusPolar * sin_theta * sin_theta * sin_theta) / + (p - _eccentricitySquared * _radiusEquator * cos_theta * cos_theta * cos_theta)); + longitude = (T) atan2(Y, X); + + T sin_latitude = (T) sin(latitude); + T N = _radiusEquator / (T) sqrt(1.0 - _eccentricitySquared * sin_latitude * sin_latitude); + + height = p / (T) cos(latitude) - N; + } + +protected: + T _radiusEquator; + T _radiusPolar; + T _eccentricitySquared; +}; + +class Rgba4Byte { +public: + + Rgba4Byte( + unsigned char r = 255, + unsigned char g = 255, + unsigned char b = 255, + unsigned char a = 255 + ) { + _r = r; + _g = g; + _b = b; + _a = a; + } + + Rgba4Byte(uint rgba) { + _color = rgba; + } + + friend bool operator==(const Rgba4Byte& left, const Rgba4Byte& right) { + return left._r == right._r && + left._g == right._g && + left._b == right._b && + left._a == right._a; + } + + friend bool operator!=(const Rgba4Byte& left, const Rgba4Byte& right) { + return left._r != right._r || + left._g != right._g || + left._b != right._b || + left._a != right._a; + } + + friend Rgba4Byte operator+(const Rgba4Byte& left, const Rgba4Byte& right) { + return Rgba4Byte(left._r * right._r + , left._g * right._g + , left._b * right._b + , left._a * right._a); + } + + operator unsigned() { + return _color; + } + + uint toUint() { + return _color; + } +public: + + union { + + struct { + unsigned char _b; + unsigned char _g; + unsigned char _r; + unsigned char _a; + }; + uint _color; + }; +}; + +typedef Rgba4Byte Rgba; + +inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s) { + Rgba4Byte color; + + color._r = (unsigned char) (c1._r + s * (c2._r - c1._r)); + color._g = (unsigned char) (c1._g + s * (c2._g - c1._g)); + color._b = (unsigned char) (c1._b + s * (c2._b - c1._b)); + color._a = (unsigned char) (c1._a + s * (c2._a - c1._a)); + return color; +} + +inline tvec2 uvLerp(const tvec2& c1, const tvec2& c2, float s) { + tvec2 color; + color.x = (c1.x + s * (c2.x - c1.x)); + color.y = (c1.y + s * (c2.y - c1.y)); + return color; +} + +template +class tAxisAlignedBox2 { +public: + + enum Extent { + EXTENT_NULL, + EXTENT_FINITE, + EXTENT_INFINITE + }; +public: + tvec2 _vMin; + tvec2 _vMax; + Extent mExtent; +public: + + tvec2 center() const { + return (_vMin + _vMax) * T(0.5); + } + + tvec2 size() const { + return _vMax - _vMin; + } + + tvec2 halfSize() const { + return (_vMax - _vMin) * T(0.5); + } + + bool intersects(tvec2 v) const { + return ( v.x >= _vMin.x && v.x <= _vMax.x && + v.y >= _vMin.y && v.y <= _vMax.y); + } + + void merge(tvec2 point) { + if (_vMin.x > point.x) { + _vMin.x = point.x; + } + + if (_vMin.y > point.y) { + _vMin.y = point.y; + } + if (_vMax.x < point.x) { + _vMax.x = point.x; + } + if (_vMax.y < point.y) { + _vMax.y = point.y; + } + } + + void merge(tAxisAlignedBox2 other) { + _vMax.makeCeil(other._vMax); + _vMin.makeFloor(other._vMin); + } + + bool contains(tvec2 point) const { + return _vMin.x <= point.x && point.x <= _vMax.x && + _vMin.y <= point.y && point.y <= _vMax.y; + } + + bool contains(tAxisAlignedBox2 other) const { + return this->_vMin.x <= other._vMin.x && + this->_vMin.y <= other._vMin.y && + other._vMax.x <= this->_vMax.x && + other._vMax.y <= this->_vMax.y; + } +}; + +template +class tray { + typedef T value_type; + typedef tray type; +protected: + tvec3 _origin; + tvec3 _direction; +public: + + tray() : + _origin(value_type(0), value_type(0), value_type(0)), + _direction(value_type(0), value_type(0), value_type(1)) { + } + + tray(const tvec3& origin, const tvec3& direction) : + _origin(origin), + _direction(direction) { + } + + /** + * �������ߵ���� + */ + void setOrigin(const tvec3& origin) { + _origin = origin; + } + + /** + * �������ߵ���� + */ + const tvec3& getOrigin(void) const { + return _origin; + } + + /** + * �������ߵķ��� + */ + void setDirection(const tvec3& dir) { + _direction = dir; + } + + /** + * �������ߵķ��� + */ + const tvec3& getDirection(void) const { + return _direction; + } + + /** + * Gets the position of a point t units along the ray. + */ + tvec3 getPoint(T time) const { + return tvec3(_origin + (_direction * time)); + } + + /** + * ��������box�ཻ + * ����ཻ,����ֵ�е�first == true.����false + * secondΪ���ߵ���ľ��� + * ����getPoint�������򷵻ؽ��� + */ + std::pair intersects(const AxisAlignedBox& box) const { + T lowt = 0.0f; + T t; + bool hit = false; + tvec3 hitpoint; + tvec3 min = box.getMinimum(); + tvec3 max = box.getMaximum(); + + /** + * ���ڰ�Χ������ + */ + if (_origin > min && _origin < max) { + return std::pair (true, 0.0f); + } + + // Check each face in turn, only check closest 3 + // Min x + if (_origin.x <= min.x && _direction.x > 0) { + t = (min.x - _origin.x) / _direction.x; + if (t >= 0) { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) { + hit = true; + lowt = t; + } + } + } + // Max x + if (_origin.x >= max.x && _direction.x < 0) { + t = (max.x - _origin.x) / _direction.x; + if (t >= 0) { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.y >= min.y && + hitpoint.y <= max.y && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) { + hit = true; + lowt = t; + } + } + } + // Min y + if (_origin.y <= min.y && _direction.y > 0) { + t = (min.y - _origin.y) / _direction.y; + if (t >= 0) { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) { + hit = true; + lowt = t; + } + } + } + // Max y + if (_origin.y >= max.y && _direction.y < 0) { + t = (max.y - _origin.y) / _direction.y; + if (t >= 0) { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.z >= min.z && + hitpoint.z <= max.z && + (!hit || t < lowt)) { + hit = true; + lowt = t; + } + } + } + // Min z + if (_origin.z <= min.z && _direction.z > 0) { + t = (min.z - _origin.z) / _direction.z; + if (t >= 0) { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) { + hit = true; + lowt = t; + } + } + } + // Max z + if (_origin.z >= max.z && _direction.z < 0) { + t = (max.z - _origin.z) / _direction.z; + if (t >= 0) { + // Substitute t back into ray and check bounds and dist + hitpoint = _origin + _direction * t; + if (hitpoint.x >= min.x && + hitpoint.x <= max.x && + hitpoint.y >= min.y && + hitpoint.y <= max.y && + (!hit || t < lowt)) { + hit = true; + lowt = t; + } + } + } + return std::pair (hit, lowt); + } +}; + +template +class Plane { +public: + tvec3 _normal; + T _distance; +public: + + Plane() { + _normal = tvec3(0, 0, 0); + _distance = 0.0f; + } + + Plane(const Plane& right) { + _normal = right._normal; + _distance = right._distance; + } + + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane(const tvec3& rkNormal, T fConstant) { + _normal = rkNormal; + _distance = -fConstant; + } + + /** Construct a plane using the 4 constants directly **/ + Plane(T x, T y, T z, T o) { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + + Plane(const tvec3& rkNormal, const tvec3& rkPoint) { + redefine(rkNormal, rkPoint); + } + + Plane(const tvec3& rkPoint0, const tvec3& rkPoint1, const tvec3& rkPoint2) { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + + /** + * ����ľ��� + */ + float distance(const tvec3 &pos) const { + return dot(_normal, pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide(const tvec3& rkPoint) const { + float fDistance = getDistance(rkPoint); + + if (fDistance < 0.0) + return Plane::NEGATIVE_SIDE; + + if (fDistance > 0.0) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + Side getSide(const tvec3& centre, const tvec3& halfSize) const { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance(const tvec3& rkPoint) const { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1, kEdge2); + _normal.normalise(); + _distance = -dot(_normal, rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) { + _normal = rkNormal; + _distance = -dot(rkNormal, rkPoint); + } + + + // tvec3 projectVector(const tvec3& p) const + // { + // matrix3 xform; + // xform[0][0] = 1.0f - _normal.x * _normal.x; + // xform[0][1] = -_normal.x * _normal.y; + // xform[0][2] = -_normal.x * _normal.z; + // xform[1][0] = -_normal.y * _normal.x; + // xform[1][1] = 1.0f - _normal.y * _normal.y; + // xform[1][2] = -_normal.y * _normal.z; + // xform[2][0] = -_normal.z * _normal.x; + // xform[2][1] = -_normal.z * _normal.y; + // xform[2][2] = 1.0f - _normal.z * _normal.z; + // return xform * p; + // } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + + bool operator==(const Plane& right) const { + return (right._distance == _distance && right._normal == _normal); + } + + bool operator!=(const Plane& right) const { + return (right._distance != _distance && right._normal != _normal); + } +}; + +template +class tfrustum { +public: + + enum { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; +public: + + /** + * project * view + */ + void loadFrustum(const tmat4x4 &mvp) { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + + bool pointInFrustum(const tvec3 &pos) const { + for (int i = 0; i < 6; i++) { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + + bool sphereInFrustum(const tvec3 &pos, const float radius) const { + for (int i = 0; i < 6; i++) { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + + bool cubeInFrustum(T minX, T maxX, T minY, T maxY, T minZ, T maxZ) const { + for (int i = 0; i < 6; i++) { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const { + return _planes[plane]; + } +protected: + Plane _planes[6]; +}; + + +typedef float real; +typedef tvec2 int2; +typedef tvec2 float2; +typedef tvec2 double2; + +typedef tvec2 real2; + + +typedef tvec3 int3; +typedef tvec3 uint3; +typedef tvec3 float3; +typedef tvec3 double3; + +typedef tvec3 real3; + + +typedef tvec4 int4; +typedef tvec4 float4; +typedef tvec4 double4; +typedef tvec4 real4; +typedef trect rect4; +typedef trect rect4i; + +typedef AxisAlignedBox aabb3d; +typedef AxisAlignedBox aabbr; + +typedef AxisAlignedBox2D AABB2D; +typedef AxisAlignedBox2D aabb2dr; +typedef AxisAlignedBox2D aabb2di; + + + +typedef tmat2x2 matrix2; +typedef tmat3x3 matrix3; +typedef tmat4x4 matrix4; +typedef tmat4x4 matrix4r; + +typedef tquat quaternion; +typedef tquat quatr; +typedef tray Ray; +typedef tfrustum Frustum; + +typedef tellipsoidModel ellipsoid; + #endif /* COMMON_H */ diff --git a/src/5threed/struct/common.h b/src/5threed/struct/common.h new file mode 100644 index 0000000..fdf1995 --- /dev/null +++ b/src/5threed/struct/common.h @@ -0,0 +1,159 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +/* + * File: common.h + * Author: Blobt + * + * Created on October 5, 2019, 4:55 PM + */ + +#ifndef COMMON_H +#define COMMON_H + +#include +#include +#include +#include +#include "Rgba.h" +#include "struct/tvec2.h" +#include "struct/tvec3.h" +#include "struct/tmat3x3.h" +#include "struct/tvec4.h" +#include "struct/tmat4x4.h" +#include "struct/tfrustum.h" + +#define die(m) do { perror(m); exit(EXIT_FAILURE); } while(0) +#define def2rad(theta) (0.01745329251994329 * (theta)) //每个角度所对应的弧度 + +typedef tvec2 float2; +typedef tvec2 int2; +typedef tvec3 float3; +typedef tvec4 float4; +typedef unsigned char byte; +typedef tmat3x3 matrix3; +typedef tmat4x4 matrix4; +typedef tfrustum Frustum; + +template inline T tmin(T a, T b) { + return a < b ? a : b; +} + +template inline T tmax(T a, T b) { + return a > b ? a : b; +} + +inline Rgba colorLerp(const Rgba& color1, const Rgba& color2, float step) { + Rgba ret; + + ret._r = (unsigned char) color1._r + step * (color2._r - color1._r); + ret._g = (unsigned char) color1._g + step * (color2._g - color1._g); + ret._b = (unsigned char) color1._b + step * (color2._b - color1._b); + ret._a = (unsigned char) color1._a + step * (color2._a - color1._a); + + return ret; +} + +inline float2 uvLerp(const float2& uv1, const float2& uv2, float step) { + + if (step < 0 || step > 1) { + printf("step : %f\n", step); + die("step must more than zero and less than 1"); + } + + float2 ret; + + ret.x = (float) uv1.x + (uv2.x - uv1.x) * step; + ret.y = (float) uv1.y + (uv2.y - uv1.y) * step; + + return ret; +} + +template +tvec3 cross(tvec3 const & x, tvec3 const & y) { + return tvec3 + ( + x.y * y.z - y.y * x.z, + x.z * y.x - y.z * x.x, + x.x * y.y - y.x * x.y + ); +} + +template +typename tvec3::value_type dot(tvec3 const & x, tvec3 const & y) { + return x.x * y.x + x.y * y.y + x.z * y.z; +} + +template +T inversesqrt(T x) { + return T(1) / sqrt(x); +} + +template +tvec2 normalize(tvec2 const & x) { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); +} + +template +tvec3 normalize(tvec3 const & x) { + typename tvec3::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z; + return x * inversesqrt(sqr); +} + +template +tvec4 normalize(tvec4 const & x) { + typename tvec4::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w; + return x * inversesqrt(sqr); +} + +template +tmat4x4 perspective(valType fovy, valType aspect, valType zNear, valType zFar) { + valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear; + valType left = -range * aspect; + valType right = range * aspect; + valType bottom = -range; + valType top = range; + + tmat4x4 res(valType(0)); + res[0][0] = (valType(2) * zNear) / (right - left); + res[1][1] = (valType(2) * zNear) / (top - bottom); + res[2][2] = -(zFar + zNear) / (zFar - zNear); + res[2][3] = -valType(1); + res[3][2] = -(valType(2) * zFar * zNear) / (zFar - zNear); + return res; +} + +template +tmat4x4 lookAt +( + tvec3 const & eye, + tvec3 const & center, + tvec3 const & up + ) { + tvec3 f = normalize(center - eye); + tvec3 u = normalize(up); + tvec3 s = normalize(cross(f, u)); + u = cross(s, f); + + tmat4x4 res(1); + res[0][0] = s.x; + res[1][0] = s.y; + res[2][0] = s.z; + res[0][1] = u.x; + res[1][1] = u.y; + res[2][1] = u.z; + res[0][2] = -f.x; + res[1][2] = -f.y; + res[2][2] = -f.z; + res[3][0] = -dot(s, eye); + res[3][1] = -dot(u, eye); + res[3][2] = dot(f, eye); + return res; +} + +#endif /* COMMON_H */ + diff --git a/src/5threed/struct/plane.h b/src/5threed/struct/plane.h new file mode 100644 index 0000000..437626c --- /dev/null +++ b/src/5threed/struct/plane.h @@ -0,0 +1,175 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +/* + * File: plane.h + * Author: Blobt + * + * Created on February 23, 2020, 11:33 AM + */ + +#ifndef PLANE_H +#define PLANE_H +#include "../common.h" + +template +class Plane { +public: + tvec3 _normal; + T _distance; +public: + + Plane() { + _normal = tvec3(0, 0, 0); + _distance = 0.0f; + } + + Plane(const Plane& right) { + _normal = right._normal; + _distance = right._distance; + } + + /** Construct a plane through a normal, and a distance to move the plane along the normal.*/ + Plane(const tvec3& rkNormal, T fConstant) { + _normal = rkNormal; + _distance = -fConstant; + } + + /** Construct a plane using the 4 constants directly **/ + Plane(T x, T y, T z, T o) { + _normal = tvec3(x, y, z); + T invLen = 1.0f / (_normal).length(); + _normal *= invLen; + _distance = o * invLen; + } + + Plane(const tvec3& rkNormal, const tvec3& rkPoint) { + redefine(rkNormal, rkPoint); + } + + Plane(const tvec3& rkPoint0, const tvec3& rkPoint1, const tvec3& rkPoint2) { + redefine(rkPoint0, rkPoint1, rkPoint2); + } + + /** + * ����ľ��� + */ + float distance(const tvec3 &pos) const { + return dot(_normal, pos) + _distance; + } + + /** The "positive side" of the plane is the half space to which the + plane normal points. The "negative side" is the other half + space. The flag "no side" indicates the plane itself. + */ + enum Side { + NO_SIDE, + POSITIVE_SIDE, + NEGATIVE_SIDE, + BOTH_SIDE + }; + + Side getSide(const tvec3& rkPoint) const { + float fDistance = getDistance(rkPoint); + + if (fDistance < 0.0) + return Plane::NEGATIVE_SIDE; + + if (fDistance > 0.0) + return Plane::POSITIVE_SIDE; + + return Plane::NO_SIDE; + } + + Side getSide(const tvec3& centre, const tvec3& halfSize) const { + // Calculate the distance between box centre and the plane + float dist = getDistance(centre); + + // Calculate the maximise allows absolute distance for + // the distance between box centre and plane + float maxAbsDist = _normal.absDot(halfSize); + + if (dist < -maxAbsDist) + return Plane::NEGATIVE_SIDE; + + if (dist > +maxAbsDist) + return Plane::POSITIVE_SIDE; + + return Plane::BOTH_SIDE; + } + + float getDistance(const tvec3& rkPoint) const { + return _normal.dot(rkPoint) + _distance; + } + + void redefine(const tvec3& rkPoint0, const tvec3& rkPoint1, + const tvec3& rkPoint2) { + tvec3 kEdge1 = rkPoint1 - rkPoint0; + tvec3 kEdge2 = rkPoint2 - rkPoint0; + _normal = cross(kEdge1, kEdge2); + _normal.normalise(); + _distance = -dot(_normal, rkPoint0); + } + + /** Redefine this plane based on a normal and a point. */ + void redefine(const tvec3& rkNormal, const tvec3& rkPoint) { + _normal = rkNormal; + _distance = -dot(rkNormal, rkPoint); + } + + + // tvec3 projectVector(const tvec3& p) const + // { + // matrix3 xform; + // xform[0][0] = 1.0f - _normal.x * _normal.x; + // xform[0][1] = -_normal.x * _normal.y; + // xform[0][2] = -_normal.x * _normal.z; + // xform[1][0] = -_normal.y * _normal.x; + // xform[1][1] = 1.0f - _normal.y * _normal.y; + // xform[1][2] = -_normal.y * _normal.z; + // xform[2][0] = -_normal.z * _normal.x; + // xform[2][1] = -_normal.z * _normal.y; + // xform[2][2] = 1.0f - _normal.z * _normal.z; + // return xform * p; + // } + + /** Normalises the plane. + @remarks + This method normalises the plane's normal and the length scale of d + is as well. + @note + This function will not crash for zero-sized vectors, but there + will be no changes made to their components. + @returns The previous length of the plane's normal. + */ + float normalise(void) { + float fLength = _normal.length(); + + // Will also work for zero-sized vectors, but will change nothing + if (fLength > 1e-08f) { + float fInvLength = 1.0f / fLength; + _normal *= fInvLength; + _distance *= fInvLength; + } + + return fLength; + } + + + + /// Comparison operator + + bool operator==(const Plane& right) const { + return (right._distance == _distance && right._normal == _normal); + } + + bool operator!=(const Plane& right) const { + return (right._distance != _distance && right._normal != _normal); + } +}; + +#endif /* PLANE_H */ + diff --git a/src/5threed/struct/tfrustum.h b/src/5threed/struct/tfrustum.h new file mode 100644 index 0000000..7f1f02e --- /dev/null +++ b/src/5threed/struct/tfrustum.h @@ -0,0 +1,87 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +/* + * File: tfrustum.h + * Author: Blobt + * + * Created on February 23, 2020, 11:22 AM + */ + +#ifndef TFRUSTUM_H +#define TFRUSTUM_H +#include "../common.h" +#include "plane.h" + +template +class tfrustum { +public: + + enum { + FRUSTUM_LEFT = 0, + FRUSTUM_RIGHT = 1, + FRUSTUM_TOP = 2, + FRUSTUM_BOTTOM = 3, + FRUSTUM_FAR = 4, + FRUSTUM_NEAR = 5, + }; +public: + + /** + * project * view + */ + void loadFrustum(const tmat4x4 &mvp) { + const T* dataPtr = mvp.data(); + _planes[FRUSTUM_LEFT ] = Plane(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]); + _planes[FRUSTUM_RIGHT ] = Plane(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]); + + _planes[FRUSTUM_TOP ] = Plane(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]); + _planes[FRUSTUM_BOTTOM] = Plane(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]); + + _planes[FRUSTUM_FAR ] = Plane(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]); + _planes[FRUSTUM_NEAR ] = Plane(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]); + } + + bool pointInFrustum(const tvec3 &pos) const { + for (int i = 0; i < 6; i++) { + if (_planes[i].distance(pos) <= 0) + return false; + } + return true; + } + + bool sphereInFrustum(const tvec3 &pos, const float radius) const { + for (int i = 0; i < 6; i++) { + if (_planes[i].distance(pos) <= -radius) + return false; + } + return true; + } + + bool cubeInFrustum(T minX, T maxX, T minY, T maxY, T minZ, T maxZ) const { + for (int i = 0; i < 6; i++) { + if (_planes[i].distance(tvec3(minX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(minX, maxY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, minY, maxZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, minZ)) > 0) continue; + if (_planes[i].distance(tvec3(maxX, maxY, maxZ)) > 0) continue; + return false; + } + return true; + } + + const Plane &getPlane(const int plane) const { + return _planes[plane]; + } +protected: + Plane _planes[6]; +}; + +#endif /* TFRUSTUM_H */ + diff --git a/src/5threed/struct/tmat4x4.h b/src/5threed/struct/tmat4x4.h index 8186a90..d080136 100644 --- a/src/5threed/struct/tmat4x4.h +++ b/src/5threed/struct/tmat4x4.h @@ -109,6 +109,10 @@ public: this->value[3] = col_type(m[3]); } + T const * data() const { + return &this->value[0][0]; + } + tmat4x4& translate(value_type x, value_type y, value_type z) { this->value[0] = col_type(1, 0, 0, 0); this->value[1] = col_type(0, 1, 0, 0); @@ -310,7 +314,7 @@ public: ); } -public: +private: col_type value[4]; }; diff --git a/src/5threed/struct/tvec3.h b/src/5threed/struct/tvec3.h index 7b7a396..68f6629 100644 --- a/src/5threed/struct/tvec3.h +++ b/src/5threed/struct/tvec3.h @@ -46,6 +46,12 @@ struct tvec3 { z = 0; } + inline tvec3(tvec3 const & v) : + x(v.x), + y(v.y), + z(v.z) { + } + tvec3(type &v) { x = v.x; y = v.y; @@ -70,9 +76,9 @@ struct tvec3 { */ template tvec3(U s) { - x = value_type(s); - y = value_type(s); - z = value_type(s); + x = value_type(s); + y = value_type(s); + z = value_type(s); } template @@ -98,6 +104,14 @@ struct tvec3 { return *this; } + template + tvec3& operator=(tvec3 const & v) { + this->x = T(v.x); + this->y = T(v.y); + this->z = T(v.z); + return *this; + } + tvec3 & operator++() { ++x; ++y; @@ -105,7 +119,54 @@ struct tvec3 { return *this; } + template + tvec3 & operator*=(U const & s) { + this->x *= T(s); + this->y *= T(s); + this->z *= T(s); + return *this; + } }; +template +tvec3 operator-(tvec3 const & v, T const & s) { + return tvec3( + v.x - T(s), + v.y - T(s), + v.z - T(s)); +} + +template +tvec3 operator-(T const & s, tvec3 const & v) { + return tvec3( + T(s) - v.x, + T(s) - v.y, + T(s) - v.z); +} + +template +tvec3 operator-(tvec3 const & v1, tvec3 const & v2) { + return tvec3( + v1.x - T(v2.x), + v1.y - T(v2.y), + v1.z - T(v2.z)); +} + +template +tvec3 operator*(tvec3 const & v, T const & s) { + return tvec3( + v.x * T(s), + v.y * T(s), + v.z * T(s)); +} + +template +tvec3 operator*(T const & s, tvec3 const & v) { + return tvec3( + T(s) * v.x, + T(s) * v.y, + T(s) * v.z); +} + #endif /* TVEC3_H */ diff --git a/src/5threed/struct/tvec4.h b/src/5threed/struct/tvec4.h index bbdb166..65d6bec 100644 --- a/src/5threed/struct/tvec4.h +++ b/src/5threed/struct/tvec4.h @@ -126,5 +126,58 @@ struct tvec4 { } }; +template +tvec4 operator+(tvec4 const & v, T const & s) { + return tvec4( + v.x + s, + v.y + s, + v.z + s, + v.w + s); +} + +template +tvec4 operator+(T const & s, tvec4 const & v) { + return tvec4( + s + v.x, + s + v.y, + s + v.z, + s + v.w); +} + +template +tvec4 operator+(tvec4 const & v1, tvec4 const & v2) { + return tvec4( + v1.x + v2.x, + v1.y + v2.y, + v1.z + v2.z, + v1.w + v2.w); +} + +template +tvec4 operator*(tvec4 const & v, T const & s) { + return tvec4( + v.x * s, + v.y * s, + v.z * s, + v.w * s); +} + +template +tvec4 operator*(T const & s, tvec4 const & v) { + return tvec4( + s * v.x, + s * v.y, + s * v.z, + s * v.w); +} + +template +tvec4 operator*(tvec4 const & v1, tvec4 const & v2) { + return tvec4( + v1.x * v2.x, + v1.y * v2.y, + v1.z * v2.z, + v1.w * v2.w); +} #endif /* TVEC4_H */ diff --git a/src/5threed/threed.cc b/src/5threed/threed.cc index 6b63b29..af7ff2f 100644 --- a/src/5threed/threed.cc +++ b/src/5threed/threed.cc @@ -9,19 +9,28 @@ using namespace std; -gint height = 50; -gint width = 50; +gint height = 600; +gint width = 800; Raster raster(width, height); struct Vertex { - float x, y; + float x, y, z; float u, v; Rgba color; }; void example1() { + Vertex vertexs[] = { + {-1.0f, 0.0f, -5.0f, 0.0f, 0.0f, Rgba(255, 0, 0, 255)}, + {0.0f, 1.0f, -5.0f, 1.0f, 1.0f, Rgba(0, 255, 0, 255)}, + {1.0f, 0.0f, -5.0f, 1.0f, 0.0f, Rgba(0, 0, 255, 255)}, + }; + raster.vertexPointer(2, DT_FLOAT, sizeof (Vertex), &vertexs[0].x); + raster.colorPointer(4, DT_BYTE, sizeof (Vertex), &vertexs[0].color); + + raster.drawArrays(DM_TRIANGES, 0, 3); } unsigned char* makeBitmap() {