From 51f056e0a9d299326b10f10a63270c81d901df21 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Fri, 2 Oct 2026 20:47:05 -0700 Subject: [PATCH] SAT benchmark (#177) --- .clang-format | 2 + benchmark/main.c | 92 + extern/imguizmo/ImGuizmo.cpp | 3537 ++++++++++++++++++++++++++++++++++ extern/imguizmo/ImGuizmo.h | 348 ++++ extern/imguizmo/LICENSE | 21 + include/box3d/collision.h | 11 + include/box3d/types.h | 17 +- samples/CMakeLists.txt | 16 +- samples/gfx/draw.c | 2 +- samples/gfx/keycodes.h | 1 + samples/main.cpp | 10 +- samples/sample.cpp | 2 +- samples/sample_benchmark.cpp | 146 ++ samples/sample_manifold.cpp | 417 ++-- shared/CMakeLists.txt | 7 + shared/sat_benchmark.c | 187 ++ shared/sat_benchmark.h | 70 + src/convex_manifold.c | 297 ++- src/hull.c | 25 +- src/physics_world.c | 281 +-- 20 files changed, 5096 insertions(+), 393 deletions(-) create mode 100644 extern/imguizmo/ImGuizmo.cpp create mode 100644 extern/imguizmo/ImGuizmo.h create mode 100644 extern/imguizmo/LICENSE create mode 100644 shared/sat_benchmark.c create mode 100644 shared/sat_benchmark.h diff --git a/.clang-format b/.clang-format index f0cdb58..e4ecbd3 100644 --- a/.clang-format +++ b/.clang-format @@ -23,6 +23,8 @@ IncludeBlocks: Regroup IncludeCategories: - Regex: '^"(box3d\/)' Priority: 2 + - Regex: '^ 0 || data->maxPlacementError > 0.01f * data->targetDistance ) + { + printf( " warning: early returns = %d, max placement error = %g\n", earlyReturnCount, + data->maxPlacementError ); + } + + DestroySatBenchmark( data ); + } + } + + printf( "\n" ); +} +#endif + static void PrintBenchmarks( const Benchmark* benchmarks, int count ) { printf( "Registered benchmarks:\n" ); @@ -143,6 +212,9 @@ static void PrintBenchmarks( const Benchmark* benchmarks, int count ) // Run benchmark 3 with 4 workers and run once. Disable continuous collision. Record the step times. // start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 -b=3 -r=1 -nc -s +// Compare GJK and SAT on hull pairs in the speculative regime. Static Box3D only. +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe --sat -r=5 + int main( int argc, char** argv ) { #ifdef TRACY_ENABLE @@ -211,6 +283,7 @@ int main( int argc, char** argv ) b3Counters counters = { 0 }; bool enableContinuous = true; bool recordStepTimes = false; + bool runSat = false; assert( maxThreadCount <= B3_MAX_WORKERS ); @@ -258,6 +331,15 @@ int main( int argc, char** argv ) { recordStepTimes = true; } + else if ( strcmp( arg, "-sat" ) == 0 || strcmp( arg, "--sat" ) == 0 ) + { +#ifdef BOX3D_INTERNAL_BENCHMARKS + runSat = true; +#else + printf( "The SAT benchmark requires static linkage\n" ); + exit( 1 ); +#endif + } else if ( strcmp( arg, "-l" ) == 0 || strcmp( arg, "--list" ) == 0 ) { PrintBenchmarks( benchmarks, benchmarkCount ); @@ -272,6 +354,7 @@ int main( int argc, char** argv ) "-r, --repeats=: number of repeats (default is 4)\n" "-nc, --no-continuous: disable continuous collision\n" "-s, --record-steps: record step times\n" + "-sat, --sat: compare GJK and SAT on hull pairs, then exit\n" "-l, --list: list the registered benchmarks\n" "-h, --help: print this help\n" ); exit( 0 ); @@ -287,6 +370,15 @@ int main( int argc, char** argv ) singleWorkerCount = b3ClampInt( singleWorkerCount, 1, maxThreadCount ); } +#ifdef BOX3D_INTERNAL_BENCHMARKS + if ( runSat ) + { + RunSatBenchmarks( runCount ); + free( profiles ); + return 0; + } +#endif + printf( "Starting benchmarks\n" ); printf( "======================================\n" ); diff --git a/extern/imguizmo/ImGuizmo.cpp b/extern/imguizmo/ImGuizmo.cpp new file mode 100644 index 0000000..01ab141 --- /dev/null +++ b/extern/imguizmo/ImGuizmo.cpp @@ -0,0 +1,3537 @@ +// https://github.com/CedricGuillemet/ImGuizmo +// v1.92.5 WIP +// +// The MIT License(MIT) +// +// Copyright(c) 2016-2026 Cedric Guillemet and contributors +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files(the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and / or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions : +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef IMGUI_DEFINE_MATH_OPERATORS +#define IMGUI_DEFINE_MATH_OPERATORS +#endif +#include "imgui.h" +#include "imgui_internal.h" +#include "ImGuizmo.h" + +#if defined(_MSC_VER) || defined(__MINGW32__) +#include +#endif +#if !defined(_MSC_VER) && !defined(__MINGW64_VERSION_MAJOR) +#define _malloca(x) alloca(x) +#define _freea(x) +#endif + +// includes patches for multiview from +// https://github.com/CedricGuillemet/ImGuizmo/issues/15 + +namespace IMGUIZMO_NAMESPACE +{ + static const float ZPI = 3.14159265358979323846f; + static const float RAD2DEG = (180.f / ZPI); + static const float DEG2RAD = (ZPI / 180.f); + const float screenRotateSize = 0.06f; + // scale a bit so translate axis do not touch when in universal + const float rotationDisplayFactor = 1.2f; + + static OPERATION operator&(OPERATION lhs, OPERATION rhs) + { + return static_cast(static_cast(lhs) & static_cast(rhs)); + } + + static bool operator!=(OPERATION lhs, int rhs) + { + return static_cast(lhs) != rhs; + } + + static bool Intersects(OPERATION lhs, OPERATION rhs) + { + return (lhs & rhs) != 0; + } + + // True if lhs contains rhs + static bool Contains(OPERATION lhs, OPERATION rhs) + { + return (lhs & rhs) == rhs; + } + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // utility and math + + void FPU_MatrixF_x_MatrixF(const float* a, const float* b, float* r) + { + r[0] = a[0] * b[0] + a[1] * b[4] + a[2] * b[8] + a[3] * b[12]; + r[1] = a[0] * b[1] + a[1] * b[5] + a[2] * b[9] + a[3] * b[13]; + r[2] = a[0] * b[2] + a[1] * b[6] + a[2] * b[10] + a[3] * b[14]; + r[3] = a[0] * b[3] + a[1] * b[7] + a[2] * b[11] + a[3] * b[15]; + + r[4] = a[4] * b[0] + a[5] * b[4] + a[6] * b[8] + a[7] * b[12]; + r[5] = a[4] * b[1] + a[5] * b[5] + a[6] * b[9] + a[7] * b[13]; + r[6] = a[4] * b[2] + a[5] * b[6] + a[6] * b[10] + a[7] * b[14]; + r[7] = a[4] * b[3] + a[5] * b[7] + a[6] * b[11] + a[7] * b[15]; + + r[8] = a[8] * b[0] + a[9] * b[4] + a[10] * b[8] + a[11] * b[12]; + r[9] = a[8] * b[1] + a[9] * b[5] + a[10] * b[9] + a[11] * b[13]; + r[10] = a[8] * b[2] + a[9] * b[6] + a[10] * b[10] + a[11] * b[14]; + r[11] = a[8] * b[3] + a[9] * b[7] + a[10] * b[11] + a[11] * b[15]; + + r[12] = a[12] * b[0] + a[13] * b[4] + a[14] * b[8] + a[15] * b[12]; + r[13] = a[12] * b[1] + a[13] * b[5] + a[14] * b[9] + a[15] * b[13]; + r[14] = a[12] * b[2] + a[13] * b[6] + a[14] * b[10] + a[15] * b[14]; + r[15] = a[12] * b[3] + a[13] * b[7] + a[14] * b[11] + a[15] * b[15]; + } + + void Frustum(float left, float right, float bottom, float top, float znear, float zfar, float* m16, bool rightHanded = true) + { + float temp, temp2, temp3, temp4; + temp = 2.0f * znear; + temp2 = right - left; + temp3 = top - bottom; + temp4 = zfar - znear; + float sign = rightHanded ? -1.0f : 1.0f; + m16[0] = temp / temp2; + m16[1] = 0.0; + m16[2] = 0.0; + m16[3] = 0.0; + m16[4] = 0.0; + m16[5] = temp / temp3; + m16[6] = 0.0; + m16[7] = 0.0; + m16[8] = (right + left) / temp2; + m16[9] = (top + bottom) / temp3; + m16[10] = sign * (zfar + znear) / temp4; + m16[11] = sign; + m16[12] = 0.0; + m16[13] = 0.0; + m16[14] = (-temp * zfar) / temp4; + m16[15] = 0.0; + } + + void Perspective(float fovyInDegrees, float aspectRatio, float znear, float zfar, float* m16, bool rightHanded = true) + { + float ymax, xmax; + ymax = znear * tanf(fovyInDegrees * DEG2RAD); + xmax = ymax * aspectRatio; + Frustum(-xmax, xmax, -ymax, ymax, znear, zfar, m16, rightHanded); + } + + void Cross(const float* a, const float* b, float* r) + { + r[0] = a[1] * b[2] - a[2] * b[1]; + r[1] = a[2] * b[0] - a[0] * b[2]; + r[2] = a[0] * b[1] - a[1] * b[0]; + } + + float Dot(const float* a, const float* b) + { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + } + + void Normalize(const float* a, float* r) + { + float il = 1.f / (sqrtf(Dot(a, a)) + FLT_EPSILON); + r[0] = a[0] * il; + r[1] = a[1] * il; + r[2] = a[2] * il; + } + + void LookAt(const float* eye, const float* at, const float* up, float* m16, bool rightHanded = true) + { + float X[3], Y[3], Z[3], tmp[3]; + + if (rightHanded) + { + tmp[0] = eye[0] - at[0]; + tmp[1] = eye[1] - at[1]; + tmp[2] = eye[2] - at[2]; + } + else + { + tmp[0] = at[0] - eye[0]; + tmp[1] = at[1] - eye[1]; + tmp[2] = at[2] - eye[2]; + } + Normalize(tmp, Z); + Normalize(up, Y); + Cross(Y, Z, tmp); + Normalize(tmp, X); + Cross(Z, X, tmp); + Normalize(tmp, Y); + + m16[0] = X[0]; + m16[1] = Y[0]; + m16[2] = Z[0]; + m16[3] = 0.0f; + m16[4] = X[1]; + m16[5] = Y[1]; + m16[6] = Z[1]; + m16[7] = 0.0f; + m16[8] = X[2]; + m16[9] = Y[2]; + m16[10] = Z[2]; + m16[11] = 0.0f; + m16[12] = -Dot(X, eye); + m16[13] = -Dot(Y, eye); + m16[14] = -Dot(Z, eye); + m16[15] = 1.0f; + } + + template T Clamp(T x, T y, T z) { return ((x < y) ? y : ((x > z) ? z : x)); } + template T max(T x, T y) { return (x > y) ? x : y; } + template T min(T x, T y) { return (x < y) ? x : y; } + template bool IsWithin(T x, T y, T z) { return (x >= y) && (x <= z); } + + struct matrix_t; + struct vec_t + { + public: + float x, y, z, w; + + vec_t() = default; + vec_t(float _x, float _y, float _z, float _w = 0.f) : x(_x), y(_y), z(_z), w(_w) {} + + void Lerp(const vec_t& v, float t) + { + x += (v.x - x) * t; + y += (v.y - y) * t; + z += (v.z - z) * t; + w += (v.w - w) * t; + } + + void Set(float v) { x = y = z = w = v; } + void Set(float _x, float _y, float _z = 0.f, float _w = 0.f) { x = _x; y = _y; z = _z; w = _w; } + + vec_t& operator -= (const vec_t& v) { x -= v.x; y -= v.y; z -= v.z; w -= v.w; return *this; } + vec_t& operator += (const vec_t& v) { x += v.x; y += v.y; z += v.z; w += v.w; return *this; } + vec_t& operator *= (const vec_t& v) { x *= v.x; y *= v.y; z *= v.z; w *= v.w; return *this; } + vec_t& operator *= (float v) { x *= v; y *= v; z *= v; w *= v; return *this; } + + vec_t operator * (float f) const; + vec_t operator - () const; + vec_t operator - (const vec_t& v) const; + vec_t operator + (const vec_t& v) const; + vec_t operator * (const vec_t& v) const; + + const vec_t& operator + () const { return (*this); } + float Length() const { return sqrtf(x * x + y * y + z * z); }; + float LengthSq() const { return (x * x + y * y + z * z); }; + vec_t Normalize() { (*this) *= (1.f / ( Length() > FLT_EPSILON ? Length() : FLT_EPSILON ) ); return (*this); } + vec_t Normalize(const vec_t& v) { this->Set(v.x, v.y, v.z, v.w); this->Normalize(); return (*this); } + vec_t Abs() const; + + void Cross(const vec_t& v) + { + vec_t res; + res.x = y * v.z - z * v.y; + res.y = z * v.x - x * v.z; + res.z = x * v.y - y * v.x; + + x = res.x; + y = res.y; + z = res.z; + w = 0.f; + } + + void Cross(const vec_t& v1, const vec_t& v2) + { + x = v1.y * v2.z - v1.z * v2.y; + y = v1.z * v2.x - v1.x * v2.z; + z = v1.x * v2.y - v1.y * v2.x; + w = 0.f; + } + + float Dot(const vec_t& v) const + { + return (x * v.x) + (y * v.y) + (z * v.z) + (w * v.w); + } + + float Dot3(const vec_t& v) const + { + return (x * v.x) + (y * v.y) + (z * v.z); + } + + void Transform(const matrix_t& matrix); + void Transform(const vec_t& s, const matrix_t& matrix); + + void TransformVector(const matrix_t& matrix); + void TransformPoint(const matrix_t& matrix); + void TransformVector(const vec_t& v, const matrix_t& matrix) { (*this) = v; this->TransformVector(matrix); } + void TransformPoint(const vec_t& v, const matrix_t& matrix) { (*this) = v; this->TransformPoint(matrix); } + + float& operator [] (size_t index) { return ((float*)&x)[index]; } + const float& operator [] (size_t index) const { return ((float*)&x)[index]; } + bool operator!=(const vec_t& other) const { return memcmp(this, &other, sizeof(vec_t)) != 0; } + }; + + vec_t makeVect(float _x, float _y, float _z = 0.f, float _w = 0.f) { vec_t res; res.x = _x; res.y = _y; res.z = _z; res.w = _w; return res; } + vec_t makeVect(ImVec2 v) { vec_t res; res.x = v.x; res.y = v.y; res.z = 0.f; res.w = 0.f; return res; } + vec_t vec_t::operator * (float f) const { return makeVect(x * f, y * f, z * f, w * f); } + vec_t vec_t::operator - () const { return makeVect(-x, -y, -z, -w); } + vec_t vec_t::operator - (const vec_t& v) const { return makeVect(x - v.x, y - v.y, z - v.z, w - v.w); } + vec_t vec_t::operator + (const vec_t& v) const { return makeVect(x + v.x, y + v.y, z + v.z, w + v.w); } + vec_t vec_t::operator * (const vec_t& v) const { return makeVect(x * v.x, y * v.y, z * v.z, w * v.w); } + vec_t vec_t::Abs() const { return makeVect(fabsf(x), fabsf(y), fabsf(z)); } + + vec_t Normalized(const vec_t& v) { vec_t res; res = v; res.Normalize(); return res; } + vec_t Cross(const vec_t& v1, const vec_t& v2) + { + vec_t res; + res.x = v1.y * v2.z - v1.z * v2.y; + res.y = v1.z * v2.x - v1.x * v2.z; + res.z = v1.x * v2.y - v1.y * v2.x; + res.w = 0.f; + return res; + } + + float Dot(const vec_t& v1, const vec_t& v2) + { + return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z); + } + + vec_t BuildPlan(const vec_t& p_point1, const vec_t& p_normal) + { + vec_t normal, res; + normal.Normalize(p_normal); + res.w = normal.Dot(p_point1); + res.x = normal.x; + res.y = normal.y; + res.z = normal.z; + return res; + } + + struct matrix_t + { + public: + + union + { + float m[4][4]; + float m16[16]; + struct + { + vec_t right, up, dir, position; + } v; + vec_t component[4]; + }; + + operator float* () { return m16; } + operator const float* () const { return m16; } + void Translation(float _x, float _y, float _z) { this->Translation(makeVect(_x, _y, _z)); } + + void Translation(const vec_t& vt) + { + v.right.Set(1.f, 0.f, 0.f, 0.f); + v.up.Set(0.f, 1.f, 0.f, 0.f); + v.dir.Set(0.f, 0.f, 1.f, 0.f); + v.position.Set(vt.x, vt.y, vt.z, 1.f); + } + + void Scale(float _x, float _y, float _z) + { + v.right.Set(_x, 0.f, 0.f, 0.f); + v.up.Set(0.f, _y, 0.f, 0.f); + v.dir.Set(0.f, 0.f, _z, 0.f); + v.position.Set(0.f, 0.f, 0.f, 1.f); + } + void Scale(const vec_t& s) { Scale(s.x, s.y, s.z); } + + matrix_t& operator *= (const matrix_t& mat) + { + matrix_t tmpMat; + tmpMat = *this; + tmpMat.Multiply(mat); + *this = tmpMat; + return *this; + } + matrix_t operator * (const matrix_t& mat) const + { + matrix_t matT; + matT.Multiply(*this, mat); + return matT; + } + + void Multiply(const matrix_t& matrix) + { + matrix_t tmp; + tmp = *this; + + FPU_MatrixF_x_MatrixF((float*)&tmp, (float*)&matrix, (float*)this); + } + + void Multiply(const matrix_t& m1, const matrix_t& m2) + { + FPU_MatrixF_x_MatrixF((float*)&m1, (float*)&m2, (float*)this); + } + + float GetDeterminant() const + { + return m[0][0] * m[1][1] * m[2][2] + m[0][1] * m[1][2] * m[2][0] + m[0][2] * m[1][0] * m[2][1] - + m[0][2] * m[1][1] * m[2][0] - m[0][1] * m[1][0] * m[2][2] - m[0][0] * m[1][2] * m[2][1]; + } + + float Inverse(const matrix_t& srcMatrix, bool affine = false); + void SetToIdentity() + { + v.right.Set(1.f, 0.f, 0.f, 0.f); + v.up.Set(0.f, 1.f, 0.f, 0.f); + v.dir.Set(0.f, 0.f, 1.f, 0.f); + v.position.Set(0.f, 0.f, 0.f, 1.f); + } + void Transpose() + { + matrix_t tmpm; + for (int l = 0; l < 4; l++) + { + for (int c = 0; c < 4; c++) + { + tmpm.m[l][c] = m[c][l]; + } + } + (*this) = tmpm; + } + + void RotationAxis(const vec_t& axis, float angle); + + void OrthoNormalize() + { + v.right.Normalize(); + v.up.Normalize(); + v.dir.Normalize(); + } + }; + + void vec_t::Transform(const matrix_t& matrix) + { + vec_t out; + + out.x = x * matrix.m[0][0] + y * matrix.m[1][0] + z * matrix.m[2][0] + w * matrix.m[3][0]; + out.y = x * matrix.m[0][1] + y * matrix.m[1][1] + z * matrix.m[2][1] + w * matrix.m[3][1]; + out.z = x * matrix.m[0][2] + y * matrix.m[1][2] + z * matrix.m[2][2] + w * matrix.m[3][2]; + out.w = x * matrix.m[0][3] + y * matrix.m[1][3] + z * matrix.m[2][3] + w * matrix.m[3][3]; + + x = out.x; + y = out.y; + z = out.z; + w = out.w; + } + + void vec_t::Transform(const vec_t& s, const matrix_t& matrix) + { + *this = s; + Transform(matrix); + } + + void vec_t::TransformPoint(const matrix_t& matrix) + { + vec_t out; + + out.x = x * matrix.m[0][0] + y * matrix.m[1][0] + z * matrix.m[2][0] + matrix.m[3][0]; + out.y = x * matrix.m[0][1] + y * matrix.m[1][1] + z * matrix.m[2][1] + matrix.m[3][1]; + out.z = x * matrix.m[0][2] + y * matrix.m[1][2] + z * matrix.m[2][2] + matrix.m[3][2]; + out.w = x * matrix.m[0][3] + y * matrix.m[1][3] + z * matrix.m[2][3] + matrix.m[3][3]; + + x = out.x; + y = out.y; + z = out.z; + w = out.w; + } + + void vec_t::TransformVector(const matrix_t& matrix) + { + vec_t out; + + out.x = x * matrix.m[0][0] + y * matrix.m[1][0] + z * matrix.m[2][0]; + out.y = x * matrix.m[0][1] + y * matrix.m[1][1] + z * matrix.m[2][1]; + out.z = x * matrix.m[0][2] + y * matrix.m[1][2] + z * matrix.m[2][2]; + out.w = x * matrix.m[0][3] + y * matrix.m[1][3] + z * matrix.m[2][3]; + + x = out.x; + y = out.y; + z = out.z; + w = out.w; + } + + float matrix_t::Inverse(const matrix_t& srcMatrix, bool affine) + { + float det = 0; + + if (affine) + { + det = GetDeterminant(); + float s = 1 / det; + m[0][0] = (srcMatrix.m[1][1] * srcMatrix.m[2][2] - srcMatrix.m[1][2] * srcMatrix.m[2][1]) * s; + m[0][1] = (srcMatrix.m[2][1] * srcMatrix.m[0][2] - srcMatrix.m[2][2] * srcMatrix.m[0][1]) * s; + m[0][2] = (srcMatrix.m[0][1] * srcMatrix.m[1][2] - srcMatrix.m[0][2] * srcMatrix.m[1][1]) * s; + m[1][0] = (srcMatrix.m[1][2] * srcMatrix.m[2][0] - srcMatrix.m[1][0] * srcMatrix.m[2][2]) * s; + m[1][1] = (srcMatrix.m[2][2] * srcMatrix.m[0][0] - srcMatrix.m[2][0] * srcMatrix.m[0][2]) * s; + m[1][2] = (srcMatrix.m[0][2] * srcMatrix.m[1][0] - srcMatrix.m[0][0] * srcMatrix.m[1][2]) * s; + m[2][0] = (srcMatrix.m[1][0] * srcMatrix.m[2][1] - srcMatrix.m[1][1] * srcMatrix.m[2][0]) * s; + m[2][1] = (srcMatrix.m[2][0] * srcMatrix.m[0][1] - srcMatrix.m[2][1] * srcMatrix.m[0][0]) * s; + m[2][2] = (srcMatrix.m[0][0] * srcMatrix.m[1][1] - srcMatrix.m[0][1] * srcMatrix.m[1][0]) * s; + m[3][0] = -(m[0][0] * srcMatrix.m[3][0] + m[1][0] * srcMatrix.m[3][1] + m[2][0] * srcMatrix.m[3][2]); + m[3][1] = -(m[0][1] * srcMatrix.m[3][0] + m[1][1] * srcMatrix.m[3][1] + m[2][1] * srcMatrix.m[3][2]); + m[3][2] = -(m[0][2] * srcMatrix.m[3][0] + m[1][2] * srcMatrix.m[3][1] + m[2][2] * srcMatrix.m[3][2]); + } + else + { + // transpose matrix + float src[16]; + for (int i = 0; i < 4; ++i) + { + src[i] = srcMatrix.m16[i * 4]; + src[i + 4] = srcMatrix.m16[i * 4 + 1]; + src[i + 8] = srcMatrix.m16[i * 4 + 2]; + src[i + 12] = srcMatrix.m16[i * 4 + 3]; + } + + // calculate pairs for first 8 elements (cofactors) + float tmp[12]; // temp array for pairs + tmp[0] = src[10] * src[15]; + tmp[1] = src[11] * src[14]; + tmp[2] = src[9] * src[15]; + tmp[3] = src[11] * src[13]; + tmp[4] = src[9] * src[14]; + tmp[5] = src[10] * src[13]; + tmp[6] = src[8] * src[15]; + tmp[7] = src[11] * src[12]; + tmp[8] = src[8] * src[14]; + tmp[9] = src[10] * src[12]; + tmp[10] = src[8] * src[13]; + tmp[11] = src[9] * src[12]; + + // calculate first 8 elements (cofactors) + m16[0] = (tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7]) - (tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7]); + m16[1] = (tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7]) - (tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7]); + m16[2] = (tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7]) - (tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7]); + m16[3] = (tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6]) - (tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6]); + m16[4] = (tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3]) - (tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3]); + m16[5] = (tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3]) - (tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3]); + m16[6] = (tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3]) - (tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3]); + m16[7] = (tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2]) - (tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2]); + + // calculate pairs for second 8 elements (cofactors) + tmp[0] = src[2] * src[7]; + tmp[1] = src[3] * src[6]; + tmp[2] = src[1] * src[7]; + tmp[3] = src[3] * src[5]; + tmp[4] = src[1] * src[6]; + tmp[5] = src[2] * src[5]; + tmp[6] = src[0] * src[7]; + tmp[7] = src[3] * src[4]; + tmp[8] = src[0] * src[6]; + tmp[9] = src[2] * src[4]; + tmp[10] = src[0] * src[5]; + tmp[11] = src[1] * src[4]; + + // calculate second 8 elements (cofactors) + m16[8] = (tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15]) - (tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15]); + m16[9] = (tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15]) - (tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15]); + m16[10] = (tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15]) - (tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15]); + m16[11] = (tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14]) - (tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14]); + m16[12] = (tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9]) - (tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10]); + m16[13] = (tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10]) - (tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8]); + m16[14] = (tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8]) - (tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9]); + m16[15] = (tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9]) - (tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8]); + + // calculate determinant + det = src[0] * m16[0] + src[1] * m16[1] + src[2] * m16[2] + src[3] * m16[3]; + + // calculate matrix inverse + float invdet = 1 / det; + for (int j = 0; j < 16; ++j) + { + m16[j] *= invdet; + } + } + + return det; + } + + void matrix_t::RotationAxis(const vec_t& axis, float angle) + { + float length2 = axis.LengthSq(); + if (length2 < FLT_EPSILON) + { + SetToIdentity(); + return; + } + + vec_t n = axis * (1.f / sqrtf(length2)); + float s = sinf(angle); + float c = cosf(angle); + float k = 1.f - c; + + float xx = n.x * n.x * k + c; + float yy = n.y * n.y * k + c; + float zz = n.z * n.z * k + c; + float xy = n.x * n.y * k; + float yz = n.y * n.z * k; + float zx = n.z * n.x * k; + float xs = n.x * s; + float ys = n.y * s; + float zs = n.z * s; + + m[0][0] = xx; + m[0][1] = xy + zs; + m[0][2] = zx - ys; + m[0][3] = 0.f; + m[1][0] = xy - zs; + m[1][1] = yy; + m[1][2] = yz + xs; + m[1][3] = 0.f; + m[2][0] = zx + ys; + m[2][1] = yz - xs; + m[2][2] = zz; + m[2][3] = 0.f; + m[3][0] = 0.f; + m[3][1] = 0.f; + m[3][2] = 0.f; + m[3][3] = 1.f; + } + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // + + static bool IsTranslateType(MOVETYPE type) + { + return type >= MT_MOVE_X && type <= MT_MOVE_SCREEN; + } + + static bool IsRotateType(MOVETYPE type) + { + return type >= MT_ROTATE_X && type <= MT_ROTATE_SCREEN; + } + + static bool IsScaleType(MOVETYPE type) + { + return type >= MT_SCALE_X && type <= MT_SCALE_XYZ; + } + + // Matches MT_MOVE_AB order + static const OPERATION TRANSLATE_PLANS[3] = { TRANSLATE_Y | TRANSLATE_Z, TRANSLATE_X | TRANSLATE_Z, TRANSLATE_X | TRANSLATE_Y }; + + Style::Style() + { + // default values + TranslationLineThickness = 3.0f; + TranslationLineArrowSize = 6.0f; + RotationLineThickness = 2.0f; + RotationOuterLineThickness = 3.0f; + ScaleLineThickness = 3.0f; + ScaleLineCircleSize = 6.0f; + HatchedAxisLineThickness = 6.0f; + CenterCircleSize = 6.0f; + + // initialize default colors + Colors[DIRECTION_X] = ImVec4(0.666f, 0.000f, 0.000f, 1.000f); + Colors[DIRECTION_Y] = ImVec4(0.000f, 0.666f, 0.000f, 1.000f); + Colors[DIRECTION_Z] = ImVec4(0.000f, 0.000f, 0.666f, 1.000f); + Colors[PLANE_X] = ImVec4(0.666f, 0.000f, 0.000f, 0.380f); + Colors[PLANE_Y] = ImVec4(0.000f, 0.666f, 0.000f, 0.380f); + Colors[PLANE_Z] = ImVec4(0.000f, 0.000f, 0.666f, 0.380f); + Colors[SELECTION] = ImVec4(1.000f, 0.500f, 0.062f, 0.541f); + Colors[INACTIVE] = ImVec4(0.600f, 0.600f, 0.600f, 0.600f); + Colors[TRANSLATION_LINE] = ImVec4(0.666f, 0.666f, 0.666f, 0.666f); + Colors[SCALE_LINE] = ImVec4(0.250f, 0.250f, 0.250f, 1.000f); + Colors[ROTATION_USING_BORDER] = ImVec4(1.000f, 0.500f, 0.062f, 1.000f); + Colors[ROTATION_USING_FILL] = ImVec4(1.000f, 0.500f, 0.062f, 0.500f); + Colors[HATCHED_AXIS_LINES] = ImVec4(0.000f, 0.000f, 0.000f, 0.500f); + Colors[TEXT] = ImVec4(1.000f, 1.000f, 1.000f, 1.000f); + Colors[TEXT_SHADOW] = ImVec4(0.000f, 0.000f, 0.000f, 1.000f); + } + + // Per-id state of a ViewManipulate widget, so several view cubes (one per viewport) + // can be manipulated independently through the PushID/PopID mechanism. + struct ViewManipulateState + { + ImGuiID mID = (ImGuiID)-1; + bool mIsDragging = false; + bool mIsClicking = false; + int mInterpolationFrames = 0; + vec_t mInterpolationUp; + vec_t mInterpolationDir; + }; + + struct TripodState + { + bool mBelowAxisLimit[3] = {}; + bool mBelowPlaneLimit[3] = {}; + float mAxisFactor[3] = { 1.f, 1.f, 1.f }; + }; + + struct Context + { + Context() : mbUsing(false), mbUsingViewManipulate(false), mbEnable(true), mIsViewManipulatorHovered(false), mbUsingBounds(false) + { + } + + ImDrawList* mDrawList; + Style mStyle; + + MODE mMode; + matrix_t mViewMat; + matrix_t mProjectionMat; + matrix_t mModel; + matrix_t mModelLocal; // orthonormalized model + matrix_t mModelInverse; + matrix_t mModelSource; + matrix_t mModelSourceInverse; + matrix_t mMVP; + matrix_t mMVPLocal; // MVP with full model matrix whereas mMVP's model matrix might only be translation in case of World space edition + matrix_t mViewProjection; + + vec_t mModelScaleOrigin; + vec_t mCameraEye; + vec_t mCameraRight; + vec_t mCameraDir; + vec_t mCameraUp; + vec_t mRayOrigin; + vec_t mRayVector; + + float mRadiusSquareCenter; + ImVec2 mScreenSquareCenter; + ImVec2 mScreenSquareMin; + ImVec2 mScreenSquareMax; + + float mScreenFactor; + vec_t mRelativeOrigin; + + bool mbUsing; + bool mbUsingViewManipulate; + bool mbEnable; + bool mbMouseOver; + bool mIsViewManipulatorHovered; + + // translation + vec_t mTranslationPlan; + vec_t mTranslationPlanOrigin; + vec_t mMatrixOrigin; + vec_t mTranslationLastDelta; + + // rotation + vec_t mRotationVectorSource; + float mRotationAngle; + float mRotationAngleOrigin; + //vec_t mWorldToLocalAxis; + + // scale + vec_t mScale; + vec_t mScaleValueOrigin; + vec_t mScaleLast; + float mSaveMousePosx; + + int mAxisMask = 0; + // Current camera-dependent state and the frozen state of the active drag. + TripodState mTripodState; + TripodState mActiveTripodState; + + float mAxisLimit=0.0025f; + float mPlaneLimit=0.02f; + + // bounds stretching + vec_t mBoundsPivot; + vec_t mBoundsAnchor; + vec_t mBoundsPlan; + vec_t mBoundsLocalPivot; + int mBoundsBestAxis; + int mBoundsAxis[2]; + bool mbUsingBounds; + matrix_t mBoundsMatrix; + + // + MOVETYPE mCurrentHandleType = MT_NONE; + MOVETYPE mHoveredHandleType = MT_NONE; + + float mX = 0.f; + float mY = 0.f; + float mWidth = 0.f; + float mHeight = 0.f; + float mXMax = 0.f; + float mYMax = 0.f; + float mDisplayRatio = 1.f; + + bool mIsOrthographic = false; + // check to not have multiple gizmo highlighted at the same time + bool mbOverGizmoHotspot = false; + // snapshot of mbOverGizmoHotspot from the previous complete frame, so IsOver() + // reports hovering over any gizmo regardless of when it is called + bool mbOverGizmoHotspotLastFrame = false; + + ImGuiWindow* mAlternativeWindow = nullptr; + ImVector mIDStack; + ImGuiID mEditingID = -1; + OPERATION mOperation = OPERATION(0); + + // per-id ViewManipulate widget states (see ViewManipulateState) + ImVector mViewManipulateStates; + + bool mAllowAxisFlip = true; + float mGizmoSizeClipSpace = 0.1f; + + inline ImGuiID GetCurrentID() + { + if (mIDStack.empty()) + { + mIDStack.push_back(-1); + } + return mIDStack.back(); + } + + inline TripodState& GetTripodState() + { + return mbUsing && GetCurrentID() == mEditingID ? mActiveTripodState : mTripodState; + } + + // Retrieve (creating if needed) the ViewManipulate state bound to the current id. + inline ViewManipulateState& GetViewManipulateState() + { + const ImGuiID id = GetCurrentID(); + for (int i = 0; i < mViewManipulateStates.Size; i++) + { + if (mViewManipulateStates[i].mID == id) + { + return mViewManipulateStates[i]; + } + } + ViewManipulateState state; + state.mID = id; + mViewManipulateStates.push_back(state); + return mViewManipulateStates.back(); + } + }; + + static Context gContext; + + static const vec_t directionUnary[3] = { makeVect(1.f, 0.f, 0.f), makeVect(0.f, 1.f, 0.f), makeVect(0.f, 0.f, 1.f) }; + static const char* translationInfoMask[] = { "X : %5.3f", "Y : %5.3f", "Z : %5.3f", + "Y : %5.3f Z : %5.3f", "X : %5.3f Z : %5.3f", "X : %5.3f Y : %5.3f", + "X : %5.3f Y : %5.3f Z : %5.3f" }; + static const char* scaleInfoMask[] = { "X : %5.2f", "Y : %5.2f", "Z : %5.2f", "XYZ : %5.2f" }; + static const char* rotationInfoMask[] = { "X : %5.2f deg %5.2f rad", "Y : %5.2f deg %5.2f rad", "Z : %5.2f deg %5.2f rad", "Screen : %5.2f deg %5.2f rad" }; + static const int translationInfoIndex[] = { 0,0,0, 1,0,0, 2,0,0, 1,2,0, 0,2,0, 0,1,0, 0,1,2 }; + static const float quadMin = 0.5f; + static const float quadMax = 0.8f; + static const float quadUV[8] = { quadMin, quadMin, quadMin, quadMax, quadMax, quadMax, quadMax, quadMin }; + static const int halfCircleSegmentCount = 64; + static const float snapTension = 0.5f; + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // + static MOVETYPE GetMoveType(OPERATION op, vec_t* gizmoHitProportion); + static MOVETYPE GetRotateType(OPERATION op); + static MOVETYPE GetScaleType(OPERATION op); + + Style& GetStyle() + { + return gContext.mStyle; + } + + static ImU32 GetColorU32(int idx) + { + IM_ASSERT(idx < COLOR::COUNT); + return ImGui::ColorConvertFloat4ToU32(gContext.mStyle.Colors[idx]); + } + + static ImVec2 worldToPos(const vec_t& worldPos, const matrix_t& mat, ImVec2 position = ImVec2(gContext.mX, gContext.mY), ImVec2 size = ImVec2(gContext.mWidth, gContext.mHeight)) + { + vec_t trans; + trans.TransformPoint(worldPos, mat); + if (fabsf(trans.w) < FLT_EPSILON) + return ImVec2(-FLT_MAX, -FLT_MAX); + trans *= 0.5f / trans.w; + trans += makeVect(0.5f, 0.5f); + trans.y = 1.f - trans.y; + trans.x *= size.x; + trans.y *= size.y; + trans.x += position.x; + trans.y += position.y; + return ImVec2(trans.x, trans.y); + } + + static void ComputeCameraRay(vec_t& rayOrigin, vec_t& rayDir, const matrix_t& viewMatrix, const matrix_t& projectionMatrix, const ImVec2& mousePosition, ImVec2 position, ImVec2 size) + { + matrix_t mViewProjInverse; + mViewProjInverse.Inverse(viewMatrix * projectionMatrix); + + const float mox = ((mousePosition.x - position.x) / size.x) * 2.f - 1.f; + const float moy = (1.f - ((mousePosition.y - position.y) / size.y)) * 2.f - 1.f; + + // Unproject the mouse at both depth extremes of the clip volume. + // A near-zero w means the extreme lies at infinity (an infinite far plane): keep the + // homogeneous coordinates in that case instead of dividing by ~0. + vec_t hA, hB; + hA.Transform(makeVect(mox, moy, 0.f, 1.f), mViewProjInverse); + hB.Transform(makeVect(mox, moy, 1.f - FLT_EPSILON, 1.f), mViewProjInverse); + const bool aAtInfinity = fabsf(hA.w) < FLT_EPSILON; + const bool bAtInfinity = fabsf(hB.w) < FLT_EPSILON; + const vec_t pointA = aAtInfinity ? hA : hA * (1.f / hA.w); + const vec_t pointB = bAtInfinity ? hB : hB * (1.f / hB.w); + + // Camera position. Derived from the view matrix so it is also correct when this is + // called for the ViewManipulate cube (which swaps the view/projection matrices). + matrix_t viewInverse; + viewInverse.Inverse(viewMatrix); + const vec_t eye = viewInverse.v.position; + + // Use the unprojected extreme closest to the camera as the ray origin. This is always + // the near plane: finite and close to the geometry, regardless of whether the + // projection uses a reversed and/or (near-)infinite depth range. Picking the far + // extreme instead (as a mis-detected "reversed" flag could) puts the origin thousands + // of units away and makes 'rayOrigin + rayDir * len' a catastrophic float cancellation, + // which is the jitter reported in issue #423. + vec_t nearPoint, farPoint; + bool farAtInfinity; + if ((pointA - eye).LengthSq() <= (pointB - eye).LengthSq()) + { + nearPoint = pointA; farPoint = pointB; farAtInfinity = bAtInfinity; + } + else + { + nearPoint = pointB; farPoint = pointA; farAtInfinity = aAtInfinity; + } + + rayOrigin = nearPoint; + // With an infinite far plane the far extreme is unusable, so take the direction from + // the camera through the near point (equivalent, and always finite for perspective). + rayDir = farAtInfinity ? Normalized(nearPoint - eye) : Normalized(farPoint - nearPoint); + } + + static void ComputeCameraRay(vec_t& rayOrigin, vec_t& rayDir, ImVec2 position = ImVec2(gContext.mX, gContext.mY), ImVec2 size = ImVec2(gContext.mWidth, gContext.mHeight)) + { + ImGuiIO& io = ImGui::GetIO(); + ComputeCameraRay(rayOrigin, rayDir, gContext.mViewMat, gContext.mProjectionMat, io.MousePos, position, size); + } + + void ComputeMouseRay(const float* view, const float* projection, const ImVec2& mousePosition, const ImVec2& rectPosition, const ImVec2& rectSize, float* rayOrigin, float* rayDirection) + { + vec_t origin, dir; + ComputeCameraRay(origin, dir, *(const matrix_t*)view, *(const matrix_t*)projection, mousePosition, rectPosition, rectSize); + rayOrigin[0] = origin.x; rayOrigin[1] = origin.y; rayOrigin[2] = origin.z; + rayDirection[0] = dir.x; rayDirection[1] = dir.y; rayDirection[2] = dir.z; + } + + static float GetSegmentLengthClipSpace(const vec_t& start, const vec_t& end, const bool localCoordinates = false) + { + vec_t startOfSegment = start; + const matrix_t& mvp = localCoordinates ? gContext.mMVPLocal : gContext.mMVP; + startOfSegment.TransformPoint(mvp); + if (fabsf(startOfSegment.w) > FLT_EPSILON) // check for axis aligned with camera direction + { + startOfSegment *= 1.f / startOfSegment.w; + } + + vec_t endOfSegment = end; + endOfSegment.TransformPoint(mvp); + if (fabsf(endOfSegment.w) > FLT_EPSILON) // check for axis aligned with camera direction + { + endOfSegment *= 1.f / endOfSegment.w; + } + + vec_t clipSpaceAxis = endOfSegment - startOfSegment; + if (gContext.mDisplayRatio < 1.0) + clipSpaceAxis.x *= gContext.mDisplayRatio; + else + clipSpaceAxis.y /= gContext.mDisplayRatio; + float segmentLengthInClipSpace = sqrtf(clipSpaceAxis.x * clipSpaceAxis.x + clipSpaceAxis.y * clipSpaceAxis.y); + return segmentLengthInClipSpace; + } + + static float GetParallelogram(const vec_t& ptO, const vec_t& ptA, const vec_t& ptB) + { + vec_t pts[] = { ptO, ptA, ptB }; + for (unsigned int i = 0; i < 3; i++) + { + pts[i].TransformPoint(gContext.mMVP); + if (fabsf(pts[i].w) > FLT_EPSILON) // check for axis aligned with camera direction + { + pts[i] *= 1.f / pts[i].w; + } + } + vec_t segA = pts[1] - pts[0]; + vec_t segB = pts[2] - pts[0]; + segA.y /= gContext.mDisplayRatio; + segB.y /= gContext.mDisplayRatio; + vec_t segAOrtho = makeVect(-segA.y, segA.x); + segAOrtho.Normalize(); + float dt = segAOrtho.Dot3(segB); + float surface = sqrtf(segA.x * segA.x + segA.y * segA.y) * fabsf(dt); + return surface; + } + + inline vec_t PointOnSegment(const vec_t& point, const vec_t& vertPos1, const vec_t& vertPos2) + { + vec_t c = point - vertPos1; + vec_t V; + + V.Normalize(vertPos2 - vertPos1); + float d = (vertPos2 - vertPos1).Length(); + float t = V.Dot3(c); + + if (t < 0.f) + { + return vertPos1; + } + + if (t > d) + { + return vertPos2; + } + + return vertPos1 + V * t; + } + + static float IntersectRayPlane(const vec_t& rOrigin, const vec_t& rVector, const vec_t& plan) + { + const float numer = plan.Dot3(rOrigin) - plan.w; + const float denom = plan.Dot3(rVector); + + if (fabsf(denom) < FLT_EPSILON) // normal is orthogonal to vector, cant intersect + { + return -1.0f; + } + + return -(numer / denom); + } + + static float DistanceToPlane(const vec_t& point, const vec_t& plan) + { + return plan.Dot3(point) + plan.w; + } + + static bool IsInContextRect(ImVec2 p) + { + return IsWithin(p.x, gContext.mX, gContext.mXMax) && IsWithin(p.y, gContext.mY, gContext.mYMax); + } + + static bool IsHoveringWindow() + { + ImGuiContext& g = *ImGui::GetCurrentContext(); + ImGuiWindow* window = ImGui::FindWindowByName(gContext.mDrawList->_OwnerName); + if (g.HoveredWindow == window) // Mouse hovering drawlist window + return true; + if (gContext.mAlternativeWindow != nullptr && g.HoveredWindow == gContext.mAlternativeWindow) + return true; + if (g.HoveredWindow != NULL) // Any other window is hovered + return false; + if (ImGui::IsMouseHoveringRect(window->InnerRect.Min, window->InnerRect.Max, false)) // Hovering drawlist window rect, while no other window is hovered (for _NoInputs windows) + return true; + return false; + } + + void SetRect(float x, float y, float width, float height) + { + gContext.mX = x; + gContext.mY = y; + gContext.mWidth = width; + gContext.mHeight = height; + gContext.mXMax = gContext.mX + gContext.mWidth; + gContext.mYMax = gContext.mY + gContext.mHeight; + gContext.mDisplayRatio = width / height; + } + + void SetOrthographic(bool isOrthographic) + { + gContext.mIsOrthographic = isOrthographic; + } + + void SetDrawlist(ImDrawList* drawlist) + { + gContext.mDrawList = drawlist ? drawlist : ImGui::GetWindowDrawList(); + } + + void SetImGuiContext(ImGuiContext* ctx) + { + ImGui::SetCurrentContext(ctx); + } + + void BeginFrame() + { + const ImU32 flags = ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_NoFocusOnAppearing | ImGuiWindowFlags_NoBringToFrontOnFocus; + +#ifdef IMGUI_HAS_VIEWPORT + ImGui::SetNextWindowSize(ImGui::GetMainViewport()->Size); + ImGui::SetNextWindowPos(ImGui::GetMainViewport()->Pos); +#else + ImGuiIO& io = ImGui::GetIO(); + ImGui::SetNextWindowSize(io.DisplaySize); + ImGui::SetNextWindowPos(ImVec2(0, 0)); +#endif + + ImGui::PushStyleColor(ImGuiCol_WindowBg, 0); + ImGui::PushStyleColor(ImGuiCol_Border, 0); + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); + + ImGui::Begin("gizmo", NULL, flags); + gContext.mDrawList = ImGui::GetWindowDrawList(); + gContext.mbOverGizmoHotspotLastFrame = gContext.mbOverGizmoHotspot; + gContext.mbOverGizmoHotspot = false; + // view manipulate flags accumulate across every ViewManipulate call of the frame + gContext.mbUsingViewManipulate = false; + gContext.mIsViewManipulatorHovered = false; + ImGui::End(); + ImGui::PopStyleVar(); + ImGui::PopStyleColor(2); + } + + bool IsUsing() + { + return (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID)) || gContext.mbUsingBounds; + } + + bool IsUsingViewManipulate() + { + return gContext.mbUsingViewManipulate; + } + + bool IsViewManipulateHovered() + { + return gContext.mIsViewManipulatorHovered; + } + + bool IsUsingAny() + { + return gContext.mbUsing || gContext.mbUsingBounds; + } + + bool IsOver() + { + // mbOverGizmoHotspotLastFrame is accumulated across every Manipulate call of the + // previous frame, so this reports hovering over any gizmo instead of only the last one. + // IsUsingAny() (not IsUsing()) is used so an active drag is detected even when IsOver() + // is queried outside the PushID scope of the gizmo being manipulated. + return gContext.mbOverGizmoHotspotLastFrame || IsUsingAny(); + } + + bool IsOver(OPERATION op) + { + if(IsUsing()) + { + return true; + } + if(Intersects(op, SCALE | SCALEU) && GetScaleType(op) != MT_NONE) + { + return true; + } + if(Intersects(op, ROTATE) && GetRotateType(op) != MT_NONE) + { + return true; + } + if(Intersects(op, TRANSLATE) && GetMoveType(op, NULL) != MT_NONE) + { + return true; + } + return false; + } + + MOVETYPE GetActiveHandleType() + { + if (!gContext.mbUsing || gContext.GetCurrentID() != gContext.mEditingID) + { + return MT_NONE; + } + return gContext.mCurrentHandleType; + } + + MOVETYPE GetHoveredHandleType() + { + return gContext.mHoveredHandleType; + } + + MOVETYPE GetActiveMoveType() + { + return GetActiveHandleType(); + } + + MOVETYPE GetHoveredMoveType() + { + return GetHoveredHandleType(); + } + + void Enable(bool enable) + { + gContext.mbEnable = enable; + if (!enable) + { + // Only cancel an ongoing interaction when the gizmo being disabled is the one + // currently edited. Otherwise disabling one gizmo would wipe the shared using + // state and break another gizmo that is still enabled/being manipulated. + if (!gContext.mbUsing || gContext.GetCurrentID() == gContext.mEditingID) + { + gContext.mbUsing = false; + gContext.mbUsingBounds = false; + gContext.mCurrentHandleType = MT_NONE; + gContext.mHoveredHandleType = MT_NONE; + } + } + } + + static void ComputeContext(const float* view, const float* projection, float* matrix, MODE mode) + { + gContext.mMode = mode; + gContext.mViewMat = *(matrix_t*)view; + gContext.mProjectionMat = *(matrix_t*)projection; + gContext.mbMouseOver = IsHoveringWindow(); + + gContext.mModelLocal = *(matrix_t*)matrix; + gContext.mModelLocal.OrthoNormalize(); + + if (mode == LOCAL) + { + gContext.mModel = gContext.mModelLocal; + } + else + { + gContext.mModel.Translation(((matrix_t*)matrix)->v.position); + } + gContext.mModelSource = *(matrix_t*)matrix; + gContext.mModelScaleOrigin.Set(gContext.mModelSource.v.right.Length(), gContext.mModelSource.v.up.Length(), gContext.mModelSource.v.dir.Length()); + + gContext.mModelInverse.Inverse(gContext.mModel); + gContext.mModelSourceInverse.Inverse(gContext.mModelSource); + gContext.mViewProjection = gContext.mViewMat * gContext.mProjectionMat; + gContext.mMVP = gContext.mModel * gContext.mViewProjection; + gContext.mMVPLocal = gContext.mModelLocal * gContext.mViewProjection; + + matrix_t viewInverse; + viewInverse.Inverse(gContext.mViewMat); + gContext.mCameraDir = viewInverse.v.dir; + gContext.mCameraEye = viewInverse.v.position; + gContext.mCameraRight = viewInverse.v.right; + gContext.mCameraUp = viewInverse.v.up; + + // compute scale from the size of camera right vector projected on screen at the matrix position + vec_t pointRight = viewInverse.v.right; + pointRight.TransformPoint(gContext.mViewProjection); + + vec_t rightViewInverse = viewInverse.v.right; + rightViewInverse.TransformVector(gContext.mModelInverse); + float rightLength = GetSegmentLengthClipSpace(makeVect(0.f, 0.f), rightViewInverse); + gContext.mScreenFactor = gContext.mGizmoSizeClipSpace / rightLength; + + ImVec2 centerSSpace = worldToPos(makeVect(0.f, 0.f), gContext.mMVP); + gContext.mScreenSquareCenter = centerSSpace; + gContext.mScreenSquareMin = ImVec2(centerSSpace.x - 10.f, centerSSpace.y - 10.f); + gContext.mScreenSquareMax = ImVec2(centerSSpace.x + 10.f, centerSSpace.y + 10.f); + + ComputeCameraRay(gContext.mRayOrigin, gContext.mRayVector); + } + + static void ComputeColors(ImU32* colors, int type, OPERATION operation) + { + if (gContext.mbEnable) + { + ImU32 selectionColor = GetColorU32(SELECTION); + + switch (operation) + { + case TRANSLATE: + colors[0] = (type == MT_MOVE_SCREEN) ? selectionColor : IM_COL32_WHITE; + for (int i = 0; i < 3; i++) + { + colors[i + 1] = (type == (int)(MT_MOVE_X + i)) ? selectionColor : GetColorU32(DIRECTION_X + i); + colors[i + 4] = (type == (int)(MT_MOVE_YZ + i)) ? selectionColor : GetColorU32(PLANE_X + i); + colors[i + 4] = (type == MT_MOVE_SCREEN) ? selectionColor : colors[i + 4]; + } + break; + case ROTATE: + colors[0] = (type == MT_ROTATE_SCREEN) ? selectionColor : IM_COL32_WHITE; + for (int i = 0; i < 3; i++) + { + colors[i + 1] = (type == (int)(MT_ROTATE_X + i)) ? selectionColor : GetColorU32(DIRECTION_X + i); + } + break; + case SCALEU: + case SCALE: + colors[0] = (type == MT_SCALE_XYZ) ? selectionColor : IM_COL32_WHITE; + for (int i = 0; i < 3; i++) + { + colors[i + 1] = (type == (int)(MT_SCALE_X + i)) ? selectionColor : GetColorU32(DIRECTION_X + i); + } + break; + // note: this internal function is only called with three possible values for operation + default: + break; + } + } + else + { + ImU32 inactiveColor = GetColorU32(INACTIVE); + for (int i = 0; i < 7; i++) + { + colors[i] = inactiveColor; + } + } + } + + static void ComputeTripodAxisAndVisibility(const int axisIndex, vec_t& dirAxis, vec_t& dirPlaneX, vec_t& dirPlaneY, bool& belowAxisLimit, bool& belowPlaneLimit, const bool localCoordinates = false) + { + dirAxis = directionUnary[axisIndex]; + dirPlaneX = directionUnary[(axisIndex + 1) % 3]; + dirPlaneY = directionUnary[(axisIndex + 2) % 3]; + TripodState& tripodState = gContext.GetTripodState(); + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID)) + { + // when using, use stored factors so the gizmo doesn't flip when we translate + + // Apply axis mask to axes and planes + belowAxisLimit = tripodState.mBelowAxisLimit[axisIndex] && !((1< FLT_EPSILON) ? -1.f : 1.f; + float mulAxisX = (allowFlip && lenDirPlaneX < lenDirMinusPlaneX&& fabsf(lenDirPlaneX - lenDirMinusPlaneX) > FLT_EPSILON) ? -1.f : 1.f; + float mulAxisY = (allowFlip && lenDirPlaneY < lenDirMinusPlaneY&& fabsf(lenDirPlaneY - lenDirMinusPlaneY) > FLT_EPSILON) ? -1.f : 1.f; + dirAxis *= mulAxis; + dirPlaneX *= mulAxisX; + dirPlaneY *= mulAxisY; + + // for axis + float axisLengthInClipSpace = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), dirAxis * gContext.mScreenFactor, localCoordinates); + + float paraSurf = GetParallelogram(makeVect(0.f, 0.f, 0.f), dirPlaneX * gContext.mScreenFactor, dirPlaneY * gContext.mScreenFactor); + // Apply axis mask to axes and planes + belowPlaneLimit = (paraSurf > gContext.mAxisLimit) && (((1< gContext.mPlaneLimit) && !((1< (1.f - snapTension)) + { + *value = *value - modulo + snap * ((*value < 0.f) ? -1.f : 1.f); + } + } + static void ComputeSnap(vec_t& value, const float* snap) + { + for (int i = 0; i < 3; i++) + { + ComputeSnap(&value[i], snap[i]); + } + } + + static float ComputeAngleOnPlan() + { + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan); + vec_t localPos = Normalized(gContext.mRayOrigin + gContext.mRayVector * len - gContext.mModel.v.position); + + vec_t perpendicularVector; + perpendicularVector.Cross(gContext.mRotationVectorSource, gContext.mTranslationPlan); + perpendicularVector.Normalize(); + float acosAngle = Clamp(Dot(localPos, gContext.mRotationVectorSource), -1.f, 1.f); + float angle = acosf(acosAngle); + angle *= (Dot(localPos, perpendicularVector) < 0.f) ? 1.f : -1.f; + return angle; + } + + static void DrawRotationGizmo(OPERATION op, MOVETYPE type) + { + if(!Intersects(op, ROTATE)) + { + return; + } + ImDrawList* drawList = gContext.mDrawList; + + bool isMultipleAxesMasked = (gContext.mAxisMask & (gContext.mAxisMask - 1)) != 0; + bool isNoAxesMasked = !gContext.mAxisMask; + + // colors + ImU32 colors[7]; + ComputeColors(colors, type, ROTATE); + + vec_t viewDirNormalized; + matrix_t viewInverse; + if (gContext.mIsOrthographic) + { + viewInverse.Inverse(*(matrix_t*)&gContext.mViewMat); + viewDirNormalized = -viewInverse.v.dir; + } + else + { + // In left-handed view matrices, mCameraDir points toward the scene instead of + // toward the camera; negate it so we always get the "toward camera" direction, + // which selects the correct front-facing half of each rotation ring. + viewInverse.Inverse(*(matrix_t*)&gContext.mViewMat); + const float handSign = (Dot(Cross(viewInverse.v.right, viewInverse.v.up), viewInverse.v.dir) >= 0.f) ? 1.f : -1.f; + viewDirNormalized = Normalized(gContext.mCameraDir) * handSign; + } + + viewDirNormalized.TransformVector(gContext.mModelInverse); + + gContext.mRadiusSquareCenter = screenRotateSize * gContext.mHeight; + + bool hasRSC = Intersects(op, ROTATE_SCREEN); + for (int axis = 0; axis < 3; axis++) + { + if(!Intersects(op, static_cast(ROTATE_Z >> axis))) + { + continue; + } + + bool isAxisMasked = ((1 << (2 - axis)) & gContext.mAxisMask) != 0; + + if ((!isAxisMasked || isMultipleAxesMasked) && !isNoAxesMasked) + { + continue; + } + const bool usingAxis = (gContext.mbUsing && type == MT_ROTATE_Z - axis); + const int circleMul = (hasRSC && !usingAxis) ? 1 : 2; + + ImVec2* circlePos = (ImVec2*)alloca(sizeof(ImVec2) * (circleMul * halfCircleSegmentCount + 1)); + const bool rightHanded = gContext.mProjectionMat.m[2][3] < 0.f; + float angleStart = atan2f(viewDirNormalized[(4 - axis) % 3], viewDirNormalized[(3 - axis) % 3]) + (gContext.mIsOrthographic ? ZPI : -ZPI) * 0.5f + (rightHanded ? 0.f : ZPI); + + for (int i = 0; i < circleMul * halfCircleSegmentCount + 1; i++) + { + float ng = angleStart + (float)circleMul * ZPI * ((float)i / (float)(circleMul * halfCircleSegmentCount)); + vec_t axisPos = makeVect(cosf(ng), sinf(ng), 0.f); + vec_t pos = makeVect(axisPos[axis], axisPos[(axis + 1) % 3], axisPos[(axis + 2) % 3]) * gContext.mScreenFactor * rotationDisplayFactor; + circlePos[i] = worldToPos(pos, gContext.mMVP); + } + if (!gContext.mbUsing || usingAxis) + { +#if IMGUI_VERSION_NUM < 19276 + drawList->AddPolyline(circlePos, circleMul* halfCircleSegmentCount + 1, colors[3 - axis], 0, gContext.mStyle.RotationLineThickness ); +#else + drawList->AddPolyline(circlePos, circleMul* halfCircleSegmentCount + 1, colors[3 - axis], gContext.mStyle.RotationLineThickness, 0 ); +#endif + } + + float radiusAxis = sqrtf((ImLengthSqr(worldToPos(gContext.mModel.v.position, gContext.mViewProjection) - circlePos[0]))); + if (radiusAxis > gContext.mRadiusSquareCenter) + { + gContext.mRadiusSquareCenter = radiusAxis; + } + } + if(hasRSC && (!gContext.mbUsing || type == MT_ROTATE_SCREEN) && (!isMultipleAxesMasked && isNoAxesMasked)) + { + drawList->AddCircle(worldToPos(gContext.mModel.v.position, gContext.mViewProjection), gContext.mRadiusSquareCenter, colors[0], 64, gContext.mStyle.RotationOuterLineThickness); + } + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID) && IsRotateType(type)) + { + ImVec2 circlePos[halfCircleSegmentCount + 1]; + + circlePos[0] = worldToPos(gContext.mModel.v.position, gContext.mViewProjection); + for (unsigned int i = 1; i < halfCircleSegmentCount + 1; i++) + { + float ng = gContext.mRotationAngle * ((float)(i - 1) / (float)(halfCircleSegmentCount - 1)); + matrix_t rotateVectorMatrix; + rotateVectorMatrix.RotationAxis(gContext.mTranslationPlan, ng); + vec_t pos; + pos.TransformPoint(gContext.mRotationVectorSource, rotateVectorMatrix); + pos *= gContext.mScreenFactor * rotationDisplayFactor; + circlePos[i] = worldToPos(pos + gContext.mModel.v.position, gContext.mViewProjection); + } + drawList->AddConvexPolyFilled(circlePos, halfCircleSegmentCount + 1, GetColorU32(ROTATION_USING_FILL)); +#if IMGUI_VERSION_NUM < 19276 + drawList->AddPolyline(circlePos, halfCircleSegmentCount + 1, GetColorU32(ROTATION_USING_BORDER), ImDrawFlags_Closed, gContext.mStyle.RotationLineThickness ); +#else + drawList->AddPolyline(circlePos, halfCircleSegmentCount + 1, GetColorU32(ROTATION_USING_BORDER), gContext.mStyle.RotationLineThickness, ImDrawFlags_Closed ); +#endif + + ImVec2 destinationPosOnScreen = circlePos[1]; + char tmps[512]; + ImFormatString(tmps, sizeof(tmps), rotationInfoMask[type - MT_ROTATE_X], (gContext.mRotationAngle / ZPI) * 180.f, gContext.mRotationAngle); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), GetColorU32(TEXT_SHADOW), tmps); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), GetColorU32(TEXT), tmps); + } + } + + static void DrawHatchedAxis(const vec_t& axis) + { + if (gContext.mStyle.HatchedAxisLineThickness <= 0.0f) + { + return; + } + + for (int j = 1; j < 10; j++) + { + ImVec2 baseSSpace2 = worldToPos(axis * 0.05f * (float)(j * 2) * gContext.mScreenFactor, gContext.mMVP); + ImVec2 worldDirSSpace2 = worldToPos(axis * 0.05f * (float)(j * 2 + 1) * gContext.mScreenFactor, gContext.mMVP); + gContext.mDrawList->AddLine(baseSSpace2, worldDirSSpace2, GetColorU32(HATCHED_AXIS_LINES), gContext.mStyle.HatchedAxisLineThickness); + } + } + + static void DrawScaleGizmo(OPERATION op, MOVETYPE type) + { + ImDrawList* drawList = gContext.mDrawList; + + if(!Intersects(op, SCALE)) + { + return; + } + + // colors + ImU32 colors[7]; + ComputeColors(colors, type, SCALE); + + // draw + vec_t scaleDisplay = { 1.f, 1.f, 1.f, 1.f }; + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID)) + { + scaleDisplay = gContext.mScale; + } + + for (int i = 0; i < 3; i++) + { + if(!Intersects(op, static_cast(SCALE_X << i))) + { + continue; + } + const bool usingAxis = (gContext.mbUsing && type == MT_SCALE_X + i); + if (!gContext.mbUsing || usingAxis) + { + vec_t dirPlaneX, dirPlaneY, dirAxis; + bool belowAxisLimit, belowPlaneLimit; + ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit, true); + + // draw axis + if (belowAxisLimit) + { + bool hasTranslateOnAxis = Contains(op, static_cast(TRANSLATE_X << i)); + float markerScale = hasTranslateOnAxis ? 1.4f : 1.0f; + ImVec2 baseSSpace = worldToPos(dirAxis * 0.1f * gContext.mScreenFactor, gContext.mMVP); + ImVec2 worldDirSSpaceNoScale = worldToPos(dirAxis * markerScale * gContext.mScreenFactor, gContext.mMVP); + ImVec2 worldDirSSpace = worldToPos((dirAxis * markerScale * scaleDisplay[i]) * gContext.mScreenFactor, gContext.mMVP); + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID)) + { + ImU32 scaleLineColor = GetColorU32(SCALE_LINE); + drawList->AddLine(baseSSpace, worldDirSSpaceNoScale, scaleLineColor, gContext.mStyle.ScaleLineThickness); + drawList->AddCircleFilled(worldDirSSpaceNoScale, gContext.mStyle.ScaleLineCircleSize, scaleLineColor); + } + + if (!hasTranslateOnAxis || gContext.mbUsing) + { + drawList->AddLine(baseSSpace, worldDirSSpace, colors[i + 1], gContext.mStyle.ScaleLineThickness); + } + drawList->AddCircleFilled(worldDirSSpace, gContext.mStyle.ScaleLineCircleSize, colors[i + 1]); + + if (gContext.GetTripodState().mAxisFactor[i] < 0.f) + { + DrawHatchedAxis(dirAxis * scaleDisplay[i]); + } + } + } + } + + // draw screen cirle + drawList->AddCircleFilled(gContext.mScreenSquareCenter, gContext.mStyle.CenterCircleSize, colors[0], 32); + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID) && IsScaleType(type)) + { + //ImVec2 sourcePosOnScreen = worldToPos(gContext.mMatrixOrigin, gContext.mViewProjection); + ImVec2 destinationPosOnScreen = worldToPos(gContext.mModel.v.position, gContext.mViewProjection); + /*vec_t dif(destinationPosOnScreen.x - sourcePosOnScreen.x, destinationPosOnScreen.y - sourcePosOnScreen.y); + dif.Normalize(); + dif *= 5.f; + drawList->AddCircle(sourcePosOnScreen, 6.f, translationLineColor); + drawList->AddCircle(destinationPosOnScreen, 6.f, translationLineColor); + drawList->AddLine(ImVec2(sourcePosOnScreen.x + dif.x, sourcePosOnScreen.y + dif.y), ImVec2(destinationPosOnScreen.x - dif.x, destinationPosOnScreen.y - dif.y), translationLineColor, 2.f); + */ + char tmps[512]; + //vec_t deltaInfo = gContext.mModel.v.position - gContext.mMatrixOrigin; + int componentInfoIndex = (type - MT_SCALE_X) * 3; + ImFormatString(tmps, sizeof(tmps), scaleInfoMask[type - MT_SCALE_X], scaleDisplay[translationInfoIndex[componentInfoIndex]]); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), GetColorU32(TEXT_SHADOW), tmps); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), GetColorU32(TEXT), tmps); + } + } + + + static void DrawScaleUniveralGizmo(OPERATION op, MOVETYPE type) + { + ImDrawList* drawList = gContext.mDrawList; + + if (!Intersects(op, SCALEU)) + { + return; + } + + // colors + ImU32 colors[7]; + ComputeColors(colors, type, SCALEU); + + // draw + vec_t scaleDisplay = { 1.f, 1.f, 1.f, 1.f }; + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID)) + { + scaleDisplay = gContext.mScale; + } + + for (int i = 0; i < 3; i++) + { + if (!Intersects(op, static_cast(SCALE_XU << i))) + { + continue; + } + const bool usingAxis = (gContext.mbUsing && type == MT_SCALE_X + i); + if (!gContext.mbUsing || usingAxis) + { + vec_t dirPlaneX, dirPlaneY, dirAxis; + bool belowAxisLimit, belowPlaneLimit; + ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit, true); + + // draw axis + if (belowAxisLimit) + { + bool hasTranslateOnAxis = Contains(op, static_cast(TRANSLATE_X << i)); + float markerScale = hasTranslateOnAxis ? 1.4f : 1.0f; + //ImVec2 baseSSpace = worldToPos(dirAxis * 0.1f * gContext.mScreenFactor, gContext.mMVPLocal); + //ImVec2 worldDirSSpaceNoScale = worldToPos(dirAxis * markerScale * gContext.mScreenFactor, gContext.mMVP); + ImVec2 worldDirSSpace = worldToPos((dirAxis * markerScale * scaleDisplay[i]) * gContext.mScreenFactor, gContext.mMVPLocal); + +#if 0 + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID)) + { + drawList->AddLine(baseSSpace, worldDirSSpaceNoScale, IM_COL32(0x40, 0x40, 0x40, 0xFF), 3.f); + drawList->AddCircleFilled(worldDirSSpaceNoScale, 6.f, IM_COL32(0x40, 0x40, 0x40, 0xFF)); + } + /* + if (!hasTranslateOnAxis || gContext.mbUsing) + { + drawList->AddLine(baseSSpace, worldDirSSpace, colors[i + 1], 3.f); + } + */ +#endif + drawList->AddCircleFilled(worldDirSSpace, 12.f, colors[i + 1]); + } + } + } + + // draw screen cirle + drawList->AddCircle(gContext.mScreenSquareCenter, 20.f, colors[0], 32, gContext.mStyle.CenterCircleSize); + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID) && IsScaleType(type)) + { + //ImVec2 sourcePosOnScreen = worldToPos(gContext.mMatrixOrigin, gContext.mViewProjection); + ImVec2 destinationPosOnScreen = worldToPos(gContext.mModel.v.position, gContext.mViewProjection); + /*vec_t dif(destinationPosOnScreen.x - sourcePosOnScreen.x, destinationPosOnScreen.y - sourcePosOnScreen.y); + dif.Normalize(); + dif *= 5.f; + drawList->AddCircle(sourcePosOnScreen, 6.f, translationLineColor); + drawList->AddCircle(destinationPosOnScreen, 6.f, translationLineColor); + drawList->AddLine(ImVec2(sourcePosOnScreen.x + dif.x, sourcePosOnScreen.y + dif.y), ImVec2(destinationPosOnScreen.x - dif.x, destinationPosOnScreen.y - dif.y), translationLineColor, 2.f); + */ + char tmps[512]; + //vec_t deltaInfo = gContext.mModel.v.position - gContext.mMatrixOrigin; + int componentInfoIndex = (type - MT_SCALE_X) * 3; + ImFormatString(tmps, sizeof(tmps), scaleInfoMask[type - MT_SCALE_X], scaleDisplay[translationInfoIndex[componentInfoIndex]]); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), GetColorU32(TEXT_SHADOW), tmps); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), GetColorU32(TEXT), tmps); + } + } + + static void DrawTranslationGizmo(OPERATION op, MOVETYPE type) + { + ImDrawList* drawList = gContext.mDrawList; + if (!drawList) + { + return; + } + + if(!Intersects(op, TRANSLATE)) + { + return; + } + + // colors + ImU32 colors[7]; + ComputeColors(colors, type, TRANSLATE); + + const ImVec2 origin = worldToPos(gContext.mModel.v.position, gContext.mViewProjection); + + // draw + bool belowAxisLimit = false; + bool belowPlaneLimit = false; + for (int i = 0; i < 3; ++i) + { + vec_t dirPlaneX, dirPlaneY, dirAxis; + ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit); + + if (!gContext.mbUsing || (gContext.mbUsing && type == MT_MOVE_X + i)) + { + // draw axis + if (belowAxisLimit && Intersects(op, static_cast(TRANSLATE_X << i))) + { + ImVec2 baseSSpace = worldToPos(dirAxis * 0.1f * gContext.mScreenFactor, gContext.mMVP); + ImVec2 worldDirSSpace = worldToPos(dirAxis * gContext.mScreenFactor, gContext.mMVP); + + drawList->AddLine(baseSSpace, worldDirSSpace, colors[i + 1], gContext.mStyle.TranslationLineThickness); + + // Arrow head begin + ImVec2 dir(origin - worldDirSSpace); + + float d = sqrtf(ImLengthSqr(dir)); + dir /= d; // Normalize + dir *= gContext.mStyle.TranslationLineArrowSize; + + ImVec2 ortogonalDir(dir.y, -dir.x); // Perpendicular vector + ImVec2 a(worldDirSSpace + dir); + drawList->AddTriangleFilled(worldDirSSpace - dir, a + ortogonalDir, a - ortogonalDir, colors[i + 1]); + // Arrow head end + + if (gContext.GetTripodState().mAxisFactor[i] < 0.f) + { + DrawHatchedAxis(dirAxis); + } + } + } + // draw plane + if (!gContext.mbUsing || (gContext.mbUsing && type == MT_MOVE_YZ + i)) + { + if (belowPlaneLimit && Contains(op, TRANSLATE_PLANS[i])) + { + ImVec2 screenQuadPts[4]; + for (int j = 0; j < 4; ++j) + { + vec_t cornerWorldPos = (dirPlaneX * quadUV[j * 2] + dirPlaneY * quadUV[j * 2 + 1]) * gContext.mScreenFactor; + screenQuadPts[j] = worldToPos(cornerWorldPos, gContext.mMVP); + } +#if IMGUI_VERSION_NUM < 19276 + drawList->AddPolyline(screenQuadPts, 4, GetColorU32(DIRECTION_X + i), ImDrawFlags_Closed, 1.0f ); +#else + drawList->AddPolyline(screenQuadPts, 4, GetColorU32(DIRECTION_X + i), 1.0f, ImDrawFlags_Closed ); +#endif + drawList->AddConvexPolyFilled(screenQuadPts, 4, colors[i + 4]); + } + } + } + + drawList->AddCircleFilled(gContext.mScreenSquareCenter, gContext.mStyle.CenterCircleSize, colors[0], 32); + + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID) && IsTranslateType(type)) + { + ImU32 translationLineColor = GetColorU32(TRANSLATION_LINE); + + ImVec2 sourcePosOnScreen = worldToPos(gContext.mMatrixOrigin, gContext.mViewProjection); + ImVec2 destinationPosOnScreen = worldToPos(gContext.mModel.v.position, gContext.mViewProjection); + vec_t dif = { destinationPosOnScreen.x - sourcePosOnScreen.x, destinationPosOnScreen.y - sourcePosOnScreen.y, 0.f, 0.f }; + dif.Normalize(); + dif *= 5.f; + drawList->AddCircle(sourcePosOnScreen, 6.f, translationLineColor); + drawList->AddCircle(destinationPosOnScreen, 6.f, translationLineColor); + drawList->AddLine(ImVec2(sourcePosOnScreen.x + dif.x, sourcePosOnScreen.y + dif.y), ImVec2(destinationPosOnScreen.x - dif.x, destinationPosOnScreen.y - dif.y), translationLineColor, 2.f); + + char tmps[512]; + vec_t deltaInfo = gContext.mModel.v.position - gContext.mMatrixOrigin; + int componentInfoIndex = (type - MT_MOVE_X) * 3; + ImFormatString(tmps, sizeof(tmps), translationInfoMask[type - MT_MOVE_X], deltaInfo[translationInfoIndex[componentInfoIndex]], deltaInfo[translationInfoIndex[componentInfoIndex + 1]], deltaInfo[translationInfoIndex[componentInfoIndex + 2]]); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), GetColorU32(TEXT_SHADOW), tmps); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), GetColorU32(TEXT), tmps); + } + } + + static bool CanActivate() + { + if (ImGui::IsMouseClicked(0) && !ImGui::IsAnyItemHovered() && !ImGui::IsAnyItemActive()) + { + return true; + } + return false; + } + + static bool HandleAndDrawLocalBounds(const float* bounds, matrix_t* matrix, const float* snapValues, OPERATION operation) + { + ImGuiIO& io = ImGui::GetIO(); + ImDrawList* drawList = gContext.mDrawList; + + bool manipulated = false; + + // compute best projection axis + vec_t axesWorldDirections[3]; + vec_t bestAxisWorldDirection = { 0.0f, 0.0f, 0.0f, 0.0f }; + int axes[3]; + unsigned int numAxes = 1; + axes[0] = gContext.mBoundsBestAxis; + int bestAxis = axes[0]; + if (!gContext.mbUsingBounds) + { + numAxes = 0; + float bestDot = 0.f; + for (int i = 0; i < 3; i++) + { + vec_t dirPlaneNormalWorld; + dirPlaneNormalWorld.TransformVector(directionUnary[i], gContext.mModelSource); + dirPlaneNormalWorld.Normalize(); + + float dt = fabsf(Dot(Normalized(gContext.mCameraEye - gContext.mModelSource.v.position), dirPlaneNormalWorld)); + if (dt >= bestDot) + { + bestDot = dt; + bestAxis = i; + bestAxisWorldDirection = dirPlaneNormalWorld; + } + + if (dt >= 0.1f) + { + axes[numAxes] = i; + axesWorldDirections[numAxes] = dirPlaneNormalWorld; + ++numAxes; + } + } + } + + if (numAxes == 0) + { + axes[0] = bestAxis; + axesWorldDirections[0] = bestAxisWorldDirection; + numAxes = 1; + } + + else if (bestAxis != axes[0]) + { + unsigned int bestIndex = 0; + for (unsigned int i = 0; i < numAxes; i++) + { + if (axes[i] == bestAxis) + { + bestIndex = i; + break; + } + } + int tempAxis = axes[0]; + axes[0] = axes[bestIndex]; + axes[bestIndex] = tempAxis; + vec_t tempDirection = axesWorldDirections[0]; + axesWorldDirections[0] = axesWorldDirections[bestIndex]; + axesWorldDirections[bestIndex] = tempDirection; + } + + for (unsigned int axisIndex = 0; axisIndex < numAxes; ++axisIndex) + { + bestAxis = axes[axisIndex]; + bestAxisWorldDirection = axesWorldDirections[axisIndex]; + + // corners + vec_t aabb[4]; + + int secondAxis = (bestAxis + 1) % 3; + int thirdAxis = (bestAxis + 2) % 3; + + for (int i = 0; i < 4; i++) + { + aabb[i][3] = aabb[i][bestAxis] = 0.f; + aabb[i][secondAxis] = bounds[secondAxis + 3 * (i >> 1)]; + aabb[i][thirdAxis] = bounds[thirdAxis + 3 * ((i >> 1) ^ (i & 1))]; + } + + // draw bounds + unsigned int anchorAlpha = gContext.mbEnable ? IM_COL32_BLACK : IM_COL32(0, 0, 0, 0x80); + + matrix_t boundsMVP = gContext.mModelSource * gContext.mViewProjection; + for (int i = 0; i < 4; i++) + { + // Clip the segment against the near plane (w >= eps) so the dashed + // line still draws when an endpoint is behind the camera or outside + // the viewport. ImDrawList clips to the 2D clip rect on its own. + vec_t p0; p0.TransformPoint(aabb[i], boundsMVP); + vec_t p1; p1.TransformPoint(aabb[(i + 1) % 4], boundsMVP); + const float wEps = 1e-3f; + bool in0 = p0.w >= wEps; + bool in1 = p1.w >= wEps; + if (!in0 && !in1) + { + continue; + } + if (!in0) + { + float t = (p1.w - wEps) / (p1.w - p0.w); + p0.x = p1.x + (p0.x - p1.x) * t; + p0.y = p1.y + (p0.y - p1.y) * t; + p0.z = p1.z + (p0.z - p1.z) * t; + p0.w = wEps; + } + else if (!in1) + { + float t = (p0.w - wEps) / (p0.w - p1.w); + p1.x = p0.x + (p1.x - p0.x) * t; + p1.y = p0.y + (p1.y - p0.y) * t; + p1.z = p0.z + (p1.z - p0.z) * t; + p1.w = wEps; + } + auto clipToScreen = [](const vec_t& c) -> ImVec2 + { + float nx = c.x * (0.5f / c.w) + 0.5f; + float ny = c.y * (0.5f / c.w) + 0.5f; + ny = 1.f - ny; + return ImVec2(gContext.mX + nx * gContext.mWidth, gContext.mY + ny * gContext.mHeight); + }; + ImVec2 worldBound1 = clipToScreen(p0); + ImVec2 worldBound2 = clipToScreen(p1); + float boundDistance = sqrtf(ImLengthSqr(worldBound1 - worldBound2)); + int stepCount = (int)(boundDistance / 10.f); + stepCount = min(stepCount, 1000); + for (int j = 0; j < stepCount; j++) + { + float stepLength = 1.f / (float)stepCount; + float t1 = (float)j * stepLength; + float t2 = (float)j * stepLength + stepLength * 0.5f; + ImVec2 worldBoundSS1 = ImLerp(worldBound1, worldBound2, ImVec2(t1, t1)); + ImVec2 worldBoundSS2 = ImLerp(worldBound1, worldBound2, ImVec2(t2, t2)); + //drawList->AddLine(worldBoundSS1, worldBoundSS2, IM_COL32(0, 0, 0, 0) + anchorAlpha, 3.f); + drawList->AddLine(worldBoundSS1, worldBoundSS2, IM_COL32(0xAA, 0xAA, 0xAA, 0) + anchorAlpha, 2.f); + } + vec_t midPoint = (aabb[i] + aabb[(i + 1) % 4]) * 0.5f; + // Per-anchor visibility: big anchor depends only on corner i; + // small anchor depends only on the midpoint of edge (i, i+1). + vec_t pCorner; pCorner.TransformPoint(aabb[i], boundsMVP); + vec_t pMid; pMid.TransformPoint(midPoint, boundsMVP); + ImVec2 worldBoundOrig = worldToPos(aabb[i], boundsMVP); + ImVec2 midBound = worldToPos(midPoint, boundsMVP); + bool bigAnchorVisible = pCorner.w >= wEps && IsInContextRect(worldBoundOrig); + bool smallAnchorVisible = pMid.w >= wEps && IsInContextRect(midBound); + + static const float AnchorBigRadius = 8.f; + static const float AnchorSmallRadius = 6.f; + bool overBigAnchor = bigAnchorVisible && ImLengthSqr(worldBoundOrig - io.MousePos) <= (AnchorBigRadius * AnchorBigRadius); + bool overSmallAnchor = smallAnchorVisible && ImLengthSqr(midBound - io.MousePos) <= (AnchorBigRadius * AnchorBigRadius); + + MOVETYPE type = MT_NONE; + vec_t gizmoHitProportion; + + if(Intersects(operation, TRANSLATE)) + { + type = GetMoveType(operation, &gizmoHitProportion); + } + if(Intersects(operation, ROTATE) && type == MT_NONE) + { + type = GetRotateType(operation); + } + if(Intersects(operation, SCALE) && type == MT_NONE) + { + type = GetScaleType(operation); + } + + if (type != MT_NONE) + { + overBigAnchor = false; + overSmallAnchor = false; + } + + ImU32 selectionColor = GetColorU32(SELECTION); + + unsigned int bigAnchorColor = overBigAnchor ? selectionColor : (IM_COL32(0xAA, 0xAA, 0xAA, 0) + anchorAlpha); + unsigned int smallAnchorColor = overSmallAnchor ? selectionColor : (IM_COL32(0xAA, 0xAA, 0xAA, 0) + anchorAlpha); + + if (bigAnchorVisible) + { + drawList->AddCircleFilled(worldBoundOrig, AnchorBigRadius, IM_COL32_BLACK); + drawList->AddCircleFilled(worldBoundOrig, AnchorBigRadius - 1.2f, bigAnchorColor); + } + + if (smallAnchorVisible) + { + drawList->AddCircleFilled(midBound, AnchorSmallRadius, IM_COL32_BLACK); + drawList->AddCircleFilled(midBound, AnchorSmallRadius - 1.2f, smallAnchorColor); + } + int oppositeIndex = (i + 2) % 4; + // big anchor on corners + if (!gContext.mbUsingBounds && gContext.mbEnable && overBigAnchor && CanActivate()) + { + gContext.mBoundsPivot.TransformPoint(aabb[(i + 2) % 4], gContext.mModelSource); + gContext.mBoundsAnchor.TransformPoint(aabb[i], gContext.mModelSource); + gContext.mBoundsPlan = BuildPlan(gContext.mBoundsAnchor, bestAxisWorldDirection); + gContext.mBoundsBestAxis = bestAxis; + gContext.mBoundsAxis[0] = secondAxis; + gContext.mBoundsAxis[1] = thirdAxis; + + gContext.mBoundsLocalPivot.Set(0.f); + gContext.mBoundsLocalPivot[secondAxis] = aabb[oppositeIndex][secondAxis]; + gContext.mBoundsLocalPivot[thirdAxis] = aabb[oppositeIndex][thirdAxis]; + + gContext.mbUsingBounds = true; + gContext.mEditingID = gContext.GetCurrentID(); + gContext.mBoundsMatrix = gContext.mModelSource; + } + // small anchor on middle of segment + if (!gContext.mbUsingBounds && gContext.mbEnable && overSmallAnchor && CanActivate()) + { + vec_t midPointOpposite = (aabb[(i + 2) % 4] + aabb[(i + 3) % 4]) * 0.5f; + gContext.mBoundsPivot.TransformPoint(midPointOpposite, gContext.mModelSource); + gContext.mBoundsAnchor.TransformPoint(midPoint, gContext.mModelSource); + gContext.mBoundsPlan = BuildPlan(gContext.mBoundsAnchor, bestAxisWorldDirection); + gContext.mBoundsBestAxis = bestAxis; + int indices[] = { secondAxis , thirdAxis }; + gContext.mBoundsAxis[0] = indices[i % 2]; + gContext.mBoundsAxis[1] = -1; + + gContext.mBoundsLocalPivot.Set(0.f); + gContext.mBoundsLocalPivot[gContext.mBoundsAxis[0]] = aabb[oppositeIndex][indices[i % 2]];// bounds[gContext.mBoundsAxis[0]] * (((i + 1) & 2) ? 1.f : -1.f); + + gContext.mbUsingBounds = true; + gContext.mEditingID = gContext.GetCurrentID(); + gContext.mBoundsMatrix = gContext.mModelSource; + } + } + + if (gContext.mbUsingBounds && (gContext.GetCurrentID() == gContext.mEditingID)) + { + matrix_t scale; + scale.SetToIdentity(); + + // compute projected mouse position on plan + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mBoundsPlan); + vec_t newPos = gContext.mRayOrigin + gContext.mRayVector * len; + + // compute a reference and delta vectors base on mouse move + vec_t deltaVector = (newPos - gContext.mBoundsPivot).Abs(); + vec_t referenceVector = (gContext.mBoundsAnchor - gContext.mBoundsPivot).Abs(); + + // for 1 or 2 axes, compute a ratio that's used for scale and snap it based on resulting length + for (int i = 0; i < 2; i++) + { + int axisIndex1 = gContext.mBoundsAxis[i]; + if (axisIndex1 == -1) + { + continue; + } + + float ratioAxis = 1.f; + vec_t axisDir = gContext.mBoundsMatrix.component[axisIndex1].Abs(); + + float dtAxis = axisDir.Dot(referenceVector); + float boundSize = bounds[axisIndex1 + 3] - bounds[axisIndex1]; + if (dtAxis > FLT_EPSILON) + { + ratioAxis = axisDir.Dot(deltaVector) / dtAxis; + } + + if (snapValues) + { + float length = boundSize * ratioAxis; + ComputeSnap(&length, snapValues[axisIndex1]); + if (boundSize > FLT_EPSILON) + { + ratioAxis = length / boundSize; + } + } + scale.component[axisIndex1] *= ratioAxis; + + if (fabsf(ratioAxis - 1.0f) > FLT_EPSILON) { + manipulated = true; + } + } + + // transform matrix + matrix_t preScale, postScale; + preScale.Translation(-gContext.mBoundsLocalPivot); + postScale.Translation(gContext.mBoundsLocalPivot); + matrix_t res = preScale * scale * postScale * gContext.mBoundsMatrix; + *matrix = res; + + // info text + char tmps[512]; + ImVec2 destinationPosOnScreen = worldToPos(gContext.mModel.v.position, gContext.mViewProjection); + ImFormatString(tmps, sizeof(tmps), "X: %.2f Y: %.2f Z: %.2f" + , (bounds[3] - bounds[0]) * gContext.mBoundsMatrix.component[0].Length() * scale.component[0].Length() + , (bounds[4] - bounds[1]) * gContext.mBoundsMatrix.component[1].Length() * scale.component[1].Length() + , (bounds[5] - bounds[2]) * gContext.mBoundsMatrix.component[2].Length() * scale.component[2].Length() + ); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), GetColorU32(TEXT_SHADOW), tmps); + drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), GetColorU32(TEXT), tmps); + } + + if (!io.MouseDown[0]) { + gContext.mbUsingBounds = false; + gContext.mEditingID = -1; + } + if (gContext.mbUsingBounds) + { + break; + } + } + + return manipulated; + } + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // + + static MOVETYPE GetScaleType(OPERATION op) + { + if (gContext.mbUsing) + { + return MT_NONE; + } + ImGuiIO& io = ImGui::GetIO(); + MOVETYPE type = MT_NONE; + + // screen + if (io.MousePos.x >= gContext.mScreenSquareMin.x && io.MousePos.x <= gContext.mScreenSquareMax.x && + io.MousePos.y >= gContext.mScreenSquareMin.y && io.MousePos.y <= gContext.mScreenSquareMax.y && + Contains(op, SCALE)) + { + type = MT_SCALE_XYZ; + } + + // compute + for (int i = 0; i < 3 && type == MT_NONE; i++) + { + if(!Intersects(op, static_cast(SCALE_X << i))) + { + continue; + } + bool isAxisMasked = ((1 << i) & gContext.mAxisMask) != 0; + + vec_t dirPlaneX, dirPlaneY, dirAxis; + bool belowAxisLimit, belowPlaneLimit; + ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit, true); + dirAxis.TransformVector(gContext.mModelLocal); + dirPlaneX.TransformVector(gContext.mModelLocal); + dirPlaneY.TransformVector(gContext.mModelLocal); + + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, BuildPlan(gContext.mModelLocal.v.position, dirAxis)); + vec_t posOnPlan = gContext.mRayOrigin + gContext.mRayVector * len; + + const float startOffset = Contains(op, static_cast(TRANSLATE_X << i)) ? 1.0f : 0.1f; + const float endOffset = Contains(op, static_cast(TRANSLATE_X << i)) ? 1.4f : 1.0f; + const ImVec2 posOnPlanScreen = worldToPos(posOnPlan, gContext.mViewProjection); + const ImVec2 axisStartOnScreen = worldToPos(gContext.mModelLocal.v.position + dirAxis * gContext.mScreenFactor * startOffset, gContext.mViewProjection); + const ImVec2 axisEndOnScreen = worldToPos(gContext.mModelLocal.v.position + dirAxis * gContext.mScreenFactor * endOffset, gContext.mViewProjection); + + vec_t closestPointOnAxis = PointOnSegment(makeVect(posOnPlanScreen), makeVect(axisStartOnScreen), makeVect(axisEndOnScreen)); + + if ((closestPointOnAxis - makeVect(posOnPlanScreen)).Length() < 12.f) // pixel size + { + if (!isAxisMasked) + type = static_cast(MT_SCALE_X + i); + } + } + + // universal + + vec_t deltaScreen = { io.MousePos.x - gContext.mScreenSquareCenter.x, io.MousePos.y - gContext.mScreenSquareCenter.y, 0.f, 0.f }; + float dist = deltaScreen.Length(); + if (Contains(op, SCALEU) && dist >= 17.0f && dist < 23.0f) + { + type = MT_SCALE_XYZ; + } + + for (int i = 0; i < 3 && type == MT_NONE; i++) + { + if (!Intersects(op, static_cast(SCALE_XU << i))) + { + continue; + } + + vec_t dirPlaneX, dirPlaneY, dirAxis; + bool belowAxisLimit, belowPlaneLimit; + ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit, true); + + // draw axis + if (belowAxisLimit) + { + bool hasTranslateOnAxis = Contains(op, static_cast(TRANSLATE_X << i)); + float markerScale = hasTranslateOnAxis ? 1.4f : 1.0f; + //ImVec2 baseSSpace = worldToPos(dirAxis * 0.1f * gContext.mScreenFactor, gContext.mMVPLocal); + //ImVec2 worldDirSSpaceNoScale = worldToPos(dirAxis * markerScale * gContext.mScreenFactor, gContext.mMVP); + ImVec2 worldDirSSpace = worldToPos((dirAxis * markerScale) * gContext.mScreenFactor, gContext.mMVPLocal); + + float distance = sqrtf(ImLengthSqr(worldDirSSpace - io.MousePos)); + if (distance < 12.f) + { + type = static_cast(MT_SCALE_X + i); + } + } + } + return type; + } + + static MOVETYPE GetRotateType(OPERATION op) + { + if (gContext.mbUsing) + { + return MT_NONE; + } + + bool isNoAxesMasked = !gContext.mAxisMask; + bool isMultipleAxesMasked = (gContext.mAxisMask & (gContext.mAxisMask - 1)) != 0; + + ImGuiIO& io = ImGui::GetIO(); + MOVETYPE type = MT_NONE; + + vec_t deltaScreen = { io.MousePos.x - gContext.mScreenSquareCenter.x, io.MousePos.y - gContext.mScreenSquareCenter.y, 0.f, 0.f }; + float dist = deltaScreen.Length(); + if (Intersects(op, ROTATE_SCREEN) && dist >= (gContext.mRadiusSquareCenter - 4.0f) && dist < (gContext.mRadiusSquareCenter + 4.0f)) + { + if (!isNoAxesMasked) + return MT_NONE; + type = MT_ROTATE_SCREEN; + } + + const vec_t planNormals[] = { gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir }; + + vec_t modelViewPos; + modelViewPos.TransformPoint(gContext.mModel.v.position, gContext.mViewMat); + + for (int i = 0; i < 3 && type == MT_NONE; i++) + { + if(!Intersects(op, static_cast(ROTATE_X << i))) + { + continue; + } + bool isAxisMasked = ((1 << i) & gContext.mAxisMask) != 0; + // pickup plan + vec_t pickupPlan = BuildPlan(gContext.mModel.v.position, planNormals[i]); + + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, pickupPlan); + const vec_t intersectWorldPos = gContext.mRayOrigin + gContext.mRayVector * len; + vec_t intersectViewPos; + intersectViewPos.TransformPoint(intersectWorldPos, gContext.mViewMat); + + if (ImAbs(modelViewPos.z) - ImAbs(intersectViewPos.z) < -FLT_EPSILON) + { + continue; + } + + const vec_t localPos = intersectWorldPos - gContext.mModel.v.position; + vec_t idealPosOnCircle = Normalized(localPos); + idealPosOnCircle.TransformVector(gContext.mModelInverse); + const ImVec2 idealPosOnCircleScreen = worldToPos(idealPosOnCircle * rotationDisplayFactor * gContext.mScreenFactor, gContext.mMVP); + + //gContext.mDrawList->AddCircle(idealPosOnCircleScreen, 5.f, IM_COL32_WHITE); + const ImVec2 distanceOnScreen = idealPosOnCircleScreen - io.MousePos; + + const float distance = makeVect(distanceOnScreen).Length(); + if (distance < 8.f) // pixel size + { + if ((!isAxisMasked || isMultipleAxesMasked) && !isNoAxesMasked) + break; + type = static_cast(MT_ROTATE_X + i); + } + } + + return type; + } + + static MOVETYPE GetMoveType(OPERATION op, vec_t* gizmoHitProportion) + { + if(!Intersects(op, TRANSLATE) || gContext.mbUsing || !gContext.mbMouseOver) + { + return MT_NONE; + } + + bool isNoAxesMasked = !gContext.mAxisMask; + bool isMultipleAxesMasked = (gContext.mAxisMask & (gContext.mAxisMask - 1)) != 0; + + ImGuiIO& io = ImGui::GetIO(); + MOVETYPE type = MT_NONE; + + // screen + if (io.MousePos.x >= gContext.mScreenSquareMin.x && io.MousePos.x <= gContext.mScreenSquareMax.x && + io.MousePos.y >= gContext.mScreenSquareMin.y && io.MousePos.y <= gContext.mScreenSquareMax.y && + Contains(op, TRANSLATE)) + { + type = MT_MOVE_SCREEN; + } + + const vec_t screenCoord = makeVect(io.MousePos - ImVec2(gContext.mX, gContext.mY)); + + // compute + for (int i = 0; i < 3 && type == MT_NONE; i++) + { + bool isAxisMasked = ((1 << i) & gContext.mAxisMask) != 0; + vec_t dirPlaneX, dirPlaneY, dirAxis; + bool belowAxisLimit, belowPlaneLimit; + ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit); + dirAxis.TransformVector(gContext.mModel); + dirPlaneX.TransformVector(gContext.mModel); + dirPlaneY.TransformVector(gContext.mModel); + + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, BuildPlan(gContext.mModel.v.position, dirAxis)); + vec_t posOnPlan = gContext.mRayOrigin + gContext.mRayVector * len; + + const ImVec2 axisStartOnScreen = worldToPos(gContext.mModel.v.position + dirAxis * gContext.mScreenFactor * 0.1f, gContext.mViewProjection) - ImVec2(gContext.mX, gContext.mY); + const ImVec2 axisEndOnScreen = worldToPos(gContext.mModel.v.position + dirAxis * gContext.mScreenFactor, gContext.mViewProjection) - ImVec2(gContext.mX, gContext.mY); + + vec_t closestPointOnAxis = PointOnSegment(screenCoord, makeVect(axisStartOnScreen), makeVect(axisEndOnScreen)); + if ((closestPointOnAxis - screenCoord).Length() < 12.f && Intersects(op, static_cast(TRANSLATE_X << i))) // pixel size + { + if (isAxisMasked) + break; + type = static_cast(MT_MOVE_X + i); + } + + const float dx = dirPlaneX.Dot3((posOnPlan - gContext.mModel.v.position) * (1.f / gContext.mScreenFactor)); + const float dy = dirPlaneY.Dot3((posOnPlan - gContext.mModel.v.position) * (1.f / gContext.mScreenFactor)); + if (belowPlaneLimit && dx >= quadUV[0] && dx <= quadUV[4] && dy >= quadUV[1] && dy <= quadUV[3] && Contains(op, TRANSLATE_PLANS[i])) + { + if ((!isAxisMasked || isMultipleAxesMasked) && !isNoAxesMasked) + break; + type = static_cast(MT_MOVE_YZ + i); + } + + if (gizmoHitProportion) + { + *gizmoHitProportion = makeVect(dx, dy, 0.f); + } + } + return type; + } + + static bool HandleTranslation(float* matrix, float* deltaMatrix, OPERATION op, MOVETYPE& type, const float* snap) + { + if(!Intersects(op, TRANSLATE) || type != MT_NONE) + { + return false; + } + const ImGuiIO& io = ImGui::GetIO(); + const bool applyRotationLocaly = gContext.mMode == LOCAL || gContext.mCurrentHandleType == MT_MOVE_SCREEN; + bool modified = false; + + // move + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID) && IsTranslateType(gContext.mCurrentHandleType)) + { +#if IMGUI_VERSION_NUM >= 18723 + ImGui::SetNextFrameWantCaptureMouse(true); +#else + ImGui::CaptureMouseFromApp(); +#endif + const float signedLength = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan); + const float len = fabsf(signedLength); // near plan + const vec_t newPos = gContext.mRayOrigin + gContext.mRayVector * len; + + // compute delta + const vec_t newOrigin = newPos - gContext.mRelativeOrigin * gContext.mScreenFactor; + vec_t delta = newOrigin - gContext.mModel.v.position; + + // 1 axis constraint + if (gContext.mCurrentHandleType >= MT_MOVE_X && gContext.mCurrentHandleType <= MT_MOVE_Z) + { + const int axisIndex = gContext.mCurrentHandleType - MT_MOVE_X; + const vec_t& axisValue = *(vec_t*)&gContext.mModel.m[axisIndex]; + const float lengthOnAxis = Dot(axisValue, delta); + delta = axisValue * lengthOnAxis; + } + + // snap + if (snap) + { + vec_t cumulativeDelta = gContext.mModel.v.position + delta - gContext.mMatrixOrigin; + if (applyRotationLocaly) + { + matrix_t modelSourceNormalized = gContext.mModelSource; + modelSourceNormalized.OrthoNormalize(); + matrix_t modelSourceNormalizedInverse; + modelSourceNormalizedInverse.Inverse(modelSourceNormalized); + cumulativeDelta.TransformVector(modelSourceNormalizedInverse); + ComputeSnap(cumulativeDelta, snap); + cumulativeDelta.TransformVector(modelSourceNormalized); + } + else + { + ComputeSnap(cumulativeDelta, snap); + } + delta = gContext.mMatrixOrigin + cumulativeDelta - gContext.mModel.v.position; + + } + + if (delta != gContext.mTranslationLastDelta) + { + modified = true; + } + gContext.mTranslationLastDelta = delta; + + // compute matrix & delta + matrix_t deltaMatrixTranslation; + deltaMatrixTranslation.Translation(delta); + if (deltaMatrix) + { + memcpy(deltaMatrix, deltaMatrixTranslation.m16, sizeof(float) * 16); + } + + const matrix_t res = gContext.mModelSource * deltaMatrixTranslation; + *(matrix_t*)matrix = res; + + if (!io.MouseDown[0]) + { + gContext.mbUsing = false; + } + + type = gContext.mCurrentHandleType; + } + else + { + // find new possible way to move + vec_t gizmoHitProportion; + type = gContext.mbOverGizmoHotspot ? MT_NONE : GetMoveType(op, &gizmoHitProportion); + gContext.mbOverGizmoHotspot |= type != MT_NONE; + if (type != MT_NONE) + { +#if IMGUI_VERSION_NUM >= 18723 + ImGui::SetNextFrameWantCaptureMouse(true); +#else + ImGui::CaptureMouseFromApp(); +#endif + } + if (CanActivate() && type != MT_NONE) + { + gContext.mActiveTripodState = gContext.mTripodState; + gContext.mbUsing = true; + gContext.mEditingID = gContext.GetCurrentID(); + gContext.mCurrentHandleType = type; + vec_t movePlanNormal[] = { gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir, + gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir, + -gContext.mCameraDir }; + + vec_t cameraToModelNormalized = Normalized(gContext.mModel.v.position - gContext.mCameraEye); + for (unsigned int i = 0; i < 3; i++) + { + vec_t orthoVector = Cross(movePlanNormal[i], cameraToModelNormalized); + movePlanNormal[i].Cross(orthoVector); + movePlanNormal[i].Normalize(); + } + // pickup plan + gContext.mTranslationPlan = BuildPlan(gContext.mModel.v.position, movePlanNormal[type - MT_MOVE_X]); + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan); + gContext.mTranslationPlanOrigin = gContext.mRayOrigin + gContext.mRayVector * len; + gContext.mMatrixOrigin = gContext.mModel.v.position; + + gContext.mRelativeOrigin = (gContext.mTranslationPlanOrigin - gContext.mModel.v.position) * (1.f / gContext.mScreenFactor); + } + } + return modified; + } + + static bool HandleScale(float* matrix, float* deltaMatrix, OPERATION op, MOVETYPE& type, const float* snap) + { + if((!Intersects(op, SCALE) && !Intersects(op, SCALEU)) || type != MT_NONE || !gContext.mbMouseOver) + { + return false; + } + ImGuiIO& io = ImGui::GetIO(); + bool modified = false; + + if (!gContext.mbUsing) + { + // find new possible way to scale + type = gContext.mbOverGizmoHotspot ? MT_NONE : GetScaleType(op); + gContext.mbOverGizmoHotspot |= type != MT_NONE; + + if (type != MT_NONE) + { +#if IMGUI_VERSION_NUM >= 18723 + ImGui::SetNextFrameWantCaptureMouse(true); +#else + ImGui::CaptureMouseFromApp(); +#endif + } + if (CanActivate() && type != MT_NONE) + { + gContext.mActiveTripodState = gContext.mTripodState; + gContext.mbUsing = true; + gContext.mEditingID = gContext.GetCurrentID(); + gContext.mCurrentHandleType = type; + const vec_t movePlanNormal[] = { gContext.mModelLocal.v.up, gContext.mModelLocal.v.dir, gContext.mModelLocal.v.right, gContext.mModelLocal.v.dir, gContext.mModelLocal.v.up, gContext.mModelLocal.v.right, -gContext.mCameraDir }; + // pickup plan + + gContext.mTranslationPlan = BuildPlan(gContext.mModelLocal.v.position, movePlanNormal[type - MT_SCALE_X]); + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan); + gContext.mTranslationPlanOrigin = gContext.mRayOrigin + gContext.mRayVector * len; + gContext.mMatrixOrigin = gContext.mModelLocal.v.position; + gContext.mScale.Set(1.f, 1.f, 1.f); + gContext.mRelativeOrigin = (gContext.mTranslationPlanOrigin - gContext.mModelLocal.v.position) * (1.f / gContext.mScreenFactor); + gContext.mScaleValueOrigin = makeVect(gContext.mModelSource.v.right.Length(), gContext.mModelSource.v.up.Length(), gContext.mModelSource.v.dir.Length()); + gContext.mSaveMousePosx = io.MousePos.x; + } + } + // scale + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID) && IsScaleType(gContext.mCurrentHandleType)) + { +#if IMGUI_VERSION_NUM >= 18723 + ImGui::SetNextFrameWantCaptureMouse(true); +#else + ImGui::CaptureMouseFromApp(); +#endif + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan); + vec_t newPos = gContext.mRayOrigin + gContext.mRayVector * len; + vec_t newOrigin = newPos - gContext.mRelativeOrigin * gContext.mScreenFactor; + vec_t delta = newOrigin - gContext.mModelLocal.v.position; + + // 1 axis constraint + if (gContext.mCurrentHandleType >= MT_SCALE_X && gContext.mCurrentHandleType <= MT_SCALE_Z) + { + int axisIndex = gContext.mCurrentHandleType - MT_SCALE_X; + const vec_t& axisValue = *(vec_t*)&gContext.mModelLocal.m[axisIndex]; + float lengthOnAxis = Dot(axisValue, delta); + delta = axisValue * lengthOnAxis; + + vec_t baseVector = gContext.mTranslationPlanOrigin - gContext.mModelLocal.v.position; + float ratio = Dot(axisValue, baseVector + delta) / Dot(axisValue, baseVector); + + gContext.mScale[axisIndex] = max(ratio, 0.001f); + } + else + { + float scaleDelta = (io.MousePos.x - gContext.mSaveMousePosx) * 0.01f; + gContext.mScale.Set(max(1.f + scaleDelta, 0.001f)); + } + + // snap + if (snap) + { + float scaleSnap[] = { snap[0], snap[0], snap[0] }; + ComputeSnap(gContext.mScale, scaleSnap); + } + + // no 0 allowed + for (int i = 0; i < 3; i++) + gContext.mScale[i] = max(gContext.mScale[i], 0.001f); + + if (gContext.mScaleLast != gContext.mScale) + { + modified = true; + } + gContext.mScaleLast = gContext.mScale; + + // compute matrix & delta + matrix_t deltaMatrixScale; + deltaMatrixScale.Scale(gContext.mScale * gContext.mScaleValueOrigin); + + matrix_t res = deltaMatrixScale * gContext.mModelLocal; + *(matrix_t*)matrix = res; + + if (deltaMatrix) + { + vec_t deltaScale = gContext.mScale * gContext.mScaleValueOrigin; + + vec_t originalScaleDivider; + originalScaleDivider.x = 1 / gContext.mModelScaleOrigin.x; + originalScaleDivider.y = 1 / gContext.mModelScaleOrigin.y; + originalScaleDivider.z = 1 / gContext.mModelScaleOrigin.z; + + deltaScale = deltaScale * originalScaleDivider; + + deltaMatrixScale.Scale(deltaScale); + memcpy(deltaMatrix, deltaMatrixScale.m16, sizeof(float) * 16); + } + + if (!io.MouseDown[0]) + { + gContext.mbUsing = false; + gContext.mScale.Set(1.f, 1.f, 1.f); + } + + type = gContext.mCurrentHandleType; + } + return modified; + } + + static bool HandleRotation(float* matrix, float* deltaMatrix, OPERATION op, MOVETYPE& type, const float* snap) + { + if(!Intersects(op, ROTATE) || type != MT_NONE || !gContext.mbMouseOver) + { + return false; + } + ImGuiIO& io = ImGui::GetIO(); + bool applyRotationLocaly = gContext.mMode == LOCAL; + bool modified = false; + + if (!gContext.mbUsing) + { + type = gContext.mbOverGizmoHotspot ? MT_NONE : GetRotateType(op); + gContext.mbOverGizmoHotspot |= type != MT_NONE; + + if (type != MT_NONE) + { +#if IMGUI_VERSION_NUM >= 18723 + ImGui::SetNextFrameWantCaptureMouse(true); +#else + ImGui::CaptureMouseFromApp(); +#endif + } + + if (type == MT_ROTATE_SCREEN) + { + applyRotationLocaly = true; + } + + if (CanActivate() && type != MT_NONE) + { + gContext.mbUsing = true; + gContext.mEditingID = gContext.GetCurrentID(); + gContext.mCurrentHandleType = type; + const vec_t rotatePlanNormal[] = { gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir, -gContext.mCameraDir }; + // pickup plan + if (applyRotationLocaly) + { + gContext.mTranslationPlan = BuildPlan(gContext.mModel.v.position, rotatePlanNormal[type - MT_ROTATE_X]); + } + else + { + gContext.mTranslationPlan = BuildPlan(gContext.mModelSource.v.position, directionUnary[type - MT_ROTATE_X]); + } + + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan); + vec_t localPos = gContext.mRayOrigin + gContext.mRayVector * len - gContext.mModel.v.position; + gContext.mRotationVectorSource = Normalized(localPos); + gContext.mRotationAngleOrigin = ComputeAngleOnPlan(); + } + } + + // rotation + if (gContext.mbUsing && (gContext.GetCurrentID() == gContext.mEditingID) && IsRotateType(gContext.mCurrentHandleType)) + { +#if IMGUI_VERSION_NUM >= 18723 + ImGui::SetNextFrameWantCaptureMouse(true); +#else + ImGui::CaptureMouseFromApp(); +#endif + gContext.mRotationAngle = ComputeAngleOnPlan(); + if (snap) + { + float snapInRadian = snap[0] * DEG2RAD; + ComputeSnap(&gContext.mRotationAngle, snapInRadian); + } + vec_t rotationAxisLocalSpace; + + rotationAxisLocalSpace.TransformVector(makeVect(gContext.mTranslationPlan.x, gContext.mTranslationPlan.y, gContext.mTranslationPlan.z, 0.f), gContext.mModelInverse); + rotationAxisLocalSpace.Normalize(); + + matrix_t deltaRotation; + deltaRotation.RotationAxis(rotationAxisLocalSpace, gContext.mRotationAngle - gContext.mRotationAngleOrigin); + if (gContext.mRotationAngle != gContext.mRotationAngleOrigin) + { + modified = true; + } + gContext.mRotationAngleOrigin = gContext.mRotationAngle; + + matrix_t scaleOrigin; + scaleOrigin.Scale(gContext.mModelScaleOrigin); + + if (applyRotationLocaly) + { + *(matrix_t*)matrix = scaleOrigin * deltaRotation * gContext.mModelLocal; + } + else + { + matrix_t res = gContext.mModelSource; + res.v.position.Set(0.f); + + *(matrix_t*)matrix = res * deltaRotation; + ((matrix_t*)matrix)->v.position = gContext.mModelSource.v.position; + } + + if (deltaMatrix) + { + *(matrix_t*)deltaMatrix = gContext.mModelInverse * deltaRotation * gContext.mModel; + } + + if (!io.MouseDown[0]) + { + gContext.mbUsing = false; + gContext.mEditingID = -1; + } + type = gContext.mCurrentHandleType; + } + return modified; + } + + void DecomposeMatrixToComponents(const float* matrix, float* translation, float* rotation, float* scale) + { + matrix_t mat = *(matrix_t*)matrix; + + scale[0] = mat.v.right.Length(); + scale[1] = mat.v.up.Length(); + scale[2] = mat.v.dir.Length(); + + mat.OrthoNormalize(); + + rotation[0] = RAD2DEG * atan2f(mat.m[1][2], mat.m[2][2]); + rotation[1] = RAD2DEG * atan2f(-mat.m[0][2], sqrtf(mat.m[1][2] * mat.m[1][2] + mat.m[2][2] * mat.m[2][2])); + rotation[2] = RAD2DEG * atan2f(mat.m[0][1], mat.m[0][0]); + + translation[0] = mat.v.position.x; + translation[1] = mat.v.position.y; + translation[2] = mat.v.position.z; + } + + void RecomposeMatrixFromComponents(const float* translation, const float* rotation, const float* scale, float* matrix) + { + matrix_t& mat = *(matrix_t*)matrix; + + matrix_t rot[3]; + for (int i = 0; i < 3; i++) + { + rot[i].RotationAxis(directionUnary[i], rotation[i] * DEG2RAD); + } + + mat = rot[0] * rot[1] * rot[2]; + + float validScale[3]; + for (int i = 0; i < 3; i++) + { + if (fabsf(scale[i]) < FLT_EPSILON) + { + validScale[i] = 0.001f; + } + else + { + validScale[i] = scale[i]; + } + } + mat.v.right *= validScale[0]; + mat.v.up *= validScale[1]; + mat.v.dir *= validScale[2]; + mat.v.position.Set(translation[0], translation[1], translation[2], 1.f); + } + + void SetAlternativeWindow(ImGuiWindow* window) + { + gContext.mAlternativeWindow = window; + } + + void SetID(int id) + { + if (gContext.mIDStack.empty()) + { + gContext.mIDStack.push_back(-1); + } + gContext.mIDStack.back() = id; + } + + ImGuiID GetID(const char* str, const char* str_end) + { + ImGuiID seed = gContext.GetCurrentID(); + ImGuiID id = ImHashStr(str, str_end ? (str_end - str) : 0, seed); + return id; + } + + ImGuiID GetID(const char* str) + { + return GetID(str, nullptr); + } + + ImGuiID GetID(const void* ptr) + { + ImGuiID seed = gContext.GetCurrentID(); + ImGuiID id = ImHashData(&ptr, sizeof(void*), seed); + return id; + } + + ImGuiID GetID(int n) + { + ImGuiID seed = gContext.GetCurrentID(); + ImGuiID id = ImHashData(&n, sizeof(n), seed); + return id; + } + + void PushID(const char* str_id) + { + ImGuiID id = GetID(str_id); + gContext.mIDStack.push_back(id); + } + + void PushID(const char* str_id_begin, const char* str_id_end) + { + ImGuiID id = GetID(str_id_begin, str_id_end); + gContext.mIDStack.push_back(id); + } + + void PushID(const void* ptr_id) + { + ImGuiID id = GetID(ptr_id); + gContext.mIDStack.push_back(id); + } + + void PushID(int int_id) + { + ImGuiID id = GetID(int_id); + gContext.mIDStack.push_back(id); + } + + void PopID() + { + IM_ASSERT(gContext.mIDStack.Size > 1); // Too many PopID(), or could be popping in a wrong/different window? + gContext.mIDStack.pop_back(); + if (gContext.mIDStack.empty()) + { + gContext.mIDStack.clear(); + } + } + + void AllowAxisFlip(bool value) + { + gContext.mAllowAxisFlip = value; + } + + void SetAxisLimit(float value) + { + gContext.mAxisLimit=value; + } + + void SetAxisMask(bool x, bool y, bool z) + { + gContext.mAxisMask = (x ? 1 : 0) + (y ? 2 : 0) + (z ? 4 : 0); + } + + void SetPlaneLimit(float value) + { + gContext.mPlaneLimit = value; + } + + bool IsOver(float* position, float pixelRadius) + { + const ImGuiIO& io = ImGui::GetIO(); + + float radius = sqrtf((ImLengthSqr(worldToPos({ position[0], position[1], position[2], 0.0f }, gContext.mViewProjection) - io.MousePos))); + return radius < pixelRadius; + } + + bool Manipulate(const float* view, const float* projection, OPERATION operation, MODE mode, float* matrix, float* deltaMatrix, const float* snap, const float* localBounds, const float* boundsSnap) + { + gContext.mDrawList->PushClipRect (ImVec2 (gContext.mX, gContext.mY), ImVec2 (gContext.mX + gContext.mWidth, gContext.mY + gContext.mHeight), false); + + // Scale is always local or matrix will be skewed when applying world scale or oriented matrix + ComputeContext(view, projection, matrix, (operation & SCALE) ? LOCAL : mode); + gContext.mHoveredHandleType = MT_NONE; + + // set delta to identity + if (deltaMatrix) + { + ((matrix_t*)deltaMatrix)->SetToIdentity(); + } + + // behind camera + vec_t camSpacePosition; + camSpacePosition.TransformPoint(makeVect(0.f, 0.f, 0.f), gContext.mMVP); + if (!gContext.mIsOrthographic && camSpacePosition.z < 0.001f && !gContext.mbUsing) + { + return false; + } + + // -- + MOVETYPE type = MT_NONE; + bool manipulated = false; + if (gContext.mbEnable) + { + if (!gContext.mbUsingBounds) + { + manipulated = HandleTranslation(matrix, deltaMatrix, operation, type, snap) || + HandleScale(matrix, deltaMatrix, operation, type, snap) || + HandleRotation(matrix, deltaMatrix, operation, type, snap); + } + } + + if (localBounds && !gContext.mbUsing) + { + manipulated |= HandleAndDrawLocalBounds(localBounds, (matrix_t*)matrix, boundsSnap, operation); + } + + gContext.mOperation = operation; + gContext.mHoveredHandleType = (!gContext.mbUsing && !gContext.mbUsingBounds) ? type : MT_NONE; + if (!gContext.mbUsingBounds) + { + DrawRotationGizmo(operation, type); + DrawTranslationGizmo(operation, type); + DrawScaleGizmo(operation, type); + DrawScaleUniveralGizmo(operation, type); + } + + gContext.mDrawList->PopClipRect (); + return manipulated; + } + + void SetGizmoSizeClipSpace(float value) + { + gContext.mGizmoSizeClipSpace = value; + } + + /////////////////////////////////////////////////////////////////////////////////////////////////// + void ComputeFrustumPlanes(vec_t* frustum, const float* clip) + { + frustum[0].x = clip[3] - clip[0]; + frustum[0].y = clip[7] - clip[4]; + frustum[0].z = clip[11] - clip[8]; + frustum[0].w = clip[15] - clip[12]; + + frustum[1].x = clip[3] + clip[0]; + frustum[1].y = clip[7] + clip[4]; + frustum[1].z = clip[11] + clip[8]; + frustum[1].w = clip[15] + clip[12]; + + frustum[2].x = clip[3] + clip[1]; + frustum[2].y = clip[7] + clip[5]; + frustum[2].z = clip[11] + clip[9]; + frustum[2].w = clip[15] + clip[13]; + + frustum[3].x = clip[3] - clip[1]; + frustum[3].y = clip[7] - clip[5]; + frustum[3].z = clip[11] - clip[9]; + frustum[3].w = clip[15] - clip[13]; + + frustum[4].x = clip[3] - clip[2]; + frustum[4].y = clip[7] - clip[6]; + frustum[4].z = clip[11] - clip[10]; + frustum[4].w = clip[15] - clip[14]; + + frustum[5].x = clip[3] + clip[2]; + frustum[5].y = clip[7] + clip[6]; + frustum[5].z = clip[11] + clip[10]; + frustum[5].w = clip[15] + clip[14]; + + for (int i = 0; i < 6; i++) + { + frustum[i].Normalize(); + } + } + + void DrawAxes(const float* view, const float* projection, const float* matrices, int matrixCount) + { + matrix_t viewM = *(matrix_t*)view; + matrix_t projM = *(matrix_t*)projection; + matrix_t viewProj = viewM * projM; + + // build frustum + vec_t frustum[6]; + ComputeFrustumPlanes(frustum, viewProj.m16); + + for (int i = 0; i < matrixCount; i++) + { + const float* matrix = &matrices[i * 16]; + matrix_t model = *(matrix_t*)matrix; + matrix_t mvp = model * viewProj; + + // world-space origin + vec_t origin; + origin.TransformPoint(vec_t(0.f, 0.f, 0.f), model); + + struct Axis + { + vec_t dir; + ImU32 color; + }; + + Axis axes[3] = { + { vec_t(1.f, 0.f, 0.f), IM_COL32(255, 0, 0, 255) }, // X - red + { vec_t(0.f, 1.f, 0.f), IM_COL32(0, 255, 0, 255) }, // Y - green + { vec_t(0.f, 0.f, 1.f), IM_COL32(0, 0, 255, 255) } // Z - blue + }; + + for (int a = 0; a < 3; a++) + { + vec_t endLocal = axes[a].dir; + + // world-space endpoint + vec_t end; + end.TransformPoint(endLocal, model); + + // frustum test + bool visible = true; + for (int f = 0; f < 6; f++) + { + float d0 = DistanceToPlane(origin, frustum[f]); + float d1 = DistanceToPlane(end, frustum[f]); + + // both outside - reject + if (d0 < 0.f && d1 < 0.f) + { + visible = false; + break; + } + } + + if (!visible) + { + continue; + } + + // project to screen + ImVec2 p0 = worldToPos(vec_t(0.f, 0.f, 0.f), mvp); + ImVec2 p1 = worldToPos(endLocal, mvp); + + // reject behind camera (clip space) + vec_t clip0, clip1; + clip0.TransformPoint(vec_t(0.f, 0.f, 0.f), mvp); + clip1.TransformPoint(endLocal, mvp); + + if (clip0.w <= 0.f && clip1.w <= 0.f) + { + continue; + } + + // draw + gContext.mDrawList->AddLine(p0, p1, axes[a].color, 2.0f); + } + } + } + + void DrawCubes(const float* view, const float* projection, const float* matrices, int matrixCount) + { + matrix_t viewInverse; + viewInverse.Inverse(*(matrix_t*)view); + + struct CubeFace + { + float z; + ImVec2 faceCoordsScreen[4]; + ImU32 color; + }; + CubeFace* faces = (CubeFace*)_malloca(sizeof(CubeFace) * matrixCount * 6); + + if (!faces) + { + return; + } + + vec_t frustum[6]; + matrix_t viewProjection = *(matrix_t*)view * *(matrix_t*)projection; + ComputeFrustumPlanes(frustum, viewProjection.m16); + + int cubeFaceCount = 0; + for (int cube = 0; cube < matrixCount; cube++) + { + const float* matrix = &matrices[cube * 16]; + + matrix_t res = *(matrix_t*)matrix * *(matrix_t*)view * *(matrix_t*)projection; + + for (int iFace = 0; iFace < 6; iFace++) + { + const int normalIndex = (iFace % 3); + const int perpXIndex = (normalIndex + 1) % 3; + const int perpYIndex = (normalIndex + 2) % 3; + const float invert = (iFace > 2) ? -1.f : 1.f; + + const vec_t faceCoords[4] = { directionUnary[normalIndex] + directionUnary[perpXIndex] + directionUnary[perpYIndex], + directionUnary[normalIndex] + directionUnary[perpXIndex] - directionUnary[perpYIndex], + directionUnary[normalIndex] - directionUnary[perpXIndex] - directionUnary[perpYIndex], + directionUnary[normalIndex] - directionUnary[perpXIndex] + directionUnary[perpYIndex], + }; + + // clipping + /* + bool skipFace = false; + for (unsigned int iCoord = 0; iCoord < 4; iCoord++) + { + vec_t camSpacePosition; + camSpacePosition.TransformPoint(faceCoords[iCoord] * 0.5f * invert, res); + if (camSpacePosition.z < 0.001f) + { + skipFace = true; + break; + } + } + if (skipFace) + { + continue; + } + */ + vec_t centerPosition, centerPositionVP; + centerPosition.TransformPoint(directionUnary[normalIndex] * 0.5f * invert, *(matrix_t*)matrix); + centerPositionVP.TransformPoint(directionUnary[normalIndex] * 0.5f * invert, res); + + bool inFrustum = true; + for (int iFrustum = 0; iFrustum < 6; iFrustum++) + { + const vec_t& plane = frustum[iFrustum]; + + bool allOutside = true; + + for (unsigned int iCoord = 0; iCoord < 4; iCoord++) + { + vec_t worldPos; + worldPos.TransformPoint(faceCoords[iCoord] * 0.5f * invert, *(matrix_t*)matrix); + + if (DistanceToPlane(worldPos, plane) >= 0.f) + { + allOutside = false; + break; + } + } + + if (allOutside) + { + continue; // face is fully outside this plane - discard + } + } + + if (!inFrustum) + { + continue; + } + CubeFace& cubeFace = faces[cubeFaceCount]; + + // 3D->2D + //ImVec2 faceCoordsScreen[4]; + for (unsigned int iCoord = 0; iCoord < 4; iCoord++) + { + cubeFace.faceCoordsScreen[iCoord] = worldToPos(faceCoords[iCoord] * 0.5f * invert, res); + } + + ImU32 directionColor = GetColorU32(DIRECTION_X + normalIndex); + cubeFace.color = directionColor | IM_COL32(0x80, 0x80, 0x80, 0); + + cubeFace.z = centerPositionVP.z / centerPositionVP.w; + cubeFaceCount++; + } + } + qsort(faces, cubeFaceCount, sizeof(CubeFace), [](void const* _a, void const* _b) { + CubeFace* a = (CubeFace*)_a; + CubeFace* b = (CubeFace*)_b; + if (a->z < b->z) + { + return 1; + } + return -1; + }); + // draw face with lighter color + for (int iFace = 0; iFace < cubeFaceCount; iFace++) + { + const CubeFace& cubeFace = faces[iFace]; + gContext.mDrawList->AddConvexPolyFilled(cubeFace.faceCoordsScreen, 4, cubeFace.color); + } + + _freea(faces); + } + + void DrawGrid(const float* view, const float* projection, const float* matrix, const float gridSize) + { + matrix_t viewProjection = *(matrix_t*)view * *(matrix_t*)projection; + vec_t frustum[6]; + ComputeFrustumPlanes(frustum, viewProjection.m16); + matrix_t res = *(matrix_t*)matrix * viewProjection; + + for (float f = -gridSize; f <= gridSize; f += 1.f) + { + for (int dir = 0; dir < 2; dir++) + { + vec_t ptA = makeVect(dir ? -gridSize : f, 0.f, dir ? f : -gridSize); + vec_t ptB = makeVect(dir ? gridSize : f, 0.f, dir ? f : gridSize); + bool visible = true; + for (int i = 0; i < 6; i++) + { + float dA = DistanceToPlane(ptA, frustum[i]); + float dB = DistanceToPlane(ptB, frustum[i]); + if (dA < 0.f && dB < 0.f) + { + visible = false; + break; + } + if (dA > 0.f && dB > 0.f) + { + continue; + } + if (dA < 0.f) + { + float len = fabsf(dA - dB); + float t = fabsf(dA) / len; + ptA.Lerp(ptB, t); + } + if (dB < 0.f) + { + float len = fabsf(dB - dA); + float t = fabsf(dB) / len; + ptB.Lerp(ptA, t); + } + } + if (visible) + { + ImU32 col = IM_COL32(0x80, 0x80, 0x80, 0xFF); + col = (fmodf(fabsf(f), 10.f) < FLT_EPSILON) ? IM_COL32(0x90, 0x90, 0x90, 0xFF) : col; + col = (fabsf(f) < FLT_EPSILON) ? IM_COL32(0x40, 0x40, 0x40, 0xFF): col; + + float thickness = 1.f; + thickness = (fmodf(fabsf(f), 10.f) < FLT_EPSILON) ? 1.5f : thickness; + thickness = (fabsf(f) < FLT_EPSILON) ? 2.3f : thickness; + + gContext.mDrawList->AddLine(worldToPos(ptA, res), worldToPos(ptB, res), col, thickness); + } + } + } + } + + void DrawGridCustom(const float* view, const float* projection, const float* matrix, const float gridSize, const float majorStep, const unsigned int subdivision) + { + const ImU32 majorCol = IM_COL32(0x80, 0x80, 0x80, 0xFF); + const ImU32 minorCol = IM_COL32(0x90, 0x90, 0x90, 0xFF); + const ImU32 centerCol = IM_COL32(0x40, 0x40, 0x40, 0xFF); + DrawGridCustomColor(view, projection, matrix, gridSize, majorStep, subdivision, majorCol, minorCol, centerCol); + } + + void DrawGridCustomColor(const float* view, const float* projection, const float* matrix, const float gridSize, const float majorStep, const unsigned int subdivision, const ImU32 majorCol, const ImU32 minorCol, const ImU32 centerCol) + { + // Must have at least 1 subdivision + IM_ASSERT(subdivision > 0 && "At least 1 segment required!"); + + matrix_t viewProjection = *(matrix_t*)view * *(matrix_t*)projection; + + vec_t frustum[6]; + ComputeFrustumPlanes(frustum, viewProjection.m16); + + matrix_t res = *(matrix_t*)matrix * viewProjection; + + const float minorStep = majorStep / (float)subdivision; + const int lineCount = (int)ceilf(gridSize / minorStep); + + for (int i = -lineCount; i <= lineCount; i++) + { + float f = i * minorStep; + + const bool isMajor = (i % (int)subdivision) == 0; + const bool isCenter = (i == 0); + + // Styling + ImU32 col = minorCol; + if (isMajor) + col = majorCol; + if (isCenter) + col = centerCol; + + float thickness = 1.0f; + if (isMajor) + thickness = 1.5f; + if (isCenter) + thickness = 2.3f; + + for (int dir = 0; dir < 2; dir++) + { + vec_t ptA = makeVect(dir ? -gridSize : f, 0.f, dir ? f : -gridSize); + vec_t ptB = makeVect(dir ? gridSize : f, 0.f, dir ? f : gridSize); + + bool visible = true; + + // Frustum clipping + for (int p = 0; p < 6; p++) + { + float dA = DistanceToPlane(ptA, frustum[p]); + float dB = DistanceToPlane(ptB, frustum[p]); + + if (dA < 0.f && dB < 0.f) + { + visible = false; + break; + } + + if (dA > 0.f && dB > 0.f) + continue; + + if (dA < 0.f) + { + float t = fabsf(dA) / fabsf(dA - dB); + ptA.Lerp(ptB, t); + } + + if (dB < 0.f) + { + float t = fabsf(dB) / fabsf(dB - dA); + ptB.Lerp(ptA, t); + } + } + + if (visible) + { + gContext.mDrawList->AddLine( + worldToPos(ptA, res), + worldToPos(ptB, res), + col, + thickness); + } + } + } + } + + void ViewManipulate(float* view, const float* projection, OPERATION operation, MODE mode, float* matrix, float length, ImVec2 position, ImVec2 size, ImU32 backgroundColor) + { + // Scale is always local or matrix will be skewed when applying world scale or oriented matrix + ComputeContext(view, projection, matrix, (operation & SCALE) ? LOCAL : mode); + ViewManipulate(view, length, position, size, backgroundColor); + } + + void ViewManipulate(float* view, float length, ImVec2 position, ImVec2 size, ImU32 backgroundColor) + { + // State is bound to the current id (PushID/PopID) so multiple view cubes are independent. + ViewManipulateState& vms = gContext.GetViewManipulateState(); + bool& isDraging = vms.mIsDragging; + bool& isClicking = vms.mIsClicking; + vec_t& interpolationUp = vms.mInterpolationUp; + vec_t& interpolationDir = vms.mInterpolationDir; + int& interpolationFrames = vms.mInterpolationFrames; + const vec_t referenceUp = makeVect(0.f, 1.f, 0.f); + + // Recompute hovering for this widget's own window instead of relying on the last + // Manipulate() call, otherwise only the most recently drawn view would react. + gContext.mbMouseOver = IsHoveringWindow(); + + matrix_t svgView, svgProjection; + svgView = gContext.mViewMat; + svgProjection = gContext.mProjectionMat; + + ImGuiIO& io = ImGui::GetIO(); + const bool viewManipulateHovered = gContext.mbMouseOver && ImRect(position, position + size).Contains(io.MousePos); + gContext.mDrawList->AddRectFilled(position, position + size, backgroundColor); + matrix_t viewInverse; + viewInverse.Inverse(*(matrix_t*)view); + const bool rightHanded = gContext.mProjectionMat.m[2][3] < 0.f; + const float handednessSign = rightHanded ? 1.f : -1.f; + const vec_t camTarget = viewInverse.v.position - viewInverse.v.dir * handednessSign * length; + + // view/projection matrices + const float distance = 3.f; + matrix_t cubeProjection, cubeView; + float fov = acosf(distance / (sqrtf(distance * distance + 3.f))) * RAD2DEG; + Perspective(fov / sqrtf(2.f), size.x / size.y, 0.01f, 1000.f, cubeProjection.m16, rightHanded); + + vec_t dir = makeVect(viewInverse.m[2][0], viewInverse.m[2][1], viewInverse.m[2][2]); + vec_t up = makeVect(viewInverse.m[1][0], viewInverse.m[1][1], viewInverse.m[1][2]); + vec_t eye = dir * handednessSign * distance; + vec_t zero = makeVect(0.f, 0.f); + LookAt(&eye.x, &zero.x, &up.x, cubeView.m16, rightHanded); + + // set context + gContext.mViewMat = cubeView; + gContext.mProjectionMat = cubeProjection; + ComputeCameraRay(gContext.mRayOrigin, gContext.mRayVector, position, size); + + const matrix_t res = cubeView * cubeProjection; + + // panels + static const ImVec2 panelPosition[9] = { ImVec2(0.75f,0.75f), ImVec2(0.25f, 0.75f), ImVec2(0.f, 0.75f), + ImVec2(0.75f, 0.25f), ImVec2(0.25f, 0.25f), ImVec2(0.f, 0.25f), + ImVec2(0.75f, 0.f), ImVec2(0.25f, 0.f), ImVec2(0.f, 0.f) }; + + static const ImVec2 panelSize[9] = { ImVec2(0.25f,0.25f), ImVec2(0.5f, 0.25f), ImVec2(0.25f, 0.25f), + ImVec2(0.25f, 0.5f), ImVec2(0.5f, 0.5f), ImVec2(0.25f, 0.5f), + ImVec2(0.25f, 0.25f), ImVec2(0.5f, 0.25f), ImVec2(0.25f, 0.25f) }; + + // tag faces + bool boxes[27]{}; + static int overBox = -1; + for (int iPass = 0; iPass < 2; iPass++) + { + for (int iFace = 0; iFace < 6; iFace++) + { + const int normalIndex = (iFace % 3); + const int perpXIndex = (normalIndex + 1) % 3; + const int perpYIndex = (normalIndex + 2) % 3; + const float invert = (iFace > 2) ? -1.f : 1.f; + const vec_t indexVectorX = directionUnary[perpXIndex] * invert; + const vec_t indexVectorY = directionUnary[perpYIndex] * invert; + const vec_t boxOrigin = directionUnary[normalIndex] * -invert - indexVectorX - indexVectorY; + + // plan local space + const vec_t n = directionUnary[normalIndex] * invert; + vec_t viewSpaceNormal = n; + vec_t viewSpacePoint = n * 0.5f; + viewSpaceNormal.TransformVector(cubeView); + viewSpaceNormal.Normalize(); + viewSpacePoint.TransformPoint(cubeView); + const vec_t viewSpaceFacePlan = BuildPlan(viewSpacePoint, viewSpaceNormal); + + // back face culling + if (viewSpaceFacePlan.w > 0.f) + { + continue; + } + + const vec_t facePlan = BuildPlan(n * 0.5f, n); + + const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, facePlan); + vec_t posOnPlan = gContext.mRayOrigin + gContext.mRayVector * len - (n * 0.5f); + + float localx = Dot(directionUnary[perpXIndex], posOnPlan) * invert + 0.5f; + float localy = Dot(directionUnary[perpYIndex], posOnPlan) * invert + 0.5f; + + // panels + const vec_t dx = directionUnary[perpXIndex]; + const vec_t dy = directionUnary[perpYIndex]; + const vec_t origin = directionUnary[normalIndex] - dx - dy; + for (int iPanel = 0; iPanel < 9; iPanel++) + { + vec_t boxCoord = boxOrigin + indexVectorX * float(iPanel % 3) + indexVectorY * float(iPanel / 3) + makeVect(1.f, 1.f, 1.f); + const ImVec2 p = panelPosition[iPanel] * 2.f; + const ImVec2 s = panelSize[iPanel] * 2.f; + ImVec2 faceCoordsScreen[4]; + vec_t panelPos[4] = { dx * p.x + dy * p.y, + dx * p.x + dy * (p.y + s.y), + dx * (p.x + s.x) + dy * (p.y + s.y), + dx * (p.x + s.x) + dy * p.y }; + + for (unsigned int iCoord = 0; iCoord < 4; iCoord++) + { + faceCoordsScreen[iCoord] = worldToPos((panelPos[iCoord] + origin) * 0.5f * invert, res, position, size); + } + + const ImVec2 panelCorners[2] = { panelPosition[iPanel], panelPosition[iPanel] + panelSize[iPanel] }; + bool insidePanel = localx > panelCorners[0].x && localx < panelCorners[1].x && localy > panelCorners[0].y && localy < panelCorners[1].y; + int boxCoordInt = int(boxCoord.x * 9.f + boxCoord.y * 3.f + boxCoord.z); + IM_ASSERT(boxCoordInt < 27); + boxes[boxCoordInt] |= insidePanel && (!isDraging) && gContext.mbMouseOver; + + // draw face with lighter color + if (iPass) + { + ImU32 directionColor = GetColorU32(DIRECTION_X + normalIndex); + gContext.mDrawList->AddConvexPolyFilled(faceCoordsScreen, 4, (directionColor | IM_COL32(0x80, 0x80, 0x80, 0x80)) | (viewManipulateHovered ? IM_COL32(0x08, 0x08, 0x08, 0) : 0)); + if (boxes[boxCoordInt]) + { + ImU32 selectionColor = GetColorU32(SELECTION); + gContext.mDrawList->AddConvexPolyFilled(faceCoordsScreen, 4, selectionColor); + + if (io.MouseDown[0] && !isClicking && !isDraging && GImGui->ActiveId == 0) { + overBox = boxCoordInt; + isClicking = true; + isDraging = true; + } + } + } + } + } + } + if (interpolationFrames) + { + interpolationFrames--; + vec_t newDir = viewInverse.v.dir * handednessSign; + newDir.Lerp(interpolationDir * handednessSign, 0.2f); + newDir.Normalize(); + + vec_t newUp = viewInverse.v.up; + newUp.Lerp(interpolationUp, 0.3f); + newUp.Normalize(); + newUp = interpolationUp; + vec_t newEye = camTarget + newDir * length; + LookAt(&newEye.x, &camTarget.x, &newUp.x, view, rightHanded); + } + gContext.mIsViewManipulatorHovered |= viewManipulateHovered; + + if (io.MouseDown[0] && (fabsf(io.MouseDelta[0]) || fabsf(io.MouseDelta[1])) && isClicking) + { + isClicking = false; + } + + if (!io.MouseDown[0]) + { + if (isClicking) + { + // apply new view direction + int cx = overBox / 9; + int cy = (overBox - cx * 9) / 3; + int cz = overBox % 3; + interpolationDir = makeVect(1.f - (float)cx, 1.f - (float)cy, 1.f - (float)cz) * handednessSign; + interpolationDir.Normalize(); + + if (fabsf(Dot(interpolationDir, referenceUp)) > 1.0f - 0.01f) + { + vec_t right = viewInverse.v.right; + if (fabsf(right.x) > fabsf(right.z)) + { + right.z = 0.f; + } + else + { + right.x = 0.f; + } + right.Normalize(); + interpolationUp = Cross(interpolationDir * handednessSign, right); + interpolationUp.Normalize(); + } + else + { + interpolationUp = referenceUp; + } + interpolationFrames = 40; + + } + isClicking = false; + isDraging = false; + } + + + if (isDraging) + { + matrix_t rx, ry, roll; + + rx.RotationAxis(referenceUp, -io.MouseDelta.x * 0.01f * handednessSign); + ry.RotationAxis(viewInverse.v.right, -io.MouseDelta.y * 0.01f * handednessSign); + + roll = rx * ry; + + vec_t newDir = viewInverse.v.dir; + newDir.TransformVector(roll); + newDir.Normalize(); + + // clamp + vec_t planDir = Cross(viewInverse.v.right, referenceUp); + planDir.y = 0.f; + planDir.Normalize(); + float dt = Dot(planDir, newDir); + if (dt < 0.0f) + { + newDir += planDir * dt; + newDir.Normalize(); + } + + vec_t newEye = camTarget + newDir * handednessSign * length; + LookAt(&newEye.x, &camTarget.x, &referenceUp.x, view, rightHanded); + } + + const bool thisUsingViewManipulate = (interpolationFrames != 0) || isDraging; + gContext.mbUsingViewManipulate |= thisUsingViewManipulate; + if (isClicking || thisUsingViewManipulate || viewManipulateHovered) { +#if IMGUI_VERSION_NUM >= 18723 + ImGui::SetNextFrameWantCaptureMouse(true); +#else + ImGui::CaptureMouseFromApp(); +#endif + } + + // restore view/projection because it was used to compute ray + ComputeContext(svgView.m16, svgProjection.m16, gContext.mModelSource.m16, gContext.mMode); + } +}; diff --git a/extern/imguizmo/ImGuizmo.h b/extern/imguizmo/ImGuizmo.h new file mode 100644 index 0000000..ab893f1 --- /dev/null +++ b/extern/imguizmo/ImGuizmo.h @@ -0,0 +1,348 @@ +// https://github.com/CedricGuillemet/ImGuizmo +// v1.92.5 WIP +// +// The MIT License(MIT) +// +// Copyright(c) 2016-2026 Cedric Guillemet and contributors +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files(the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and / or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions : +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// ------------------------------------------------------------------------------------------- +// History : +// 2026/05/03 v1.9 release. CMake build system, vcpkg support, many fixes and new features from contributors. +// 2019/11/03 View gizmo +// 2016/09/11 Behind camera culling. Scaling Delta matrix not multiplied by source matrix scales. local/world rotation and translation fixed. Display message is incorrect (X: ... Y:...) in local mode. +// 2016/09/09 Hatched negative axis. Snapping. Documentation update. +// 2016/09/04 Axis switch and translation plan autohiding. Scale transform stability improved +// 2016/09/01 Mogwai changed to Manipulate. Draw debug cube. Fixed inverted scale. Mixing scale and translation/rotation gives bad results. +// 2016/08/31 First version +// +// ------------------------------------------------------------------------------------------- +// Future (no order): +// +// - Multi view +// - display rotation/translation/scale infos in local/world space and not only local +// - finish local/world matrix application +// - OPERATION as bitmask +// +// ------------------------------------------------------------------------------------------- +// Example +#if 0 +void EditTransform(const Camera& camera, matrix_t& matrix) +{ + static ImGuizmo::OPERATION mCurrentGizmoOperation(ImGuizmo::ROTATE); + static ImGuizmo::MODE mCurrentGizmoMode(ImGuizmo::WORLD); + if (ImGui::IsKeyPressed(90)) + mCurrentGizmoOperation = ImGuizmo::TRANSLATE; + if (ImGui::IsKeyPressed(69)) + mCurrentGizmoOperation = ImGuizmo::ROTATE; + if (ImGui::IsKeyPressed(82)) // r Key + mCurrentGizmoOperation = ImGuizmo::SCALE; + if (ImGui::RadioButton("Translate", mCurrentGizmoOperation == ImGuizmo::TRANSLATE)) + mCurrentGizmoOperation = ImGuizmo::TRANSLATE; + ImGui::SameLine(); + if (ImGui::RadioButton("Rotate", mCurrentGizmoOperation == ImGuizmo::ROTATE)) + mCurrentGizmoOperation = ImGuizmo::ROTATE; + ImGui::SameLine(); + if (ImGui::RadioButton("Scale", mCurrentGizmoOperation == ImGuizmo::SCALE)) + mCurrentGizmoOperation = ImGuizmo::SCALE; + float matrixTranslation[3], matrixRotation[3], matrixScale[3]; + ImGuizmo::DecomposeMatrixToComponents(matrix.m16, matrixTranslation, matrixRotation, matrixScale); + ImGui::InputFloat3("Tr", matrixTranslation, 3); + ImGui::InputFloat3("Rt", matrixRotation, 3); + ImGui::InputFloat3("Sc", matrixScale, 3); + ImGuizmo::RecomposeMatrixFromComponents(matrixTranslation, matrixRotation, matrixScale, matrix.m16); + + if (mCurrentGizmoOperation != ImGuizmo::SCALE) + { + if (ImGui::RadioButton("Local", mCurrentGizmoMode == ImGuizmo::LOCAL)) + mCurrentGizmoMode = ImGuizmo::LOCAL; + ImGui::SameLine(); + if (ImGui::RadioButton("World", mCurrentGizmoMode == ImGuizmo::WORLD)) + mCurrentGizmoMode = ImGuizmo::WORLD; + } + static bool useSnap(false); + if (ImGui::IsKeyPressed(83)) + useSnap = !useSnap; + ImGui::Checkbox("", &useSnap); + ImGui::SameLine(); + vec_t snap; + switch (mCurrentGizmoOperation) + { + case ImGuizmo::TRANSLATE: + snap = config.mSnapTranslation; + ImGui::InputFloat3("Snap", &snap.x); + break; + case ImGuizmo::ROTATE: + snap = config.mSnapRotation; + ImGui::InputFloat("Angle Snap", &snap.x); + break; + case ImGuizmo::SCALE: + snap = config.mSnapScale; + ImGui::InputFloat("Scale Snap", &snap.x); + break; + } + ImGuiIO& io = ImGui::GetIO(); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + ImGuizmo::Manipulate(camera.mView.m16, camera.mProjection.m16, mCurrentGizmoOperation, mCurrentGizmoMode, matrix.m16, NULL, useSnap ? &snap.x : NULL); +} +#endif +#pragma once + +#ifdef USE_IMGUI_API +#include "imconfig.h" +#endif +#ifndef IMGUI_API +#define IMGUI_API +#endif + +#ifndef IMGUIZMO_NAMESPACE +#define IMGUIZMO_NAMESPACE ImGuizmo +#endif + +struct ImGuiWindow; + +namespace IMGUIZMO_NAMESPACE +{ + // call inside your own window and before Manipulate() in order to draw gizmo to that window. + // Or pass a specific ImDrawList to draw to (e.g. ImGui::GetForegroundDrawList()). + IMGUI_API void SetDrawlist(ImDrawList* drawlist = nullptr); + + // call BeginFrame right after ImGui_XXXX_NewFrame(); + IMGUI_API void BeginFrame(); + + // this is necessary because when imguizmo is compiled into a dll, and imgui into another + // globals are not shared between them. + // More details at https://stackoverflow.com/questions/19373061/what-happens-to-global-and-static-variables-in-a-shared-library-when-it-is-dynam + // expose method to set imgui context + IMGUI_API void SetImGuiContext(ImGuiContext* ctx); + + // return true if mouse cursor is over any gizmo control (axis, plan or screen component) + IMGUI_API bool IsOver(); + + // return true if mouse IsOver or if the gizmo is in moving state + IMGUI_API bool IsUsing(); + + // return true if the view gizmo is in moving state + IMGUI_API bool IsUsingViewManipulate(); + // only check if your mouse is over the view manipulator - no matter whether it's active or not + IMGUI_API bool IsViewManipulateHovered(); + + // return true if any gizmo is in moving state + IMGUI_API bool IsUsingAny(); + + // enable/disable the gizmo. Stay in the state until next call to Enable. + // gizmo is rendered with gray half transparent color when disabled + IMGUI_API void Enable(bool enable); + + // helper functions for manualy editing translation/rotation/scale with an input float + // translation, rotation and scale float points to 3 floats each + // Angles are in degrees (more suitable for human editing) + // example: + // float matrixTranslation[3], matrixRotation[3], matrixScale[3]; + // ImGuizmo::DecomposeMatrixToComponents(gizmoMatrix.m16, matrixTranslation, matrixRotation, matrixScale); + // ImGui::InputFloat3("Tr", matrixTranslation, 3); + // ImGui::InputFloat3("Rt", matrixRotation, 3); + // ImGui::InputFloat3("Sc", matrixScale, 3); + // ImGuizmo::RecomposeMatrixFromComponents(matrixTranslation, matrixRotation, matrixScale, gizmoMatrix.m16); + // + // These functions have some numerical stability issues for now. Use with caution. + IMGUI_API void DecomposeMatrixToComponents(const float* matrix, float* translation, float* rotation, float* scale); + IMGUI_API void RecomposeMatrixFromComponents(const float* translation, const float* rotation, const float* scale, float* matrix); + + IMGUI_API void SetRect(float x, float y, float width, float height); + // default is false + IMGUI_API void SetOrthographic(bool isOrthographic); + + // Render coordinate system axes (red X, green Y and blue Z). Usefull for debug/tests + IMGUI_API void DrawAxes(const float* view, const float* projection, const float* matrices, int matrixCount); + // Render a cube with face color corresponding to face normal. Usefull for debug/tests + IMGUI_API void DrawCubes(const float* view, const float* projection, const float* matrices, int matrixCount); + IMGUI_API void DrawGrid(const float* view, const float* projection, const float* matrix, const float gridSize); + // Render grid with customizable major line step and amount of segments between major lines. + // NOTE(m.wlasiuk) : calling this function with majorStep = 1.0f and subdivision = 1 is equivalent to DrawGrid in terms of the end result but performs more calculations + IMGUI_API void DrawGridCustom(const float* view, const float* projection, const float* matrix, const float gridSize, const float majorStep, const unsigned int subdivision); + // Render grid with customizable major line step and amount of segments between major lines and with possibility to set custom colors for major, minor and center lines + IMGUI_API void DrawGridCustomColor(const float* view, const float* projection, const float* matrix, const float gridSize, const float majorStep, const unsigned int subdivision, const ImU32 majorCol, const ImU32 minorCol, const ImU32 centerCol); + + // call it when you want a gizmo + // Needs view and projection matrices. + // matrix parameter is the source matrix (where will be gizmo be drawn) and might be transformed by the function. Return deltaMatrix is optional + // translation is applied in world space + enum OPERATION + { + TRANSLATE_X = (1u << 0), + TRANSLATE_Y = (1u << 1), + TRANSLATE_Z = (1u << 2), + ROTATE_X = (1u << 3), + ROTATE_Y = (1u << 4), + ROTATE_Z = (1u << 5), + ROTATE_SCREEN = (1u << 6), + SCALE_X = (1u << 7), + SCALE_Y = (1u << 8), + SCALE_Z = (1u << 9), + BOUNDS = (1u << 10), + SCALE_XU = (1u << 11), + SCALE_YU = (1u << 12), + SCALE_ZU = (1u << 13), + + TRANSLATE = TRANSLATE_X | TRANSLATE_Y | TRANSLATE_Z, + ROTATE = ROTATE_X | ROTATE_Y | ROTATE_Z | ROTATE_SCREEN, + SCALE = SCALE_X | SCALE_Y | SCALE_Z, + SCALEU = SCALE_XU | SCALE_YU | SCALE_ZU, // universal + UNIVERSAL = TRANSLATE | ROTATE | SCALEU + }; + + inline OPERATION operator|(OPERATION lhs, OPERATION rhs) + { + return static_cast(static_cast(lhs) | static_cast(rhs)); + } + + enum MODE + { + LOCAL, + WORLD + }; + + IMGUI_API bool Manipulate(const float* view, const float* projection, OPERATION operation, MODE mode, float* matrix, float* deltaMatrix = NULL, const float* snap = NULL, const float* localBounds = NULL, const float* boundsSnap = NULL); + // + // Please note that this cubeview is patented by Autodesk : https://patents.google.com/patent/US7782319B2/en + // It seems to be a defensive patent in the US. I don't think it will bring troubles using it as + // other software are using the same mechanics. But just in case, you are now warned! + // + IMGUI_API void ViewManipulate(float* view, float length, ImVec2 position, ImVec2 size, ImU32 backgroundColor); + + // use this version if you did not call Manipulate before and you are just using ViewManipulate + IMGUI_API void ViewManipulate(float* view, const float* projection, OPERATION operation, MODE mode, float* matrix, float length, ImVec2 position, ImVec2 size, ImU32 backgroundColor); + + IMGUI_API void SetAlternativeWindow(ImGuiWindow* window); + + [[deprecated("Use PushID/PopID instead.")]] + IMGUI_API void SetID(int id); + + // ID stack/scopes + // Read the FAQ (docs/FAQ.md or http://dearimgui.org/faq) for more details about how ID are handled in dear imgui. + // - Those questions are answered and impacted by understanding of the ID stack system: + // - "Q: Why is my widget not reacting when I click on it?" + // - "Q: How can I have widgets with an empty label?" + // - "Q: How can I have multiple widgets with the same label?" + // - Short version: ID are hashes of the entire ID stack. If you are creating widgets in a loop you most likely + // want to push a unique identifier (e.g. object pointer, loop index) to uniquely differentiate them. + // - You can also use the "Label##foobar" syntax within widget label to distinguish them from each others. + // - In this header file we use the "label"/"name" terminology to denote a string that will be displayed + used as an ID, + // whereas "str_id" denote a string that is only used as an ID and not normally displayed. + IMGUI_API void PushID(const char* str_id); // push string into the ID stack (will hash string). + IMGUI_API void PushID(const char* str_id_begin, const char* str_id_end); // push string into the ID stack (will hash string). + IMGUI_API void PushID(const void* ptr_id); // push pointer into the ID stack (will hash pointer). + IMGUI_API void PushID(int int_id); // push integer into the ID stack (will hash integer). + IMGUI_API void PopID(); // pop from the ID stack. + IMGUI_API ImGuiID GetID(const char* str_id); // calculate unique ID (hash of whole ID stack + given parameter). e.g. if you want to query into ImGuiStorage yourself + IMGUI_API ImGuiID GetID(const char* str_id_begin, const char* str_id_end); + IMGUI_API ImGuiID GetID(const void* ptr_id); + + // return true if the cursor is over the operation's gizmo + IMGUI_API bool IsOver(OPERATION op); + IMGUI_API void SetGizmoSizeClipSpace(float value); + + // Handle type used by the translate/rotate/scale gizmos. + enum MOVETYPE + { + MT_NONE, + MT_MOVE_X, + MT_MOVE_Y, + MT_MOVE_Z, + MT_MOVE_YZ, + MT_MOVE_ZX, + MT_MOVE_XY, + MT_MOVE_SCREEN, + MT_ROTATE_X, + MT_ROTATE_Y, + MT_ROTATE_Z, + MT_ROTATE_SCREEN, + MT_SCALE_X, + MT_SCALE_Y, + MT_SCALE_Z, + MT_SCALE_XYZ + }; + + // Returns which handle is actively being dragged, or MT_NONE. + IMGUI_API MOVETYPE GetActiveHandleType(); + // Returns which handle is currently hovered, or MT_NONE. + IMGUI_API MOVETYPE GetHoveredHandleType(); + // Aliases matching the MOVETYPE enum name. + IMGUI_API MOVETYPE GetActiveMoveType(); + IMGUI_API MOVETYPE GetHoveredMoveType(); + + // Allow axis to flip + // When true (default), the guizmo axis flip for better visibility + // When false, they always stay along the positive world/local axis + IMGUI_API void AllowAxisFlip(bool value); + + // Configure the limit where axis are hidden + IMGUI_API void SetAxisLimit(float value); + // Set an axis mask to permanently hide a given axis (true -> hidden, false -> shown) + IMGUI_API void SetAxisMask(bool x, bool y, bool z); + // Configure the limit where planes are hiden + IMGUI_API void SetPlaneLimit(float value); + // from a x,y,z point in space and using Manipulation view/projection matrix, check if mouse is in pixel radius distance of that projected point + IMGUI_API bool IsOver(float* position, float pixelRadius); + + // Compute the world-space mouse picking ray from explicit inputs, without reading ImGui IO. + // Useful for tests/headless usage. view and projection are column-major float[16] (same + // layout as Manipulate). rayOrigin and rayDirection receive a float[3] each. + IMGUI_API void ComputeMouseRay(const float* view, const float* projection, const ImVec2& mousePosition, const ImVec2& rectPosition, const ImVec2& rectSize, float* rayOrigin, float* rayDirection); + + enum COLOR + { + DIRECTION_X, // directionColor[0] + DIRECTION_Y, // directionColor[1] + DIRECTION_Z, // directionColor[2] + PLANE_X, // planeColor[0] + PLANE_Y, // planeColor[1] + PLANE_Z, // planeColor[2] + SELECTION, // selectionColor + INACTIVE, // inactiveColor + TRANSLATION_LINE, // translationLineColor + SCALE_LINE, + ROTATION_USING_BORDER, + ROTATION_USING_FILL, + HATCHED_AXIS_LINES, + TEXT, + TEXT_SHADOW, + COUNT + }; + + struct Style + { + IMGUI_API Style(); + + float TranslationLineThickness; // Thickness of lines for translation gizmo + float TranslationLineArrowSize; // Size of arrow at the end of lines for translation gizmo + float RotationLineThickness; // Thickness of lines for rotation gizmo + float RotationOuterLineThickness; // Thickness of line surrounding the rotation gizmo + float ScaleLineThickness; // Thickness of lines for scale gizmo + float ScaleLineCircleSize; // Size of circle at the end of lines for scale gizmo + float HatchedAxisLineThickness; // Thickness of hatched axis lines + float CenterCircleSize; // Size of circle at the center of the translate/scale gizmo + + ImVec4 Colors[COLOR::COUNT]; + }; + + IMGUI_API Style& GetStyle(); +} diff --git a/extern/imguizmo/LICENSE b/extern/imguizmo/LICENSE new file mode 100644 index 0000000..dcbee45 --- /dev/null +++ b/extern/imguizmo/LICENSE @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2016 Cedric Guillemet + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/include/box3d/collision.h b/include/box3d/collision.h index cb99349..ab1e86f 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -212,6 +212,17 @@ B3_INLINE const float* b3GetHullSoaNormals( const b3HullData* hull ) return (const float*)( (intptr_t)hull + hull->soaNormalOffset ); } +/// Get the dot products of the two adjacent face normals for each full edge. +B3_INLINE const float* b3GetHullEdgeCosines( const b3HullData* hull ) +{ + if ( hull->edgeCosineOffset == 0 ) + { + return NULL; + } + + return (const float*)( (intptr_t)hull + hull->edgeCosineOffset ); +} + /// Create a tessellated cylinder as a hull. B3_API b3HullData* b3CreateCylinder( float height, float radius, float yOffset, int sides ); diff --git a/include/box3d/types.h b/include/box3d/types.h index fb88971..09552f7 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -2019,7 +2019,7 @@ typedef struct b3HullFace } b3HullFace; /// 64-bit hull version. Useful for validating serialized data. -#define B3_HULL_VERSION 0x4A4C9587DE57485Cull +#define B3_HULL_VERSION 0x9D3E61B7C2A4F085ull /// A convex hull. /// @note This data structure has data hanging off the end and cannot be directly copied. @@ -2043,7 +2043,7 @@ typedef struct b3HullData /// The radius of the largest sphere at the center. float innerRadius; - /// The local centroid + /// The local centroid. b3Vec3 center; /// The inertia tensor about the centroid. @@ -2058,7 +2058,7 @@ typedef struct b3HullData /// Offset of the point array in bytes from the struct address. int32_t pointOffset; - /// This is the half-edge count (double the edge count) + /// This is the half-edge count (double the edge count). int32_t edgeCount; /// Offset of the edge array in bytes from the struct address. @@ -2073,15 +2073,21 @@ typedef struct b3HullData /// Offset of the face array in bytes from the struct address. int32_t faceOffset; - /// Offset of structure of array (SOA) vertices + /// Offset of structure of array (SOA) vertices. int32_t soaVertexOffset; - /// Offset of structure of array (SOA) unit normal vectors + /// Offset of structure of array (SOA) unit normal vectors. int32_t soaNormalOffset; + /// Offset of dot(n1, n2) for each full edge. + int32_t edgeCosineOffset; + /// The total number of bytes for this hull. int32_t byteCount; + /// Explicit padding for determinism. + int32_t padding; + /// Any padding must be explicit. } b3HullData; @@ -2102,6 +2108,7 @@ typedef struct b3BoxHull float nx[8]; ///< normal x, padded to multiple of 4 float ny[8]; ///< normal y, padded to multiple of 4 float nz[8]; ///< normal z, padded to multiple of 4 + float edgeCosines[12]; ///< dot(n1, n2) for each full edge. } b3BoxHull; /**@}*/ // hull diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 5a628e8..257de2e 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -80,6 +80,20 @@ set_target_properties(implot PROPERTIES POSITION_INDEPENDENT_CODE ON ) +# ImGuizmo vendored from github.com/CedricGuillemet/ImGuizmo at 18cef5e (2026-08-08), ImGuizmo.cpp/.h only. +add_library(imguizmo STATIC + "${CMAKE_SOURCE_DIR}/extern/imguizmo/ImGuizmo.cpp" + "${CMAKE_SOURCE_DIR}/extern/imguizmo/ImGuizmo.h" +) +target_link_libraries(imguizmo PUBLIC imgui) +target_include_directories(imguizmo SYSTEM PUBLIC "${CMAKE_SOURCE_DIR}/extern/imguizmo") +set_target_properties(imguizmo PROPERTIES + CXX_STANDARD 20 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS OFF + POSITION_INDEPENDENT_CODE ON +) + # --- sokol: vendored headers INTERFACE target + per-platform backend. -------- set(sokol_SOURCE_DIR "${CMAKE_SOURCE_DIR}/extern/sokol") add_library(sokol INTERFACE) @@ -204,7 +218,7 @@ target_include_directories(samples PRIVATE # gfx: the renderer + Draw*/adapter. shared: human, utils (Random*), the # benchmark/determinism helpers the samples lean on. -target_link_libraries(samples PRIVATE gfx shared imgui implot sokol nfd) +target_link_libraries(samples PRIVATE gfx shared imgui implot imguizmo sokol nfd) # MSVC auto-links winmm via a #pragma comment in main.cpp. MinGW ignores that # pragma, so link the timeBeginPeriod/timeEndPeriod library explicitly. diff --git a/samples/gfx/draw.c b/samples/gfx/draw.c index c9afc3b..a870a5b 100644 --- a/samples/gfx/draw.c +++ b/samples/gfx/draw.c @@ -295,7 +295,7 @@ void DrawGrid( b3Pos center, b3Vec3 normal, float halfExtent, int divisions, Vec void DrawGroundGrid( int size ) { - Vec4 color = MakeVec4( 0.3f, 0.3f, 0.3f, 1.0f ); + Vec4 color = MakeVec4( 0.3f, 0.3f, 0.3f, 0.3f ); DrawGrid( b3Pos_zero, b3Vec3_axisY, (float)size, size, color ); } diff --git a/samples/gfx/keycodes.h b/samples/gfx/keycodes.h index cea7766..a2a62c8 100644 --- a/samples/gfx/keycodes.h +++ b/samples/gfx/keycodes.h @@ -15,6 +15,7 @@ enum KEY_B = SAPP_KEYCODE_B, KEY_D = SAPP_KEYCODE_D, KEY_F = SAPP_KEYCODE_F, + KEY_G = SAPP_KEYCODE_G, KEY_L = SAPP_KEYCODE_L, KEY_M = SAPP_KEYCODE_M, KEY_O = SAPP_KEYCODE_O, diff --git a/samples/main.cpp b/samples/main.cpp index 5e0fda5..a92df26 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -554,22 +554,18 @@ static sapp_desc BuildAppDesc( int argc, char** argv ) return desc; } -// We own main (SOKOL_NO_ENTRY) so the leak dump can run after sapp_run returns, -// once sokol has torn down its window and context. On Windows sapp_run returns -// after that teardown; on macOS it may not return, so the dump is Windows only. +// The leak dump runs at CRT exit, after static destructors. Libraries such as ImGuizmo keep +// heap backed containers at file scope, and a dump right after sapp_run reports them. int main( int argc, char** argv ) { #if defined( _MSC_VER ) _CrtSetReportMode( _CRT_WARN, _CRTDBG_MODE_DEBUG | _CRTDBG_MODE_FILE ); _CrtSetReportFile( _CRT_WARN, _CRTDBG_FILE_STDOUT ); + _CrtSetDbgFlag( _CrtSetDbgFlag( _CRTDBG_REPORT_FLAG ) | _CRTDBG_LEAK_CHECK_DF ); #endif sapp_desc desc = BuildAppDesc( argc, argv ); sapp_run( &desc ); -#if defined( _MSC_VER ) - _CrtDumpMemoryLeaks(); -#endif - return s_exitCode; } diff --git a/samples/sample.cpp b/samples/sample.cpp index 4f6e21b..1d8dcbf 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -1064,7 +1064,7 @@ void Sample::DrawMetrics() ImGui::SetItemTooltip( "tree height" ); ImGui::Text( "movable tree %d", s.treeHeight ); ImGui::SetItemTooltip( "tree height" ); - ImGui::Text( "alloc %lld K", (long long)( s.byteCount / 1024 ) ); + ImGui::Text( "world %lld K", (long long)( s.byteCount / 1024 ) ); ImGui::Text( "stack %d K", s.stackUsed / 1024 ); ImGui::Text( "arena %d K", s.arenaCapacity / 1024 ); diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index fd13ae7..35e235e 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -12,6 +12,10 @@ #include +#ifdef BOX3D_INTERNAL_BENCHMARKS +#include "sat_benchmark.h" +#endif + inline bool operator<( b3BodyId a, b3BodyId b ) { uint64_t ua = b3StoreBodyId( a ); @@ -1561,3 +1565,145 @@ public: }; static int benchmarkSleep = RegisterSample( "Benchmark", "Sleep", BenchmarkSleep::Create ); + +#ifdef BOX3D_INTERNAL_BENCHMARKS + +// GJK versus SAT on hull pairs held inside the speculative margin. Times are per query and smoothed over frames. +class BenchmarkSeparatingAxis : public Sample +{ +public: + explicit BenchmarkSeparatingAxis( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 4.0f, b3Pos_zero ); + } + + m_data = CreateSatBenchmark( (SatHullType)m_typeA, (SatHullType)m_typeB ); + } + + ~BenchmarkSeparatingAxis() override + { + DestroySatBenchmark( m_data ); + } + + bool HasSolverControls() const override + { + return false; + } + + bool HasProfile() const override + { + return false; + } + + bool DrawControls() override + { + const char* hullNames[] = { "Complex", "Rock", "Cylinder" }; + static_assert( IM_ARRAYSIZE( hullNames ) == satHull_count ); + + ImGui::PushItemWidth( 8.0f * ImGui::GetFontSize() ); + + bool changed = ImGui::Combo( "Hull A", &m_typeA, hullNames, IM_ARRAYSIZE( hullNames ) ); + changed = ImGui::Combo( "Hull B", &m_typeB, hullNames, IM_ARRAYSIZE( hullNames ) ) || changed; + if ( changed ) + { + DestroySatBenchmark( m_data ); + m_data = CreateSatBenchmark( (SatHullType)m_typeA, (SatHullType)m_typeB ); + m_resetAverage = true; + } + + m_resetAverage = ImGui::Checkbox( "Inscribed Sphere", &m_inscribedSphere ) || m_resetAverage; + m_resetAverage = ImGui::Checkbox( "Warm Start GJK", &m_warmStart ) || m_resetAverage; + ImGui::SliderInt( "Pair", &m_pairIndex, 0, SAT_PAIR_COUNT - 1 ); + + ImGui::PopItemWidth(); + return true; + } + + void Render() override + { + b3Transform transformB = m_data->transforms[m_pairIndex]; + b3WorldTransform worldA = b3MakeWorldTransform( b3Transform_identity ); + b3WorldTransform worldB = b3MakeWorldTransform( transformB ); + + DrawHull( worldA, m_data->hullA, MakeColor( b3_colorGreen ) ); + DrawHull( worldB, m_data->hullB, MakeColor( b3_colorCyan ) ); + + b3DistanceInput input = {}; + input.proxyA = { b3GetHullPoints( m_data->hullA ), m_data->hullA->vertexCount, 0.0f }; + input.proxyB = { b3GetHullPoints( m_data->hullB ), m_data->hullB->vertexCount, 0.0f }; + input.transform = transformB; + input.useRadii = false; + + b3SimplexCache cache = {}; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, nullptr, 0 ); + + b3Pos pA = b3TransformWorldPoint( worldA, output.pointA ); + b3Pos pB = b3TransformWorldPoint( worldA, output.pointB ); + DrawPoint( pA, 8.0f, MakeColor( b3_colorRed ) ); + DrawPoint( pB, 8.0f, MakeColor( b3_colorYellow ) ); + DrawLine( pA, pB, MakeColor( b3_colorWhite ) ); + + Sample::Render(); + } + + void Step() override + { + EnableSatInscribedSphere( m_data, m_inscribedSphere ); + + int repeatCount = m_isDebug ? 1 : 10; + SatBenchmarkResult result = RunSatBenchmark( m_data, repeatCount, m_warmStart ); + + float scale = 1000.0f / result.queryCount; + float distanceTime = scale * result.distanceMs; + float satTime = scale * result.satMs; + + if ( m_resetAverage ) + { + m_distanceTime = distanceTime; + m_satTime = satTime; + m_resetAverage = false; + } + else + { + float alpha = 0.05f; + m_distanceTime = b3LerpFloat( m_distanceTime, distanceTime, alpha ); + m_satTime = b3LerpFloat( m_satTime, satTime, alpha ); + } + + DrawTextLine( "pairs = %d, queries per frame = %d", SAT_PAIR_COUNT, result.queryCount ); + DrawTextLine( "GJK (us) = %.3f, iterations = %.2f", m_distanceTime, result.averageIterations ); + DrawTextLine( "SAT (us) = %.3f", m_satTime ); + DrawTextLine( "SAT / GJK = %.2f", m_satTime / m_distanceTime ); + DrawTextLine( "target dist = %.4f, avg dist = %.4f, avg sep = %.4f", m_data->targetDistance, + result.averageDistance, result.averageSeparation ); + + if ( result.earlyReturnCount > 0 ) + { + DrawTextLine( "SAT early returns = %d", result.earlyReturnCount ); + } + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkSeparatingAxis( context ); + } + + SatBenchmarkData* m_data; + int m_typeA = satHull_complex; + int m_typeB = satHull_complex; + int m_pairIndex = 0; + bool m_inscribedSphere = true; + bool m_warmStart = false; + bool m_resetAverage = true; + float m_distanceTime = 0.0f; + float m_satTime = 0.0f; +}; + +static int sampleSeparatingAxis = RegisterSample( "Benchmark", "SAT vs GJK", BenchmarkSeparatingAxis::Create ); + +#endif diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 12d77b2..08e8847 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -10,14 +10,55 @@ #include -// Trackball about the camera axes so the near side of the shape follows the cursor regardless of the -// shape or camera orientation. A drag across the window height is a full turn. -static b3Quat DragRotation( const Camera* camera, b3Quat q, float dx, float dy ) +#include + +static void DrawGizmoControls( bool* show, bool* local ) { - float scale = 2.0f * B3_PI / b3MaxFloat( (float)camera->m_height, 1.0f ); - b3Quat yaw = b3MakeQuatFromAxisAngle( camera->GetUp(), scale * dx ); - b3Quat pitch = b3MakeQuatFromAxisAngle( camera->GetRight(), scale * dy ); - return b3NormalizeQuat( b3MulQuat( b3MulQuat( pitch, yaw ), q ) ); + ImGui::Checkbox( "Gizmo (G)", show ); + ImGui::Checkbox( "Local axes", local ); +} + +// Must run inside the ImGui frame. The view is eye relative, so the shape is placed against the draw origin. +static void DrawTransformGizmo( const Camera* camera, b3WorldTransform* transform, bool local ) +{ + ImGuizmo::BeginFrame(); + + // Behind the panels like the in-world labels + ImGuizmo::SetDrawlist( ImGui::GetBackgroundDrawList() ); + + ImGuiViewport* viewport = ImGui::GetMainViewport(); + ImGuizmo::SetRect( viewport->Pos.x, viewport->Pos.y, viewport->Size.x, viewport->Size.y ); + ImGuizmo::SetOrthographic( false ); + + // Alt drag orbits the camera + ImGuizmo::Enable( ImGui::GetIO().KeyAlt == false ); + + ImGuizmo::OPERATION operation = ImGuizmo::TRANSLATE | ImGuizmo::ROTATE; + + b3Pos origin = GetDrawOrigin(); + b3Vec3 offset = b3SubPos( transform->p, origin ); + b3Matrix3 r = b3MakeMatrixFromQuat( transform->q ); + Mat4 matrix = { + { r.cx.x, r.cx.y, r.cx.z, 0.0f }, + { r.cy.x, r.cy.y, r.cy.z, 0.0f }, + { r.cz.x, r.cz.y, r.cz.z, 0.0f }, + { offset.x, offset.y, offset.z, 1.0f }, + }; + + Mat4 view = camera->View(); + Mat4 proj = camera->Proj(); + ImGuizmo::MODE space = local ? ImGuizmo::LOCAL : ImGuizmo::WORLD; + if ( ImGuizmo::Manipulate( &view.cx.x, &proj.cx.x, operation, space, &matrix.cx.x ) ) + { + // Rotation drags accumulate in the matrix, the quaternion keeps it orthonormal + b3Matrix3 m = { + { matrix.cx.x, matrix.cx.y, matrix.cx.z }, + { matrix.cy.x, matrix.cy.y, matrix.cy.z }, + { matrix.cz.x, matrix.cz.y, matrix.cz.z }, + }; + transform->q = b3NormalizeQuat( b3MakeQuatFromMatrix( &m ) ); + transform->p = b3OffsetPos( origin, { matrix.cw.x, matrix.cw.y, matrix.cw.z } ); + } } class Manifold : public Sample @@ -48,22 +89,18 @@ public: m_simplexCache = {}; m_satCache = {}; m_manualFeature = 0; - m_baseTranslation = b3Pos_zero; - m_lastX = 0.0f; - m_lastY = 0.0f; - m_origin = b3Pos_zero; m_drawPoints = true; m_useCache = false; - m_tracking = false; - m_rotating = false; + m_showGizmo = true; + m_gizmoLocal = false; } void Render() override { - DrawTextLine( "origin: %g %g %g", m_origin.x, m_origin.y, m_origin.z ); DrawTextLine( "count = %d", m_manifold.pointCount ); DrawAxes( b3WorldTransform_identity, 0.5f ); + DrawGroundGrid( 80 ); if ( m_manifold.pointCount == 0 || m_drawPoints == false ) { @@ -118,51 +155,30 @@ public: } ImGui::Checkbox( "Draw Points", &m_drawPoints ); + DrawGizmoControls( &m_showGizmo, &m_gizmoLocal ); return true; } + void Keyboard( int key, int action, int modifiers ) override + { + if ( key == KEY_G && action == ACTION_PRESS && modifiers == 0 ) + { + m_showGizmo = !m_showGizmo; + } + } + + void DrawSampleWindows() override + { + if ( m_showGizmo ) + { + DrawTransformGizmo( m_camera, &m_transformB, m_gizmoLocal ); + } + } + + // The gizmo moves the shape, so keep the picking in Sample off void MouseDown( b3Vec2 p, int button, int modifiers ) override { - if ( button == 0 && ( modifiers & MOD_ALT ) == 0 ) - { - if ( modifiers & MOD_SHIFT ) - { - m_lastX = p.x; - m_lastY = p.y; - m_rotating = true; - } - else - { - PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); - m_origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); - m_baseTranslation = m_transformB.p; - m_tracking = true; - } - } - } - - void MouseUp( b3Vec2 p, int button ) override - { - m_tracking = false; - m_rotating = false; - } - - void MouseMove( b3Vec2 p ) override - { - if ( m_tracking ) - { - PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); - b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); - m_transformB.p = m_baseTranslation + b3SubPos( origin, m_origin ); - } - - if ( m_rotating ) - { - m_transformB.q = DragRotation( m_camera, m_transformB.q, p.x - m_lastX, p.y - m_lastY ); - m_lastX = p.x; - m_lastY = p.y; - } } static constexpr int m_pointCapacity = 64; @@ -170,17 +186,13 @@ public: b3LocalManifoldPoint m_points[m_pointCapacity]; b3WorldTransform m_transformA; b3WorldTransform m_transformB; - b3Pos m_baseTranslation; - b3Pos m_origin; b3SimplexCache m_simplexCache; b3SATCache m_satCache; int m_manualFeature; - float m_lastX; - float m_lastY; bool m_useCache; - bool m_tracking; - bool m_rotating; bool m_drawPoints; + bool m_showGizmo; + bool m_gizmoLocal; }; class TriangleManifold : public Sample @@ -210,25 +222,21 @@ public: m_simplexCache = {}; m_satCache = {}; - m_baseTranslation = b3Pos_zero; - m_lastX = 0.0f; - m_lastY = 0.0f; - m_origin = b3Pos_zero; m_useCache = false; - m_tracking = false; - m_rotating = false; m_manualFeature = 0; + m_showGizmo = true; + m_gizmoLocal = false; } void Render() override { - DrawTextLine( "origin: %g %g %g", m_origin.x, m_origin.y, m_origin.z ); DrawTextLine( "count = %d", m_manifold.pointCount ); DrawTextLine( "feature = %d", m_manifold.feature ); DrawTextLine( "cache hit = %d", m_satCache.hit ); DrawAxes( b3WorldTransform_identity, 1.0f ); + DrawGroundGrid( 10 ); if ( m_manifold.pointCount > 0 ) { @@ -291,50 +299,30 @@ public: ImGui::RadioButton( "edgePair", &m_manualFeature, 3 ); } + DrawGizmoControls( &m_showGizmo, &m_gizmoLocal ); + return true; } + void Keyboard( int key, int action, int modifiers ) override + { + if ( key == KEY_G && action == ACTION_PRESS && modifiers == 0 ) + { + m_showGizmo = !m_showGizmo; + } + } + + void DrawSampleWindows() override + { + if ( m_showGizmo ) + { + DrawTransformGizmo( m_camera, &m_transformB, m_gizmoLocal ); + } + } + + // The gizmo moves the shape, so keep the picking in Sample off void MouseDown( b3Vec2 p, int button, int modifiers ) override { - if ( button == 0 && ( modifiers & MOD_ALT ) == 0 ) - { - if ( modifiers & MOD_SHIFT ) - { - m_lastX = p.x; - m_lastY = p.y; - m_rotating = true; - } - else - { - PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); - m_origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); - m_baseTranslation = m_transformB.p; - m_tracking = true; - } - } - } - - void MouseUp( b3Vec2 p, int button ) override - { - m_tracking = false; - m_rotating = false; - } - - void MouseMove( b3Vec2 p ) override - { - if ( m_tracking ) - { - PickRay pickRay = m_camera->BuildPickRay( p.x, p.y ); - b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation ); - m_transformB.p = m_baseTranslation + b3SubPos( origin, m_origin ); - } - - if ( m_rotating ) - { - m_transformB.q = DragRotation( m_camera, m_transformB.q, p.x - m_lastX, p.y - m_lastY ); - m_lastX = p.x; - m_lastY = p.y; - } } static constexpr int m_pointCapacity = 8; @@ -348,16 +336,12 @@ public: b3WorldTransform m_transformB; b3Vec3 m_triangle[3] = {}; - b3Pos m_baseTranslation; - b3Pos m_origin; b3SimplexCache m_simplexCache; b3SATCache m_satCache; int m_manualFeature; - float m_lastX; - float m_lastY; bool m_useCache; - bool m_tracking; - bool m_rotating; + bool m_showGizmo; + bool m_gizmoLocal; }; class SphereAndSphere : public Manifold @@ -811,10 +795,13 @@ public: static int sampleCollideHulls = RegisterSample( "Manifold", "Hull vs Hull", HullAndHull::Create ); -// Shows the candidate axes culled by the inscribed sphere bound in the separating axis test. No axis n -// can separate the hulls by more than dot(n, centerB - centerA) - innerRadiusA - innerRadiusB, so any -// face or edge whose bound is below the best separation found so far is skipped. This recomputes the -// culling decisions of b3ComputeSeparatingAxis from public hull data and must be kept in sync with it. +// Shows the candidate axes culled in the separating axis test. No axis n can separate the hulls by more +// than dot(n, centerB - centerA) - innerRadiusA - innerRadiusB, so any face whose bound is below the best +// separation found so far is skipped. An edge survives only if the plane separation of the other hull's +// inscribed sphere can beat the best face separation somewhere over the Gauss map arc of the edge. The +// support points of the best faces lie on the other hull, so they give a second, tighter arc test. +// This recomputes the culling decisions of b3ComputeSeparatingAxis from public hull data and must be +// kept in sync with it. class HullCulling : public Manifold { public: @@ -825,6 +812,17 @@ public: e_tested, }; + enum EdgeBound + { + e_noBound = 0, + e_sphereBound, + e_supportBound, + e_bothBounds, + }; + + // Mirrors B3_EDGE_PROBE_MIN_TESTS + static constexpr int probeMinTests = 10; + explicit HullCulling( SampleContext* context ) : Manifold( context ) { @@ -847,6 +845,7 @@ public: m_showSpheres = true; m_showCulled = true; m_showNormals = true; + m_edgeBound = e_bothBounds; ResetCounts(); } @@ -884,6 +883,10 @@ public: ImGui::Checkbox( "Inscribed spheres", &m_showSpheres ); ImGui::Checkbox( "Culled features", &m_showCulled ); ImGui::Checkbox( "Face normals", &m_showNormals ); + const char* edgeBounds[] = { "None", "Sphere", "Support", "Sphere + Support" }; + ImGuiStyle& style = ImGui::GetStyle(); + ImGui::SetNextItemWidth( ImGui::CalcTextSize( edgeBounds[3] ).x + ImGui::GetFrameHeight() + 2.0f * style.FramePadding.x ); + ImGui::Combo( "Edge bound", &m_edgeBound, edgeBounds, IM_ARRAYSIZE( edgeBounds ) ); bool rebuild = ImGui::Checkbox( "Cylinders", &m_useCylinders ); if ( m_useCylinders ) @@ -912,52 +915,110 @@ public: memset( m_edgeStateB, 0, sizeof( m_edgeStateB ) ); m_seedA = 0; m_seedB = 0; + m_probeVertexA = 0; + m_probeVertexB = 0; m_keptEdgeCountA = 0; m_keptEdgeCountB = 0; + m_sphereKeptEdgeCountA = 0; + m_sphereKeptEdgeCountB = 0; m_separationA = -FLT_MAX; m_separationB = -FLT_MAX; m_gap = 0.0f; + m_probeApplied = false; m_separatedFeature = b3_invalidAxis; } - // Face of A against the vertices of B, in frame A - static float FaceSeparationA( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, int faceIndex ) + // Face of A against the vertices of B, in frame A. The deepest vertex of B is the support point. + static float FaceSeparationA( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, int faceIndex, + int* vertexIndex = nullptr ) { b3Plane plane = b3GetHullPlanes( hullA )[faceIndex]; const b3Vec3* points = b3GetHullPoints( hullB ); float separation = FLT_MAX; + int bestIndex = 0; for ( int i = 0; i < hullB->vertexCount; ++i ) { b3Vec3 point = b3TransformPoint( transformBtoA, points[i] ); - separation = b3MinFloat( separation, b3Dot( plane.normal, point ) - plane.offset ); + float s = b3Dot( plane.normal, point ) - plane.offset; + if ( s < separation ) + { + separation = s; + bestIndex = i; + } } + + if ( vertexIndex != nullptr ) + { + *vertexIndex = bestIndex; + } + return separation; } - // Face of B against the vertices of A, in frame B - static float FaceSeparationB( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, int faceIndex ) + // Face of B against the vertices of A, in frame B. The deepest vertex of A is the support point. + static float FaceSeparationB( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, int faceIndex, + int* vertexIndex = nullptr ) { b3Plane plane = b3GetHullPlanes( hullB )[faceIndex]; const b3Vec3* points = b3GetHullPoints( hullA ); float separation = FLT_MAX; + int bestIndex = 0; for ( int i = 0; i < hullA->vertexCount; ++i ) { b3Vec3 point = b3InvTransformPoint( transformBtoA, points[i] ); - separation = b3MinFloat( separation, b3Dot( plane.normal, point ) - plane.offset ); + float s = b3Dot( plane.normal, point ) - plane.offset; + if ( s < separation ) + { + separation = s; + bestIndex = i; + } } + + if ( vertexIndex != nullptr ) + { + *vertexIndex = bestIndex; + } + return separation; } - // Mirrors b3ArcCanReach - static bool ArcCanReach( float a, float b, float c, float length, float bound ) + // Mirrors b3TestEdgeCandidateSorted + static bool ArcCanReachSorted( float a, float b, float c, float bound ) { const float parallelTolerance = 1.0e-4f; + float hi = b3MaxFloat( a, b ); + float lo = b3MinFloat( a, b ); + float u = lo - c * hi; float s = 1.0f - c * c; - float t = a * a + b * b - 2.0f * a * b * c; - bool endpoint = b3MaxFloat( a, b ) >= bound; - bool interior = - a >= c * b && b >= c * a && length >= bound && ( bound <= 0.0f || s < parallelTolerance || t >= bound * bound * s ); - return endpoint || interior; + bool exterior = hi >= bound; + bool interior = u >= 0.0f && ( u * u >= ( bound - hi ) * ( bound + hi ) * s || s < parallelTolerance ); + return exterior || interior; + } + + // Arc test of every surviving edge against the plane separations of a point on the other hull. + // Mirrors the gather loops and b3FilterEdgeCandidates, edges that fail are culled. + static int CullEdges( const b3HullData* hull, b3Vec3 point, float bound, uint8_t* states ) + { + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + const float* cosines = b3GetHullEdgeCosines( hull ); + int keptCount = 0; + for ( int i = 0; i < hull->edgeCount; i += 2 ) + { + if ( states[i / 2] == e_culled ) + { + continue; + } + + b3Plane plane1 = planes[edges[i].face]; + b3Plane plane2 = planes[edges[i + 1].face]; + float d1 = b3Dot( plane1.normal, point ) - plane1.offset; + float d2 = b3Dot( plane2.normal, point ) - plane2.offset; + bool kept = ArcCanReachSorted( d1, d2, cosines[i >> 1], bound ); + states[i / 2] = kept ? e_tested : e_culled; + keptCount += kept ? 1 : 0; + } + return keptCount; } // Mirrors the culling in b3ComputeSeparatingAxis with early return enabled @@ -984,7 +1045,9 @@ public: m_seedA = dotA[i] > dotA[m_seedA] ? i : m_seedA; } - float floorA = b3MinFloat( FaceSeparationA( hullA, hullB, transformBtoA, m_seedA ), speculativeDistance ); + // The seed support vertex stands in for the probe if no face improves on it + float seedSeparationA = FaceSeparationA( hullA, hullB, transformBtoA, m_seedA, &m_probeVertexB ); + float floorA = b3MinFloat( seedSeparationA, speculativeDistance ); for ( int i = 0; i < hullA->faceCount; ++i ) { if ( dotA[i] - radiusBound < b3MaxFloat( floorA, m_separationA ) ) @@ -994,10 +1057,12 @@ public: } m_faceStateA[i] = e_tested; - float separation = FaceSeparationA( hullA, hullB, transformBtoA, i ); + int vertexIndex; + float separation = FaceSeparationA( hullA, hullB, transformBtoA, i, &vertexIndex ); if ( separation > m_separationA ) { m_separationA = separation; + m_probeVertexB = vertexIndex; if ( separation > speculativeDistance ) { m_separatedFeature = b3_faceAxisA; @@ -1014,7 +1079,8 @@ public: m_seedB = dotB[i] > dotB[m_seedB] ? i : m_seedB; } - float floorB = b3MaxFloat( FaceSeparationB( hullA, hullB, transformBtoA, m_seedB ), m_separationA ); + float seedSeparationB = FaceSeparationB( hullA, hullB, transformBtoA, m_seedB, &m_probeVertexA ); + float floorB = b3MaxFloat( seedSeparationB, m_separationA ); floorB = b3MinFloat( floorB, speculativeDistance ); for ( int i = 0; i < hullB->faceCount; ++i ) { @@ -1025,10 +1091,12 @@ public: } m_faceStateB[i] = e_tested; - float separation = FaceSeparationB( hullA, hullB, transformBtoA, i ); + int vertexIndex; + float separation = FaceSeparationB( hullA, hullB, transformBtoA, i, &vertexIndex ); if ( separation > m_separationB ) { m_separationB = separation; + m_probeVertexA = vertexIndex; if ( separation > speculativeDistance ) { m_separatedFeature = b3_faceAxisB; @@ -1037,28 +1105,42 @@ public: } } - float edgeBound = b3MaxFloat( m_separationA, m_separationB ) + radiusBound; + bool useSphere = m_edgeBound == e_sphereBound || m_edgeBound == e_bothBounds; + bool useSupport = m_edgeBound == e_supportBound || m_edgeBound == e_bothBounds; + float maxFaceSeparation = b3MaxFloat( m_separationA, m_separationB ); - const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); - for ( int i = 0; i < hullA->edgeCount; i += 2 ) + if ( useSphere ) { - int face1 = edgesA[i].face; - int face2 = edgesA[i + 1].face; - float c = b3Dot( planesA[face1].normal, planesA[face2].normal ); - bool kept = ArcCanReach( dotA[face1], dotA[face2], c, centerDistance, edgeBound ); - m_edgeStateA[i / 2] = kept ? e_tested : e_culled; - m_keptEdgeCountA += kept ? 1 : 0; + // Inscribed sphere bound, the arc inputs are plane separations of the other hull's center + float boundSlack = radius - radiusBound; + float thresholdA = maxFaceSeparation + hullB->innerRadius - boundSlack; + float thresholdB = maxFaceSeparation + hullA->innerRadius - boundSlack; + b3Vec3 centerBinA = b3TransformPoint( transformBtoA, hullB->center ); + b3Vec3 centerAinB = b3InvTransformPoint( transformBtoA, hullA->center ); + m_keptEdgeCountA = CullEdges( hullA, centerBinA, thresholdA, m_edgeStateA ); + m_keptEdgeCountB = CullEdges( hullB, centerAinB, thresholdB, m_edgeStateB ); + } + else + { + m_keptEdgeCountA = hullA->edgeCount / 2; + m_keptEdgeCountB = hullB->edgeCount / 2; + memset( m_edgeStateA, e_tested, m_keptEdgeCountA ); + memset( m_edgeStateB, e_tested, m_keptEdgeCountB ); } - const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); - for ( int i = 0; i < hullB->edgeCount; i += 2 ) + m_sphereKeptEdgeCountA = m_keptEdgeCountA; + m_sphereKeptEdgeCountB = m_keptEdgeCountB; + + // The probe is gated on the edge pair work in SIMD groups of A edges + int testCount = m_keptEdgeCountB * ( ( m_keptEdgeCountA + 3 ) >> 2 ); + m_probeApplied = useSupport && testCount >= probeMinTests; + if ( m_probeApplied ) { - int face1 = edgesB[i].face; - int face2 = edgesB[i + 1].face; - float c = b3Dot( planesB[face1].normal, planesB[face2].normal ); - bool kept = ArcCanReach( dotB[face1], dotB[face2], c, centerDistance, edgeBound ); - m_edgeStateB[i / 2] = kept ? e_tested : e_culled; - m_keptEdgeCountB += kept ? 1 : 0; + float probeBound = maxFaceSeparation - ( 0.1f * B3_LINEAR_SLOP + 0.001f * b3AbsFloat( centerDistance + radius ) ); + b3Vec3 probeB = b3TransformPoint( transformBtoA, b3GetHullPoints( hullB )[m_probeVertexB] ); + b3Vec3 probeA = b3InvTransformPoint( transformBtoA, b3GetHullPoints( hullA )[m_probeVertexA] ); + m_keptEdgeCountA = CullEdges( hullA, probeB, probeBound, m_edgeStateA ); + m_keptEdgeCountB = CullEdges( hullB, probeA, probeBound, m_edgeStateB ); } } @@ -1177,6 +1259,14 @@ public: DrawWireSphere( m_transformB, &sphereB, 32, MakeColor( b3_colorDeepSkyBlue ) ); } + if ( m_probeApplied ) + { + b3Pos probeA = b3TransformWorldPoint( m_transformA, b3GetHullPoints( hullA )[m_probeVertexA] ); + b3Pos probeB = b3TransformWorldPoint( m_transformB, b3GetHullPoints( hullB )[m_probeVertexB] ); + DrawPoint( probeA, 12.0f, MakeColor( b3_colorMagenta ) ); + DrawPoint( probeB, 12.0f, MakeColor( b3_colorMagenta ) ); + } + int faceCountA = hullA->faceCount; int faceCountB = hullB->faceCount; int edgeCountA = hullA->edgeCount / 2; @@ -1188,7 +1278,6 @@ public: bool edgesReached = m_separatedFeature == b3_invalidAxis; int seedCount = m_separatedFeature == b3_faceAxisA ? 1 : 2; - DrawTextLine( "drag to move B, shift + drag to rotate B" ); DrawTextLine( "vertices %d, faces %d, edges %d", hullA->vertexCount, faceCountA, edgeCountA ); DrawTextLine( "|d| - rA - rB = %.3f", m_gap ); DrawTextLine( "faces A: tested %d, culled %d of %d, best separation %.4f", testedA, culledA, faceCountA, m_separationA ); @@ -1217,6 +1306,22 @@ public: int totalPairCount = edgeCountA * edgeCountB; DrawTextLine( "edges kept: A %d of %d, B %d of %d", m_keptEdgeCountA, edgeCountA, m_keptEdgeCountB, edgeCountB ); DrawTextLine( "edge pairs: %d of %d (%.1f%%)", pairCount, totalPairCount, 100.0f * pairCount / totalPairCount ); + + if ( m_probeApplied && m_edgeBound == e_bothBounds ) + { + int spherePairCount = m_sphereKeptEdgeCountA * m_sphereKeptEdgeCountB; + DrawTextLine( "sphere bound: A %d, B %d, edge pairs %d (%.1f%%)", m_sphereKeptEdgeCountA, m_sphereKeptEdgeCountB, + spherePairCount, 100.0f * spherePairCount / totalPairCount ); + } + + if ( m_probeApplied ) + { + DrawTextLine( "magenta points are the support probes" ); + } + else if ( m_edgeBound == e_supportBound || m_edgeBound == e_bothBounds ) + { + DrawTextLine( "probe skipped, fewer than %d edge tests", probeMinTests ); + } } DrawTextLine( "SAT type: %d", m_satCache.type ); @@ -1237,8 +1342,15 @@ public: uint8_t m_edgeStateB[B3_MAX_HULL_EDGES]; int m_seedA; int m_seedB; + + // Support vertex on each hull for the best face of the other, it probes the edges of the other hull + int m_probeVertexA; + int m_probeVertexB; + int m_keptEdgeCountA; int m_keptEdgeCountB; + int m_sphereKeptEdgeCountA; + int m_sphereKeptEdgeCountB; float m_separationA; float m_separationB; float m_gap; @@ -1250,6 +1362,8 @@ public: bool m_showSpheres; bool m_showCulled; bool m_showNormals; + int m_edgeBound; + bool m_probeApplied; }; static int sampleHullCulling = RegisterSample( "Manifold", "Hull Culling", HullCulling::Create ); @@ -1574,6 +1688,8 @@ public: Search(); } + DrawGizmoControls( &m_showGizmo, &m_gizmoLocal ); + return true; } @@ -1649,7 +1765,6 @@ public: b3Pos centerB = b3TransformWorldPoint( m_transformB, m_hullB->center ); DrawLine( centerA, centerB, MakeColor( b3_colorWhite ) ); - DrawTextLine( "drag to move B, shift + drag to rotate B" ); DrawTextLine( "search: %s after %d poses", m_searchFound ? "found" : "not found", m_searchCount ); DrawTextLine( "best separation: face A %.4f, face B %.4f, edge %.4f -> %s", m_faceSeparationA, m_faceSeparationB, m_edge.separation, m_edgeWins ? "EDGE WINS" : "face wins" ); diff --git a/shared/CMakeLists.txt b/shared/CMakeLists.txt index c33e6dc..3a663ee 100644 --- a/shared/CMakeLists.txt +++ b/shared/CMakeLists.txt @@ -24,4 +24,11 @@ set_target_properties(shared PROPERTIES target_include_directories(shared PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(shared PRIVATE box3d) +# The SAT benchmark calls internal functions that a shared Box3D does not export +if (NOT BUILD_SHARED_LIBS) + target_sources(shared PRIVATE sat_benchmark.c sat_benchmark.h) + target_include_directories(shared PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../src) + target_compile_definitions(shared PUBLIC BOX3D_INTERNAL_BENCHMARKS) +endif() + source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} PREFIX "" FILES ${BOX3D_SHARED_FILES}) \ No newline at end of file diff --git a/shared/sat_benchmark.c b/shared/sat_benchmark.c new file mode 100644 index 0000000..740e872 --- /dev/null +++ b/shared/sat_benchmark.c @@ -0,0 +1,187 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "sat_benchmark.h" + +#include "utils.h" + +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include "manifold.h" + +#include + +const char* GetSatHullName( SatHullType type ) +{ + switch ( type ) + { + case satHull_complex: + return "complex"; + case satHull_rock: + return "rock"; + case satHull_cylinder: + return "cylinder"; + default: + return "unknown"; + } +} + +static b3HullData* CreateSatHull( SatHullType type ) +{ + switch ( type ) + { + case satHull_complex: + return b3CreateComplexHull( 0.5f ); + case satHull_rock: + return b3CreateRock( 0.5f ); + case satHull_cylinder: + return b3CreateCylinder( 1.0f, 0.1f, -0.5f, 32 ); + default: + B3_ASSERT( false ); + return NULL; + } +} + +static float GetBoundingRadius( const b3HullData* hull ) +{ + b3Vec3 center = b3AABB_Center( hull->aabb ); + b3Vec3 extents = b3AABB_Extents( hull->aabb ); + return b3Length( center ) + b3Length( extents ); +} + +SatBenchmarkData* CreateSatBenchmark( SatHullType typeA, SatHullType typeB ) +{ + SatBenchmarkData* data = calloc( 1, sizeof( SatBenchmarkData ) ); + data->hullA = CreateSatHull( typeA ); + data->hullB = CreateSatHull( typeB ); + data->innerRadiusA = data->hullA->innerRadius; + data->innerRadiusB = data->hullB->innerRadius; + + const b3HullData* hullA = data->hullA; + const b3HullData* hullB = data->hullB; + + b3DistanceInput input = { + .proxyA = { b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }, + .proxyB = { b3GetHullPoints( hullB ), hullB->vertexCount, 0.0f }, + .useRadii = false, + }; + + float target = 0.5f * B3_SPECULATIVE_DISTANCE; + float tolerance = 0.01f * target; + float reach = GetBoundingRadius( hullA ) + GetBoundingRadius( hullB ) + 0.5f; + + data->targetDistance = target; + data->maxPlacementError = 0.0f; + + uint32_t savedSeed = g_randomSeed; + g_randomSeed = 1234; + + for ( int i = 0; i < SAT_PAIR_COUNT; ++i ) + { + b3Quat q = RandomQuat(); + b3Vec3 direction = b3RotateVector( RandomQuat(), b3Vec3_axisX ); + b3Transform transform = { b3MulSV( reach, direction ), q }; + + // Newton iteration, the gradient of distance with respect to the position of B is the + // normal. Distance is 1-Lipschitz so each step lands at or above the target and never overlaps. + b3SimplexCache cache = { 0 }; + float error = 0.0f; + for ( int iteration = 0; iteration < 20; ++iteration ) + { + input.transform = transform; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + error = output.distance - target; + if ( b3AbsFloat( error ) < tolerance ) + { + break; + } + + transform.p = b3MulSub( transform.p, error, output.normal ); + } + + data->transforms[i] = transform; + data->caches[i] = cache; + data->maxPlacementError = b3MaxFloat( data->maxPlacementError, b3AbsFloat( error ) ); + } + + g_randomSeed = savedSeed; + + return data; +} + +void DestroySatBenchmark( SatBenchmarkData* data ) +{ + b3DestroyHull( data->hullA ); + b3DestroyHull( data->hullB ); + free( data ); +} + +void EnableSatInscribedSphere( SatBenchmarkData* data, bool flag ) +{ + // A hugely negative radius drives the SAT upper bound so high that no candidate is skipped. + // It must stay finite or the bound becomes NaN. + float disabledRadius = -B3_HUGE; + data->hullA->innerRadius = flag ? data->innerRadiusA : disabledRadius; + data->hullB->innerRadius = flag ? data->innerRadiusB : disabledRadius; +} + +SatBenchmarkResult RunSatBenchmark( const SatBenchmarkData* data, int repeatCount, bool warmStartDistance ) +{ + const b3HullData* hullA = data->hullA; + const b3HullData* hullB = data->hullB; + + b3DistanceInput input = { + .proxyA = { b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }, + .proxyB = { b3GetHullPoints( hullB ), hullB->vertexCount, 0.0f }, + .useRadii = false, + }; + + float distanceSum = 0.0f; + int iterationSum = 0; + + uint64_t ticks = b3GetTicks(); + + for ( int repeat = 0; repeat < repeatCount; ++repeat ) + { + for ( int i = 0; i < SAT_PAIR_COUNT; ++i ) + { + input.transform = data->transforms[i]; + b3SimplexCache cache = warmStartDistance ? data->caches[i] : (b3SimplexCache){ 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + distanceSum += output.distance; + iterationSum += output.iterations; + } + } + + float distanceMs = b3GetMillisecondsAndReset( &ticks ); + + float separationSum = 0.0f; + int earlyReturnCount = 0; + + for ( int repeat = 0; repeat < repeatCount; ++repeat ) + { + for ( int i = 0; i < SAT_PAIR_COUNT; ++i ) + { + // Early return enabled to match the manifold path. The placement keeps it from firing. + b3AxisQuery query = b3ComputeSeparatingAxis( hullA, hullB, data->transforms[i], true ); + b3SeparatingAxis axis = b3GetBestAxis( &query ); + separationSum += axis.separation; + earlyReturnCount += query.separatedFeature != b3_invalidAxis ? 1 : 0; + } + } + + float satMs = b3GetMilliseconds( ticks ); + + int queryCount = repeatCount * SAT_PAIR_COUNT; + SatBenchmarkResult result = { + .distanceMs = distanceMs, + .satMs = satMs, + .queryCount = queryCount, + .averageDistance = distanceSum / queryCount, + .averageSeparation = separationSum / queryCount, + .averageIterations = (float)iterationSum / queryCount, + .earlyReturnCount = earlyReturnCount, + }; + return result; +} diff --git a/shared/sat_benchmark.h b/shared/sat_benchmark.h new file mode 100644 index 0000000..70d1a36 --- /dev/null +++ b/shared/sat_benchmark.h @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "box3d/collision.h" + +#include + +// Compares GJK (b3ShapeDistance) against SAT (b3ComputeSeparatingAxis) on hull pairs held +// inside the speculative margin, so neither query can exit early. SAT is internal, so this +// is only available when Box3D is linked statically, see BOX3D_INTERNAL_BENCHMARKS. + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef enum SatHullType +{ + satHull_complex, + satHull_rock, + satHull_cylinder, + satHull_count +} SatHullType; + +#define SAT_PAIR_COUNT 1000 + +typedef struct SatBenchmarkData +{ + b3HullData* hullA; + b3HullData* hullB; + float innerRadiusA; + float innerRadiusB; + + // Hull A sits at the origin so these are the poses of B in the frame of A + b3Transform transforms[SAT_PAIR_COUNT]; + + // Converged GJK caches for a warm started distance query + b3SimplexCache caches[SAT_PAIR_COUNT]; + + float targetDistance; + float maxPlacementError; +} SatBenchmarkData; + +typedef struct SatBenchmarkResult +{ + float distanceMs; + float satMs; + int queryCount; + float averageDistance; + float averageSeparation; + float averageIterations; + + // Should be zero, otherwise the placement let SAT exit early + int earlyReturnCount; +} SatBenchmarkResult; + +const char* GetSatHullName( SatHullType type ); + +SatBenchmarkData* CreateSatBenchmark( SatHullType typeA, SatHullType typeB ); +void DestroySatBenchmark( SatBenchmarkData* data ); + +// Disabling the inscribed sphere bound makes SAT evaluate every face and edge candidate +void EnableSatInscribedSphere( SatBenchmarkData* data, bool flag ); + +SatBenchmarkResult RunSatBenchmark( const SatBenchmarkData* data, int repeatCount, bool warmStartDistance ); + +#ifdef __cplusplus +} +#endif diff --git a/src/convex_manifold.c b/src/convex_manifold.c index f5bfde0..e450bd0 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -10,6 +10,7 @@ #include "box3d/collision.h" #include "box3d/constants.h" +#include #include #include @@ -1353,85 +1354,124 @@ static inline void b3GetFaceDots( const b3HullData* hull, b3Vec3 d, float* dots #define B3_PARALLEL_TOL 1e-4f -// A hull edge is bounded by two face normals. On the Gauss map the edge becomes an arc between -// those two face normals. An edge can only build the best separating axis if a normal on that arc -// can beat the best separation value seen so far. This function determines if the upper bound -// separation on that arc can possibly beat the current maximum face separation. -// -// This follows the upper bound used for hull faces: -// separation_upper_bound = dot(axis, centerB - centerA) - innerRadiusA - innerRadiusB -// -// Inputs: -// d the vector connecting the hull centers -// a1 = dot(n1, d) -// a2 = dot(n2, d) -// c = dot(n1, n2) -// bound = maxFaceSeparation + radiusBound -// -// This returns 1 if the edges is a candidate and 0 otherwise. -static inline int b3TestEdgeCandidate( float a1, float a2, float c, float bound ) +// Inscribed sphere edge test. https://box2d.org/posts/2026/09/inscribed-spheres/ +// The implementation here is a mix of the versions from Cairn Overturf and Dirk Gregorius: +// https://gist.github.com/cairnc/dee7a2866da0709f2d9a77b6493b57b5 +// https://gist.github.com/dgregorius/e6751b5c00937cd21af63cba3c53c861 +// This benchmarks faster than the version from the blog post. +static inline int b3TestEdgeCandidateSorted( float a1, float a2, float c, float bound ) { - // c = cos(theta), the angle between the normals. - // s = sin(theta)^2 >= 0 + // We want to maximize + // + // f(t) = dot(d, nlerp(n1, n2, t)) + // + // over t in [0, 1], where + // + // a1 = dot(n1, d) + // a2 = dot(n2, d) + // c = dot(n1, n2). + // + // The maximum can occur either at an endpoint or at an interior + // critical point where f'(t) = 0. + + // Since the endpoint values are a1 and a2, only the larger endpoint + // needs to be tested against the bound. + float hi = b3MaxFloat( a1, a2 ); + float lo = b3MinFloat( a1, a2 ); + int exterior = hi >= bound; + + // Differentiating f(t) gives + // + // a2 - c*a1 - (1 - c)*(a1 + a2)*t + // f'(t) = -------------------------------------- + // ((1 - t)^2 + t^2 + 2*c*t*(1 - t))^(3/2) + // + // The denominator is positive and the numerator is linear in t. + // An interior maximum therefore requires + // + // f'(0) >= 0 -> a2 >= c*a1 + // f'(1) <= 0 -> a1 >= c*a2. + // + // After sorting these become + // + // hi >= c*lo + // lo >= c*hi. + // + // The second is always the stricter condition since + // + // (hi - c*lo) - (lo - c*hi) + // = (1 + c)*(hi - lo) >= 0. + // + // Thus both conditions reduce to u >= 0. + float u = lo - c * hi; + int maxIsInterior = u >= 0.0f; + + // Solving f'(t) = 0 and evaluating f(t) at tmax gives + // + // f(tmax)^2 = + // (a1^2 + a2^2 - 2*c*a1*a2) / (1 - c^2). + // + // After sorting, + // + // hi^2 + lo^2 - 2*c*hi*lo + // = hi^2*(1 - c^2) + u^2. + // + // Let s = 1 - c^2 and rearrange f(tmax) >= bound: + // + // u^2 >= (bound^2 - hi^2)*s. float s = 1.0f - c * c; + float boundTerm = ( bound - hi ) * ( bound + hi ); + float lhs = u * u; + float rhs = boundTerm * s; + int maxBeatsBound = lhs >= rhs; - // This is coincidentally the law of cosines. Break out your protractor. - float t = a1 * a1 + a2 * a2 - 2.0f * a1 * a2 * c; + // The interior expression contains 1 - c^2 in its denominator. + // When this approaches zero the arc is degenerate or ill-conditioned, + // so conservatively keep the edge as a candidate. + int nearlyParallel = s < B3_PARALLEL_TOL; - // Exterior conditions: - // Can either face normal beat the best separation? These cover the case where - // d is outside the arc. Note that d pointing outside the arc does not cull this - // edge. It can still generate the maximum separation (happens commonly). - // Note: when earlyReturn == false, bound == -INFINITY and this is always true. - int exterior = b3MaxFloat( a1, a2 ) >= bound; - - // Project d into the plane that holds both n1 and n2, call that vector dp. - // Introduce the coordinates b1 and b2 (these have units of length). - // - // dp = b1*n1 + b2*n2 - // - // Since dp is the projection of d into the plane formed by the cross(n1, n2): - // dot(n1, dp) == dot(n1, d) == a1 - // dot(n2, dp) == dot(n2, d) == a2 - // - // Dot the equation with n1 and n2: - // a1 = b1 + b2*c - // a2 = b1*c + b2 - // - // Solve for b1 and b2 using Cramer's Rule - // b1 = (a1 - a2 * c) / s - // b2 = (a2 - a1 * c) / s - // - // s = 1 - c * c is positive, so b1 and b2 must be positive for d to live in - // the arc between n1 and n2. - // - // The peak value along d is then norm(dp): - // dot(dp, dp) = dot(b1*n1 + b2*n2, d) - // = b1*a1 + b2*a2 - // = (a1*a1 - a1*a2*c + a2*a2 - a1*a2*c) / s - // = (a1*a1 + a2*a2 - 2*a1*a2*c) / s - // = t / s - // - // The interior is a candidate if: - // norm(dp) > bound - // sqrt(t / s) > bound - // If bound < 0 this is always true. Otherwise - // t > bound^2 * s - // - // Interior conditions: - // b1 and b2 positive (interior arc): a1 >= c * a2 & a2 >= c * a1 - // bound <= 0.0: the interior arc is automatically a candidate because it is positive - // s < B3_PARALLEL_TOL : n1 and n2 are nearly parallel so give up and pass the edge to the next stage - // t >= bound * bound * s : bound is positive and the interior normal direction is a candidate - // - // Using bit ops here to avoid branches. - - int interior = - ( a1 >= c * a2 ) & ( a2 >= c * a1 ) & ( ( bound <= 0.0f ) | ( s < B3_PARALLEL_TOL ) | ( t >= bound * bound * s ) ); + int interior = maxIsInterior & ( maxBeatsBound | nearlyParallel ); return exterior | interior; } +// dot(n, otherCenter) - offset for all faces of the hull, padded to the SIMD width. +static inline void b3GetFacePlaneSeparations( const b3HullData* hull, b3Vec3 otherCenter, float* separations ) +{ + b3GetFaceDots( hull, otherCenter, separations ); + + const b3Plane* planes = b3GetHullPlanes( hull ); + int faceCount = hull->faceCount; + for ( int i = 0; i < faceCount; ++i ) + { + separations[i] -= planes[i].offset; + } +} + +// The number of edge pair tests needed to make the extra culling pass worthwhile. +#define B3_EDGE_PROBE_MIN_TESTS 10 + +// Re-test edge candidates against the plane separations of a probe point on the other hull +static inline int b3FilterEdgeCandidates( const b3HullData* hull, const float* planeSeparations, float bound, int* edgeIndices, + int count ) +{ + const b3HullHalfEdge* halfEdges = b3GetHullEdges( hull ); + const float* cosines = b3GetHullEdgeCosines( hull ); + int keptCount = 0; + + for ( int k = 0; k < count; ++k ) + { + int i = edgeIndices[k]; + int i1 = halfEdges[i].face; + int i2 = halfEdges[i + 1].face; + float c = cosines[i >> 1]; + edgeIndices[keptCount] = i; + keptCount += b3TestEdgeCandidateSorted( planeSeparations[i1], planeSeparations[i2], c, bound ); + } + + return keptCount; +} + // Temporary abbreviations for convenience. #define NE ( B3_MAX_HULL_EDGES + B3_SIMD_WIDTH ) #define NF ( B3_MAX_HULL_FACES + B3_SIMD_WIDTH ) @@ -1518,15 +1558,16 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* // Use the seed to get a lower bound on the separation for the faces of hullA. float floorA = -INFINITY; + float seedSeparationA = -INFINITY; + int seedVertexB = 0; if ( earlyReturn ) { b3Plane plane = planesA[seedIndexA]; b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; - int vertexIndex; - float separation = - b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, soaVertexCountB, cB, hB, &vertexIndex ); - floorA = b3MinFloat( separation, speculativeDistance ); + seedSeparationA = + b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, soaVertexCountB, cB, hB, &seedVertexB ); + floorA = b3MinFloat( seedSeparationA, speculativeDistance ); } // Test A's face planes against B's vertices. @@ -1539,11 +1580,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* } b3Plane plane = planesA[i]; - b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); - float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; - int vertexIndex; - float separation = - b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, soaVertexCountB, cB, hB, &vertexIndex ); + int vertexIndex = seedVertexB; + float separation = seedSeparationA; + + // Avoid recomputing the seed face separation. + if ( earlyReturn == false || i != seedIndexA ) + { + b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); + float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; + separation = + b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, soaVertexCountB, cB, hB, &vertexIndex ); + } + if ( separation > res.faceA.separation ) { res.faceA.normal = plane.normal; @@ -1588,17 +1636,18 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* // Get a lower bound on the separation for the faces of hullB. float floorB = -INFINITY; + float seedSeparationB = -INFINITY; + int seedVertexA = 0; if ( earlyReturn ) { b3Plane plane = planesB[seedIndexB]; b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; - int vertexIndex; - float separation = - b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, soaVertexCountA, cA, hA, &vertexIndex ); + seedSeparationB = + b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, soaVertexCountA, cA, hA, &seedVertexA ); // Include the floor set by hull A faces. - floorB = b3MaxFloat( separation, res.faceA.separation ); + floorB = b3MaxFloat( seedSeparationB, res.faceA.separation ); floorB = b3MinFloat( floorB, speculativeDistance ); } @@ -1612,10 +1661,17 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* b3Plane plane = planesB[i]; b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); - float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; - int vertexIndex; - float separation = - b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, soaVertexCountA, cA, hA, &vertexIndex ); + int vertexIndex = seedVertexA; + float separation = seedSeparationB; + + // Avoid recomputing the seed face separation. + if ( earlyReturn == false || i != seedIndexB ) + { + float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; + separation = + b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, soaVertexCountA, cA, hA, &vertexIndex ); + } + if ( separation > res.faceB.separation ) { res.faceB.normal = direction; @@ -1635,38 +1691,71 @@ b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* _Static_assert( ( B3_MAX_HULL_FACES & ( B3_SIMD_WIDTH - 1 ) ) == 0, "must be multiple of SIMD width" ); _Static_assert( ( B3_MAX_HULL_VERTICES & ( B3_SIMD_WIDTH - 1 ) ) == 0, "must be multiple of SIMD width" ); - // Bound to skip edge tests. Derived from: - // dot(edgeNormal, deltaCenter) - radiusBound > maxSep - float edgeBound = earlyReturn ? b3MaxFloat( res.faceA.separation, res.faceB.separation ) + radiusBound : -INFINITY; - - B3_VALIDATE( earlyReturn == false || centerDistance >= edgeBound ); + B3_VALIDATE( earlyReturn == false || + centerDistance >= b3MaxFloat( res.faceA.separation, res.faceB.separation ) + radiusBound ); // Gather edges of A that can feasibly create a separating axis that beats the maximum face separation. int halfEdgeCountA = hullA->edgeCount; const b3HullHalfEdge* halfEdgesA = b3GetHullEdges( hullA ); int edgeIndicesA[NE]; int na = 0; - for ( int i = 0; i < halfEdgeCountA; i += 2 ) - { - int i1 = halfEdgesA[i].face; - int i2 = halfEdgesA[i + 1].face; - float c = b3Dot( planesA[i1].normal, planesA[i2].normal ); - edgeIndicesA[na] = i; - na += b3TestEdgeCandidate( dotA[i1], dotA[i2], c, edgeBound ); - } - // Similar for edges of B. int halfEdgeCountB = hullB->edgeCount; const b3HullHalfEdge* halfEdgesB = b3GetHullEdges( hullB ); int edgeIndicesB[B3_MAX_HULL_EDGES]; int nb = 0; + + float maxFaceSeparation = b3MaxFloat( res.faceA.separation, res.faceB.separation ); + float boundSlack = radius - radiusBound; + float thresholdA = earlyReturn ? maxFaceSeparation + hullB->innerRadius - boundSlack : -INFINITY; + float thresholdB = earlyReturn ? maxFaceSeparation + hullA->innerRadius - boundSlack : -INFINITY; + + b3Vec3 centerBinA = b3Add( b3MulMV( R, hullB->center ), xfB.p ); + b3Vec3 centerAinB = b3MulMV( invR, b3Sub( hullA->center, xfB.p ) ); + + _Alignas( 16 ) float planeDotA[NF]; + _Alignas( 16 ) float planeDotB[NF]; + b3GetFacePlaneSeparations( hullA, centerBinA, planeDotA ); + b3GetFacePlaneSeparations( hullB, centerAinB, planeDotB ); + + const float* cosinesA = b3GetHullEdgeCosines( hullA ); + const float* cosinesB = b3GetHullEdgeCosines( hullB ); + + for ( int i = 0; i < halfEdgeCountA; i += 2 ) + { + int i1 = halfEdgesA[i].face; + int i2 = halfEdgesA[i + 1].face; + float c = cosinesA[i >> 1]; + edgeIndicesA[na] = i; + na += b3TestEdgeCandidateSorted( planeDotA[i1], planeDotA[i2], c, thresholdA ); + } + for ( int i = 0; i < halfEdgeCountB; i += 2 ) { int i1 = halfEdgesB[i].face; int i2 = halfEdgesB[i + 1].face; - float c = b3Dot( planesB[i1].normal, planesB[i2].normal ); + float c = cosinesB[i >> 1]; edgeIndicesB[nb] = i; - nb += b3TestEdgeCandidate( dotB[i1], dotB[i2], c, edgeBound ); + nb += b3TestEdgeCandidateSorted( planeDotB[i1], planeDotB[i2], c, thresholdB ); + } + + // Apply additional culling use the support points for the best face axes. + // The support vertices of the best faces are points on the other hull, so an edge pair axis cannot + // have a larger separation than the plane separation of these points over the arc of the edge. + // This can slow down boxes so skip this if there are not enough edge candidates. + if ( earlyReturn && nb * ( ( na + 3 ) >> 2 ) >= B3_EDGE_PROBE_MIN_TESTS ) + { + float probeBound = maxFaceSeparation - ( 0.1f * B3_LINEAR_SLOP + 0.001f * b3AbsFloat( centerDistance + radius ) ); + + int vertexB = res.faceA.indexB != B3_NULL_INDEX ? res.faceA.indexB : seedVertexB; + b3Vec3 probeB = { vxB[vertexB], vyB[vertexB], vzB[vertexB] }; + b3GetFacePlaneSeparations( hullA, b3Add( b3MulMV( R, probeB ), xfB.p ), planeDotA ); + na = b3FilterEdgeCandidates( hullA, planeDotA, probeBound, edgeIndicesA, na ); + + int vertexA = res.faceB.indexA != B3_NULL_INDEX ? res.faceB.indexA : seedVertexA; + b3Vec3 probeA = { vxA[vertexA], vyA[vertexA], vzA[vertexA] }; + b3GetFacePlaneSeparations( hullB, b3MulMV( invR, b3Sub( probeA, xfB.p ) ), planeDotB ); + nb = b3FilterEdgeCandidates( hullB, planeDotB, probeBound, edgeIndicesB, nb ); } if ( na == 0 || nb == 0 ) diff --git a/src/hull.c b/src/hull.c index 97026d9..58c37bc 100644 --- a/src/hull.c +++ b/src/hull.c @@ -1628,6 +1628,19 @@ static float* b3GetHullSoaNormalsWrite( b3HullData* hull ) return (float*)( (intptr_t)hull + hull->soaNormalOffset ); } +static void b3UpdateHullEdgeCosines( b3HullData* hull ) +{ + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + float* cosines = (float*)( (intptr_t)hull + hull->edgeCosineOffset ); + int count = hull->edgeCount / 2; + + for ( int i = 0; i < count; ++i ) + { + cosines[i] = b3Dot( planes[edges[2 * i].face].normal, planes[edges[2 * i + 1].face].normal ); + } +} + int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction ) { int bestIndex = B3_NULL_INDEX; @@ -2224,6 +2237,8 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou byteCount += b3AlignUp8( 3 * soaVertexCount * (int)sizeof( float ) ); int soaNormalOffset = (int)byteCount; byteCount += b3AlignUp8( 3 * soaNormalCount * (int)sizeof( float ) ); + int edgeCosineOffset = (int)byteCount; + byteCount += b3AlignUp8( ( edgeCount / 2 ) * (int)sizeof( float ) ); b3HullData* hull = b3Alloc( byteCount ); memset( hull, 0, byteCount ); @@ -2236,6 +2251,7 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou hull->faceOffset = faceOffset; hull->soaVertexOffset = soaVertexOffset; hull->soaNormalOffset = soaNormalOffset; + hull->edgeCosineOffset = edgeCosineOffset; hull->vertexCount = vertexCount; hull->edgeCount = edgeCount; @@ -2317,6 +2333,8 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou // All builder pointers are dead from here on. b3Free( work, sizes.totalBytes ); + b3UpdateHullEdgeCosines( hull ); + b3UpdateHullBounds( hull ); bool success = b3UpdateHullBulkProperties( hull ); if ( success == false ) @@ -2372,8 +2390,8 @@ bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 ) // Hull identity covers every byte, so the structs carry explicit padding. These lock // the layout, re-audit padding if a size changes. -_Static_assert( sizeof( b3HullData ) == 144, "unexpected hull data size" ); -_Static_assert( sizeof( b3BoxHull ) == 640, "unexpected box hull size" ); +_Static_assert( sizeof( b3HullData ) == 152, "unexpected hull data size" ); +_Static_assert( sizeof( b3BoxHull ) == 696, "unexpected box hull size" ); // Implement b3HullMap. #define NAME b3HullMap @@ -2558,6 +2576,8 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra nz[i] = 0.0f; } + b3UpdateHullEdgeCosines( hull ); + b3UpdateHullBounds( hull ); bool success = b3UpdateHullBulkProperties( hull ); if ( success == false ) @@ -2812,6 +2832,7 @@ static const b3BoxHull s_boxHull = { .faceOffset = offsetof( b3BoxHull, boxFaces ), .soaVertexOffset = offsetof( b3BoxHull, vx ), .soaNormalOffset = offsetof( b3BoxHull, nx ), + .edgeCosineOffset = offsetof( b3BoxHull, edgeCosines ), }, .boxVertices = { diff --git a/src/physics_world.c b/src/physics_world.c index 8dbc2f7..c91f946 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -599,9 +599,6 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* float speculativeDistance = B3_SPECULATIVE_DISTANCE; float recycleDistanceNonTouching = b3MinFloat( recycleDistance, speculativeDistance ); - // Prefetch contact[i + contactPrefetchDistance] each iteration so the random - // 216 B contact load lands in L1 by the time we reach it. Distance picked to - // cover ~200 cycles of memory latency without overshooting the L1 working set. const int contactPrefetchDistance = 4; int prefetchEnd = endIndex - contactPrefetchDistance; @@ -2215,70 +2212,6 @@ b3Profile b3World_GetProfile( b3WorldId worldId ) return world->profile; } -b3Counters b3World_GetCounters( b3WorldId worldId ) -{ - b3World* world = b3GetUnlockedWorldFromId( worldId ); - if ( world == NULL ) - { - return (b3Counters){ 0 }; - } - - b3Counters s = { 0 }; - s.bodyCount = b3GetIdCount( &world->bodyIdPool ); - s.shapeCount = b3GetIdCount( &world->shapeIdPool ); - s.contactCount = b3GetIdCount( &world->contactIdPool ); - s.jointCount = b3GetIdCount( &world->jointIdPool ); - s.islandCount = b3GetIdCount( &world->islandIdPool ); - - b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; - s.staticTreeHeight = b3DynamicTree_GetHeight( staticTree ); - - b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; - b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; - s.treeHeight = b3MaxInt( b3DynamicTree_GetHeight( dynamicTree ), b3DynamicTree_GetHeight( kinematicTree ) ); - - s.satCallCount = world->satCallCount; - s.satCacheHitCount = world->satCacheHitCount; - memcpy( s.manifoldCounts, world->manifoldCounts, B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); - s.stackUsed = world->stack.maxAllocation; - s.byteCount = b3GetByteCount(); - s.taskCount = world->taskCount; - - _Static_assert( B3_GRAPH_COLOR_COUNT == sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ), "colorCounts size mismatch" ); - - s.awakeContactCount = 0; - for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) - { - b3GraphColor* color = world->constraintGraph.colors + i; - int colorContactCount = color->convexContacts.count + color->contacts.count; - s.colorCounts[i] = colorContactCount + color->jointSims.count; - s.awakeContactCount += colorContactCount; - } - s.awakeContactCount += world->solverSets.data[b3_awakeSet].contactIndices.count; - - s.recycledContactCount = 0; - s.arenaCapacity = 0; - s.distanceIterations = 0; - s.pushBackIterations = 0; - s.rootIterations = 0; - for ( int i = 0; i < world->workerCount; ++i ) - { - s.recycledContactCount += world->taskContexts.data[i].recycledContactCount; - - s.distanceIterations = b3MaxInt( s.distanceIterations, world->taskContexts.data[i].distanceIterations ); - s.pushBackIterations = b3MaxInt( s.pushBackIterations, world->taskContexts.data[i].pushBackIterations ); - s.rootIterations = b3MaxInt( s.rootIterations, world->taskContexts.data[i].rootIterations ); - - int peak = world->taskContexts.data[i].arena.shared->peakDemand; - if ( peak > s.arenaCapacity ) - { - s.arenaCapacity = peak; - } - } - - return s; -} - b3Capacity b3World_GetMaxCapacity( b3WorldId worldId ) { b3World* world = b3GetUnlockedWorldFromId( worldId ); @@ -2376,15 +2309,8 @@ void b3World_StopRecording( b3WorldId worldId ) b3StopRecordingInternal( world ); } -void b3World_DumpMemoryStats( b3WorldId worldId ) +static uint64_t b3GetWorldByteCount( b3World* world, bool log ) { - b3World* world = b3GetUnlockedWorldFromId( worldId ); - if ( world == NULL ) - { - return; - } - - // Large worlds can exceed 2GB, sum in 64 bits uint64_t total = 0; // id pools @@ -2396,13 +2322,16 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) int shapeIdBytes = b3GetIdBytes( &world->shapeIdPool ); total += (uint64_t)bodyIdBytes + solverSetIdBytes + jointIdBytes + contactIdBytes + islandIdBytes + shapeIdBytes; - b3Log( "id pools" ); - b3Log( "body ids: %d", bodyIdBytes ); - b3Log( "solver set ids: %d", solverSetIdBytes ); - b3Log( "joint ids: %d", jointIdBytes ); - b3Log( "contact ids: %d", contactIdBytes ); - b3Log( "island ids: %d", islandIdBytes ); - b3Log( "shape ids: %d", shapeIdBytes ); + if ( log ) + { + b3Log( "id pools" ); + b3Log( "body ids: %d", bodyIdBytes ); + b3Log( "solver set ids: %d", solverSetIdBytes ); + b3Log( "joint ids: %d", jointIdBytes ); + b3Log( "contact ids: %d", contactIdBytes ); + b3Log( "island ids: %d", islandIdBytes ); + b3Log( "shape ids: %d", shapeIdBytes ); + } // Islands own per-island body/contact/joint link arrays int islandLinkBytes = 0; @@ -2425,15 +2354,18 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) total += (uint64_t)bodyArrayBytes + solverSetArrayBytes + jointArrayBytes + contactArrayBytes + islandArrayBytes + islandLinkBytes + shapeArrayBytes + sensorArrayBytes; - b3Log( "world arrays" ); - b3Log( "bodies: %d", bodyArrayBytes ); - b3Log( "solver sets: %d", solverSetArrayBytes ); - b3Log( "joints: %d", jointArrayBytes ); - b3Log( "contacts: %d", contactArrayBytes ); - b3Log( "islands: %d", islandArrayBytes ); - b3Log( "island links: %d", islandLinkBytes ); - b3Log( "shapes: %d", shapeArrayBytes ); - b3Log( "sensors: %d", sensorArrayBytes ); + if ( log ) + { + b3Log( "world arrays" ); + b3Log( "bodies: %d", bodyArrayBytes ); + b3Log( "solver sets: %d", solverSetArrayBytes ); + b3Log( "joints: %d", jointArrayBytes ); + b3Log( "contacts: %d", contactArrayBytes ); + b3Log( "islands: %d", islandArrayBytes ); + b3Log( "island links: %d", islandLinkBytes ); + b3Log( "shapes: %d", shapeArrayBytes ); + b3Log( "sensors: %d", sensorArrayBytes ); + } // Sensors own overlap tracking arrays. The sensor array is dense. int sensorOverlapBytes = 0; @@ -2446,8 +2378,11 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) } total += sensorOverlapBytes; - b3Log( "owned arrays" ); - b3Log( "sensor overlaps: %d", sensorOverlapBytes ); + if ( log ) + { + b3Log( "owned arrays" ); + b3Log( "sensor overlaps: %d", sensorOverlapBytes ); + } // Shared hull database. The map owns a combined bucket and metadata allocation // plus the small map struct. Each stored key is an owned clone sized by byteCount. @@ -2462,9 +2397,12 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) } total += hullMapBytes + hullDataBytes; - b3Log( "hulls" ); - b3Log( "database: %d (%d, %d)", (int)hullMapBytes, hullCount, hullBucketCount ); - b3Log( "hull data: %d", (int)hullDataBytes ); + if ( log ) + { + b3Log( "hulls" ); + b3Log( "database: %d (%d, %d)", (int)hullMapBytes, hullCount, hullBucketCount ); + b3Log( "hull data: %d", (int)hullDataBytes ); + } // broad-phase int staticTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_staticBody ); @@ -2474,11 +2412,14 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) int pairSetBytes = b3GetHashSetBytes( pairSet ); total += (uint64_t)staticTreeBytes + kinematicTreeBytes + dynamicTreeBytes + pairSetBytes; - b3Log( "broad-phase" ); - b3Log( "static tree: %d", staticTreeBytes ); - b3Log( "kinematic tree: %d", kinematicTreeBytes ); - b3Log( "dynamic tree: %d", dynamicTreeBytes ); - b3Log( "pairSet: %d (%d, %d)", pairSetBytes, pairSet->count, pairSet->capacity ); + if ( log ) + { + b3Log( "broad-phase" ); + b3Log( "static tree: %d", staticTreeBytes ); + b3Log( "kinematic tree: %d", kinematicTreeBytes ); + b3Log( "dynamic tree: %d", dynamicTreeBytes ); + b3Log( "pairSet: %d (%d, %d)", pairSetBytes, pairSet->count, pairSet->capacity ); + } // Manifold block allocators, one per manifold point count int manifoldArrayBytes = b3Array_ByteCount( world->manifoldAllocators ); @@ -2491,9 +2432,12 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) } total += (uint64_t)manifoldArrayBytes + manifoldBlockBytes; - b3Log( "manifold allocators" ); - b3Log( "allocator array: %d", manifoldArrayBytes ); - b3Log( "blocks: %d", manifoldBlockBytes ); + if ( log ) + { + b3Log( "manifold allocators" ); + b3Log( "allocator array: %d", manifoldArrayBytes ); + b3Log( "blocks: %d", manifoldBlockBytes ); + } // solver sets int bodySimCapacity = 0; @@ -2524,12 +2468,15 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) int setIslandSimBytes = islandSimCapacity * (int)sizeof( b3IslandSim ); total += (uint64_t)setBodySimBytes + setBodyStateBytes + setJointSimBytes + setContactSimBytes + setIslandSimBytes; - b3Log( "solver sets" ); - b3Log( "body sim: %d", setBodySimBytes ); - b3Log( "body state: %d", setBodyStateBytes ); - b3Log( "joint sim: %d", setJointSimBytes ); - b3Log( "contact sim: %d", setContactSimBytes ); - b3Log( "island sim: %d", setIslandSimBytes ); + if ( log ) + { + b3Log( "solver sets" ); + b3Log( "body sim: %d", setBodySimBytes ); + b3Log( "body state: %d", setBodyStateBytes ); + b3Log( "joint sim: %d", setJointSimBytes ); + b3Log( "contact sim: %d", setContactSimBytes ); + b3Log( "island sim: %d", setIslandSimBytes ); + } // constraint graph int bodyBitSetBytes = 0; @@ -2544,10 +2491,13 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) } total += (uint64_t)bodyBitSetBytes + graphJointSimBytes + graphContactBytes; - b3Log( "constraint graph" ); - b3Log( "body bit sets: %d", bodyBitSetBytes ); - b3Log( "joint sim: %d", graphJointSimBytes ); - b3Log( "contact sim: %d", graphContactBytes ); + if ( log ) + { + b3Log( "constraint graph" ); + b3Log( "body bit sets: %d", bodyBitSetBytes ); + b3Log( "joint sim: %d", graphJointSimBytes ); + b3Log( "contact sim: %d", graphContactBytes ); + } // Per worker task storage and its bit sets int taskContextBytes = b3Array_ByteCount( world->taskContexts ); @@ -2570,9 +2520,12 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) } total += (uint64_t)taskContextBytes + sensorTaskContextBytes; - b3Log( "task contexts" ); - b3Log( "worker: %d", taskContextBytes ); - b3Log( "sensor: %d", sensorTaskContextBytes ); + if ( log ) + { + b3Log( "task contexts" ); + b3Log( "worker: %d", taskContextBytes ); + b3Log( "sensor: %d", sensorTaskContextBytes ); + } // Double buffered event arrays int eventBytes = 0; @@ -2587,7 +2540,10 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) eventBytes += b3Array_ByteCount( world->jointEvents ); total += eventBytes; - b3Log( "events: %d", eventBytes ); + if ( log ) + { + b3Log( "events: %d", eventBytes ); + } // Debug draw bit sets int debugBytes = 0; @@ -2597,13 +2553,96 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) debugBytes += b3GetBitSetBytes( &world->debugIslandSet ); total += debugBytes; - b3Log( "debug draw: %d", debugBytes ); + if ( log ) + { + b3Log( "debug draw: %d", debugBytes ); + } // stack allocator total += world->stack.capacity; - b3Log( "stack allocator: %d", world->stack.capacity ); - b3Log( "total: %u KB", (uint32_t)( total / 1024 ) ); + if ( log ) + { + b3Log( "stack allocator: %d", world->stack.capacity ); + b3Log( "total: %u KB", (uint32_t)( total / 1024 ) ); + } + + return total; +} + +void b3World_DumpMemoryStats( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + b3GetWorldByteCount( world, true ); +} + +b3Counters b3World_GetCounters( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Counters){ 0 }; + } + + b3Counters s = { 0 }; + s.bodyCount = b3GetIdCount( &world->bodyIdPool ); + s.shapeCount = b3GetIdCount( &world->shapeIdPool ); + s.contactCount = b3GetIdCount( &world->contactIdPool ); + s.jointCount = b3GetIdCount( &world->jointIdPool ); + s.islandCount = b3GetIdCount( &world->islandIdPool ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + s.staticTreeHeight = b3DynamicTree_GetHeight( staticTree ); + + b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; + b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; + s.treeHeight = b3MaxInt( b3DynamicTree_GetHeight( dynamicTree ), b3DynamicTree_GetHeight( kinematicTree ) ); + + s.satCallCount = world->satCallCount; + s.satCacheHitCount = world->satCacheHitCount; + memcpy( s.manifoldCounts, world->manifoldCounts, B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + s.stackUsed = world->stack.maxAllocation; + s.byteCount = (int64_t)b3GetWorldByteCount( world, false ); + s.taskCount = world->taskCount; + + _Static_assert( B3_GRAPH_COLOR_COUNT == sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ), "colorCounts size mismatch" ); + + s.awakeContactCount = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = world->constraintGraph.colors + i; + int colorContactCount = color->convexContacts.count + color->contacts.count; + s.colorCounts[i] = colorContactCount + color->jointSims.count; + s.awakeContactCount += colorContactCount; + } + s.awakeContactCount += world->solverSets.data[b3_awakeSet].contactIndices.count; + + s.recycledContactCount = 0; + s.arenaCapacity = 0; + s.distanceIterations = 0; + s.pushBackIterations = 0; + s.rootIterations = 0; + for ( int i = 0; i < world->workerCount; ++i ) + { + s.recycledContactCount += world->taskContexts.data[i].recycledContactCount; + + s.distanceIterations = b3MaxInt( s.distanceIterations, world->taskContexts.data[i].distanceIterations ); + s.pushBackIterations = b3MaxInt( s.pushBackIterations, world->taskContexts.data[i].pushBackIterations ); + s.rootIterations = b3MaxInt( s.rootIterations, world->taskContexts.data[i].rootIterations ); + + int peak = world->taskContexts.data[i].arena.shared->peakDemand; + if ( peak > s.arenaCapacity ) + { + s.arenaCapacity = peak; + } + } + + return s; } typedef struct WorldQueryContext