├── .codecov.sh ├── codecov.yml ├── .build.vcxproj ├── display.lldb ├── .gitignore ├── .github └── workflows │ └── test.yaml ├── display.natvis ├── license.md ├── readme.md ├── quat.h ├── util.h ├── mat.h ├── swizzle.h ├── vec.h └── maths.h /.codecov.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | for filename in `find . | egrep '\.cpp'`; 3 | do 4 | gcov-5 -n -o . $filename > /dev/null; 5 | done 6 | -------------------------------------------------------------------------------- /codecov.yml: -------------------------------------------------------------------------------- 1 | codecov: 2 | notify: 3 | require_ci_to_pass: no 4 | 5 | coverage: 6 | precision: 2 7 | round: down 8 | range: "70...100" 9 | 10 | status: 11 | project: yes 12 | patch: yes 13 | changes: no 14 | 15 | parsers: 16 | gcov: 17 | branch_detection: 18 | conditional: yes 19 | loop: yes 20 | method: no 21 | macro: no 22 | 23 | comment: 24 | layout: "header, diff" 25 | behavior: default 26 | require_changes: no 27 | 28 | ignore: 29 | - ".test/catch.hpp" 30 | -------------------------------------------------------------------------------- /.build.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Debug 5 | Win32 6 | 7 | 8 | 9 | 10 | Application 11 | v140 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /display.lldb: -------------------------------------------------------------------------------- 1 | type summary add --summary-string "${var.x}, ${var.y}" vec2f 2 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}" vec3f 3 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}, ${var.w}" vec4f 4 | type summary add --summary-string "${var.x}, ${var.y}" vec2d 5 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}" vec3d 6 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}, ${var.w}" vec4d 7 | type summary add --summary-string "${var.x}, ${var.y}" vec2i 8 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}" vec3i 9 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}, ${var.w}" vec4i 10 | type summary add --summary-string "${var.x}, ${var.y}" vec2u 11 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}" vec3u 12 | type summary add --summary-string "${var.x}, ${var.y}, ${var.z}, ${var.w}" vec4u 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .test/test 2 | *.gcno 3 | *.gcda 4 | 5 | # Windows image file caches 6 | Thumbs.db 7 | ehthumbs.db 8 | 9 | # Folder config file 10 | Desktop.ini 11 | 12 | # Recycle Bin used on file shares 13 | $RECYCLE.BIN/ 14 | 15 | # Windows Installer files 16 | *.cab 17 | *.msi 18 | *.msm 19 | *.msp 20 | 21 | # Windows shortcuts 22 | *.lnk 23 | 24 | # ========================= 25 | # Operating System Files 26 | # ========================= 27 | 28 | # OSX 29 | # ========================= 30 | 31 | .DS_Store 32 | .AppleDouble 33 | .LSOverride 34 | 35 | examples/*ios_files* 36 | **__pycache__** 37 | *.pyc 38 | **.idea/ 39 | tools/venv 40 | 41 | # Thumbnails 42 | ._* 43 | 44 | # Files that might appear in the root of a volume 45 | .DocumentRevisions-V100 46 | .fseventsd 47 | .Spotlight-V100 48 | .TemporaryItems 49 | .Trashes 50 | .VolumeIcon.icns 51 | 52 | # Directories potentially created on remote AFP share 53 | .AppleDB 54 | .AppleDesktop 55 | Network Trash Folder 56 | Temporary Items 57 | .apdisk -------------------------------------------------------------------------------- /.github/workflows/test.yaml: -------------------------------------------------------------------------------- 1 | name: gcc/clang/msvc 2 | on: [push] 3 | jobs: 4 | gcc: 5 | runs-on: ubuntu-latest 6 | steps: 7 | - uses: actions/checkout@v2 8 | - run: g++ --version 9 | - run: g++ --std=c++17 -Wno-braced-scalar-init .test/test.cpp -o .test/test && ./".test/test" 10 | - run: g++ --std=c++14 -Wno-braced-scalar-init .test/test.cpp -o .test/test && ./".test/test" 11 | - run: g++ --std=c++11 -Wno-braced-scalar-init -fprofile-arcs -ftest-coverage -fPIC -fno-inline -fno-inline-small-functions -fno-default-inline --coverage .test/test.cpp -o .test/test && ./".test/test" 12 | clang: 13 | runs-on: ubuntu-latest 14 | steps: 15 | - uses: actions/checkout@v2 16 | - run: clang++ --version 17 | - run: clang++ --std=c++17 -Wno-braced-scalar-init .test/test.cpp -o .test/test && ./".test/test" 18 | - run: clang++ --std=c++14 -Wno-braced-scalar-init .test/test.cpp -o .test/test && ./".test/test" 19 | - run: clang++ --std=c++11 -Wno-braced-scalar-init .test/test.cpp -o .test/test && ./".test/test" 20 | msvc: 21 | runs-on: windows-latest 22 | steps: 23 | - uses: actions/checkout@v2 24 | - uses: microsoft/setup-msbuild@v1.1 25 | - run: msbuild .build.vcxproj /p:PlatformToolset=v142 26 | - run: msbuild .build.vcxproj /p:PlatformToolset=v143 27 | -------------------------------------------------------------------------------- /display.natvis: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | {{{v[0]}, {v[1]}}} 5 | 6 | 7 | 8 | {{{v[0]}, {v[1]}, {v[2]}}} 9 | 10 | 11 | 12 | {{{v[0]}, {v[1]}, {v[2]}, {v[3]}}} 13 | 14 | 15 | 16 | {{{v[0]}, {v[1]}, {v[2]}, {v[3]}}} 17 | 18 | 19 | 20 | {{{v[0]}, {v[1]}, {v[2]}}} 21 | 22 | 23 | 24 | {{{v[0]}, {v[1]}}} 25 | 26 | 27 | 28 | {{{v[0]}, {v[1]}, {v[2]}, {v[3]}}} 29 | 30 | 31 | 32 | {{{v[0]}, {v[1]}, {v[2]}}} 33 | 34 | 35 | 36 | {{{v[0]}, {v[1]}}} 37 | 38 | 39 | 40 | {{ {v[$T3]} }} 41 | {{ {v[$T3]}, {v[$T4]} }} 42 | {{ {v[$T3]}, {v[$T4]}, {v[$T5]} }} 43 | {{ {v[$T3]}, {v[$T4]}, {v[$T5]}, {v[$T6]} }} 44 | 45 | 46 | -------------------------------------------------------------------------------- /license.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Alex Dixon 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | 24 | # SDFGen 25 | 26 | ### vec.h and util.h are heavily modified versions of the math api in sdf-gen 27 | 28 | The MIT License (MIT) 29 | 30 | Copyright (c) 2015, Christopher Batty 31 | 32 | Permission is hereby granted, free of charge, to any person obtaining a copy 33 | of this software and associated documentation files (the "Software"), to deal 34 | in the Software without restriction, including without limitation the rights 35 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 36 | copies of the Software, and to permit persons to whom the Software is 37 | furnished to do so, subject to the following conditions: 38 | 39 | The above copyright notice and this permission notice shall be included in all 40 | copies or substantial portions of the Software. 41 | 42 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 43 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 44 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 45 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 46 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 47 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 48 | SOFTWARE. 49 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | 2 | # maths 3 | 4 | [![gcc_clang](https://github.com/polymonster/maths/actions/workflows/test.yaml/badge.svg)](https://github.com/polymonster/maths/actions) 5 | [![vc2017](https://ci.appveyor.com/api/projects/status/uny5ae4bf3kp2p0m?svg=true)](https://ci.appveyor.com/project/polymonster/maths) 6 | [![codecov](https://codecov.io/gh/polymonster/maths/branch/master/graph/badge.svg)](https://codecov.io/gh/polymonster/maths) [![License: MIT](https://img.shields.io/badge/License-MIT-blue.svg)](https://opensource.org/licenses/MIT) 7 | 8 | A C++ maths library... you might find this useful for games and graphics dev, it has a lot of useful intersection, geometric test and conversion functions, vector swizzling, gjk implementation and other handy features. 9 | 10 | There is a [Live Demo](https://www.polymonster.co.uk/pmtech/examples/maths_functions.html) via WebAssembly and WebGL. 11 | 12 | ## Requirements 13 | 14 | Supported Compilers: MSVC 2017+, GCC 7.0+, Clang 6.0+, EMCC 2.0. 15 | 16 | C++11 or later is required. Tested with C++20, C++17, C++14 and C++11. 17 | 18 | ## Features 19 | 20 | The entire library is header only, add the maths directory to your include search path and simply include: 21 | 22 | ```c++ 23 | #include "maths.h" // intersection, geometric tests and conversion functions 24 | #include "util.h" // min, max, swap, smoothstep, scalar functions.. etc 25 | #include "vec.h" // vector of any dimension and type 26 | #include "mat.h" // matrix of any dimension and type 27 | #include "quat.h" // quaternion of any type 28 | 29 | void maths() { 30 | // templated by size and type 31 | Vec<3, float> float3; 32 | Vec<2, int> int3; 33 | Mat<4, 4, double> double4x4; 34 | Quat q; 35 | 36 | // common abbreviations for convenience 37 | Vec<3, float> vec3f; 38 | Vec<2, float> vec2f; 39 | Mat<3, 3, float> mat3f; 40 | // + common sizes and types for 2, 3, 4 dimensions, float, doubles, ints 41 | 42 | // quick common constructors 43 | vec3f zero = vec3f::zero(); 44 | vec3f one = vec3f::one(); 45 | vec3f red = vec3f::red(); 46 | mat4f identity = mat4f::identity(); 47 | // ... and so on 48 | 49 | // arithmetic operators 50 | vec3f result_add = zero + one; 51 | vec3f result_mul = zero * one; 52 | result_add += red; 53 | mat4f mat_result = mata * matb; 54 | // etc 55 | 56 | // overloaded functions and operations that feel lightweight and expressive 57 | vec3f norm = normalize(va); 58 | f32 dp = dot(va, vb); 59 | vec3f cp = cross(va, vb); 60 | quat q = normalize(q2); 61 | quat qd = dot(q, q); 62 | // + more 63 | 64 | // shader style funcs with scalar variants 65 | vec3f lerp = lerp(va, vb, 0.5f); 66 | f32 lerp = lerp(fa, fb, 0.75f); 67 | vec3f sat = saturate(va); 68 | quat q = slerp(qa, qb, 0.25f); 69 | // yeah! 70 | 71 | // cmath functions for vectors and swizzles 72 | vec2f v2_sin = sin(result_add.xy); 73 | vec3f v3_cos = cos(result_mul); 74 | vec2f v2_floor = floor(v2); 75 | // ... you get it! 76 | 77 | // swizzles for that shader like feeling! 78 | vec4f swizz = v.wzyx; // construct from swizzle 79 | swizz = v.xxxx; // assign from swizzle 80 | swizz.wyxz = v.xxyy; // assign swizzle to swizzle 81 | vec2f v2 = swizz.yz; // construct truncated 82 | swizz.wx = v.xy; // assign truncated 83 | swizz.xyz *= swizz2.www; // arithmetic on swizzles 84 | vec2 v2 = swizz.xy * 2.0f; // swizzle / scalar arithmetic 85 | 86 | // sometimes you may need to cast from swizzle to vec if c++ cant apply implicit casts 87 | f32 dp = dot((vec2f)swizz.xz, (vec2f)swizz.yy): 88 | } 89 | ``` 90 | 91 | ### Functions 92 | 93 | Plane Classification: `point_vs_plane, aabb_vs_plane, sphere_vs_plane, capsule_vs_plane, cone_vs_plane`. 94 | 95 | Overlaps: `sphere_vs_sphere, sphere_vs_aabb, sphere_vs_obb, aabb_vs_aabb, aabb_vs_frustum, sphere_vs_frustum, sphere_vs_capsule, capsule_vs_capsule, obb_vs_obb, aabb_vs_obb, convex_hull_vs_convex_hull, gjk_2d, gjk_3d`. 96 | 97 | Point Inside: `point_inside_aabb, point_inside_sphere, point_inside_obb, point_inside_triangle, point_inside_cone, point_inside_convex_hull, point_inside_poly, point_inside_frustum`. 98 | 99 | Closest Point: `closest_point_on_aabb, closest_point_on_line, closest_point_on_plane, closest_point_on_obb, closest_point_on_sphere, closest_point_on_ray, closest_point_on_triangle, closest_point_on_polygon, closest_point_on_convex_hull, closest_point_on_cone`. 100 | 101 | Point Distance: `point_aabb_distance, point_segment_distance, point_triangle_distance, distance_on_line, point_plane_distance, plane_distance, point_sphere_distance, point_polygon_distance, point_convex_hull_distance, point_cone_distance, point_obb_distance`. 102 | 103 | Ray / Line: `ray_vs_plane, ray_vs_triangle, ray_vs_sphere, ray_vs_line_segment, ray_vs_aabb, ray_vs_obb, ray_vs_capsule, ray_vs_cylinder, line_vs_line, line_vs_poly, shortest_line_segment_between_lines, shortest_line_segment_between_line_segments`. 104 | 105 | Shader Style Functions: `dot, cross, normalize, mag, mag2, dist, dist2, triple, vector_triple, lerp, nlerp, slerp, saturate, clamp, normalize, chebyshev_normalize, all, any, min, max, min_union, max_union, smoothstep, step, round, floor, ceil, abs, frac, trunc, exp, exp2, log, log2, sin, cos, tan, asin, acos, atan, sinh, cosh, tanh`. 106 | 107 | Graph Functions: `smooth_start, smooth_stop, impulse, cubic_pulse, exp_step, parabola, pcurve, exp_sustained_impulse, sinc, gain, almost_identity, integral_smoothstep, quad_impulse, poly_impulse`. 108 | 109 | \+ More included! 110 | 111 | ### Running Tests 112 | 113 | ```shell 114 | c++ --std=c++11 -Wno-braced-scalar-init .test/test.cpp -o .test/test && ./".test/test" 115 | ``` 116 | 117 | ### Debugger Tools 118 | 119 | There is a provided [display.natvis](https://github.com/polymonster/maths/blob/master/display.natvis) file which can be used with visual studio or vscode, this will display swizzles correctly when hovering in the debugger and prevent the huge union expansion from the swizzles. 120 | 121 | Append the contents of [display.lldb](https://github.com/polymonster/maths/blob/master/display.lldb) to your `~/.lldbinit` for improved readability in xcode or commandline lldb debugging. 122 | -------------------------------------------------------------------------------- /quat.h: -------------------------------------------------------------------------------- 1 | // quat.h 2 | // Copyright 2014 - 2020 Alex Dixon. 3 | // License: https://github.com/polymonster/maths/blob/master/license.md 4 | 5 | #pragma once 6 | 7 | #include "mat.h" 8 | #include "vec.h" 9 | 10 | #include 11 | #include 12 | 13 | template 14 | struct Quat 15 | { 16 | union 17 | { 18 | struct { 19 | T x, y, z, w; 20 | }; 21 | 22 | struct { 23 | T v[4]; 24 | }; 25 | }; 26 | 27 | Quat(); 28 | Quat(T z_theta, T y_theta, T x_theta); 29 | Quat(T x, T y, T z, T w); 30 | 31 | Quat operator*(const T& scale) const; 32 | Quat operator/(const T& scale) const; 33 | Quat operator+(const Quat& q) const; 34 | Quat operator=(const Vec<4, T>& v) const; 35 | Quat operator-() const; 36 | Quat operator*(const Quat& rhs) const; 37 | Quat& operator*=(const Quat& rhs); 38 | Quat& operator*=(const T& scale); 39 | 40 | void euler_angles(T z_theta, T y_theta, T x_theta); 41 | void axis_angle(Vec<3, T> axis, T w); 42 | void axis_angle(T lx, T ly, T lz, T lw); 43 | void axis_angle(Vec<4, T> v); 44 | void get_matrix(Mat<4, 4, T>& lmatrix); 45 | void get_matrix(Mat<4, 4, T>& lmatrix) const; 46 | void from_matrix(Mat<4, 4, T> m); 47 | Vec<3, T> to_euler() const; 48 | }; 49 | 50 | // free funcs 51 | template 52 | maths_inline T dot(const Quat& l, const Quat& r) 53 | { 54 | return l.x * r.x + l.y * r.y + l.z * r.z + l.w * r.w; 55 | } 56 | 57 | template 58 | maths_inline Quat normalize(const Quat& q) 59 | { 60 | Quat q2; 61 | T rmag = (T)1 / sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); 62 | for(size_t i = 0; i < 4; ++i) 63 | q2.v[i] = q.v[i] * rmag; 64 | return q2; 65 | } 66 | 67 | template 68 | maths_inline T mag2(const Quat& q) 69 | { 70 | return dot(q, q); 71 | } 72 | 73 | template 74 | maths_inline Quat lerp(const Quat& l, const Quat& r, T t) 75 | { 76 | return (l * ((T)1 - t) + r * t); 77 | } 78 | 79 | template 80 | maths_inline Quat nlerp(const Quat& l, const Quat& r, T t) 81 | { 82 | return normalize(lerp(l, r, t)); 83 | } 84 | 85 | template 86 | maths_inline Quat slerp(Quat q1, Quat q2, T t) 87 | { 88 | T w1, x1, y1, z1, w2, x2, y2, z2; 89 | T theta, mult1, mult2; 90 | 91 | w1 = q1.w; x1 = q1.x; y1 = q1.y; z1 = q1.z; 92 | w2 = q2.w; x2 = q2.x; y2 = q2.y; z2 = q2.z; 93 | 94 | // reverse the sign of q2 if q1.q2 < 0. 95 | if (w1*w2 + x1*x2 + y1*y2 + z1*z2 < 0) 96 | { 97 | w2 = -w2; x2 = -x2; y2 = -y2; z2 = -z2; 98 | } 99 | 100 | theta = acos(w1*w2 + x1*x2 + y1*y2 + z1*z2); 101 | 102 | constexpr T k_epsilon = (T)0.000001; 103 | if (theta > k_epsilon) 104 | { 105 | mult1 = sin( (1-t)*theta ) / sin( theta ); 106 | mult2 = sin( t*theta ) / sin( theta ); 107 | } 108 | else 109 | { 110 | mult1 = 1 - t; 111 | mult2 = t; 112 | } 113 | 114 | Quat out_quat; 115 | out_quat.w = mult1*w1 + mult2*w2; 116 | out_quat.x = mult1*x1 + mult2*x2; 117 | out_quat.y = mult1*y1 + mult2*y2; 118 | out_quat.z = mult1*z1 + mult2*z2; 119 | 120 | return out_quat; 121 | } 122 | 123 | // constructors 124 | template 125 | maths_inline Quat::Quat() 126 | { 127 | x = (T)0; 128 | y = (T)0; 129 | z = (T)0; 130 | w = (T)1; 131 | }; 132 | 133 | template 134 | maths_inline Quat::Quat(T z_theta, T y_theta, T x_theta) 135 | { 136 | euler_angles(z_theta, y_theta, x_theta); 137 | } 138 | 139 | template 140 | maths_inline Quat::Quat(T x, T y, T z, T w) 141 | { 142 | this->x = x; 143 | this->y = y; 144 | this->z = z; 145 | this->w = w; 146 | } 147 | 148 | // operators 149 | template 150 | maths_inline Quat Quat::operator*(const T& scale) const 151 | { 152 | Quat out_quat; 153 | for(size_t i = 0; i < 4; ++i) 154 | out_quat.v[i] = v[i] * scale; 155 | 156 | return out_quat; 157 | } 158 | 159 | template 160 | maths_inline Quat Quat::operator/(const T& scale) const 161 | { 162 | Quat out_quat; 163 | for(size_t i = 0; i < 4; ++i) 164 | out_quat.v[i] = v[i] / scale; 165 | 166 | return out_quat; 167 | } 168 | 169 | template 170 | maths_inline Quat Quat::operator+(const Quat& q) const 171 | { 172 | Quat out_quat; 173 | for(size_t i = 0; i < 4; ++i) 174 | out_quat.v[i] = v[i] + q.v[i]; 175 | 176 | return out_quat; 177 | } 178 | 179 | template 180 | maths_inline Quat Quat::operator=(const Vec<4, T>& _v) const 181 | { 182 | Quat out_quat; 183 | for(size_t i = 0; i < 4; ++i) 184 | out_quat.v[i] = _v[i]; 185 | 186 | return out_quat; 187 | } 188 | 189 | template 190 | maths_inline Quat Quat::operator-() const // Unary minus 191 | { 192 | Quat out_quat; 193 | for(size_t i = 0; i < 4; ++i) 194 | out_quat.v[i] = -v[i]; 195 | 196 | return out_quat; 197 | } 198 | 199 | // non commutative multiply 200 | template 201 | maths_inline Quat Quat::operator*(const Quat& rhs) const 202 | { 203 | Quat res; 204 | res.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z; 205 | res.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y; 206 | res.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.z; 207 | res.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w; 208 | 209 | return res; 210 | } 211 | 212 | template 213 | maths_inline Quat& Quat::operator*=(const Quat& rhs) 214 | { 215 | Quat res; 216 | res.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z; 217 | res.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y; 218 | res.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.z; 219 | res.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w; 220 | 221 | *this = res; 222 | return *this; 223 | } 224 | 225 | template 226 | maths_inline Quat& Quat::operator*=(const T& scale) 227 | { 228 | for(size_t i = 0; i < 4; ++i) 229 | v[i] *= scale; 230 | return *this; 231 | } 232 | 233 | // member funcs 234 | template 235 | inline void Quat::euler_angles(T z_theta, T y_theta, T x_theta) 236 | { 237 | T half_z = (T)0.5 * z_theta; 238 | T half_x = (T)0.5 * x_theta; 239 | T half_y = (T)0.5 * y_theta; 240 | 241 | T cos_z_2 = (T)cos(half_z); 242 | T cos_y_2 = (T)cos(half_y); 243 | T cos_x_2 = (T)cos(half_x); 244 | 245 | T sin_z_2 = (T)sin(half_z); 246 | T sin_y_2 = (T)sin(half_y); 247 | T sin_x_2 = (T)sin(half_x); 248 | 249 | // compute quat 250 | w = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2; 251 | x = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2; 252 | y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2; 253 | z = sin_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2; 254 | 255 | *this = normalize(*this); 256 | } 257 | 258 | template 259 | maths_inline void Quat::axis_angle(Vec<3, T> axis, T w) 260 | { 261 | axis_angle(axis.x, axis.y, axis.z, w); 262 | } 263 | 264 | template 265 | maths_inline void Quat::axis_angle(T lx, T ly, T lz, T lw) 266 | { 267 | T half_angle = lw * (T)0.5; 268 | 269 | w = (T)cos(half_angle); 270 | x = lx * (T)sin(half_angle); 271 | y = ly * (T)sin(half_angle); 272 | z = lz * (T)sin(half_angle); 273 | 274 | *this = normalize(*this); 275 | } 276 | 277 | template 278 | maths_inline void Quat::axis_angle(Vec<4, T> v) 279 | { 280 | axis_angle(v.x, v.y, v.z, v.w); 281 | } 282 | 283 | template 284 | inline void Quat::get_matrix(Mat<4, 4, T>& lmatrix) 285 | { 286 | *this = normalize(*this); 287 | 288 | static const T _0 = (T)0; 289 | static const T _1 = (T)1; 290 | static const T _2 = (T)2; 291 | 292 | lmatrix.m[0] = _1 - _2 * y * y - _2 * z * z; 293 | lmatrix.m[1] = _2 * x * y - _2 * z * w; 294 | lmatrix.m[2] = _2 * x * z + _2 * y * w; 295 | lmatrix.m[3] = _0; 296 | 297 | lmatrix.m[4] = _2 * x * y + _2 * z * w; 298 | lmatrix.m[5] = _1 - _2 * x * x - _2 * z * z; 299 | lmatrix.m[6] = _2 * y * z - _2 * x * w; 300 | lmatrix.m[7] = _0; 301 | 302 | lmatrix.m[8] = _2 * x * z - _2 * y * w; 303 | lmatrix.m[9] = _2 * y * z + _2 * x * w; 304 | lmatrix.m[10] = _1 - _2 * x * x - _2 * y * y; 305 | lmatrix.m[11] = _0; 306 | 307 | lmatrix.m[12] = _0; 308 | lmatrix.m[13] = _0; 309 | lmatrix.m[14] = _0; 310 | lmatrix.m[15] = _1; 311 | } 312 | 313 | template 314 | inline void Quat::get_matrix(Mat<4, 4, T>& lmatrix) const 315 | { 316 | auto cp = normalize(*this); 317 | cp.get_matrix(lmatrix); 318 | } 319 | 320 | template 321 | inline void Quat::from_matrix(Mat<4, 4, T> m) 322 | { 323 | // thanks! 324 | // .. https://math.stackexchange.com/questions/893984/conversion-of-rotation-matrix-to-quaternion 325 | 326 | const T& m00 = m.m[0]; 327 | const T& m01 = m.m[4]; 328 | const T& m02 = m.m[8]; 329 | const T& m10 = m.m[1]; 330 | const T& m11 = m.m[5]; 331 | const T& m12 = m.m[9]; 332 | const T& m20 = m.m[2]; 333 | const T& m21 = m.m[6]; 334 | const T& m22 = m.m[10]; 335 | 336 | T t = 0.0f; 337 | 338 | if (m22 < 0) 339 | { 340 | if (m00 > m11) 341 | { 342 | t = 1 + m00 -m11 -m22; 343 | 344 | x = t; 345 | y = m01 + m10; 346 | z = m20 + m02; 347 | w = m12 - m21; 348 | } 349 | else 350 | { 351 | t = 1 -m00 + m11 -m22; 352 | 353 | x = m01+m10; 354 | y = t; 355 | z = m12+m21; 356 | w = m20-m02; 357 | } 358 | } 359 | else 360 | { 361 | if (m00 < -m11) 362 | { 363 | t = 1 -m00 -m11 + m22; 364 | 365 | x = m20+m02; 366 | y = m12+m21; 367 | z = t; 368 | w = m01-m10; 369 | } 370 | else 371 | { 372 | t = 1 + m00 + m11 + m22; 373 | 374 | x = m12-m21; 375 | y = m20-m02; 376 | z = m01-m10; 377 | w = t; 378 | } 379 | } 380 | 381 | T srt = (T)0.5 / (T)sqrt(t); 382 | *this *= srt; 383 | } 384 | 385 | template 386 | inline Vec<3, T> Quat::to_euler() const 387 | { 388 | Vec<3, T> euler; 389 | 390 | T two = (T)2; 391 | T one = (T)1; 392 | 393 | // roll (x-axis rotation) 394 | T sinr = two * (T)(w * x + y * z); 395 | T cosr = one - two * (T)(x * x + y * y); 396 | euler.x = atan2(sinr, cosr); 397 | 398 | // pitch (y-axis rotation) 399 | T sinp = two * (w * y - z * x); 400 | if (abs(sinp) >= 1) 401 | euler.y = (T)copysign(M_PI / two, sinp); // use 90 degrees if out of range 402 | else 403 | euler.y = asin(sinp); 404 | 405 | // yaw (z-axis rotation) 406 | T siny = two * (w * z + x * y); 407 | T cosy = one - two * (y * y + z * z); 408 | euler.z = atan2(siny, cosy); 409 | 410 | return euler; 411 | } 412 | 413 | template 414 | maths_inline std::ostream& operator<<(std::ostream& out, const Quat& q) 415 | { 416 | out << q.v[0]; 417 | for (size_t i = 1; i < 4; ++i) 418 | out << ", " << q.v[i]; 419 | return out; 420 | } 421 | 422 | typedef Quat quat; 423 | typedef Quat quatf; 424 | typedef Quat quatd; 425 | -------------------------------------------------------------------------------- /util.h: -------------------------------------------------------------------------------- 1 | // util.h 2 | // Copyright 2014 - 2020 Alex Dixon. 3 | // License: https://github.com/polymonster/maths/blob/master/license.md 4 | // The initial implementation and some functions came from https://github.com/christopherbatty/SDFGen 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #if __linux__ 15 | #include 16 | #endif 17 | 18 | #define MATHS_ALWAYS_INLINE 19 | 20 | #ifndef MATHS_ALWAYS_INLINE 21 | #define maths_inline inline 22 | #else 23 | #ifdef _MSC_VER 24 | #define maths_inline __forceinline 25 | #else 26 | #define maths_inline inline __attribute__((always_inline)) 27 | #endif 28 | #endif 29 | 30 | #if defined(__GNUC__) || defined(__clang__) 31 | #define maths_deprecated __attribute__((deprecated)) 32 | #elif defined(_MSC_VER) 33 | #define maths_deprecated __declspec(deprecated) 34 | #else 35 | #pragma message("WARNING: deprecated not implemented for this compiler") 36 | #define maths_deprecated 37 | #endif 38 | 39 | #ifndef M_PI 40 | const double M_PI = 3.1415926535897932384626433832795; 41 | #endif 42 | 43 | #define F_PI 3.1415926535897932384626433832795f 44 | 45 | #ifdef WIN32 46 | #undef min 47 | #undef max 48 | #elif __linux__ 49 | #include 50 | #endif 51 | 52 | typedef uint64_t u64; 53 | typedef uint32_t u32; 54 | typedef float f32; 55 | typedef double f64; 56 | 57 | using std::max; 58 | using std::min; 59 | using std::swap; 60 | 61 | template 62 | maths_inline T sgn(T val) 63 | { 64 | return (T)(T(0) < val) - (val < T(0)); 65 | } 66 | 67 | template 68 | maths_inline T sqr(const T& x) 69 | { 70 | return x * x; 71 | } 72 | 73 | template 74 | maths_inline T cube(const T& x) 75 | { 76 | return x * x * x; 77 | } 78 | 79 | template 80 | maths_inline T rsqrt(const T& x) 81 | { 82 | return (T)1 / sqrt(x); 83 | } 84 | 85 | template 86 | maths_inline T min(T a1, T a2, T a3) 87 | { 88 | return min(a1, min(a2, a3)); 89 | } 90 | 91 | template 92 | maths_inline T min(T a1, T a2, T a3, T a4) 93 | { 94 | return min(min(a1, a2), min(a3, a4)); 95 | } 96 | 97 | template 98 | maths_inline T min(T a1, T a2, T a3, T a4, T a5) 99 | { 100 | return min(min(a1, a2), min(a3, a4), a5); 101 | } 102 | 103 | template 104 | maths_inline T min(T a1, T a2, T a3, T a4, T a5, T a6) 105 | { 106 | return min(min(a1, a2), min(a3, a4), min(a5, a6)); 107 | } 108 | 109 | template 110 | maths_inline T max(T a1, T a2, T a3) 111 | { 112 | return max(a1, max(a2, a3)); 113 | } 114 | 115 | template 116 | maths_inline T max(T a1, T a2, T a3, T a4) 117 | { 118 | return max(max(a1, a2), max(a3, a4)); 119 | } 120 | 121 | template 122 | maths_inline T max(T a1, T a2, T a3, T a4, T a5) 123 | { 124 | return max(max(a1, a2), max(a3, a4), a5); 125 | } 126 | 127 | template 128 | maths_inline T max(T a1, T a2, T a3, T a4, T a5, T a6) 129 | { 130 | return max(max(a1, a2), max(a3, a4), max(a5, a6)); 131 | } 132 | 133 | template 134 | maths_inline void minmax(T a1, T a2, T& amin, T& amax) 135 | { 136 | if (a1 < a2) 137 | { 138 | amin = a1; 139 | amax = a2; 140 | } 141 | else 142 | { 143 | amin = a2; 144 | amax = a1; 145 | } 146 | } 147 | 148 | template 149 | inline void minmax(T a1, T a2, T a3, T& amin, T& amax) 150 | { 151 | if (a1 < a2) 152 | { 153 | if (a1 < a3) 154 | { 155 | amin = a1; 156 | if (a2 < a3) 157 | amax = a3; 158 | else 159 | amax = a2; 160 | } 161 | else 162 | { 163 | amin = a3; 164 | if (a1 < a2) 165 | amax = a2; 166 | else 167 | amax = a1; 168 | } 169 | } 170 | else 171 | { 172 | if (a2 < a3) 173 | { 174 | amin = a2; 175 | if (a1 < a3) 176 | amax = a3; 177 | else 178 | amax = a1; 179 | } 180 | else 181 | { 182 | amin = a3; 183 | amax = a1; 184 | } 185 | } 186 | } 187 | 188 | template 189 | inline void minmax(T a1, T a2, T a3, T a4, T& amin, T& amax) 190 | { 191 | if (a1 < a2) 192 | { 193 | if (a3 < a4) 194 | { 195 | amin = min(a1, a3); 196 | amax = max(a2, a4); 197 | } 198 | else 199 | { 200 | amin = min(a1, a4); 201 | amax = max(a2, a3); 202 | } 203 | } 204 | else 205 | { 206 | if (a3 < a4) 207 | { 208 | amin = min(a2, a3); 209 | amax = max(a1, a4); 210 | } 211 | else 212 | { 213 | amin = min(a2, a4); 214 | amax = max(a1, a3); 215 | } 216 | } 217 | } 218 | 219 | template 220 | maths_inline void minmax(T a1, T a2, T a3, T a4, T a5, T& amin, T& amax) 221 | { 222 | amin = min(a1, a2, a3, a4, a5); 223 | amax = max(a1, a2, a3, a4, a5); 224 | } 225 | 226 | template 227 | maths_inline void minmax(T a1, T a2, T a3, T a4, T a5, T a6, T& amin, T& amax) 228 | { 229 | amin = min(a1, a2, a3, a4, a5, a6); 230 | amax = max(a1, a2, a3, a4, a5, a6); 231 | } 232 | 233 | template 234 | maths_inline void update_minmax(T a1, T& amin, T& amax) 235 | { 236 | if (a1 < amin) 237 | amin = a1; 238 | else if (a1 > amax) 239 | amax = a1; 240 | } 241 | 242 | template 243 | inline void sort(T& a, T& b, T& c) 244 | { 245 | T temp; 246 | if (a < b) 247 | { 248 | if (a < c) 249 | { 250 | if (c < b) 251 | { // a 293 | maths_inline T clamp(T a, T lower, T upper) 294 | { 295 | if (a < lower) 296 | return lower; 297 | else if (a > upper) 298 | return upper; 299 | else 300 | return a; 301 | } 302 | 303 | // only makes sense with T=float or double 304 | template 305 | maths_inline T smooth_step(T r) 306 | { 307 | if (r < 0) 308 | return 0; 309 | else if (r > 1) 310 | return 1; 311 | return r * r * r * (10 + r * (-15 + r * 6)); 312 | } 313 | 314 | template 315 | maths_inline T smooth_step(T r, T r_lower, T r_upper, T value_lower, T value_upper) 316 | { 317 | return value_lower + smooth_step((r - r_lower) / (r_upper - r_lower)) * (value_upper - value_lower); 318 | } 319 | 320 | // its like smooth step but with linear interpolation 321 | template 322 | maths_inline T linear_step(T l, T r, T v) 323 | { 324 | if (v <= l) 325 | return 0.0; 326 | 327 | if (v >= r) 328 | return 1.0; 329 | 330 | return (v - l) / (r - l); 331 | } 332 | 333 | // only makes sense with T=float or double 334 | template 335 | maths_inline T ramp(T r) 336 | { 337 | return smooth_step((r + 1) / 2) * 2 - 1; 338 | } 339 | 340 | template 341 | maths_inline T saturate(T v) 342 | { 343 | v = max(v, (T)0); 344 | v = min(v, (T)1); 345 | return v; 346 | } 347 | 348 | #ifdef WIN32__ 349 | maths_inline int lround(double x) 350 | { 351 | if (x > 0) 352 | return (x - floor(x) < 0.5) ? (int)floor(x) : (int)ceil(x); 353 | else 354 | return (x - floor(x) <= 0.5) ? (int)floor(x) : (int)ceil(x); 355 | } 356 | 357 | maths_inline double remainder(double x, double y) 358 | { 359 | return x - std::floor(x / y + 0.5) * y; 360 | } 361 | #endif 362 | 363 | maths_inline unsigned int round_up_to_power_of_two(unsigned int n) 364 | { 365 | int exponent = 0; 366 | --n; 367 | while (n) 368 | { 369 | ++exponent; 370 | n >>= 1; 371 | } 372 | return 1 << exponent; 373 | } 374 | 375 | maths_inline unsigned int round_down_to_power_of_two(unsigned int n) 376 | { 377 | int exponent = 0; 378 | while (n > 1) 379 | { 380 | ++exponent; 381 | n >>= 1; 382 | } 383 | return 1 << exponent; 384 | } 385 | 386 | // return the morton index from x,y position 387 | inline uint64_t morton_xy(uint64_t x, uint64_t y) 388 | { 389 | x = (x | (x << 16)) & 0x0000FFFF0000FFFF; 390 | x = (x | (x << 8)) & 0x00FF00FF00FF00FF; 391 | x = (x | (x << 4)) & 0x0F0F0F0F0F0F0F0F; 392 | x = (x | (x << 2)) & 0x3333333333333333; 393 | x = (x | (x << 1)) & 0x5555555555555555; 394 | 395 | y = (y | (y << 16)) & 0x0000FFFF0000FFFF; 396 | y = (y | (y << 8)) & 0x00FF00FF00FF00FF; 397 | y = (y | (y << 4)) & 0x0F0F0F0F0F0F0F0F; 398 | y = (y | (y << 2)) & 0x3333333333333333; 399 | y = (y | (y << 1)) & 0x5555555555555555; 400 | 401 | return x | (y << 1); 402 | } 403 | 404 | /// returns the morten order index from x,y,z position 405 | inline uint64_t morton_xyz(uint64_t x, uint64_t y, uint64_t z) 406 | { 407 | x = (x | (x << 16)) & 0xFFFF00000000FFFF; 408 | x = (x | (x << 8)) & 0xF00F00F00F00F; 409 | x = (x | (x << 4)) & 0x30C30C30C30C30C3; 410 | x = (x | (x << 2)) & 0x9249249249249249; 411 | 412 | y = (y | (y << 16)) & 0xFFFF00000000FFFF; 413 | y = (y | (y << 8)) & 0xF00F00F00F00F; 414 | y = (y | (y << 4)) & 0x30C30C30C30C30C3; 415 | y = (y | (y << 2)) & 0x9249249249249249; 416 | 417 | z = (z | (z << 16)) & 0xFFFF00000000FFFF; 418 | z = (z | (z << 8)) & 0xF00F00F00F00F; 419 | z = (z | (z << 4)) & 0x30C30C30C30C30C3; 420 | z = (z | (z << 2)) & 0x9249249249249249; 421 | 422 | return (x << 0) | (y << 1) | (z << 2); 423 | } 424 | 425 | 426 | // morton_1 - extract even bits value 0b010101 returns 0b111 427 | inline uint64_t morton_1(uint64_t x) 428 | { 429 | x = x & 0x5555555555555555; 430 | x = (x | (x >> 1)) & 0x3333333333333333; 431 | x = (x | (x >> 2)) & 0x0F0F0F0F0F0F0F0F; 432 | x = (x | (x >> 4)) & 0x00FF00FF00FF00FF; 433 | x = (x | (x >> 8)) & 0x0000FFFF0000FFFF; 434 | x = (x | (x >> 16)) & 0x00000000FFFFFFFF; 435 | return x; 436 | } 437 | 438 | // returns the number of bits divisible by 3. value 0b001001001 returns 0b111 439 | inline uint64_t morton_2(uint64_t x) { 440 | x = x & 0x9249249249249249; 441 | x = (x | (x >> 2)) & 0x30C30C30C30C30C3; 442 | x = (x | (x >> 4)) & 0xF00F00F00F00F; 443 | x = (x | (x >> 8)) & 0xFF0000FF0000FF; 444 | x = (x | (x >> 16)) & 0xFFFF00000000FFFF; 445 | x = (x | (x >> 32)) & 0x00000000FFFFFFFF; 446 | return x; 447 | } 448 | 449 | // returns the x,y grid position for morton order index d 450 | inline void morton_to_xy(uint64_t d, uint64_t &x, uint64_t &y) 451 | { 452 | x = morton_1(d); 453 | y = morton_1(d >> 1); 454 | } 455 | 456 | // returns the x,y,z grid position for morton order index d 457 | inline void morton_to_xyz(uint64_t d, uint64_t &x, uint64_t &y, uint64_t &z) 458 | { 459 | x = morton_2(d >> 0); 460 | y = morton_2(d >> 1); 461 | z = morton_2(d >> 2); 462 | } 463 | 464 | inline int intlog2(int x) 465 | { 466 | int exp = -1; 467 | while (x) 468 | { 469 | x >>= 1; 470 | ++exp; 471 | } 472 | return exp; 473 | } 474 | 475 | template 476 | inline void get_barycentric(T x, int& i, T& f, int i_low, int i_high) 477 | { 478 | T s = std::floor(x); 479 | i = (int)s; 480 | if (i < i_low) 481 | { 482 | i = i_low; 483 | f = 0; 484 | } 485 | else if (i > i_high - 2) 486 | { 487 | i = i_high - 2; 488 | f = 1; 489 | } 490 | else 491 | f = (T)(x - s); 492 | } 493 | 494 | template 495 | maths_inline S lerp(const S& value0, const S& value1, T f) 496 | { 497 | return (1 - f) * value0 + f * value1; 498 | } 499 | 500 | template 501 | maths_inline S bilerp(const S& v00, const S& v10, const S& v01, const S& v11, T fx, T fy) 502 | { 503 | return lerp(lerp(v00, v10, fx), lerp(v01, v11, fx), fy); 504 | } 505 | 506 | template 507 | inline S trilerp(const S& v000, const S& v100, const S& v010, const S& v110, const S& v001, const S& v101, const S& v011, 508 | const S& v111, T fx, T fy, T fz) 509 | { 510 | return lerp(bilerp(v000, v100, v010, v110, fx, fy), bilerp(v001, v101, v011, v111, fx, fy), fz); 511 | } 512 | 513 | template 514 | inline S quadlerp(const S& v0000, const S& v1000, const S& v0100, const S& v1100, const S& v0010, const S& v1010, 515 | const S& v0110, const S& v1110, const S& v0001, const S& v1001, const S& v0101, const S& v1101, 516 | const S& v0011, const S& v1011, const S& v0111, const S& v1111, T fx, T fy, T fz, T ft) 517 | { 518 | return lerp(trilerp(v0000, v1000, v0100, v1100, v0010, v1010, v0110, v1110, fx, fy, fz), 519 | trilerp(v0001, v1001, v0101, v1101, v0011, v1011, v0111, v1111, fx, fy, fz), ft); 520 | } 521 | 522 | // f should be between 0 and 1, with f=0.5 corresponding to balanced weighting between w0 and w2 523 | template 524 | inline void quadratic_bspline_weights(T f, T& w0, T& w1, T& w2) 525 | { 526 | w0 = T(0.5) * sqr(f - 1); 527 | w1 = T(0.75) - sqr(f - T(0.5)); 528 | w2 = T(0.5) * sqr(f); 529 | } 530 | 531 | // f should be between 0 and 1 532 | template 533 | inline void cubic_interp_weights(T f, T& wneg1, T& w0, T& w1, T& w2) 534 | { 535 | T f2(f * f), f3(f2 * f); 536 | wneg1 = -T(1. / 3) * f + T(1. / 2) * f2 - T(1. / 6) * f3; 537 | w0 = 1 - f2 + T(1. / 2) * (f3 - f); 538 | w1 = f + T(1. / 2) * (f2 - f3); 539 | w2 = T(1. / 6) * (f3 - f); 540 | } 541 | 542 | template 543 | inline S cubic_interp(const S& value_neg1, const S& value0, const S& value1, const S& value2, T f) 544 | { 545 | T wneg1, w0, w1, w2; 546 | cubic_interp_weights(f, wneg1, w0, w1, w2); 547 | return wneg1 * value_neg1 + w0 * value0 + w1 * value1 + w2 * value2; 548 | } 549 | 550 | template 551 | inline T map_to_range(T in_range_start, T in_range_end, T out_range_start, T out_range_end, T v) 552 | { 553 | T slope = 1.0f * (out_range_end - out_range_start) / (in_range_end - in_range_start); 554 | return out_range_start + slope * (v - in_range_start); 555 | } 556 | 557 | template 558 | inline T catmul_rom_spline(float t, T p0, T p1, T p2, T p3) 559 | { 560 | return 0.5f * ((2.0f * p1) + 561 | (p2 - p0) * t + 562 | (2.0f * p0 - 5.0f * p1 + 4.0f * p2 - p3) * (t * t) + 563 | (-1.0f * p0 + 3.0f * p1 - 3.0f * p2 + p3) * (t * t * t)); 564 | } 565 | 566 | template 567 | inline T catmul_rom_spline(float t, T p0, T p1, T p2, T p3, float alpha) 568 | { 569 | auto getT = [](float t, T p0, T p1, float alpha) -> float 570 | { 571 | T square = sqr(p1-p0); 572 | 573 | float sum = 0.0f; 574 | size_t numComponents = sizeof(p1) / sizeof(((float*)&p1)[0]); 575 | for(size_t i = 0; i < numComponents; ++i) 576 | sum += ((float*)&square)[i]; 577 | 578 | float b = sqrt(sum); 579 | float c = pow(b, alpha); 580 | return c + t; 581 | }; 582 | 583 | float t0 = 0.0f; 584 | float t1 = getT(t0, p0, p1, alpha); 585 | float t2 = getT(t1, p1, p2, alpha); 586 | float t3 = getT(t2, p2, p3, alpha); 587 | 588 | t = lerp(t1, t2, t); 589 | 590 | T a1 = (t1-t)/(t1-t0)*p0 + (t-t0)/(t1-t0)*p1; 591 | T a2 = (t2-t)/(t2-t1)*p1 + (t-t1)/(t2-t1)*p2; 592 | T a3 = (t3-t)/(t3-t2)*p2 + (t-t2)/(t3-t2)*p3; 593 | 594 | T b1 = (t2-t)/(t2-t0)*a1 + (t-t0)/(t2-t0)*a2; 595 | T b2 = (t3-t)/(t3-t1)*a2 + (t-t1)/(t3-t1)*a3; 596 | 597 | return (t2-t)/(t2-t1)*b1 + (t-t1)/(t2-t1)*b2; 598 | } 599 | 600 | template 601 | inline T catmul_rom_spline_centripital(float t, T p0, T p1, T p2, T p3) 602 | { 603 | return catmul_rom_spline(t, p0, p1, p2, p3, 0.5f); 604 | } 605 | 606 | template 607 | maths_inline T smooth_start2(T t, T b = 0.0, T c = 1.0, T d = 1.0) 608 | { 609 | t /= d; 610 | return c * t*t + b; 611 | } 612 | 613 | template 614 | maths_inline T smooth_start3(T t, T b = 0.0, T c = 1.0, T d = 1.0) 615 | { 616 | t /= d; 617 | return c * t*t*t + b; 618 | } 619 | 620 | template 621 | maths_inline T smooth_start4(T t, T b = 0.0, T c = 1.0, T d = 1.0) 622 | { 623 | t /= d; 624 | return c * t*t*t*t + b; 625 | } 626 | 627 | template 628 | maths_inline T smooth_start5(T t, T b = 0.0, T c = 1.0, T d = 1.0) 629 | { 630 | t /= d; 631 | return c * t*t*t*t*t + b; 632 | } 633 | 634 | template 635 | maths_inline T smooth_stop2(T t, T b = 0.0, T c = 1.0, T d = 1.0) 636 | { 637 | t /= d; 638 | return -c * t * (t - 2) + b; 639 | } 640 | 641 | template 642 | maths_inline T smooth_stop3(T t, T b = 0.0, T c = 1.0, T d = 1.0) 643 | { 644 | t = t / d - 1; 645 | return c * (t*t*t + (T)1) + b; 646 | } 647 | 648 | template 649 | maths_inline T smooth_stop4(T t, T b = 0.0, T c = 1.0, T d = 1.0) 650 | { 651 | t = t / d - 1; 652 | return -c * (t*t*t*t - (T)1) + b; 653 | } 654 | 655 | template 656 | maths_inline T smooth_stop5(T t, T b = 0.0, T c = 1.0, T d = 1.0) 657 | { 658 | t = t / d - 1; 659 | return c * (t*t*t*t*t + (T)1) + b; 660 | } 661 | 662 | template 663 | maths_inline T soften_towards_edge(T c, T p, T e, T r) 664 | { 665 | T pd = abs(e - p); 666 | T cd = abs(e - c); 667 | if (cd < pd) 668 | { 669 | T s = smooth_step(cd, (T)0.0, r, (T)0.0, (T)1.0); 670 | return lerp(p, c, s); 671 | } 672 | return c; 673 | } 674 | 675 | template 676 | maths_inline T soften_towards_edges(T c, T p, T e0, T e1, T r) 677 | { 678 | c = soften_towards_edge(c, p, e0, r); 679 | c = soften_towards_edge(c, p, e1, r); 680 | return c; 681 | } 682 | 683 | // thanks to inigo quilez for the following functions: https://iquilezles.org/articles/functions/ 684 | // returns an exponential impulse (y position on a graph for x); k controls the stretching of the function 685 | template 686 | maths_inline T impulse(T k, T x) 687 | { 688 | const T h = k * x; 689 | return h * exp((T)1.0 - h); 690 | } 691 | 692 | // returns a cubic pulse (y position on a graph for x); equivalent to: smoothstep(c-w,c,x)-smoothstep(c,c+w,x) 693 | template 694 | maths_inline T cubic_pulse(T c, T w, T x) 695 | { 696 | x = fabs(x - c); 697 | if (x > w) return (T)0.0; 698 | x /= w; 699 | return (T)1.0 - x * x*((T)3.0 - (T)2.0*x); 700 | } 701 | 702 | // returns an exponential step (y position on a graph for x); k is control parameter, n is power which gives sharper curves. 703 | template 704 | maths_inline T exp_step(T x, T k, T n) 705 | { 706 | return exp(-k * pow(x, n)); 707 | } 708 | 709 | // returns a parabola (y position on a graph for x); use k to control its shape 710 | template 711 | maths_inline T parabola(T x, T k) 712 | { 713 | return pow((T)4.0 * x * ((T)1.0 - x), k); 714 | } 715 | 716 | // returns a power curve (y position on a graph for x); this is a generalziation of the parabola 717 | template 718 | maths_inline T pcurve(T x, T a, T b) 719 | { 720 | T k = pow(a + b, a + b) / (pow(a, a)*pow(b, b)); 721 | return k * pow(x, a) * pow((T)1.0 - x, b); 722 | } 723 | 724 | // returns an exponential sustained impulse (y position on a graph for x); control on the width of attack with k and release with f 725 | template 726 | maths_inline T exp_sustained_impulse(T x, T f, T k) 727 | { 728 | T s = max(x-f, (T)0); 729 | return min(x*x/(f*f), (T)1 + ((T)2/f)*s*(T)exp(-k*s)); 730 | } 731 | 732 | // returns a sin curve (y position on a graph for x); can be used for some bouncing behaviors. give k different integer values to tweak the amount of bounces 733 | template 734 | maths_inline T sinc(T x, T k) 735 | { 736 | auto a = (T)M_PI * (k*x-(T)1); 737 | return std::sin(a)/a; 738 | } 739 | 740 | // returns gain (y position on a graph for x); remapping the unit interval into the unit interval by expanding the sides and compressing the center 741 | template 742 | maths_inline T gain(T x, T k) 743 | { 744 | const T a = (T)0.5*pow((T)2.0*((x<(T)0.5)?x:(T)1.0-x), k); 745 | return (x<(T)0.5)?a:(T)1.0-a; 746 | } 747 | 748 | // returns soft clipping (in a cubic fashion) of x; let m be the threshold (anything above m stays unchanged), and n the value things will take when the signal is zero 749 | template 750 | maths_inline T almost_identity(T x, T m, T n) 751 | { 752 | if(x>m) 753 | return x; 754 | const T a = (T)2.0*n - m; 755 | const T b = (T)2.0*m - (T)3.0*n; 756 | const T t = x/m; 757 | return (a*t + b)*t*t + n; 758 | } 759 | 760 | // returns the integral smoothstep of x it's derivative is never larger than 1 761 | template 762 | maths_inline T integral_smoothstep(T x, T t) 763 | { 764 | if(x>t) 765 | return x - t/(T)2.0; 766 | return x*x*x*((T)1.0-x*(T)0.5/t)/t/t; 767 | } 768 | 769 | // returns an quadratic impulse (y position on a graph for x); k controls the stretching of the function 770 | template 771 | maths_inline T quad_impulse(T k, T x) 772 | { 773 | return (T)2.0*sqrt(k)*x/((T)1.0+k*x*x); 774 | } 775 | 776 | // returns a quadratic impulse (y position on a graph for x); n is the degree of the polynomial and k controls the stretching of the function 777 | template 778 | maths_inline T poly_impulse(T k, T n, T x) 779 | { 780 | return (n/(n-(T)1.0))*pow((n-(T)1.0)*k,(T)1.0/n)*x/((T)1.0+k*pow(x,n)); 781 | } 782 | -------------------------------------------------------------------------------- /mat.h: -------------------------------------------------------------------------------- 1 | // mat.h 2 | // Copyright 2014 - 2020 Alex Dixon. 3 | // License: https://github.com/polymonster/maths/blob/master/license.md 4 | 5 | #pragma once 6 | 7 | #include "vec.h" 8 | #include // memcpy linux 9 | 10 | template 11 | struct Mat 12 | { 13 | T m[R * C]; 14 | 15 | // Constructors 16 | Mat(){}; 17 | 18 | Mat(T* data) 19 | { 20 | for (size_t i = 0; i < R * C; ++i) 21 | m[i] = data[i]; 22 | } 23 | 24 | template 25 | Mat(const Mat& other) 26 | { 27 | for (size_t r = 0; r < R; ++r) 28 | { 29 | for (size_t c = 0; c < C; ++c) 30 | { 31 | m.at(r, c) = other.at(r, c); 32 | } 33 | } 34 | } 35 | 36 | // common ctrs for initializer lists 37 | template < size_t R2 = R, size_t C2 = C, typename = typename std::enable_if< R2 == 2 && C2 == 2 >::type > 38 | Mat(T v00, T v01, 39 | T v10, T v11) 40 | { 41 | m[0] = v00; 42 | m[1] = v01; 43 | m[2] = v10; 44 | m[3] = v11; 45 | } 46 | 47 | template < size_t R2 = R, size_t C2 = C, typename = typename std::enable_if< R2 == 3 && C2 == 3 >::type > 48 | Mat(T v00, T v01, T v02, 49 | T v10, T v11, T v12, 50 | T v20, T v21, T v22) 51 | { 52 | m[0] = v00; 53 | m[1] = v01; 54 | m[2] = v02; 55 | m[3] = v10; 56 | m[4] = v11; 57 | m[5] = v12; 58 | m[6] = v20; 59 | m[7] = v21; 60 | m[8] = v22; 61 | } 62 | 63 | template < size_t R2 = R, size_t C2 = C, typename = typename std::enable_if< R2 == 4 && C2 == 4 >::type > 64 | Mat(T v00, T v01, T v02, T v03, 65 | T v10, T v11, T v12, T v13, 66 | T v20, T v21, T v22, T v23, 67 | T v30, T v31, T v32, T v33) 68 | { 69 | m[0] = v00; 70 | m[1] = v01; 71 | m[2] = v02; 72 | m[3] = v03; 73 | m[4] = v10; 74 | m[5] = v11; 75 | m[6] = v12; 76 | m[7] = v13; 77 | m[8] = v20; 78 | m[9] = v21; 79 | m[10] = v22; 80 | m[11] = v23; 81 | m[12] = v30; 82 | m[13] = v31; 83 | m[14] = v32; 84 | m[15] = v33; 85 | } 86 | 87 | static Mat create_identity(); 88 | 89 | // Operators 90 | Mat operator*(T rhs) const; 91 | Mat& operator*=(T rhs); 92 | Mat operator*(const Mat& rhs) const; 93 | Mat& operator*=(const Mat& rhs); 94 | Vec operator*(const Vec& rhs) const; 95 | T& operator()(size_t r, size_t c); 96 | const T& operator()(size_t r, size_t c) const; 97 | 98 | // Accessors 99 | T& at(size_t r, size_t c); 100 | const T& at(size_t r, size_t c) const; 101 | Vec get_row(size_t index) const; 102 | Vec get_column(size_t index) const; 103 | Vec<3, T> get_translation() const; 104 | void set_row(size_t index, const Vec& row); 105 | void set_column(size_t index, const Vec& col); 106 | void set_translation(const Vec<3, T>& t); 107 | void set_vectors(const Vec<3, T>& right, const Vec<3, T>& up, const Vec<3, T>& at, const Vec<3, T>& pos); 108 | 109 | // Computation 110 | Mat multiply(T scalar) const; 111 | Mat multiply(const Mat& rhs) const; 112 | Vec multiply(const Vec& rhs) const; 113 | Vec<4, T> transform_vector(const Vec<4, T>& v) const; 114 | Vec<3, T> transform_vector(const Vec<3, T>& v, T& w) const; 115 | Vec<3, T> transform_vector(const Vec<3, T>& v) const; 116 | Mat transposed(); 117 | void transpose(); 118 | }; 119 | 120 | // Accessor Functions 121 | template 122 | maths_inline T& Mat::at(size_t r, size_t c) 123 | { 124 | return m[r * C + c]; 125 | } 126 | 127 | template 128 | maths_inline const T& Mat::at(size_t r, size_t c) const 129 | { 130 | return m[r * C + c]; 131 | } 132 | 133 | template 134 | maths_inline Vec Mat::get_row(size_t index) const 135 | { 136 | return Vec(&m[index * C]); 137 | } 138 | 139 | template 140 | maths_inline Vec Mat::get_column(size_t index) const 141 | { 142 | Vec col; 143 | for (size_t i = 0; i < R; ++i) 144 | col[i] = at(i, index); 145 | 146 | return col; 147 | } 148 | 149 | template 150 | maths_inline Vec<3, T> Mat::get_translation() const 151 | { 152 | return Vec<3, T>(m[3], m[7], m[11]); 153 | } 154 | 155 | template 156 | maths_inline void Mat::set_row(size_t index, const Vec& row) 157 | { 158 | size_t i = index * C; 159 | memcpy(&m[i], &row.v, sizeof(T) * C); 160 | } 161 | 162 | template 163 | maths_inline void Mat::set_column(size_t index, const Vec& col) 164 | { 165 | for (size_t r = 0; r < R; ++r) 166 | at(r, index) = col[r]; 167 | } 168 | 169 | template 170 | maths_inline void Mat::set_translation(const Vec<3, T>& t) 171 | { 172 | m[3] = t.x; 173 | m[7] = t.y; 174 | m[11] = t.z; 175 | } 176 | 177 | template 178 | void Mat::set_vectors(const Vec<3, T>& right, const Vec<3, T>& up, const Vec<3, T>& at, const Vec<3, T>& pos) 179 | { 180 | set_row(0, Vec<4, T>(right, pos.x)); 181 | set_row(1, Vec<4, T>(up, pos.y)); 182 | set_row(2, Vec<4, T>(at, pos.z)); 183 | set_row(3, Vec<4, T>(0.0f, 0.0f, 0.0f, 1.0f)); 184 | } 185 | 186 | // Operators 187 | template 188 | maths_inline Vec Mat::operator*(const Vec& rhs) const 189 | { 190 | return multiply(rhs); 191 | } 192 | 193 | template 194 | maths_inline Mat Mat::operator*(const Mat& rhs) const 195 | { 196 | return multiply(rhs); 197 | } 198 | 199 | template 200 | maths_inline Mat& Mat::operator*=(const Mat& rhs) 201 | { 202 | *this = multiply(rhs); 203 | return *this; 204 | } 205 | 206 | template 207 | maths_inline Mat Mat::operator*(T rhs) const 208 | { 209 | return multiply(rhs); 210 | } 211 | 212 | template 213 | maths_inline Mat& Mat::operator*=(T rhs) 214 | { 215 | *this = multiply(rhs); 216 | return *this; 217 | } 218 | 219 | template 220 | maths_inline T& Mat::operator()(size_t r, size_t c) 221 | { 222 | return at(r, c); 223 | } 224 | 225 | template 226 | maths_inline const T& Mat::operator()(size_t r, size_t c) const 227 | { 228 | return at(r, c); 229 | } 230 | 231 | // Computation functions 232 | template 233 | inline Mat Mat::multiply(const Mat& rhs) const 234 | { 235 | Mat result; 236 | 237 | for (size_t r = 0; r < R; ++r) 238 | { 239 | for (size_t c = 0; c < C; ++c) 240 | { 241 | T& element = result.at(r, c); 242 | 243 | Vec vr = get_row(r); 244 | Vec vc = rhs.get_column(c); 245 | 246 | element = dot(vr, vc); 247 | } 248 | } 249 | 250 | return result; 251 | } 252 | 253 | template 254 | maths_inline Mat Mat::multiply(T scalar) const 255 | { 256 | Mat result; 257 | for (size_t i = 0; i < R * C; ++i) 258 | { 259 | result.m[i] = m[i] * scalar; 260 | } 261 | 262 | return result; 263 | } 264 | 265 | template 266 | maths_inline Vec Mat::multiply(const Vec& v) const 267 | { 268 | Vec result; 269 | for (size_t r = 0; r < R; ++r) 270 | { 271 | result[r] = dot(v, get_row(r)); 272 | } 273 | 274 | return result; 275 | } 276 | 277 | template 278 | maths_inline Vec<4, T> Mat::transform_vector(const Vec<4, T>& v) const 279 | { 280 | Vec<4, T> result; 281 | for (size_t r = 0; r < R; ++r) 282 | { 283 | result[r] = dot(v, get_row(r)); 284 | } 285 | 286 | return result; 287 | } 288 | 289 | template 290 | maths_inline Vec<3, T> Mat::transform_vector(const Vec<3, T>& v, T& w) const 291 | { 292 | Vec<4, T> result = Vec<4, T>(v, w); 293 | Vec<4, T> v4 = Vec<4, T>(v, w); 294 | for (size_t r = 0; r < R; ++r) 295 | { 296 | result[r] = dot(v4, get_row(r)); 297 | } 298 | 299 | w = result.w; 300 | return result.xyz; 301 | } 302 | 303 | template 304 | maths_inline Vec<3, T> Mat::transform_vector(const Vec<3, T>& v) const 305 | { 306 | Vec<4, T> result = Vec<4, T>(v, 1.0); 307 | Vec<4, T> v4 = Vec<4, T>(v, 1.0); 308 | for (size_t r = 0; r < R; ++r) 309 | { 310 | result[r] = dot(v4, get_row(r)); 311 | } 312 | 313 | return result.xyz; 314 | } 315 | 316 | template 317 | maths_inline void Mat::transpose() 318 | { 319 | Mat t = this->transposed(); 320 | *this = t; 321 | } 322 | 323 | template 324 | maths_inline Mat Mat::transposed() 325 | { 326 | Mat t; 327 | 328 | for (size_t r = 0; r < R; ++r) 329 | for (size_t c = 0; c < C; ++c) 330 | t.at(c, r) = at(r, c); 331 | 332 | return t; 333 | } 334 | 335 | template 336 | maths_inline Mat Mat::create_identity() 337 | { 338 | Mat identity; 339 | memset(&identity, 0x0, sizeof(Mat)); 340 | 341 | for (size_t r = 0; r < R; ++r) 342 | for (size_t c = 0; c < C; ++c) 343 | if (r == c) 344 | identity.at(r, c) = 1; 345 | 346 | return identity; 347 | } 348 | 349 | template 350 | std::ostream& operator<<(std::ostream& out, const Mat& m) 351 | { 352 | out << m.m[0]; 353 | for (size_t i = 1; i < R * C; ++i) 354 | out << ", " << m.m[i]; 355 | return out; 356 | } 357 | 358 | template 359 | std::ostream& operator<<(std::ostream& out, const Mat& m) 360 | { 361 | out << "(float)" << m.m[0]; 362 | for (size_t i = 1; i < R * C; ++i) 363 | out << ", " << "(float)" << m.m[i]; 364 | return out; 365 | } 366 | 367 | namespace mat 368 | { 369 | template 370 | inline T compute_determinant(const Mat<2, 2, T>& matrix_) 371 | { 372 | const T& a = matrix_(0, 0); 373 | const T& b = matrix_(0, 1); 374 | const T& c = matrix_(1, 0); 375 | const T& d = matrix_(1, 1); 376 | return a * d - b * c; 377 | } 378 | 379 | template 380 | inline T compute_determinant(const Mat<3, 3, T>& m_) 381 | { 382 | return m_(0, 0) * (m_(1, 1) * m_(2, 2) - m_(1, 2) * m_(2, 1)) + 383 | m_(0, 1) * (m_(1, 2) * m_(2, 0) - m_(1, 0) * m_(2, 2)) + 384 | m_(0, 2) * (m_(1, 0) * m_(2, 1) - m_(1, 1) * m_(2, 0)); 385 | } 386 | 387 | template 388 | inline T compute_determinant(const Mat<4, 4, T>& m) 389 | { 390 | T m00 = m(0, 0); 391 | T m10 = m(1, 0); 392 | T m20 = m(2, 0); 393 | T m30 = m(3, 0); 394 | T m01 = m(0, 1); 395 | T m11 = m(1, 1); 396 | T m21 = m(2, 1); 397 | T m31 = m(3, 1); 398 | T m02 = m(0, 2); 399 | T m12 = m(1, 2); 400 | T m22 = m(2, 2); 401 | T m32 = m(3, 2); 402 | T m03 = m(0, 3); 403 | T m13 = m(1, 3); 404 | T m23 = m(2, 3); 405 | T m33 = m(3, 3); 406 | 407 | return m03 * m12 * m21 * m30 - m02 * m13 * m21 * m30 - m03 * m11 * m22 * m30 + m01 * m13 * m22 * m30 + 408 | m02 * m11 * m23 * m30 - m01 * m12 * m23 * m30 - m03 * m12 * m20 * m31 + m02 * m13 * m20 * m31 + 409 | m03 * m10 * m22 * m31 - m00 * m13 * m22 * m31 - m02 * m10 * m23 * m31 + m00 * m12 * m23 * m31 + 410 | m03 * m11 * m20 * m32 - m01 * m13 * m20 * m32 - m03 * m10 * m21 * m32 + m00 * m13 * m21 * m32 + 411 | m01 * m10 * m23 * m32 - m00 * m11 * m23 * m32 - m02 * m11 * m20 * m33 + m01 * m12 * m20 * m33 + 412 | m02 * m10 * m21 * m33 - m00 * m12 * m21 * m33 - m01 * m10 * m22 * m33 + m00 * m11 * m22 * m33; 413 | } 414 | 415 | template 416 | Mat<4, 4, T> inverse3x3(const Mat<4, 4, T>& mat) 417 | { 418 | const T* m = &mat.m[0]; 419 | 420 | // determinant 421 | T one_over_det = (T)1 / compute_determinant(mat); 422 | 423 | Mat<4, 4, T> inverse = Mat<4, 4, T>::create_identity(); 424 | 425 | // find the adjoint matrix (transposed) and multiply by 1/det to get the inverse 426 | inverse.m[0] = (m[5] * m[10] - m[6] * m[9]) * one_over_det; 427 | inverse.m[1] = -(m[1] * m[10] - m[2] * m[9]) * one_over_det; 428 | inverse.m[2] = (m[1] * m[6] - m[2] * m[5]) * one_over_det; 429 | 430 | inverse.m[4] = -(m[4] * m[10] - m[6] * m[8]) * one_over_det; 431 | inverse.m[5] = (m[0] * m[10] - m[2] * m[8]) * one_over_det; 432 | inverse.m[6] = -(m[0] * m[6] - m[2] * m[4]) * one_over_det; 433 | 434 | inverse.m[8] = (m[4] * m[9] - m[5] * m[8]) * one_over_det; 435 | inverse.m[9] = -(m[0] * m[9] - m[1] * m[8]) * one_over_det; 436 | inverse.m[10] = (m[0] * m[5] - m[1] * m[4]) * one_over_det; 437 | 438 | return inverse; 439 | } 440 | 441 | template 442 | Mat<4, 4, T> inverse3x4(const Mat<4, 4, T>& mat) 443 | { 444 | const T* m = &mat.m[0]; 445 | 446 | // determinant 447 | T det = (m[0] * (m[5] * m[10] - m[9] * m[6])) - (m[4] * (m[1] * m[10] - m[9] * m[2])) + 448 | (m[8] * (m[1] * m[6] - m[5] * m[2])); 449 | 450 | T one_over_det = (T)1 / det; 451 | 452 | Mat<4, 4, T> inverse = Mat<4, 4, T>::create_identity(); 453 | 454 | // find the adjoint matrix (transposed) and multiply by 1/det to get the inverse 455 | inverse.m[0] = (m[5] * m[10] - m[6] * m[9]) * one_over_det; 456 | inverse.m[1] = -(m[1] * m[10] - m[2] * m[9]) * one_over_det; 457 | inverse.m[2] = (m[1] * m[6] - m[2] * m[5]) * one_over_det; 458 | 459 | inverse.m[4] = -(m[4] * m[10] - m[6] * m[8]) * one_over_det; 460 | inverse.m[5] = (m[0] * m[10] - m[2] * m[8]) * one_over_det; 461 | inverse.m[6] = -(m[0] * m[6] - m[2] * m[4]) * one_over_det; 462 | 463 | inverse.m[8] = (m[4] * m[9] - m[5] * m[8]) * one_over_det; 464 | inverse.m[9] = -(m[0] * m[9] - m[1] * m[8]) * one_over_det; 465 | inverse.m[10] = (m[0] * m[5] - m[1] * m[4]) * one_over_det; 466 | 467 | // take into account inverse the translation portion (inverse translation portion * inverse rotation) 468 | Vec<3, T> t(-m[3], -m[7], -m[11]); 469 | inverse.m[3] = t.x * inverse.m[0] + t.y * inverse.m[1] + t.z * inverse.m[2]; 470 | inverse.m[7] = t.x * inverse.m[4] + t.y * inverse.m[5] + t.z * inverse.m[6]; 471 | inverse.m[11] = t.x * inverse.m[8] + t.y * inverse.m[9] + t.z * inverse.m[10]; 472 | 473 | return inverse; 474 | } 475 | 476 | template 477 | Mat<4, 4, T> inverse4x4(const Mat<4, 4, T>& mat) 478 | { 479 | const T* m = &mat.m[0]; 480 | 481 | // laplace expansion theorum 482 | T s0 = ((m[0] * m[5]) - (m[1] * m[4])); 483 | T s1 = ((m[0] * m[6]) - (m[2] * m[4])); 484 | T s2 = ((m[0] * m[7]) - (m[3] * m[4])); 485 | T s3 = ((m[1] * m[6]) - (m[2] * m[5])); 486 | T s4 = ((m[1] * m[7]) - (m[3] * m[5])); 487 | T s5 = ((m[2] * m[7]) - (m[3] * m[6])); 488 | 489 | T c5 = ((m[10] * m[15]) - (m[11] * m[14])); 490 | T c4 = ((m[9] * m[15]) - (m[11] * m[13])); 491 | T c3 = ((m[9] * m[14]) - (m[10] * m[13])); 492 | T c2 = ((m[8] * m[15]) - (m[11] * m[12])); 493 | T c1 = ((m[8] * m[14]) - (m[10] * m[12])); 494 | T c0 = ((m[8] * m[13]) - (m[9] * m[12])); 495 | 496 | T det = (s0 * c5) - (s1 * c4) + (s2 * c3) + (s3 * c2) - (s4 * c1) + (s5 * c0); 497 | 498 | T one_over_det = (T)1 / det; 499 | 500 | Mat<4, 4, T> inverse; 501 | 502 | inverse.m[0] = +(m[5] * c5 - m[6] * c4 + m[7] * c3) * one_over_det; 503 | inverse.m[1] = -(m[1] * c5 - m[2] * c4 + m[3] * c3) * one_over_det; 504 | inverse.m[2] = +(m[13] * s5 - m[14] * s4 + m[15] * s3) * one_over_det; 505 | inverse.m[3] = -(m[9] * s5 - m[10] * s4 + m[11] * s3) * one_over_det; 506 | 507 | inverse.m[4] = -(m[4] * c5 - m[6] * c2 + m[7] * c1) * one_over_det; 508 | inverse.m[5] = +(m[0] * c5 - m[2] * c2 + m[3] * c1) * one_over_det; 509 | inverse.m[6] = -(m[12] * s5 - m[14] * s2 + m[15] * s1) * one_over_det; 510 | inverse.m[7] = +(m[8] * s5 - m[10] * s2 + m[11] * s1) * one_over_det; 511 | 512 | inverse.m[8] = +(m[4] * c4 - m[5] * c2 + m[7] * c0) * one_over_det; 513 | inverse.m[9] = -(m[0] * c4 - m[1] * c2 + m[3] * c0) * one_over_det; 514 | inverse.m[10] = +(m[12] * s4 - m[13] * s2 + m[15] * s0) * one_over_det; 515 | inverse.m[11] = -(m[8] * s4 - m[9] * s2 + m[11] * s0) * one_over_det; 516 | 517 | inverse.m[12] = -(m[4] * c3 - m[5] * c1 + m[6] * c0) * one_over_det; 518 | inverse.m[13] = +(m[0] * c3 - m[1] * c1 + m[2] * c0) * one_over_det; 519 | inverse.m[14] = -(m[12] * s3 - m[13] * s1 + m[14] * s0) * one_over_det; 520 | inverse.m[15] = +(m[8] * s3 - m[9] * s1 + m[10] * s0) * one_over_det; 521 | 522 | return inverse; 523 | } 524 | 525 | template 526 | inline Mat<4, 4, T> create_translation(const Vec<3, T>& t) 527 | { 528 | Mat<4, 4, T> m; 529 | 530 | m.m[0] = 1; 531 | m.m[1] = 0; 532 | m.m[2] = 0; 533 | m.m[3] = t.x; 534 | m.m[4] = 0; 535 | m.m[5] = 1; 536 | m.m[6] = 0; 537 | m.m[7] = t.y; 538 | m.m[8] = 0; 539 | m.m[9] = 0; 540 | m.m[10] = 1; 541 | m.m[11] = t.z; 542 | m.m[12] = 0; 543 | m.m[13] = 0; 544 | m.m[14] = 0; 545 | m.m[15] = 1; 546 | 547 | return m; 548 | } 549 | 550 | template 551 | inline Mat<4, 4, T> create_x_rotation(T theta) 552 | { 553 | Mat<4, 4, T> m; 554 | 555 | // get sin / cos theta once 556 | T theta_rad = theta; 557 | T sin_theta = sin(theta_rad); 558 | T cos_theta = cos(theta_rad); 559 | 560 | m.m[0] = 1; 561 | m.m[1] = 0; 562 | m.m[2] = 0; 563 | m.m[3] = 0; 564 | m.m[4] = 0; 565 | m.m[5] = cos_theta; 566 | m.m[6] = -sin_theta; 567 | m.m[7] = 0; 568 | m.m[8] = 0; 569 | m.m[9] = sin_theta; 570 | m.m[10] = cos_theta; 571 | m.m[11] = 0; 572 | m.m[12] = 0; 573 | m.m[13] = 0; 574 | m.m[14] = 0; 575 | m.m[15] = 1; 576 | 577 | return m; 578 | } 579 | 580 | template 581 | inline Mat<4, 4, T> create_y_rotation(T theta) 582 | { 583 | Mat<4, 4, T> m; 584 | 585 | // get sin / cos theta once 586 | T theta_rad = theta; 587 | T sin_theta = sin(theta_rad); 588 | T cos_theta = cos(theta_rad); 589 | 590 | m.m[0] = cos_theta; 591 | m.m[1] = 0; 592 | m.m[2] = sin_theta; 593 | m.m[3] = 0; 594 | m.m[4] = 0; 595 | m.m[5] = 1; 596 | m.m[6] = 0; 597 | m.m[7] = 0; 598 | m.m[8] = -sin_theta; 599 | m.m[9] = 0; 600 | m.m[10] = cos_theta; 601 | m.m[11] = 0; 602 | m.m[12] = 0; 603 | m.m[13] = 0; 604 | m.m[14] = 0; 605 | m.m[15] = 1; 606 | 607 | return m; 608 | } 609 | 610 | template 611 | inline Mat<4, 4, T> create_z_rotation(T theta) 612 | { 613 | Mat<4, 4, T> m; 614 | 615 | // get sin / cos theta once 616 | T theta_rad = theta; 617 | T sin_theta = sin(theta_rad); 618 | T cos_theta = cos(theta_rad); 619 | 620 | m.m[0] = cos_theta; 621 | m.m[1] = -sin_theta; 622 | m.m[2] = 0; 623 | m.m[3] = 0; 624 | m.m[4] = sin_theta; 625 | m.m[5] = cos_theta; 626 | m.m[6] = 0.0f; 627 | m.m[7] = 0; 628 | m.m[8] = 0; 629 | m.m[9] = 0; 630 | m.m[10] = 1; 631 | m.m[11] = 0; 632 | m.m[12] = 0; 633 | m.m[13] = 0; 634 | m.m[14] = 0; 635 | m.m[15] = 1; 636 | 637 | return m; 638 | } 639 | 640 | template 641 | inline Mat<4, 4, T> create_rotation(const Vec<3, T>& axis, T theta) 642 | { 643 | Mat<4, 4, T> m; 644 | 645 | T theta_rad = theta; 646 | T sin_theta = sin(theta_rad); 647 | T cos_theta = cos(theta_rad); 648 | T inv_cos_theta = 1 - cos(theta_rad); 649 | 650 | m.m[0] = inv_cos_theta * axis.x * axis.x + cos_theta; 651 | m.m[1] = inv_cos_theta * axis.x * axis.y - sin_theta * axis.z; 652 | m.m[2] = inv_cos_theta * axis.x * axis.z + sin_theta * axis.y; 653 | m.m[3] = 0; 654 | 655 | m.m[4] = inv_cos_theta * axis.x * axis.y + sin_theta * axis.z; 656 | m.m[5] = inv_cos_theta * axis.y * axis.y + cos_theta; 657 | m.m[6] = inv_cos_theta * axis.y * axis.z - sin_theta * axis.x; 658 | m.m[7] = 0; 659 | 660 | m.m[8] = inv_cos_theta * axis.x * axis.z - sin_theta * axis.y; 661 | m.m[9] = inv_cos_theta * axis.y * axis.z + sin_theta * axis.x; 662 | m.m[10] = inv_cos_theta * axis.z * axis.z + cos_theta; 663 | m.m[11] = 0; 664 | 665 | m.m[12] = 0; 666 | m.m[13] = 0; 667 | m.m[14] = 0; 668 | m.m[15] = 1; 669 | 670 | return m; 671 | } 672 | 673 | template 674 | inline Mat<4, 4, T> create_scale(const Vec<3, T>& s) 675 | { 676 | Mat<4, 4, T> m; 677 | 678 | m.m[0] = s.x; 679 | m.m[1] = 0; 680 | m.m[2] = 0; 681 | m.m[3] = 0; 682 | m.m[4] = 0; 683 | m.m[5] = s.y; 684 | m.m[6] = 0; 685 | m.m[7] = 0; 686 | m.m[8] = 0; 687 | m.m[9] = 0; 688 | m.m[10] = s.z; 689 | m.m[11] = 0; 690 | m.m[12] = 0; 691 | m.m[13] = 0; 692 | m.m[14] = 0; 693 | m.m[15] = 1; 694 | 695 | return m; 696 | } 697 | 698 | template 699 | inline Mat<4, 4, T> create_bias() 700 | { 701 | Mat<4, 4, T> m; 702 | 703 | static T half = 1 / 2; 704 | 705 | m.m[0] = half; 706 | m.m[1] = 0; 707 | m.m[2] = 0; 708 | m.m[3] = half; 709 | m.m[4] = 0; 710 | m.m[5] = half; 711 | m.m[6] = 0; 712 | m.m[7] = half; 713 | m.m[8] = 0; 714 | m.m[9] = 0; 715 | m.m[10] = half; 716 | m.m[11] = half; 717 | m.m[12] = 0; 718 | m.m[13] = 0; 719 | m.m[14] = 0; 720 | m.m[15] = 1; 721 | 722 | return m; 723 | } 724 | 725 | template 726 | inline Mat<4, 4, T> create_axis_swap(const Vec<3, T>& x, const Vec<3, T>& y, const Vec<3, T>& z) 727 | { 728 | Mat<4, 4, T> m; 729 | 730 | m.m[0] = x.x; 731 | m.m[1] = y.x; 732 | m.m[2] = z.x; 733 | m.m[3] = 0.0f; 734 | m.m[4] = x.y; 735 | m.m[5] = y.y; 736 | m.m[6] = z.y; 737 | m.m[7] = 0.0f; 738 | m.m[8] = x.z; 739 | m.m[9] = y.z; 740 | m.m[10] = z.z; 741 | m.m[11] = 0.0f; 742 | m.m[12] = 0.0f; 743 | m.m[13] = 0.0f; 744 | m.m[14] = 0.0f; 745 | m.m[15] = 1.0f; 746 | 747 | return m; 748 | } 749 | 750 | template 751 | inline Mat<4, 4, T> create_perspective_projection(T left, T right, T bottom, T top, T znear, T zfar) 752 | { 753 | Mat<4, 4, T> m; 754 | 755 | m.m[0] = ((T)2 * znear) / (right - left); 756 | m.m[1] = 0; 757 | m.m[2] = (right + left) / (right - left); 758 | m.m[3] = 0; 759 | m.m[4] = 0; 760 | m.m[5] = (2 * znear) / (top - bottom); 761 | m.m[6] = (top + bottom) / (top - bottom); 762 | m.m[7] = 0; 763 | m.m[8] = 0; 764 | m.m[9] = 0; 765 | m.m[10] = (-zfar - znear) / (zfar - znear); 766 | m.m[11] = (-(2 * znear) * zfar) / (zfar - znear); 767 | m.m[12] = 0; 768 | m.m[13] = 0; 769 | m.m[14] = -1; 770 | m.m[15] = 0; 771 | 772 | return m; 773 | } 774 | 775 | template 776 | inline Mat<4, 4, T> create_perspective_projection_yup(T fov, T aspect, T znear, T zfar) 777 | { 778 | T tfov = (T)tan(fov * 0.5); 779 | T right = tfov * aspect * znear; 780 | T left = -right; 781 | 782 | T top = tfov * znear; 783 | T bottom = -top; 784 | 785 | return create_perspective_projection(left, right, bottom, top, znear, zfar); 786 | } 787 | 788 | // y-is down 789 | template 790 | inline Mat<4, 4, T> create_perspective_projection(T fov, T aspect, T znear, T zfar) 791 | { 792 | T tfov = (T)tan(fov * 0.5); 793 | T right = tfov * aspect * znear; 794 | T left = -right; 795 | 796 | T bottom = tfov * znear; 797 | T top = -bottom; 798 | 799 | return create_perspective_projection(left, right, bottom, top, znear, zfar); 800 | } 801 | 802 | template 803 | inline Mat<4, 4, T> create_perspective_projection_inverse_depth(T left, T right, T bottom, T top, T znear, T zfar) 804 | { 805 | Mat<4, 4, T> m; 806 | 807 | m.m[0] = ((T)2 * znear) / (right - left); 808 | m.m[1] = 0; 809 | m.m[2] = (right + left) / (right - left); 810 | m.m[3] = 0; 811 | m.m[4] = 0; 812 | m.m[5] = (2 * znear) / (top - bottom); 813 | m.m[6] = (top + bottom) / (top - bottom); 814 | m.m[7] = 0; 815 | m.m[8] = 0; 816 | m.m[9] = 0; 817 | m.m[10] = (-znear) / (znear - zfar); 818 | m.m[11] = (-znear * zfar) / (znear - zfar); 819 | m.m[12] = 0; 820 | m.m[13] = 0; 821 | m.m[14] = -1; 822 | m.m[15] = 0; 823 | 824 | return m; 825 | } 826 | 827 | template 828 | inline Mat<4, 4, T> create_perspective_projection_yup_inverse_depth(T fov, T aspect, T znear, T zfar) 829 | { 830 | Mat<4, 4, T> m; 831 | 832 | T tfov = (T)tan(fov * 0.5); 833 | T right = tfov * aspect * znear; 834 | T left = -right; 835 | 836 | T top = tfov * znear; 837 | T bottom = -top; 838 | 839 | return create_perspective_projection_inverse_depth(left, right, bottom, top, znear, zfar); 840 | } 841 | 842 | template 843 | inline Mat<4, 4, T> create_perspective_projection_inverse_depth(T fov, T aspect, T znear, T zfar) 844 | { 845 | T tfov = (T)tan(fov * 0.5); 846 | T right = tfov * aspect * znear; 847 | T left = -right; 848 | 849 | T bottom = tfov * znear; 850 | T top = -bottom; 851 | 852 | return create_perspective_projection_inverse_depth(left, right, bottom, top, znear, zfar); 853 | } 854 | 855 | template 856 | inline Mat<4, 4, T> create_orthographic_projection(T left, T right, T bottom, T top, T znear, T zfar) 857 | { 858 | T& l = left; 859 | T& r = right; 860 | 861 | T& t = top; 862 | T& b = bottom; 863 | 864 | T& n = znear; 865 | T& f = zfar; 866 | 867 | Mat<4, 4, T> m = Mat<4, 4, T>::create_identity(); 868 | 869 | m.m[0] = (T)2 / (r - l); 870 | m.m[5] = (T)2 / (t - b); 871 | m.m[10] = (T)1 / (n - f); 872 | 873 | m.m[3] = (l + r) / (l - r); 874 | m.m[7] = (t + b) / (b - t); 875 | m.m[11] = n / (n - f); 876 | 877 | return m; 878 | } 879 | 880 | template 881 | Mat<3, 3, T> to3x3(const Mat<4, 4, T>& rhs) 882 | { 883 | Mat<3, 3, T> mm; 884 | for(size_t r = 0; r < 3; ++r) 885 | { 886 | for(size_t c = 0; c < 3; ++c) 887 | { 888 | mm.at(r, c) = rhs.at(r, c); 889 | } 890 | } 891 | return mm; 892 | } 893 | } // namespace mat 894 | 895 | // abbreviations 896 | typedef Mat<3, 3, float> Mat3f; 897 | typedef Mat<4, 4, float> Mat4f; 898 | typedef Mat<3, 4, float> Mat34f; 899 | typedef Mat<3, 3, float> mat3; 900 | typedef Mat<4, 4, float> mat4; 901 | typedef Mat<2, 2, float> mat2; 902 | typedef Mat<3, 3, float> mat3f; 903 | typedef Mat<4, 4, float> mat4f; 904 | typedef Mat<2, 2, float> mat2f; 905 | typedef Mat<4, 4, float> float4x4; 906 | typedef Mat<3, 4, float> float3x4; 907 | typedef Mat<4, 3, float> float4x3; 908 | typedef Mat<3, 3, float> float3x3; 909 | typedef Mat<2, 2, float> float2x2; 910 | -------------------------------------------------------------------------------- /swizzle.h: -------------------------------------------------------------------------------- 1 | // swizzle.h 2 | // Copyright 2014 - 2020 Alex Dixon. 3 | // License: https://github.com/polymonster/maths/blob/master/license.md 4 | 5 | #pragma once 6 | 7 | #if __cplusplus >= 201402L 8 | // c++14 onwards 9 | template 10 | constexpr size_t max_elem() { 11 | std::initializer_list l = {SW...}; 12 | return std::max(l)+1; 13 | } 14 | #define SW_TYPE_SIZE max_elem() 15 | #define SW_ASSIGN(v) v 16 | #else 17 | // c++11.. 18 | // W is the width of the swizzle, not the width of the vector, the swizzle of W=2 (.zw) would need to write to 19 | // index 2 and 3, this is ok because the swizzle is backed by a union with a vec containing T v[4] 20 | // we know the pointer to v[0] and the sizeof the struct is such that a v4 is 16 bytes... 21 | // this is undefined behaviour so proceed with caution, but has been tested and works on clang, gcc and msvc. 22 | // when writing to a swizzle in operator=, requires a cast to a T* (ie. float*) and the write, to avoid UB sanitizer warning 23 | #define SW_ASSIGN(v) &v[0] 24 | #define SW_TYPE_SIZE W 25 | #endif 26 | 27 | template 28 | struct Swizzle 29 | { 30 | // in c++14 we know the min size of the array we require for a swizzle at compile time. 31 | // in c++11 it might be possible, but I havent figured it out! so it requires a hack to work around UBSan 32 | T v[SW_TYPE_SIZE]; 33 | 34 | template 35 | Swizzle& operator=(const Swizzle& lhs) 36 | { 37 | static_assert(W == W2, "error: assigning swizzle of different dimensions"); 38 | size_t i1[] = {SW...}; 39 | size_t i2[] = {SW2...}; 40 | 41 | auto vw = SW_ASSIGN(v); 42 | auto vr = SW_ASSIGN(lhs.v); 43 | for(size_t x = 0; x < W; ++x) 44 | vw[i1[x]] = vr[i2[x]]; 45 | 46 | return *this; 47 | } 48 | 49 | template 50 | Swizzle& operator=(const Vec& lhs) 51 | { 52 | static_assert(W == N, "error: assigning vector to swizzle of different dimensions"); 53 | size_t i1[] = {SW...}; 54 | 55 | auto vw = SW_ASSIGN(v); 56 | for(size_t x = 0; x < W; ++x) 57 | vw[i1[x]] = (T)lhs.v[x]; 58 | 59 | return *this; 60 | } 61 | 62 | operator Vec () const 63 | { 64 | Vec vec; 65 | size_t i1[] = {SW...}; 66 | for(size_t x = 0; x < W; ++x) 67 | vec[x] = v[i1[x]]; 68 | return vec; 69 | } 70 | 71 | // vec 72 | template 73 | Vec operator+(const Swizzle& rhs) const 74 | { 75 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 76 | return Vec(Vec(*this) + Vec(rhs)); 77 | } 78 | 79 | template 80 | Vec operator-(const Swizzle& rhs) const 81 | { 82 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 83 | return Vec(Vec(*this) - Vec(rhs)); 84 | } 85 | 86 | template 87 | Vec operator/(const Swizzle& rhs) const 88 | { 89 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 90 | return Vec(Vec(*this) / Vec(rhs)); 91 | } 92 | 93 | template 94 | Vec operator*(const Swizzle& rhs) const 95 | { 96 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 97 | return Vec(Vec(*this) * Vec(rhs)); 98 | } 99 | 100 | // compund swizzle 101 | template 102 | Swizzle& operator+=(const Swizzle& rhs) 103 | { 104 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 105 | *this = Vec(*this) + Vec(rhs); 106 | return *this; 107 | } 108 | 109 | template 110 | Swizzle& operator-=(const Swizzle& rhs) 111 | { 112 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 113 | *this = Vec(*this) - Vec(rhs); 114 | return *this; 115 | } 116 | 117 | template 118 | Swizzle& operator/=(const Swizzle& rhs) 119 | { 120 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 121 | *this = Vec(*this) / Vec(rhs); 122 | return *this; 123 | } 124 | 125 | template 126 | Swizzle& operator*=(const Swizzle& rhs) 127 | { 128 | static_assert(W == W2, "error: performing arithmetic on swizzles of different sizes"); 129 | *this = Vec(*this) * Vec(rhs); 130 | return *this; 131 | } 132 | 133 | // scalar 134 | Vec operator+(T rhs) const 135 | { 136 | return Vec(Vec(*this) + (rhs)); 137 | } 138 | 139 | Vec operator-(T rhs) const 140 | { 141 | return Vec(Vec(*this) - (rhs)); 142 | } 143 | 144 | Vec operator/(T rhs) const 145 | { 146 | return Vec(Vec(*this) / (rhs)); 147 | } 148 | 149 | Vec operator*(T rhs) const 150 | { 151 | return Vec(Vec(*this) * (rhs)); 152 | } 153 | }; 154 | 155 | // unary minus 156 | template 157 | maths_inline Vec operator-(const Swizzle& lhs) 158 | { 159 | return -Vec(lhs); 160 | } 161 | 162 | // add 163 | template 164 | maths_inline Vec operator+(const Vec& rhs, const Swizzle& lhs) 165 | { 166 | return Vec(lhs) + Vec(rhs); 167 | } 168 | 169 | template 170 | maths_inline Vec operator+(const Swizzle& lhs, const Vec& rhs) 171 | { 172 | return Vec(lhs) + Vec(rhs); 173 | } 174 | 175 | template 176 | maths_inline Vec operator+(const Swizzle& lhs, const T& rhs) 177 | { 178 | return Vec(lhs) + rhs; 179 | } 180 | 181 | // compound add 182 | template 183 | maths_inline Swizzle& operator+=(Swizzle& lhs, const Vec& rhs) 184 | { 185 | lhs = Vec(lhs) + Vec(rhs); 186 | return lhs; 187 | } 188 | 189 | template 190 | maths_inline Vec& operator+=(Vec& lhs, const Swizzle& rhs) 191 | { 192 | lhs = Vec(lhs) + Vec(rhs); 193 | return lhs; 194 | } 195 | 196 | template 197 | maths_inline Swizzle& operator+=(Swizzle& lhs, const T& rhs) 198 | { 199 | lhs = Vec(lhs) + rhs; 200 | return lhs; 201 | } 202 | 203 | // subtract 204 | template 205 | maths_inline Vec operator-(const Swizzle& lhs, const Vec& rhs) 206 | { 207 | return Vec(lhs) - Vec(rhs); 208 | } 209 | 210 | template