From b2a5c72568ff1773d01f1f2d4ec3640bc9e476fc Mon Sep 17 00:00:00 2001 From: blobt <380255922@qq.com> Date: Tue, 25 Feb 2020 11:01:46 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=9F=A9=E9=98=B5=E9=A2=9C?= =?UTF-8?q?=E8=89=B2=E9=94=99=E4=B9=B1=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 - src/1raster/draw_triangle.cc | 4 +- src/5threed/Raster.cc | 1 + src/5threed/common.h | 5446 +--------------------------------- src/7threed/Raster.cc | 202 +- src/7threed/Raster.h | 17 +- src/7threed/threed.cc | 44 +- 7 files changed, 293 insertions(+), 5422 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e1b90c4..e28c558 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,6 @@ cmake_minimum_required (VERSION 3.10) project(3dbase) - set(CMAKE_CXX_COMPILER "g++") set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) diff --git a/src/1raster/draw_triangle.cc b/src/1raster/draw_triangle.cc index 6da94a5..d55e15b 100644 --- a/src/1raster/draw_triangle.cc +++ b/src/1raster/draw_triangle.cc @@ -81,7 +81,7 @@ void example4() { int2 p[3] = { int2(100, 150), int2(250, 250), - int2(350, 50) + int2(350, 150) }; Ege e0(p[0].x, p[0].y, p[1].x, p[1].y, Rgba(255, 0, 0, 0), Rgba(0, 255, 0, 0)); @@ -102,7 +102,7 @@ void example4() { // raster.drawEge(e1, e2); - raster.drawTriangle(p[0], p[1], p[2], Rgba(255, 0, 0, 0), Rgba(0, 255, 0, 0), Rgba(0, 0, 255, 0)); + raster.drawTriangle(p[0], p[1], p[2], Rgba(0, 0, 255,0), Rgba(0, 255, 0, 0), Rgba(255, 0, 0, 0)); } unsigned char* makeBitmap() { diff --git a/src/5threed/Raster.cc b/src/5threed/Raster.cc index e562b5b..3e499b8 100644 --- a/src/5threed/Raster.cc +++ b/src/5threed/Raster.cc @@ -451,6 +451,7 @@ float3 Raster::piplineTransform(float3 pos) { screen.x = screen.x * _viewPort.x; screen.y = screen.y * _viewPort.y; + //screen.y = _viewPort.y - (screen.y * _viewPort.y); return float3(screen.x, screen.y, screen.z); } diff --git a/src/5threed/common.h b/src/5threed/common.h index 07994db..fdf1995 100644 --- a/src/5threed/common.h +++ b/src/5threed/common.h @@ -18,41 +18,25 @@ #include #include #include -#include -#include -#include -#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)) //每个角度所对应的弧度 -#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 long long int64; -typedef unsigned short ushort; -typedef unsigned int uint; -typedef unsigned long ulong; +typedef tmat3x3 matrix3; +typedef tmat4x4 matrix4; +typedef tfrustum Frustum; template inline T tmin(T a, T b) { return a < b ? a : b; @@ -62,5376 +46,114 @@ template inline T tmax(T a, T b) { return a > b ? a : b; } -union LargeInt { - - struct __LARGE_INT { - unsigned int LowPart; - unsigned int HighPart; - } _largeInt; - int64 int64Data; -}; - -inline float unitRandom() { - return float(rand()) / float( RAND_MAX); -} - -//----------------------------------------------------------------------- +inline Rgba colorLerp(const Rgba& color1, const Rgba& color2, float step) { + Rgba ret; -inline float rangeRandom(float fLow, float fHigh) { - return (fHigh - fLow)*unitRandom() + fLow; -} + 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); -/** - * ����64λ���� - */ -inline int64 makeInt64(unsigned low, unsigned hi) { - LargeInt intdata; - intdata._largeInt.HighPart = low; - intdata._largeInt.LowPart = hi; - return intdata.int64Data; + return ret; } -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; - } +inline float2 uvLerp(const float2& uv1, const float2& uv2, float step) { - void makeFloor(tvec2 cmp) { - if (cmp.x < x) x = cmp.x; - if (cmp.y < y) y = cmp.y; + if (step < 0 || step > 1) { + printf("step : %f\n", step); + die("step must more than zero and less than 1"); } -}; - -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)); -} + float2 ret; -template -tvec2 operator/(tvec2 const & v, T const & s) { - return tvec2( - v.x / T(s), - v.y / T(s)); -} + ret.x = (float) uv1.x + (uv2.x - uv1.x) * step; + ret.y = (float) uv1.y + (uv2.y - uv1.y) * step; -template -tvec2 operator/(T const & s, tvec2 const & v) { - return tvec2( - T(s) / v.x, - T(s) / v.y); + return ret; } template -tvec2 operator/(tvec2 const & v1, tvec2 const & v2) { - return tvec2( - v1.x / T(v2.x), - v1.y / T(v2.y) +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 -tvec2 operator-(tvec2 const & v) { - return tvec2 ( - -v.x, - -v.y - ); +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 -tvec2 operator++(tvec2 const & v, int) { - return tvec2( - v.x + T(1), - v.y + T(1) - ); +T inversesqrt(T x) { + return T(1) / sqrt(x); } template -tvec2 operator--(tvec2 const & v, int) { - return tvec2( - v.x - T(1), - v.y - T(1) - ); +tvec2 normalize(tvec2 const & x) { + typename tvec2::value_type sqr = x.x * x.x + x.y * x.y; + return x * inversesqrt(sqr); } 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; +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 -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; +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 -tvec3 rotateY(tvec3 const & v, T angle) { - tvec3 res = v; - - T c = cos(T(DEG2RAD(angle))); - T s = sin(T(DEG2RAD(angle))); +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; - res.x = v.x * c + v.z * s; - res.z = -v.x * s + v.z * c; + 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 rotateZ(tvec3 const & v, T angle) { - - tvec3 res = v; - - T c = cos(DEG2RAD(angle)); - - T s = sin(DEG2RAD(angle)); +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); - res.x = v.x * c - v.y * s; - res.y = v.x * s + v.y * c; + 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; } -/** - * ���������ļн� - * ������������ 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(); - - // Divide by zero check - if (lenProduct < 1e-6f) - lenProduct = 1e-6f; - - 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; -} - -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 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)); -} - -template -tvec3 operator-(tvec3 const & v) { - return tvec3( - -v.x, - -v.y, - -v.z); -} - -template -tvec3 operator++(tvec3 const & v, int) { - return tvec3( - v.x + T(1), - v.y + T(1), - v.z + T(1)); -} - -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/7threed/Raster.cc b/src/7threed/Raster.cc index efbddd1..44195f3 100644 --- a/src/7threed/Raster.cc +++ b/src/7threed/Raster.cc @@ -42,6 +42,8 @@ Raster::Raster(int width, int height) { _defaultUvPointer.data = _defaultUvArray; _matModel = matrix4(1); + _matView = matrix4(1); + _matProj = matrix4(1); } Raster::~Raster() { @@ -185,7 +187,7 @@ void Raster::drawSpan(const Span &span, Image* image) { - setPixel(i, span.y, color); + setPixel(i, span.y, blendColor); } } @@ -224,7 +226,7 @@ void Raster::drawEge(const Ege& e1, const Ege& e2, Image* image) { } else { x2 = e2.x2 - xOffset2 * scale2; } - color2 = colorLerp(e2.color1, e2.color2, scale2); + color2 = colorLerp(e2.color2, e2.color1, scale2); uvE2 = uvLerp(e2.uv2, e2.uv1, scale2); scale2 += setp2; //setPixel(x2, y, Rgba(255,0,0)); @@ -236,7 +238,7 @@ void Raster::drawEge(const Ege& e1, const Ege& e2, Image* image) { } else { x1 = e1.x2 - xOffset1 * scale1; } - color1 = colorLerp(e1.color1, e1.color2, scale1); + color1 = colorLerp(e1.color2, e1.color1, scale1); uvE1 = uvLerp(e1.uv2, e1.uv1, scale1); scale1 += setp1; //setPixel(x1, y, Rgba(255,0,0)); @@ -263,8 +265,12 @@ void Raster::drawTriangle(Ege eges[3], Image* image) { int iShort1 = (iMax + 1) % 3; int iShort2 = (iMax + 2) % 3; + + //printf("max:%d (%d,%d) (%d,%d) (%d,%d,%d) (%d,%d,%d)\n", iMax,eges[iMax].x1, eges[iMax].y1, eges[iMax].x2, eges[iMax].y2, eges[iMax].color1._r, eges[iMax].color1._g, eges[iMax].color1._b, eges[iMax].color2._r, eges[iMax].color2._g, eges[iMax].color2._b); + //printf("sho:%d (%d,%d) (%d,%d) (%d,%d,%d) (%d,%d,%d)\n", iShort1,eges[iShort1].x1, eges[iShort1].y1, eges[iShort1].x2, eges[iShort1].y2, eges[iShort1].color1._r, eges[iShort1].color1._g, eges[iShort1].color1._b, eges[iShort1].color2._r, eges[iShort1].color2._g, eges[iShort1].color2._b); + drawEge(eges[iMax], eges[iShort1], image); - drawEge(eges[iMax], eges[iShort2], image); + //drawEge(eges[iMax], eges[iShort2], image); } void Raster::drawTriangle(const Vertex& vertex, Image* image) { @@ -320,67 +326,154 @@ 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]); 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]); posData += _positionPointer.stride; pp = (float*) posData; - float4 mp2(pp[0], pp[1], pp[2], 1); - mp2 = _matModel * mp2; + float3 mp2(pp[0], pp[1], pp[2]); - 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; - } + mp0 = mp0 * _matModel; + mp1 = mp1 * _matModel; + mp2 = mp2 * _matModel; + + if (_frust.pointInFrustum(mp0) || _frust.pointInFrustum(mp1) || _frust.pointInFrustum(mp2)) { + + mp0 = piplineTransform(mp0); + mp1 = piplineTransform(mp1); + mp2 = piplineTransform2(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), + }; - if (_uvPointer.data == 0) { - uvData = (char*) uvPointerdesc.data; + + drawTriangle(eges, _texture); + + if (_colorPointer.data == 0) { + cData = (char*) colorPointerdesc.data; + } + + if (_uvPointer.data == 0) { + uvData = (char*) uvPointerdesc.data; + } } } } +float3 Raster::piplineTransform2(float3 pos) { + float4 world(pos.x, pos.y, pos.z, 1); + + float4 screen = _matProjView * world; + if (screen.w == 0.0f) { + return false; + } + + //printf("1 %f %f %f\n",screen.x, screen.y, screen.z); + + screen.x /= screen.w; + screen.y /= screen.w; + screen.z /= screen.w; + + + + + // map to range 0 - 1 + screen.x = screen.x * 0.5f + 0.5f; + screen.y = screen.y * 0.5f + 0.5f; + screen.z = screen.z * 0.5f + 0.5f; + + // map to viewport + screen.x = screen.x * _viewPort.x; + screen.y = screen.y * _viewPort.y; + //screen.y = _viewPort.y - (screen.y * _viewPort.y); + + + + return float3(screen.x, screen.y, screen.z); +} + +float3 Raster::piplineTransform(float3 pos) { + float4 world(pos.x, pos.y, pos.z, 1); + + float4 screen = _matProjView * world; + if (screen.w == 0.0f) { + return false; + } + screen.x /= screen.w; + screen.y /= screen.w; + screen.z /= screen.w; + + // map to range 0 - 1 + screen.x = screen.x * 0.5f + 0.5f; + screen.y = screen.y * 0.5f + 0.5f; + screen.z = screen.z * 0.5f + 0.5f; + + // map to viewport + screen.x = screen.x * _viewPort.x; + screen.y = screen.y * _viewPort.y; + //screen.y = _viewPort.y - (screen.y * _viewPort.y); + + return float3(screen.x, screen.y, screen.z); +} + +void Raster::loadProjMatrix(const matrix4& mat) { + _matProj = mat; +} + +void Raster::loadProjIdentity() { + _matProj = matrix4(1); +} + +void Raster::loadViewMatrix(const matrix4& mat) { + _matView = mat; +} + +void Raster::loadViewIdentity() { + _matView = matrix4(1); +} + void Raster::loadMatrix(const matrix4& mat) { _matModel = mat; } @@ -389,6 +482,19 @@ void Raster::loadIdentity() { _matModel = matrix4(1); } +void Raster::setPerspective(float fovy, float aspect, float zNear, float zFar) { + _matProj = perspective(fovy, aspect, zNear, zFar); +} + +void Raster::lookat(float3 const & eye, float3 const & center, float3 const & up) { + _matView = lookAt(eye, center, up); +} + +void Raster::setViewPort(int x, int y, int w, int h) { + _viewPort.x = w; + _viewPort.y = h; +} + void Raster::bindTexture(Image* image) { _texture = image; } diff --git a/src/7threed/Raster.h b/src/7threed/Raster.h index 15bc58e..8e3fa03 100644 --- a/src/7threed/Raster.h +++ b/src/7threed/Raster.h @@ -139,9 +139,18 @@ public: void drawTriangle(const Vertex& vertex, Image* image); void drawTriangle(Ege eges[3], Image* image); void drawArrays(DRAWMODE pri, int start, int count); + float3 piplineTransform(float3 pos); + float3 piplineTransform2(float3 pos); void bindTexture(Image* image); + void loadViewMatrix(const matrix4 &mat); + void loadViewIdentity(); + void loadProjMatrix(const matrix4 &mat); + void loadProjIdentity(); void loadMatrix(const matrix4& mat); void loadIdentity(); + void setPerspective(float fovy, float aspect, float zNear, float zFar); + void lookat(float3 const & eye, float3 const & center, float3 const & up); + void setViewPort(int x, int y, int w, int h); int size(); void clean(); bool setPixel(int x, int y, Rgba color); @@ -152,8 +161,14 @@ private: int _height; Rgba _color; Image* _texture; - + matrix4 _matModel; + matrix4 _matView; + matrix4 _matProj; + matrix4 _matProjView; // _matProj * _matView + float2 _viewPort; + Frustum _frust; //台体 + DataElementDes _positionPointer; DataElementDes _colorPointer; diff --git a/src/7threed/threed.cc b/src/7threed/threed.cc index 7c6d1f0..9be67ea 100644 --- a/src/7threed/threed.cc +++ b/src/7threed/threed.cc @@ -20,14 +20,39 @@ struct Vertex { }; void example3() { + Vertex vertexs[] = { + {-1.0f, 0.0f, -2.0f, 0.0f, 0.0f, Rgba(255, 0, 0, 255)}, + { 0.0f, 1.0f, -9.0f, 1.0f, 1.0f, Rgba(0, 255, 0, 255)}, + { 1.0f, 0.0f, -2.0f, 1.0f, 0.0f, Rgba(0, 0, 255, 255)} + }; + + raster.loadIdentity(); + + Image* image = Image::loadFromFile("/home/blobt/Documents/dev/cpp/3dbase/image/scale.jpg"); + + + raster.bindTexture(image); + + raster.vertexPointer(2, DT_FLOAT, sizeof (Vertex), &vertexs[0].x); + raster.colorPointer(4, DT_BYTE, sizeof (Vertex), &vertexs[0].color); + raster.textureCoordPointer(2, DT_FLOAT, sizeof (Vertex), &vertexs[0].u); + + raster.drawArrays(DM_TRIANGES, 0, 3); +} + +/** + * if (!_frust.pointInFrustum(mp0) || _frust.pointInFrustum(mp1) || _frust.pointInFrustum(mp2)) { + + //mp0 = piplineTransform(mp0); + //mp1 = piplineTransform(mp1); + //mp2 = piplineTransform2(mp2); + * 保留用来测试绘制，测试前要做上述修改 + */ +void example2() { Vertex vertexs[] = { {10, 10, 0, 0.0f, 0.0f, Rgba(255, 0, 0, 255)}, {110, 110, 0, 1.0f, 1.0f, Rgba(255, 0, 0, 255)}, - {110, 10, 0, 1.0f, 0.0f, Rgba(255, 0, 0, 255)}, - - {10, 10, 0, 0.0f, 0.0f, Rgba(255, 255, 255, 255)}, - {10, 110, 0, 0.0f, 1.0f, Rgba(255, 255, 255, 255)}, - {110, 110, 0, 1.0f, 1.0f, Rgba(255, 255, 255, 255)}, + {110, 10, 0, 1.0f, 0.0f, Rgba(255, 0, 0, 255)} }; raster.loadIdentity(); @@ -41,11 +66,9 @@ void example3() { raster.colorPointer(4, DT_BYTE, sizeof (Vertex), &vertexs[0].color); raster.textureCoordPointer(2, DT_FLOAT, sizeof (Vertex), &vertexs[0].u); - - raster.drawArrays(DM_TRIANGES, 0, 6); + raster.drawArrays(DM_TRIANGES, 0, 3); } - unsigned char* makeBitmap() { raster.clean(); @@ -94,6 +117,11 @@ int main(int argc, char* argv[]) { gtk_widget_show_all(window); + + + raster.setViewPort(0, 0, width, height); + raster.setPerspective(60, (float) (width) / (float) (height), 0.1, 1000); + gtk_main(); return 0;