From 40da94c42222ebdf9db471ac8bed3b86a6923a6a Mon Sep 17 00:00:00 2001 From: ecker Date: Sat, 30 Aug 2025 15:52:30 -0500 Subject: [PATCH] wasted a week on trying to roll out my own physics system just to be humbled when it almost works but doesn't work well enough (probably a bad solver, will probably revisit it at a later date) --- Makefile | 2 +- bin/data/entities/player.json | 13 +- bin/data/scenes/sourceengine/test_grid.json | 8 + bin/exe/default/cc | 2 +- engine/inc/uf/engine/entity/entity.h | 1 + engine/inc/uf/ext/reactphysics/reactphysics.h | 20 +- engine/inc/uf/macros.h | 3 + engine/inc/uf/utils/math/collision.h | 31 - .../inc/uf/utils/math/collision/boundingbox.h | 29 - engine/inc/uf/utils/math/collision/gjk.h | 90 -- engine/inc/uf/utils/math/collision/mesh.h | 37 - engine/inc/uf/utils/math/collision/modular.h | 36 - engine/inc/uf/utils/math/collision/sphere.h | 26 - engine/inc/uf/utils/math/physics.h | 12 +- engine/inc/uf/utils/math/physics/impl.h | 201 +++++ engine/inc/uf/utils/math/physics/pod.inl | 12 +- engine/inc/uf/utils/math/physics/stub.h | 108 --- engine/inc/uf/utils/math/shapes.h | 441 +--------- engine/inc/uf/utils/math/transform.h | 6 +- .../inc/uf/utils/math/transform/transform.inl | 44 + engine/inc/uf/utils/math/vector.h | 7 +- engine/inc/uf/utils/math/vector/class.inl | 3 +- engine/inc/uf/utils/math/vector/pod.inl | 16 + engine/inc/uf/utils/mesh/clip.h | 406 +++++++++ engine/inc/uf/utils/mesh/grid.h | 3 +- engine/inc/uf/utils/mesh/mesh.h | 37 +- engine/inc/uf/utils/tests/tests.h | 73 ++ engine/src/engine/ext/ext.cpp | 1 - engine/src/engine/ext/player/behavior.cpp | 66 +- .../src/engine/ext/region/chunk/behavior.cpp | 5 +- engine/src/engine/ext/scene/behavior.cpp | 1 - engine/src/engine/graph/graph.cpp | 37 +- engine/src/engine/object/behavior.cpp | 75 +- engine/src/engine/object/object.cpp | 10 +- engine/src/engine/scene/scene.cpp | 2 + engine/src/ext/lua/usertypes/physics.cpp | 10 +- engine/src/ext/reactphysics/reactphysics.cpp | 201 ++--- engine/src/ext/xatlas/xatlas.cpp | 102 +-- .../src/utils/math/collision/boundingbox.cpp | 159 ---- engine/src/utils/math/collision/gjk.cpp | 249 ------ engine/src/utils/math/collision/mesh.cpp | 44 - engine/src/utils/math/collision/modular.cpp | 57 -- engine/src/utils/math/collision/sphere.cpp | 51 -- engine/src/utils/math/physics.cpp | 2 +- engine/src/utils/math/physics/aabb.inl | 264 ++++++ engine/src/utils/math/physics/bvh.inl | 249 ++++++ engine/src/utils/math/physics/capsule.inl | 109 +++ engine/src/utils/math/physics/epa.inl | 135 +++ engine/src/utils/math/physics/gjk.inl | 198 +++++ engine/src/utils/math/physics/helpers.inl | 260 ++++++ engine/src/utils/math/physics/impl.cpp | 410 +++++++++ engine/src/utils/math/physics/integration.inl | 308 +++++++ engine/src/utils/math/physics/mesh.inl | 303 +++++++ engine/src/utils/math/physics/plane.inl | 57 ++ engine/src/utils/math/physics/ray.inl | 237 +++++ engine/src/utils/math/physics/sphere.inl | 66 ++ engine/src/utils/math/physics/tests.inl | 815 ++++++++++++++++++ engine/src/utils/mesh/mesh.cpp | 51 ++ engine/src/utils/tests/tests.cpp | 28 + ext/main.cpp | 5 +- 60 files changed, 4626 insertions(+), 1608 deletions(-) delete mode 100644 engine/inc/uf/utils/math/collision.h delete mode 100644 engine/inc/uf/utils/math/collision/boundingbox.h delete mode 100644 engine/inc/uf/utils/math/collision/gjk.h delete mode 100644 engine/inc/uf/utils/math/collision/mesh.h delete mode 100644 engine/inc/uf/utils/math/collision/modular.h delete mode 100644 engine/inc/uf/utils/math/collision/sphere.h create mode 100644 engine/inc/uf/utils/math/physics/impl.h delete mode 100644 engine/inc/uf/utils/math/physics/stub.h create mode 100644 engine/inc/uf/utils/mesh/clip.h create mode 100644 engine/inc/uf/utils/tests/tests.h delete mode 100644 engine/src/utils/math/collision/boundingbox.cpp delete mode 100644 engine/src/utils/math/collision/gjk.cpp delete mode 100644 engine/src/utils/math/collision/mesh.cpp delete mode 100644 engine/src/utils/math/collision/modular.cpp delete mode 100644 engine/src/utils/math/collision/sphere.cpp create mode 100644 engine/src/utils/math/physics/aabb.inl create mode 100644 engine/src/utils/math/physics/bvh.inl create mode 100644 engine/src/utils/math/physics/capsule.inl create mode 100644 engine/src/utils/math/physics/epa.inl create mode 100644 engine/src/utils/math/physics/gjk.inl create mode 100644 engine/src/utils/math/physics/helpers.inl create mode 100644 engine/src/utils/math/physics/impl.cpp create mode 100644 engine/src/utils/math/physics/integration.inl create mode 100644 engine/src/utils/math/physics/mesh.inl create mode 100644 engine/src/utils/math/physics/plane.inl create mode 100644 engine/src/utils/math/physics/ray.inl create mode 100644 engine/src/utils/math/physics/sphere.inl create mode 100644 engine/src/utils/math/physics/tests.inl create mode 100644 engine/src/utils/tests/tests.cpp diff --git a/Makefile b/Makefile index f6bb3060..19a4ffdb 100644 --- a/Makefile +++ b/Makefile @@ -66,7 +66,7 @@ ifneq (,$(findstring win64,$(ARCH))) REQ_DEPS += meshoptimizer toml xatlas curl ffx:fsr dc:texconv # vall_e cpptrace # openvr # ncurses draco discord bullet ultralight-ux FLAGS += -march=native -g # -flto # -g endif - REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit reactphysics simd ctti gltf imgui fmt freetype openal ogg wav + REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit reactphysics simd ctti gltf imgui fmt freetype openal ogg wav FLAGS += -DUF_ENV_WINDOWS -DUF_ENV_WIN64 -DWIN32_LEAN_AND_MEAN DEPS += -lgdi32 -ldwmapi LINKS += #-Wl,-subsystem,windows diff --git a/bin/data/entities/player.json b/bin/data/entities/player.json index 853973bf..1a45ac99 100644 --- a/bin/data/entities/player.json +++ b/bin/data/entities/player.json @@ -63,12 +63,12 @@ "air": 0.1, "crouch": 1, - "jump": [ 0, 6, 0 ], + "jump": [ 0, 8, 0 ], "look": 1, "floored": { - "feet": [ 0, -1.5, 0 ], + "feet": [ 0, -1, 0 ], // "floor": [ 0, -0.5, 0 ], - "floor": [ 0, -1.0, 0 ], + "floor": [ 0, -1, 0 ], "print": false }, "strafe": true @@ -77,10 +77,17 @@ "gravity": [ 0, -9.81, 0 ], "inertia": [ 0, 0, 0 ], + // "type": "sphere", + // "radius": 2, + "type": "capsule", "radius": 1, "height": 2, + // "type": "bounding box", + // "min": [ -1.0, -2.0, -1.0 ], + // "max": [ 1.0, 0.0, 1.0 ], + "mass": 100, "friction": 0.95, "restitution": 0.0, diff --git a/bin/data/scenes/sourceengine/test_grid.json b/bin/data/scenes/sourceengine/test_grid.json index a7845900..e8add596 100644 --- a/bin/data/scenes/sourceengine/test_grid.json +++ b/bin/data/scenes/sourceengine/test_grid.json @@ -7,6 +7,14 @@ "metadata": { "graph": { "tags": { + " worldspawn": { + // "physics": { "type": "bounding box", "static": true }, + "physics": { "type": "plane", "static": true, "direction": [ 0, 1, 0 ], "offset": 0 }, + "grid": { "size": [8,1,8], "epsilon": 0.001, "cleanup": true, "print": true, "clip": true }, + "optimize meshlets": { "simplify": 0.125, "print": false }, + "unwrap mesh": true + }, + "/^prop_/": { "action": "load", "payload": { "import": "/prop.json", "metadata": { "physics": { "gravity": [ 0, 0, 0 ] } } } }, "/^func_/": { "action": "load", "payload": { "import": "/prop.json", "metadata": { "physics": { "gravity": [ 0, 0, 0 ] } } } } } diff --git a/bin/exe/default/cc b/bin/exe/default/cc index 060d289b..b08d5af5 100644 --- a/bin/exe/default/cc +++ b/bin/exe/default/cc @@ -1 +1 @@ -clang \ No newline at end of file +gcc \ No newline at end of file diff --git a/engine/inc/uf/engine/entity/entity.h b/engine/inc/uf/engine/entity/entity.h index 20d1470e..de02b3ca 100644 --- a/engine/inc/uf/engine/entity/entity.h +++ b/engine/inc/uf/engine/entity/entity.h @@ -67,6 +67,7 @@ namespace uf { void setParent(); void setParent( uf::Entity& parent ); + uf::Entity& addChild( uf::Entity& child ); void addChild( uf::Entity* child ); void moveChild( uf::Entity& child ); diff --git a/engine/inc/uf/ext/reactphysics/reactphysics.h b/engine/inc/uf/ext/reactphysics/reactphysics.h index 4da21d17..7903287d 100644 --- a/engine/inc/uf/ext/reactphysics/reactphysics.h +++ b/engine/inc/uf/ext/reactphysics/reactphysics.h @@ -4,7 +4,6 @@ #include #include #include -#include #include #if UF_USE_REACTPHYSICS @@ -25,21 +24,18 @@ namespace pod { rp3d::PhysicsWorld* world = NULL; pod::Transform<> transform = {}; - - struct Linear { - pod::Vector3f velocity; - pod::Vector3f acceleration; - } linear; - struct Angular { - pod::Quaternion<> velocity; - pod::Quaternion<> acceleration; - } rotational; + pod::Vector3f velocity; + pod::Vector3f acceleration; + pod::Quaternion<> angularVelocity; + pod::Quaternion<> angularAcceleration; struct { struct { pod::Transform<> transform = {}; - Linear linear; - Angular rotational; + pod::Vector3f velocity; + pod::Vector3f acceleration; + pod::Quaternion<> angularVelocity; + pod::Quaternion<> angularAcceleration; } current, previous; } internal; diff --git a/engine/inc/uf/macros.h b/engine/inc/uf/macros.h index 25776da9..72bd3e51 100644 --- a/engine/inc/uf/macros.h +++ b/engine/inc/uf/macros.h @@ -4,6 +4,9 @@ #define UF_NS_GET_LAST(name) uf::string::namespaceGetLast(#name) #define UF_NS_REMOVE_FIRST(name) uf::string::namespaceRemoveFirst(#name) +#define S_1(x) #x +#define S_2(x) S_1(x) + #define TIMER(x, ...)\ static bool first = true;\ static uf::Timer timer(false);\ diff --git a/engine/inc/uf/utils/math/collision.h b/engine/inc/uf/utils/math/collision.h deleted file mode 100644 index fcdc7598..00000000 --- a/engine/inc/uf/utils/math/collision.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include -#if UF_USE_UNUSED -#include "./collision/gjk.h" -#include "./collision/boundingbox.h" -#include "./collision/sphere.h" -#include "./collision/mesh.h" -#include - -namespace uf { - class UF_API Collider { - public: - typedef uf::stl::vector container_t; - protected: - uf::Collider::container_t m_container; - public: - ~Collider(); - void clear(); - - void add( pod::Collider* ); - uf::Collider::container_t& getContainer(); - const uf::Collider::container_t& getContainer() const; - - std::size_t getSize() const; - - uf::stl::vector intersects( const uf::Collider&, bool = false ) const; - uf::stl::vector intersects( const pod::Collider&, bool = false ) const; - }; -} -#endif \ No newline at end of file diff --git a/engine/inc/uf/utils/math/collision/boundingbox.h b/engine/inc/uf/utils/math/collision/boundingbox.h deleted file mode 100644 index 0028304a..00000000 --- a/engine/inc/uf/utils/math/collision/boundingbox.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#if UF_USE_UNUSED -#include "gjk.h" - -namespace uf { - class UF_API BoundingBox : public pod::Collider { - protected: - pod::Vector3 m_origin; - pod::Vector3 m_corner; - public: - BoundingBox( const pod::Vector3& = {}, const pod::Vector3& = {} ); - - const pod::Vector3& getOrigin() const; - const pod::Vector3& getCorner() const; - void setOrigin( const pod::Vector3& ); - void setCorner( const pod::Vector3& ); - - pod::Vector3 min() const; - pod::Vector3 max() const; - pod::Vector3 closest( const pod::Vector3f& ) const; - - virtual uf::stl::string type() const; - virtual pod::Vector3* expand() const; - virtual pod::Vector3 support( const pod::Vector3& ) const; - pod::Collider::Manifold intersects( const uf::BoundingBox& ) const; - }; -} -#endif \ No newline at end of file diff --git a/engine/inc/uf/utils/math/collision/gjk.h b/engine/inc/uf/utils/math/collision/gjk.h deleted file mode 100644 index eba30344..00000000 --- a/engine/inc/uf/utils/math/collision/gjk.h +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -#include - -#if UF_USE_UNUSED -#include -#include - -namespace pod { - struct UF_API Simplex { - public: - struct UF_API SupportPoint { - pod::Vector3 a, b, v; - bool operator==( const pod::Simplex::SupportPoint& r ) const { - return v == r.v; - } - pod::Vector3 operator-( const pod::Simplex::SupportPoint& r ) const { - return v - r.v; - } - pod::Vector3 operator*( float r ) const { - return v * r; - } - pod::Vector3 operator-( const pod::Vector3& r ) const { - return v - r; - } - pod::Math::num_t dot( const pod::Vector3& r ) const { - return uf::vector::dot(v, r); - } - pod::Vector3 cross( const pod::Simplex::SupportPoint& r ) const { - return uf::vector::cross(v, r.v); - } - pod::Vector3 cross( const pod::Vector3& r ) const { - return uf::vector::cross(v, r); - } - }; - int size = 0; - pod::Simplex::SupportPoint b, c, d; - /* - pod::Vector3 b, c, d; - Simplex( const pod::Vector3& = pod::Vector3(), const pod::Vector3& = pod::Vector3(), const pod::Vector3& = pod::Vector3() ); - void add( const pod::Vector3& ); - void set( const pod::Vector3& = pod::Vector3(), const pod::Vector3& = pod::Vector3(), const pod::Vector3& = pod::Vector3() ); - */ - }; -} -namespace pod { - class UF_API Collider { - protected: - pod::Transform<> m_transform; - public: - struct UF_API Manifold { - const pod::Collider* a; - const pod::Collider* b; - pod::Vector3 normal; - float depth; - bool colliding; - - Manifold() { - this->normal = pod::Vector3{0.0f, 0.0f, 0.0f}; - this->depth = -0.1f; - this->colliding = false; - } - Manifold( const pod::Collider& a, const pod::Collider& b ) { - this->a = &a; - this->b = &b; - this->normal = pod::Vector3{0.0f, 0.0f, 0.0f}; - this->depth = -0.1f; - this->colliding = false; - } - Manifold( const pod::Collider& a, const pod::Collider& b, const pod::Vector3& normal, float depth, bool colliding ) { - this->a = &a; - this->b = &b; - this->normal = normal; - this->depth = depth; - this->colliding = colliding; - } - }; - virtual ~Collider(); - virtual uf::stl::string type() const; - virtual pod::Vector3* expand() const = 0; - virtual pod::Vector3 support( const pod::Vector3& ) const = 0; - pod::Collider::Manifold intersects( const pod::Collider& ) const; - - pod::Vector3f getPosition() const; - pod::Transform<>& getTransform(); - const pod::Transform<>& getTransform() const; - void setTransform( const pod::Transform<>& ); - }; -} -#endif \ No newline at end of file diff --git a/engine/inc/uf/utils/math/collision/mesh.h b/engine/inc/uf/utils/math/collision/mesh.h deleted file mode 100644 index cf2029bf..00000000 --- a/engine/inc/uf/utils/math/collision/mesh.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#if UF_USE_UNUSED -#include "gjk.h" -#include - -namespace uf { - class UF_API MeshCollider : public pod::Collider { - protected: - uf::stl::vector m_positions; - public: - MeshCollider( const pod::Transform<>& = {}, const uf::stl::vector& = {} ); - - uf::stl::vector& getPositions(); - const uf::stl::vector& getPositions() const; - - void setPositions( const uf::stl::vector& ); - - #if 0 - template - void setPositions( const uf::Mesh& mesh ) { - this->m_positions.clear(); - this->m_positions.reserve( std::max( mesh.vertices.size(), mesh.indices.size() ) ); - if ( !mesh.indices.empty() ) { - for ( auto& index : mesh.indices ) this->m_positions.push_back( mesh.vertices[index].position ); - } else { - for ( auto& vertex : mesh.vertices ) this->m_positions.push_back( vertex.position ); - } - } - #endif - - virtual uf::stl::string type() const; - virtual pod::Vector3* expand() const; - virtual pod::Vector3 support( const pod::Vector3& ) const; - }; -} -#endif \ No newline at end of file diff --git a/engine/inc/uf/utils/math/collision/modular.h b/engine/inc/uf/utils/math/collision/modular.h deleted file mode 100644 index db854e4f..00000000 --- a/engine/inc/uf/utils/math/collision/modular.h +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#if UF_USE_UNUSED -#include "gjk.h" -#include - -namespace uf { - class UF_API ModularCollider : public pod::Collider { - public: - typedef std::function function_expand_t; - typedef std::function function_support_t; - protected: - uint m_len; - pod::Vector3* m_container; - - bool m_should_free; - - uf::ModularCollider::function_expand_t m_function_expand; - uf::ModularCollider::function_support_t m_function_support; - public: - ModularCollider( uint len = 0, pod::Vector3* = NULL, bool = false, const uf::ModularCollider::function_expand_t& = NULL, const uf::ModularCollider::function_support_t& = NULL ); - ~ModularCollider(); - - void setExpand( const uf::ModularCollider::function_expand_t& = NULL ); - void setSupport( const uf::ModularCollider::function_support_t& = NULL ); - - pod::Vector3* getContainer(); - uint getSize() const; - void setContainer( pod::Vector3*, uint ); - - virtual uf::stl::string type() const; - virtual pod::Vector3* expand() const; - virtual pod::Vector3 support( const pod::Vector3& ) const; - }; -} -#endif \ No newline at end of file diff --git a/engine/inc/uf/utils/math/collision/sphere.h b/engine/inc/uf/utils/math/collision/sphere.h deleted file mode 100644 index a7fcc7b4..00000000 --- a/engine/inc/uf/utils/math/collision/sphere.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#if UF_USE_UNUSED -#include "gjk.h" - -namespace uf { - class UF_API SphereCollider : public pod::Collider { - protected: - float m_radius; - pod::Vector3 m_origin; - public: - SphereCollider( float = 1.0f, const pod::Vector3& = pod::Vector3{} ); - - float getRadius() const; - const pod::Vector3& getOrigin() const; - - void setRadius( float = 1.0f ); - void setOrigin( const pod::Vector3& ); - - virtual uf::stl::string type() const; - virtual pod::Vector3* expand() const; - virtual pod::Vector3 support( const pod::Vector3& ) const; - pod::Collider::Manifold intersects( const uf::SphereCollider& ) const; - }; -} -#endif \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics.h b/engine/inc/uf/utils/math/physics.h index 38b7b6b7..1ae95cde 100644 --- a/engine/inc/uf/utils/math/physics.h +++ b/engine/inc/uf/utils/math/physics.h @@ -5,12 +5,10 @@ #include #include -#if UF_USE_BULLET - #include -#elif UF_USE_REACTPHYSICS +#if UF_USE_REACTPHYSICS #include #else - #include "physics/stub.h" + #include "physics/impl.h" #endif namespace uf { @@ -25,9 +23,13 @@ namespace uf { void UF_API initialize( uf::Object& ); void UF_API tick( uf::Object& ); void UF_API terminate( uf::Object& ); + #if 0 template pod::Transform& update( pod::Transform& transform, pod::Physics& physics ); template pod::Transform& update( pod::Physics& physics, pod::Transform& transform ); + #endif } } -#include "physics/pod.inl" \ No newline at end of file +#if 0 +#include "physics/pod.inl" +#endif \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h new file mode 100644 index 00000000..56a61221 --- /dev/null +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -0,0 +1,201 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace uf { + class Mesh; +} + +namespace pod { + enum class ShapeType { + AABB, + SPHERE, + PLANE, + CAPSULE, + MESH, + }; + + struct SupportPoint { + pod::Vector3f p; // Minkowski difference point + pod::Vector3f pA; // original support point in A + pod::Vector3f pB; // original support point in B + }; + + struct Simplex { + //uf::stl::vector pts; + uf::stl::vector pts; + }; + + struct Face { + // pod::Vector3f a,b,c; + pod::SupportPoint a, b, c; + pod::Vector3f normal; + float distance; + }; + + struct BVH { + typedef uf::stl::vector> pair_t; + struct Node { + pod::AABB bounds = {}; + int left = -1; + int right = -1; + int start = 0; + int count = 0; + }; + uf::stl::vector indices; + uf::stl::vector nodes; + }; + + struct MeshCollider { + pod::BVH* bvh; + const uf::Mesh* mesh; + }; + + typedef uint32_t CollisionMask; + + struct Collider { + pod::ShapeType type; + pod::CollisionMask mask = 0xFFFFFFFF; + union { + pod::Sphere sphere; + pod::AABB aabb; + pod::Plane plane; + pod::Capsule capsule; + pod::MeshCollider mesh; + } u; + }; + + struct PhysicsMaterial { + float restitution = 0.2f; + float staticFriction = 0.5f; + float dynamicFriction = 0.3f; + }; + + struct World; // forward declare + + struct RigidBody { + pod::World* world = NULL; + uf::Object* object = NULL; + pod::Transform<>* transform = NULL; + + bool isStatic = false; + + float mass = 1.0f; + float inverseMass = 1.0f; + + pod::Vector3f velocity = {}; + pod::Vector3f forceAccumulator = {}; + + pod::Vector3f angularVelocity = {}; + pod::Vector3f torqueAccumulator = {}; + + pod::Vector3f inertiaTensor = { 1, 1, 1 }; + pod::Vector3f inverseInertiaTensor = { 1, 1, 1 }; + + pod::AABB bounds; + pod::Collider collider; + pod::PhysicsMaterial material; + }; + + struct Contact { + pod::Vector3f point; + pod::Vector3f normal; + float penetration; + + // Warm-start cached values + int lifetime = 0; + pod::Vector3f tangent = {}; + float accumulatedNormalImpulse = 0.0f; + float accumulatedTangentImpulse = 0.0f; + }; + + struct Manifold { + pod::RigidBody* a = NULL; + pod::RigidBody* b = NULL; + float dt = 0; + uf::stl::vector points; + }; + + struct RayQuery { + bool hit = false; + const pod::RigidBody* body; + pod::Contact contact = { pod::Vector3f{}, pod::Vector3f{}, FLT_MAX }; + }; + + struct World { + uf::stl::vector bodies; + + pod::Vector3f gravity = { 0, -9.81f, 0 }; + pod::BVH bvh; + }; +} + +namespace uf { + namespace physics { + namespace impl { + extern UF_API float timescale; + extern UF_API bool interpolate; + extern UF_API bool shared; + extern UF_API bool globalStorage; + + extern UF_API pod::World world; + + void UF_API initialize(); + void UF_API initialize( uf::Object& ); + void UF_API initialize( pod::World& ); + + void UF_API tick( float = 0 ); + void UF_API tick( uf::Object&, float = 0 ); + void UF_API tick( pod::World&, float = 0 ); + + void UF_API terminate(); + void UF_API terminate( uf::Object& ); + void UF_API terminate( pod::World& ); + + void UF_API step( pod::World&, float dt ); + void UF_API substep( pod::World&, float dt, int substeps ); + + void UF_API setMass( pod::RigidBody& body, float mass = 0.0f ); + void UF_API setInertia( pod::RigidBody& body ); + void UF_API applyForce( pod::RigidBody& body, const pod::Vector3f& force ); + void UF_API applyForceAtPoint( pod::RigidBody body, const pod::Vector3f& force, const pod::Vector3f& point ); + void UF_API applyImpulse( pod::RigidBody& body, const pod::Vector3f& impulse ); + void UF_API applyTorque( pod::RigidBody& body, const pod::Vector3f& torque ); + + void UF_API setVelocity( pod::RigidBody& body, const pod::Vector3f& velocity ); + void UF_API applyRotation( pod::RigidBody& body, const pod::Quaternion<>& q ); + void UF_API applyRotation( pod::RigidBody& body, const pod::Vector3f& axis, float angle ); + + pod::RigidBody& UF_API create( uf::Object&, float mass = 0.0f ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::AABB& aabb, float mass = 0 ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::Sphere& sphere, float mass = 0 ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::Plane& plane, float mass = 0 ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::Capsule& capsule, float mass = 0 ); + pod::RigidBody& UF_API create( uf::Object&, const uf::Mesh& mesh, float mass = 0 ); + + pod::RigidBody& UF_API create( pod::World&, uf::Object&, float mass = 0.0f ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::AABB& aabb, float mass = 0 ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Sphere& sphere, float mass = 0 ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Plane& plane, float mass = 0 ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Capsule& capsule, float mass = 0 ); + pod::RigidBody& UF_API create( pod::World&, uf::Object&, const uf::Mesh& mesh, float mass = 0 ); + + void UF_API destroy( uf::Object& ); + void UF_API destroy( pod::RigidBody& ); + + pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::RigidBody&, float = FLT_MAX ); + pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::World&, float = FLT_MAX ); + pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::World&, const pod::RigidBody*, float = FLT_MAX ); + } + } +} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/pod.inl b/engine/inc/uf/utils/math/physics/pod.inl index b4a3b83b..cb269f6c 100644 --- a/engine/inc/uf/utils/math/physics/pod.inl +++ b/engine/inc/uf/utils/math/physics/pod.inl @@ -2,14 +2,14 @@ template pod::Transform& uf::physics::update( pod::Transform& transform, pod::Physics& physics ) { // physics.internal.previous = transform; - if ( physics.linear.acceleration != pod::Vector3t{0,0,0} ) - physics.linear.velocity += (physics.linear.acceleration * uf::physics::time::delta); - if ( physics.rotational.acceleration != pod::Quaternion{0,0,0,0} ) { - physics.rotational.velocity = uf::quaternion::multiply(physics.rotational.velocity, physics.rotational.acceleration*uf::physics::time::delta); + if ( physics.acceleration != pod::Vector3t{0,0,0} ) + physics.velocity += (physics.acceleration * uf::physics::time::delta); + if ( physics.angularAcceleration != pod::Quaternion{0,0,0,0} ) { + physics.angularVelocity = uf::quaternion::multiply(physics.angularVelocity, physics.angularAcceleration*uf::physics::time::delta); } - transform = uf::transform::move( transform, physics.linear.velocity*uf::physics::time::delta ); - transform = uf::transform::rotate( transform, uf::vector::normalize(physics.rotational.velocity*uf::physics::time::delta)); + transform = uf::transform::move( transform, physics.velocity*uf::physics::time::delta ); + transform = uf::transform::rotate( transform, uf::vector::normalize(physics.angularVelocity*uf::physics::time::delta)); return transform; } diff --git a/engine/inc/uf/utils/math/physics/stub.h b/engine/inc/uf/utils/math/physics/stub.h deleted file mode 100644 index afd97b58..00000000 --- a/engine/inc/uf/utils/math/physics/stub.h +++ /dev/null @@ -1,108 +0,0 @@ -// enough things to get it to compile without a physics lib -namespace uf { - struct Mesh; -} - -namespace pod { - struct UF_API Physics { - size_t uid = 0; - uf::Object* object = NULL; - - bool shared = false; // share control of the transform both in-engine and bullet, set to true if you're directly modifying the transform - void* body = NULL; - void* shape = NULL; - - void* world = NULL; - - pod::Transform<> transform = {}; - - struct Linear { - pod::Vector3f velocity; - pod::Vector3f acceleration; - } linear; - struct Angular { - pod::Quaternion<> velocity; - pod::Quaternion<> acceleration; - } rotational; - - struct { - struct { - pod::Transform<> transform = {}; - Linear linear; - Angular rotational; - } current, previous; - } internal; - - - struct { - uint32_t flags = 0; - float mass = 0.0f; - float friction = 0.8f; - float restitution = 0.0f; - pod::Vector3f inertia = {0, 0, 0}; - pod::Vector3f gravity = {0, 0, 0}; - } stats; - }; - - typedef Physics PhysicsState; -} - -namespace uf { - namespace physics { - namespace impl { - typedef void* WorldState; - inline void UF_API initialize() {} - inline void UF_API initialize( uf::Object& ) {} - inline void UF_API tick( float = 0 ) {} - inline void UF_API tick( uf::Object&, float = 0 ) {} - inline void UF_API terminate() {} - inline void UF_API terminate( uf::Object& ) {} - - // base collider creation - inline pod::PhysicsState& UF_API create( uf::Object& ) { static pod::PhysicsState null; return null; } - - inline void UF_API destroy( uf::Object& ) { return; } - inline void UF_API destroy( pod::PhysicsState& ) { return; } - - inline void UF_API attach( pod::PhysicsState& ) { return; } - inline void UF_API detach( pod::PhysicsState& ) { return; } - - // collider for mesh (static or dynamic) - inline pod::PhysicsState& create( uf::Object&, const uf::Mesh&, bool ) { static pod::PhysicsState null; return null; } - // collider for boundingbox - inline pod::PhysicsState& UF_API create( uf::Object&, const pod::Vector3f& ) { static pod::PhysicsState null; return null; } - // collider for capsule - inline pod::PhysicsState& UF_API create( uf::Object&, float, float ) { static pod::PhysicsState null; return null; } - /* - // synchronize engine transforms to bullet transforms - inline void UF_API syncTo( ext::reactphysics::WorldState& ) { return; } - // synchronize bullet transforms to engine transforms - inline void UF_API syncFrom( ext::reactphysics::WorldState&, float = 1 ) { return; } - */ - // apply impulse - inline void UF_API setImpulse( pod::PhysicsState&, const pod::Vector3f& = {} ) { return; } - inline void UF_API applyImpulse( pod::PhysicsState&, const pod::Vector3f& ) { return; } - // directly move a transform - inline void UF_API applyMovement( pod::PhysicsState&, const pod::Vector3f& ) { return; } - // directly apply a velocity - inline void UF_API setVelocity( pod::PhysicsState&, const pod::Vector3f& ) { return; } - inline void UF_API applyVelocity( pod::PhysicsState&, const pod::Vector3f& ) { return; } - // directly rotate a transform - inline void UF_API applyRotation( pod::PhysicsState&, const pod::Quaternion<>& ) { return; } - inline void UF_API applyRotation( pod::PhysicsState&, const pod::Vector3f&, float ) { return; } - - // ray casting - inline uf::Object* UF_API rayCast( const pod::Vector3f&, const pod::Vector3f& ) { return NULL; } - inline uf::Object* UF_API rayCast( pod::PhysicsState&, const pod::Vector3f&, const pod::Vector3f& ) { return NULL; } - inline uf::Object* UF_API rayCast( pod::PhysicsState&, const pod::Vector3f&, const pod::Vector3f&, float& ) { return NULL; } - inline uf::Object* UF_API rayCast( const pod::Vector3f&, const pod::Vector3f&, uf::Object*, float& ) { return NULL; } - - // allows noclip - inline void UF_API activateCollision( pod::PhysicsState&, bool = true ) { return; } - - // - inline float UF_API getMass( pod::PhysicsState& ) { return {}; } - inline void UF_API setMass( pod::PhysicsState&, float ) { return; } - } - } -} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/shapes.h b/engine/inc/uf/utils/math/shapes.h index edae8c3c..85baafd0 100644 --- a/engine/inc/uf/utils/math/shapes.h +++ b/engine/inc/uf/utils/math/shapes.h @@ -6,446 +6,39 @@ namespace pod { struct Plane { pod::Vector3f normal; - float distance; + float offset; + }; + + struct AABB { + pod::Vector3f min; + pod::Vector3f max; }; struct Sphere { - pod::Vector3f origin; float radius; }; - struct Cube { + struct Capsule { + float radius; + float halfHeight; + }; + + struct Ray { pod::Vector3f origin; - pod::Vector3f size; + pod::Vector3f direction; }; struct Triangle { pod::Vector3f points[3]; }; + struct TriangleWithNormal : Triangle { // to-do: find a better name + pod::Vector3f normals[3]; + }; + template struct Polygon { uf::stl::vector points; }; - - struct Cone { - pod::Vector3f origin; - pod::Vector3f direction; - float height; - float radius; - }; - - struct Torus { - pod::Vector3f origin; - float majorRadius; - float minorRadius; - }; - - struct Cylinder { - pod::Vector3f origin; - pod::Vector3f direction; - float height; - float radius; - }; } -namespace uf { - namespace shapes { - inline float planePointSide( const pod::Plane& plane, const pod::Vector3f& point ) { - return uf::vector::dot( point, plane.normal ) - plane.distance; - } - - inline float planeEdgeIntersection( const pod::Plane& plane, const pod::Vector3f& p1, const pod::Vector3f& p2 ) { - return (plane.distance - uf::vector::dot(p1, plane.normal)) / uf::vector::dot(uf::vector::subtract(p2, p1), plane.normal); - } - - template - uf::stl::vector> verticesPolygonate( const uf::stl::vector& vertices, const uf::stl::vector& indices ) { - uf::stl::vector> polygons; - polygons.reserve( indices.size() / 3 ); - - for ( auto i = 0; i < indices.size(); i += 3 ) { - polygons.emplace_back( pod::Polygon{ { vertices[indices[i+0]], vertices[indices[i+1]], vertices[indices[i+2]] } }); - } - - return polygons; - } - - template - uf::stl::vector> verticesPolygonate( const uf::stl::vector& vertices ) { - uf::stl::vector> polygons; - polygons.reserve( vertices.size() / 3 ); - - for ( auto i = 0; i < vertices.size(); i += 3 ) { - polygons.emplace_back( pod::Polygon{ { vertices[i+0], vertices[i+1], vertices[i+2] } }); - } - - return polygons; - } - - template - uf::stl::vector polygonsTriangulate( const uf::stl::vector>& polygons ) { - uf::stl::vector vertices; - vertices.reserve( polygons.size() * 3 ); - - for ( const auto& polygon : polygons ) { - for (size_t i = 1; i < polygon.points.size() - 1; ++i) { - vertices.emplace_back(polygon.points[0]); - vertices.emplace_back(polygon.points[i]); - vertices.emplace_back(polygon.points[i + 1]); - } - } - - // rewind - for ( auto i = 0; i < vertices.size(); i += 3 ) { - for ( const auto& polygon : polygons ) { - const auto& p0 = vertices[i+0]; - const auto& p1 = vertices[i+1]; - const auto& p2 = vertices[i+2]; - - auto edge1 = p1.position - p0.position; - auto edge2 = p2.position - p0.position; - auto polygonNormal = uf::vector::cross(edge1, edge2); - - if ( uf::vector::dot( polygonNormal, p0.normal ) < 0 ) { - std::swap( vertices[i+1], vertices[i+2] ); - } - } - } - - return vertices; - } - - template - uf::stl::vector verticesIndex( const uf::stl::vector& vertices ) { - uf::stl::vector indices; - indices.reserve( vertices.size() ); - - for ( auto i = 0; i < vertices.size(); ++i ) { - indices.emplace_back( i ); - } - - return indices; - } - - template - uf::stl::vector indexedVertices( const uf::stl::vector& vertices, const uf::stl::vector& indices ) { - uf::stl::vector newVertices; - - for ( auto idx : indices ) { - newVertices.emplace_back( vertices[idx] ); - } - - return newVertices; - } - - - // to-do: fix why these causes problems - // the polygon approach works fine enough - template - uf::stl::vector verticesPlaneClip( const uf::stl::vector& vertices, const pod::Plane& plane ) { - uf::stl::vector newVertices; - - for ( auto i = 0; i < vertices.size(); i += 3 ) { - const auto& v0 = vertices[i+0]; - const auto& v1 = vertices[i+1]; - const auto& v2 = vertices[i+2]; - - /* - float s0 = uf::shapes::planePointSide( plane, v0.position ) > 0; - float s1 = uf::shapes::planePointSide( plane, v1.position ) > 0; - float s2 = uf::shapes::planePointSide( plane, v2.position ) > 0; - - if ( s0 == s1 && s1 == s2 ) { - newVertices.emplace_back( v0 ); - newVertices.emplace_back( v1 ); - newVertices.emplace_back( v2 ); - - continue; - } - - if ( s0 != s1 ) { - float t = planeEdgeIntersection( plane, v0.position, v1.position ); - T intersection = T::interpolate( v0, v1, t ); - - if ( s0 ) { - newVertices.emplace_back( v0 ); - newVertices.emplace_back( intersection ); - newVertices.emplace_back( v1 ); - } else { - newVertices.emplace_back( v1 ); - newVertices.emplace_back( intersection ); - newVertices.emplace_back( v0 ); - } - } - - if ( s1 != s2 ) { - float t = planeEdgeIntersection( plane, v1.position, v2.position ); - T intersection = T::interpolate( v1, v2, t ); - - if ( s1 ) { - newVertices.emplace_back( v1 ); - newVertices.emplace_back( intersection ); - newVertices.emplace_back( v2 ); - } else { - newVertices.emplace_back( v2 ); - newVertices.emplace_back( intersection ); - newVertices.emplace_back( v1 ); - } - } - - if ( s2 != s0 ) { - float t = planeEdgeIntersection( plane, v2.position, v0.position ); - T intersection = T::interpolate( v2, v0, t ); - - if ( s2 ) { - newVertices.emplace_back( v2 ); - newVertices.emplace_back( intersection ); - newVertices.emplace_back( v0 ); - } else { - newVertices.emplace_back( v0 ); - newVertices.emplace_back( intersection ); - newVertices.emplace_back( v2 ); - } - } - */ - float d0 = uf::shapes::planePointSide( plane, v0.position ); - float d1 = uf::shapes::planePointSide( plane, v1.position ); - float d2 = uf::shapes::planePointSide( plane, v2.position ); - - if (d0 < 0 && d1 < 0 && d2 < 0) { - continue; - } - - if (d0 >= 0 && d1 >= 0 && d2 >= 0) { - newVertices.emplace_back( v0 ); - newVertices.emplace_back( v1 ); - newVertices.emplace_back( v2 ); - continue; - } - - { - // Some vertices are in front and some are behind - bool edge0Crosses = (d0 >= 0) != (d1 >= 0); - bool edge1Crosses = (d1 >= 0) != (d2 >= 0); - bool edge2Crosses = (d2 >= 0) != (d0 >= 0); - - T intersection01; - T intersection12; - T intersection20; - - if ( edge0Crosses ) { - auto t = planeEdgeIntersection( plane, v0.position, v1.position ); - intersection01 = T::interpolate( v0, v1, t ); - } - if ( edge1Crosses ) { - auto t = planeEdgeIntersection( plane, v1.position, v2.position ); - intersection12 = T::interpolate( v1, v2, t ); - } - if ( edge2Crosses ) { - auto t = planeEdgeIntersection( plane, v2.position, v0.position ); - intersection20 = T::interpolate( v2, v0, t ); - } - - if (edge0Crosses && edge1Crosses && edge2Crosses) { - // All edges cross the plane: split into three triangles - newVertices.emplace_back( intersection01 ); - newVertices.emplace_back( intersection12 ); - newVertices.emplace_back( intersection20 ); - } else if (edge0Crosses && edge1Crosses) { - // Edges 0 and 1 cross: create a quad - newVertices.emplace_back( intersection01 ); - newVertices.emplace_back( intersection12 ); - newVertices.emplace_back( v1 ); - - newVertices.emplace_back( intersection01 ); - newVertices.emplace_back( v1 ); - newVertices.emplace_back( v0 ); - } else if (edge1Crosses && edge2Crosses) { - // Edges 1 and 2 cross: create a quad - newVertices.emplace_back( intersection12 ); - newVertices.emplace_back( intersection20 ); - newVertices.emplace_back( v2 ); - - newVertices.emplace_back( intersection12 ); - newVertices.emplace_back( v2 ); - newVertices.emplace_back( v1 ); - } else if (edge2Crosses && edge0Crosses) { - // Edges 2 and 0 cross: create a quad - newVertices.emplace_back( intersection20 ); - newVertices.emplace_back( intersection01 ); - newVertices.emplace_back( v0 ); - - newVertices.emplace_back( intersection20 ); - newVertices.emplace_back( v0 ); - newVertices.emplace_back( v2 ); - } else if (edge0Crosses) { - // Only edge 0 crosses: create a triangle - newVertices.emplace_back( intersection01 ); - newVertices.emplace_back( v1 ); - newVertices.emplace_back( v0 ); - } else if (edge1Crosses) { - // Only edge 1 crosses: create a triangle - newVertices.emplace_back( intersection12 ); - newVertices.emplace_back( v2 ); - newVertices.emplace_back( v1 ); - } else if (edge2Crosses) { - // Only edge 2 crosses: create a triangle - newVertices.emplace_back( intersection20 ); - newVertices.emplace_back( v0 ); - newVertices.emplace_back( v2 ); - } - } - } - - return newVertices; - } - - template - pod::Polygon polygonPlaneClip( const pod::Polygon& polygon, const pod::Plane& plane ) { - pod::Polygon clipped; - - auto count = polygon.points.size(); - bool modified = false; - for (auto i = 0; i < count; ++i) { - const T& current = polygon.points[i]; - const T& previous = polygon.points[(i - 1 + count) % count]; - - float currentSide = uf::shapes::planePointSide( plane, current.position ); - float previousSide = uf::shapes::planePointSide( plane, previous.position ); - - if (currentSide >= 0) { - if (previousSide < 0) { - float t = uf::shapes::planeEdgeIntersection( plane, previous.position, current.position ); - T interpolated = T::interpolate( previous, current, t ); - clipped.points.emplace_back(interpolated); - - modified = true; - } - clipped.points.emplace_back(current); - } else if (previousSide >= 0) { - float t = uf::shapes::planeEdgeIntersection( plane, previous.position, current.position ); - T interpolated = T::interpolate( previous, current, t ); - clipped.points.emplace_back(interpolated); - - modified = true; - } - } - - if ( modified ) { - std::reverse( clipped.points.begin(), clipped.points.end() ); - } - - return clipped; - } - - template - uf::stl::vector>& clip( - uf::stl::vector>& polygons, - - const pod::Vector3f& min, - const pod::Vector3f& max - ) { - // inverted because its needed - uf::stl::vector planes = { - // Right plane: x = max.x - { pod::Vector3f{-1, 0, 0}, -max.x }, - // Left plane: x = min.x - { pod::Vector3f{1, 0, 0}, min.x }, - // Top plane: y = max.y - { pod::Vector3f{0, -1, 0}, -max.y }, - // Bottom plane: y = min.y - { pod::Vector3f{0, 1, 0}, min.y }, - // Front plane: z = max.z - { pod::Vector3f{0, 0, -1}, -max.z }, - // Back plane: z = min.z - { pod::Vector3f{0, 0, 1}, min.z } - }; - - // clip against planes - for ( const auto& plane : planes ) { - uf::stl::vector> clippedPolygons; - for ( const auto& polygon : polygons ) { - auto clipped = uf::shapes::polygonPlaneClip( polygon, plane ); - if ( clipped.points.size() >= 3 ) clippedPolygons.emplace_back( clipped ); - } - polygons = std::move(clippedPolygons); - } - - return polygons; - } - - template - uf::stl::vector& clip( - uf::stl::vector& vertices, - - const pod::Vector3f& min, - const pod::Vector3f& max, - - bool polygons = true - ) { - if ( polygons ) { - // convert to polys - auto polygons = uf::shapes::verticesPolygonate( vertices ); - // clip against planes - uf::shapes::clip( polygons, min, max ); - // triangulate - vertices = uf::shapes::polygonsTriangulate( polygons ); - - return vertices; - } - - // inverted because its needed - uf::stl::vector planes = { - // Right plane: x = max.x - { pod::Vector3f{-1, 0, 0}, -max.x }, - // Left plane: x = min.x - { pod::Vector3f{1, 0, 0}, min.x }, - // Top plane: y = max.y - { pod::Vector3f{0, -1, 0}, -max.y }, - // Bottom plane: y = min.y - { pod::Vector3f{0, 1, 0}, min.y }, - // Front plane: z = max.z - { pod::Vector3f{0, 0, -1}, -max.z }, - // Back plane: z = min.z - { pod::Vector3f{0, 0, 1}, min.z } - }; - - // clip against planes - for ( const auto& plane : planes ) { - vertices = uf::shapes::verticesPlaneClip( vertices, plane ); - } - - return vertices; - } - - template - void clip( - uf::stl::vector& vertices, - uf::stl::vector& indices, - - const pod::Vector3f& min, - const pod::Vector3f& max, - - bool polygons = true - ) { - if ( polygons ) { - // convert to polys - auto polygons = uf::shapes::verticesPolygonate( vertices, indices ); - // clip against planes - uf::shapes::clip( polygons, min, max ); - // triangulate - vertices = uf::shapes::polygonsTriangulate( polygons ); - indices = uf::shapes::verticesIndex( vertices ); - return; - } - - // deindex vertices - vertices = uf::shapes::indexedVertices( vertices, indices ); - // clip vertices - uf::shapes::clip( vertices, min, max ); - // reindex - indices = uf::shapes::verticesIndex( vertices ); - } - } -} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/transform.h b/engine/inc/uf/utils/math/transform.h index 5ceae10c..4d0ca165 100644 --- a/engine/inc/uf/utils/math/transform.h +++ b/engine/inc/uf/utils/math/transform.h @@ -53,7 +53,11 @@ namespace uf { template pod::Transform /*UF_API*/ fromMatrix( const pod::Matrix4t& matrix ); template pod::Transform& /*UF_API*/ reference( pod::Transform& transform, const pod::Transform& parent, bool reorient = true ); template pod::Transform /*UF_API*/ interpolate( const pod::Transform& from, const pod::Transform& to, float factor, bool reorient = true ); - + template pod::Transform /*UF_API*/ inverse(const pod::Transform& t); + template pod::Vector3t /*UF_API*/ apply( const pod::Transform& transform, const pod::Vector3t& point ); + template pod::Vector3t /*UF_API*/ applyInverse(const pod::Transform& t, const pod::Vector3t& worldPoint); + + template uf::stl::string /*UF_API*/ toString( const pod::Transform&, bool flatten = true ); template ext::json::Value /*UF_API*/ encode( const pod::Transform&, bool flatten = true, const ext::json::EncodingSettings& = {} ); template pod::Transform& /*UF_API*/ decode( const ext::json::Value&, pod::Transform& ); diff --git a/engine/inc/uf/utils/math/transform/transform.inl b/engine/inc/uf/utils/math/transform/transform.inl index b513903b..0ffffdb1 100644 --- a/engine/inc/uf/utils/math/transform/transform.inl +++ b/engine/inc/uf/utils/math/transform/transform.inl @@ -156,6 +156,50 @@ template pod::Transform /*UF_API*/ uf::transform::interpolate( co transform.orientation = uf::quaternion::slerp( from.orientation, to.orientation, factor ); return reorient ? uf::transform::reorient( transform ) : transform; } + +template +pod::Transform uf::transform::inverse(const pod::Transform& t) { + pod::Transform inv; + + // Inverse orientation = conjugate of normalized quaternion + pod::Quaternion qInv = uf::quaternion::conjugate(uf::vector::normalize(t.orientation)); + + // Inverse scale is reciprocal (with protection against zero) + pod::Vector3t sInv { + (fabs(t.scale.x) > 1e-8) ? (1.0f / t.scale.x) : 0.0f, + (fabs(t.scale.y) > 1e-8) ? (1.0f / t.scale.y) : 0.0f, + (fabs(t.scale.z) > 1e-8) ? (1.0f / t.scale.z) : 0.0f + }; + + inv.scale = sInv; + inv.orientation = qInv; + + // To invert translation: apply inverse orientation/scale + pod::Vector3t negPos = -t.position; + inv.position = uf::quaternion::rotate(qInv, pod::Vector3f{ negPos.x * sInv.x, + negPos.y * sInv.y, + negPos.z * sInv.z }); + + // Update basis vectors + inv = uf::transform::reorient(inv); + + return inv; +} + + +template // Normalizes a vector +pod::Vector3t /*UF_API*/ uf::transform::apply( const pod::Transform& transform, const pod::Vector3t& point ) { + // return uf::transform::model( transform ) * point; + return uf::matrix::multiply( uf::transform::model( transform ), point ); +} + +// Apply inverse transform to a point +template +pod::Vector3t uf::transform::applyInverse(const pod::Transform& t, const pod::Vector3t& worldPoint) { + pod::Transform inv = inverse(t); + return uf::transform::apply(inv, worldPoint); +} + template // Normalizes a vector uf::stl::string /*UF_API*/ uf::transform::toString( const pod::Transform& t, bool flatten ) { pod::Transform transform = flatten ? uf::transform::flatten(t) : t; diff --git a/engine/inc/uf/utils/math/vector.h b/engine/inc/uf/utils/math/vector.h index f46610f2..71452e60 100644 --- a/engine/inc/uf/utils/math/vector.h +++ b/engine/inc/uf/utils/math/vector.h @@ -125,6 +125,8 @@ namespace uf { // Equality checking template int /*UF_API*/ compareTo( const T& left, const T& right ); // Equality check between two vectors (less than) template bool /*UF_API*/ equals( const T& left, const T& right ); // Equality check between two vectors (equals) + + template bool /*UF_API*/ isValid( const T& v ); // Checks if all components are valid (non NaN, inf, etc.) // Basic arithmetic template T /*UF_API*/ add( const T& left, const T& right ); // Adds two vectors of same type and size together template T /*UF_API*/ add( const T& left, /*const typename T::type_t&*/ typename T::type_t scalar ); // Adds two vectors of same type and size together @@ -172,8 +174,9 @@ namespace uf { template typename T::type_t /*UF_API*/ norm( const T& vector ); // Compute the norm of the vector template typename T::type_t /*UF_API*/ magnitude( const T& vector ); // Gets the magnitude of the vector template T /*UF_API*/ normalize( const T& vector ); // Normalizes a vector - template void /*UF_API*/ orthonormalize( T& x, T& y ); // Normalizes a vector - template T /*UF_API*/ orthonormalize( const T& x, const T& y ); // Normalizes a vector + template T /*UF_API*/ clampMagnitude( const T& vector ); // Clamps the magnitude of a vector + template void /*UF_API*/ orthonormalize( T& x, T& y ); // Orthonormalizes a vector against another vector + template T /*UF_API*/ orthonormalize( const T& x, const T& y ); // Orthonormalizes a vector against another vector template uf::stl::string /*UF_API*/ toString( const T& vector ); // Parses a vector as a string template ext::json::Value encode( const pod::Vector& v, const ext::json::EncodingSettings& = {} ); // Parses a vector into a JSON value diff --git a/engine/inc/uf/utils/math/vector/class.inl b/engine/inc/uf/utils/math/vector/class.inl index 650ebf3d..3419974b 100644 --- a/engine/inc/uf/utils/math/vector/class.inl +++ b/engine/inc/uf/utils/math/vector/class.inl @@ -57,8 +57,7 @@ T& uf::Vector::setComponent( std::size_t i, const T& value ) { // Validation template // Checks if all components are valid (non NaN, inf, etc.) bool uf::Vector::isValid() const { - for ( std::size_t i = 0; i < N; ++i ) if ( this->m_pod[i] != this->m_pod[i] ) return false; - return true; + return uf::vector::isValid( this->m_pod ); } // Basic arithmetic template // Adds two vectors of same type and size together diff --git a/engine/inc/uf/utils/math/vector/pod.inl b/engine/inc/uf/utils/math/vector/pod.inl index f1bc1af5..e14bd0e9 100644 --- a/engine/inc/uf/utils/math/vector/pod.inl +++ b/engine/inc/uf/utils/math/vector/pod.inl @@ -42,6 +42,10 @@ bool /*UF_API*/ uf::vector::equals( const T& left, const T& right ) { if ( left[i] != right[i] ) return false; return true; } +template // +bool /*UF_API*/ uf::vector::isValid( const T& v ) { + return uf::vector::equals( v, v ); +} // Basic arithmetic template // Adds two vectors of same type and size together T /*UF_API*/ uf::vector::add( const T& left, const T& right ) { @@ -494,6 +498,18 @@ T /*UF_API*/ uf::vector::normalize( const T& vector ) { #endif return uf::vector::divide(vector, norm); } + +template // Clamps the length of a vector +T /*UF_API*/ uf::vector::clampMagnitude( const T& v, float maxMag ) { + T res = v; + float mag = uf::vector::magnitude( res ); + if ( mag > maxMag ) { + res /= (maxMag / sqrt(mag)); + } + + return res; +} + template // Normalizes a vector void /*UF_API*/ uf::vector::orthonormalize( T& normal, T& tangent ) { normal = uf::vector::normalize( normal ); diff --git a/engine/inc/uf/utils/mesh/clip.h b/engine/inc/uf/utils/mesh/clip.h new file mode 100644 index 00000000..86cde0c5 --- /dev/null +++ b/engine/inc/uf/utils/mesh/clip.h @@ -0,0 +1,406 @@ +#pragma once + +#include +#include +#include + +namespace uf { + namespace shapes { + inline float planePointSide( const pod::Plane& plane, const pod::Vector3f& point ) { + return uf::vector::dot( point, plane.normal ) - plane.offset; + } + + inline float planeEdgeIntersection( const pod::Plane& plane, const pod::Vector3f& p1, const pod::Vector3f& p2 ) { + return (plane.offset - uf::vector::dot(p1, plane.normal)) / uf::vector::dot(uf::vector::subtract(p2, p1), plane.normal); + } + + template + uf::stl::vector> verticesPolygonate( const uf::stl::vector& vertices, const uf::stl::vector& indices ) { + uf::stl::vector> polygons; + polygons.reserve( indices.size() / 3 ); + + for ( auto i = 0; i < indices.size(); i += 3 ) { + polygons.emplace_back( pod::Polygon{ { vertices[indices[i+0]], vertices[indices[i+1]], vertices[indices[i+2]] } }); + } + + return polygons; + } + + template + uf::stl::vector> verticesPolygonate( const uf::stl::vector& vertices ) { + uf::stl::vector> polygons; + polygons.reserve( vertices.size() / 3 ); + + for ( auto i = 0; i < vertices.size(); i += 3 ) { + polygons.emplace_back( pod::Polygon{ { vertices[i+0], vertices[i+1], vertices[i+2] } }); + } + + return polygons; + } + + template + uf::stl::vector polygonsTriangulate( const uf::stl::vector>& polygons ) { + uf::stl::vector vertices; + vertices.reserve( polygons.size() * 3 ); + + for ( const auto& polygon : polygons ) { + for (size_t i = 1; i < polygon.points.size() - 1; ++i) { + vertices.emplace_back(polygon.points[0]); + vertices.emplace_back(polygon.points[i]); + vertices.emplace_back(polygon.points[i + 1]); + } + } + + // rewind + for ( auto i = 0; i < vertices.size(); i += 3 ) { + for ( const auto& polygon : polygons ) { + const auto& p0 = vertices[i+0]; + const auto& p1 = vertices[i+1]; + const auto& p2 = vertices[i+2]; + + auto edge1 = p1.position - p0.position; + auto edge2 = p2.position - p0.position; + auto polygonNormal = uf::vector::cross(edge1, edge2); + + if ( uf::vector::dot( polygonNormal, p0.normal ) < 0 ) { + std::swap( vertices[i+1], vertices[i+2] ); + } + } + } + + return vertices; + } + + template + uf::stl::vector verticesIndex( const uf::stl::vector& vertices ) { + uf::stl::vector indices; + indices.reserve( vertices.size() ); + + for ( auto i = 0; i < vertices.size(); ++i ) { + indices.emplace_back( i ); + } + + return indices; + } + + template + uf::stl::vector indexedVertices( const uf::stl::vector& vertices, const uf::stl::vector& indices ) { + uf::stl::vector newVertices; + + for ( auto idx : indices ) { + newVertices.emplace_back( vertices[idx] ); + } + + return newVertices; + } + + + // to-do: fix why these causes problems + // the polygon approach works fine enough + template + uf::stl::vector verticesPlaneClip( const uf::stl::vector& vertices, const pod::Plane& plane ) { + uf::stl::vector newVertices; + + for ( auto i = 0; i < vertices.size(); i += 3 ) { + const auto& v0 = vertices[i+0]; + const auto& v1 = vertices[i+1]; + const auto& v2 = vertices[i+2]; + + /* + float s0 = uf::shapes::planePointSide( plane, v0.position ) > 0; + float s1 = uf::shapes::planePointSide( plane, v1.position ) > 0; + float s2 = uf::shapes::planePointSide( plane, v2.position ) > 0; + + if ( s0 == s1 && s1 == s2 ) { + newVertices.emplace_back( v0 ); + newVertices.emplace_back( v1 ); + newVertices.emplace_back( v2 ); + + continue; + } + + if ( s0 != s1 ) { + float t = planeEdgeIntersection( plane, v0.position, v1.position ); + T intersection = T::interpolate( v0, v1, t ); + + if ( s0 ) { + newVertices.emplace_back( v0 ); + newVertices.emplace_back( intersection ); + newVertices.emplace_back( v1 ); + } else { + newVertices.emplace_back( v1 ); + newVertices.emplace_back( intersection ); + newVertices.emplace_back( v0 ); + } + } + + if ( s1 != s2 ) { + float t = planeEdgeIntersection( plane, v1.position, v2.position ); + T intersection = T::interpolate( v1, v2, t ); + + if ( s1 ) { + newVertices.emplace_back( v1 ); + newVertices.emplace_back( intersection ); + newVertices.emplace_back( v2 ); + } else { + newVertices.emplace_back( v2 ); + newVertices.emplace_back( intersection ); + newVertices.emplace_back( v1 ); + } + } + + if ( s2 != s0 ) { + float t = planeEdgeIntersection( plane, v2.position, v0.position ); + T intersection = T::interpolate( v2, v0, t ); + + if ( s2 ) { + newVertices.emplace_back( v2 ); + newVertices.emplace_back( intersection ); + newVertices.emplace_back( v0 ); + } else { + newVertices.emplace_back( v0 ); + newVertices.emplace_back( intersection ); + newVertices.emplace_back( v2 ); + } + } + */ + float d0 = uf::shapes::planePointSide( plane, v0.position ); + float d1 = uf::shapes::planePointSide( plane, v1.position ); + float d2 = uf::shapes::planePointSide( plane, v2.position ); + + if (d0 < 0 && d1 < 0 && d2 < 0) { + continue; + } + + if (d0 >= 0 && d1 >= 0 && d2 >= 0) { + newVertices.emplace_back( v0 ); + newVertices.emplace_back( v1 ); + newVertices.emplace_back( v2 ); + continue; + } + + { + // Some vertices are in front and some are behind + bool edge0Crosses = (d0 >= 0) != (d1 >= 0); + bool edge1Crosses = (d1 >= 0) != (d2 >= 0); + bool edge2Crosses = (d2 >= 0) != (d0 >= 0); + + T intersection01; + T intersection12; + T intersection20; + + if ( edge0Crosses ) { + auto t = planeEdgeIntersection( plane, v0.position, v1.position ); + intersection01 = T::interpolate( v0, v1, t ); + } + if ( edge1Crosses ) { + auto t = planeEdgeIntersection( plane, v1.position, v2.position ); + intersection12 = T::interpolate( v1, v2, t ); + } + if ( edge2Crosses ) { + auto t = planeEdgeIntersection( plane, v2.position, v0.position ); + intersection20 = T::interpolate( v2, v0, t ); + } + + if (edge0Crosses && edge1Crosses && edge2Crosses) { + // All edges cross the plane: split into three triangles + newVertices.emplace_back( intersection01 ); + newVertices.emplace_back( intersection12 ); + newVertices.emplace_back( intersection20 ); + } else if (edge0Crosses && edge1Crosses) { + // Edges 0 and 1 cross: create a quad + newVertices.emplace_back( intersection01 ); + newVertices.emplace_back( intersection12 ); + newVertices.emplace_back( v1 ); + + newVertices.emplace_back( intersection01 ); + newVertices.emplace_back( v1 ); + newVertices.emplace_back( v0 ); + } else if (edge1Crosses && edge2Crosses) { + // Edges 1 and 2 cross: create a quad + newVertices.emplace_back( intersection12 ); + newVertices.emplace_back( intersection20 ); + newVertices.emplace_back( v2 ); + + newVertices.emplace_back( intersection12 ); + newVertices.emplace_back( v2 ); + newVertices.emplace_back( v1 ); + } else if (edge2Crosses && edge0Crosses) { + // Edges 2 and 0 cross: create a quad + newVertices.emplace_back( intersection20 ); + newVertices.emplace_back( intersection01 ); + newVertices.emplace_back( v0 ); + + newVertices.emplace_back( intersection20 ); + newVertices.emplace_back( v0 ); + newVertices.emplace_back( v2 ); + } else if (edge0Crosses) { + // Only edge 0 crosses: create a triangle + newVertices.emplace_back( intersection01 ); + newVertices.emplace_back( v1 ); + newVertices.emplace_back( v0 ); + } else if (edge1Crosses) { + // Only edge 1 crosses: create a triangle + newVertices.emplace_back( intersection12 ); + newVertices.emplace_back( v2 ); + newVertices.emplace_back( v1 ); + } else if (edge2Crosses) { + // Only edge 2 crosses: create a triangle + newVertices.emplace_back( intersection20 ); + newVertices.emplace_back( v0 ); + newVertices.emplace_back( v2 ); + } + } + } + + return newVertices; + } + + template + pod::Polygon polygonPlaneClip( const pod::Polygon& polygon, const pod::Plane& plane ) { + pod::Polygon clipped; + + auto count = polygon.points.size(); + bool modified = false; + for (auto i = 0; i < count; ++i) { + const T& current = polygon.points[i]; + const T& previous = polygon.points[(i - 1 + count) % count]; + + float currentSide = uf::shapes::planePointSide( plane, current.position ); + float previousSide = uf::shapes::planePointSide( plane, previous.position ); + + if (currentSide >= 0) { + if (previousSide < 0) { + float t = uf::shapes::planeEdgeIntersection( plane, previous.position, current.position ); + T interpolated = T::interpolate( previous, current, t ); + clipped.points.emplace_back(interpolated); + + modified = true; + } + clipped.points.emplace_back(current); + } else if (previousSide >= 0) { + float t = uf::shapes::planeEdgeIntersection( plane, previous.position, current.position ); + T interpolated = T::interpolate( previous, current, t ); + clipped.points.emplace_back(interpolated); + + modified = true; + } + } + + if ( modified ) { + std::reverse( clipped.points.begin(), clipped.points.end() ); + } + + return clipped; + } + + template + uf::stl::vector>& clip( + uf::stl::vector>& polygons, + + const pod::Vector3f& min, + const pod::Vector3f& max + ) { + // inverted because its needed + uf::stl::vector planes = { + // Right plane: x = max.x + { pod::Vector3f{-1, 0, 0}, -max.x }, + // Left plane: x = min.x + { pod::Vector3f{1, 0, 0}, min.x }, + // Top plane: y = max.y + { pod::Vector3f{0, -1, 0}, -max.y }, + // Bottom plane: y = min.y + { pod::Vector3f{0, 1, 0}, min.y }, + // Front plane: z = max.z + { pod::Vector3f{0, 0, -1}, -max.z }, + // Back plane: z = min.z + { pod::Vector3f{0, 0, 1}, min.z } + }; + + // clip against planes + for ( const auto& plane : planes ) { + uf::stl::vector> clippedPolygons; + for ( const auto& polygon : polygons ) { + auto clipped = uf::shapes::polygonPlaneClip( polygon, plane ); + if ( clipped.points.size() >= 3 ) clippedPolygons.emplace_back( clipped ); + } + polygons = std::move(clippedPolygons); + } + + return polygons; + } + + template + uf::stl::vector& clip( + uf::stl::vector& vertices, + + const pod::Vector3f& min, + const pod::Vector3f& max, + + bool polygons = true + ) { + if ( polygons ) { + // convert to polys + auto polygons = uf::shapes::verticesPolygonate( vertices ); + // clip against planes + uf::shapes::clip( polygons, min, max ); + // triangulate + vertices = uf::shapes::polygonsTriangulate( polygons ); + + return vertices; + } + + // inverted because its needed + uf::stl::vector planes = { + // Right plane: x = max.x + { pod::Vector3f{-1, 0, 0}, -max.x }, + // Left plane: x = min.x + { pod::Vector3f{1, 0, 0}, min.x }, + // Top plane: y = max.y + { pod::Vector3f{0, -1, 0}, -max.y }, + // Bottom plane: y = min.y + { pod::Vector3f{0, 1, 0}, min.y }, + // Front plane: z = max.z + { pod::Vector3f{0, 0, -1}, -max.z }, + // Back plane: z = min.z + { pod::Vector3f{0, 0, 1}, min.z } + }; + + // clip against planes + for ( const auto& plane : planes ) { + vertices = uf::shapes::verticesPlaneClip( vertices, plane ); + } + + return vertices; + } + + template + void clip( + uf::stl::vector& vertices, + uf::stl::vector& indices, + + const pod::Vector3f& min, + const pod::Vector3f& max, + + bool polygons = true + ) { + if ( polygons ) { + // convert to polys + auto polygons = uf::shapes::verticesPolygonate( vertices, indices ); + // clip against planes + uf::shapes::clip( polygons, min, max ); + // triangulate + vertices = uf::shapes::polygonsTriangulate( polygons ); + indices = uf::shapes::verticesIndex( vertices ); + return; + } + + // deindex vertices + vertices = uf::shapes::indexedVertices( vertices, indices ); + // clip vertices + uf::shapes::clip( vertices, min, max ); + // reindex + indices = uf::shapes::verticesIndex( vertices ); + } + } +} \ No newline at end of file diff --git a/engine/inc/uf/utils/mesh/grid.h b/engine/inc/uf/utils/mesh/grid.h index ddc34d2c..d53f2185 100644 --- a/engine/inc/uf/utils/mesh/grid.h +++ b/engine/inc/uf/utils/mesh/grid.h @@ -1,11 +1,12 @@ #pragma once #include +#include #include +#include #include #include - namespace uf { namespace meshgrid { struct Node { diff --git a/engine/inc/uf/utils/mesh/mesh.h b/engine/inc/uf/utils/mesh/mesh.h index 31e18c60..78e4b94c 100644 --- a/engine/inc/uf/utils/mesh/mesh.h +++ b/engine/inc/uf/utils/mesh/mesh.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include @@ -160,6 +159,38 @@ namespace uf { size_t offset = 0; // bytes to offset from within the associated buffer int32_t interleaved = -1; // index to interleaved buffer if in bounds } vertex, index, instance, indirect; + + struct AttributeView { + Attribute attribute; + + const void* data(size_t first = 0) const { + if ( !valid() ) return NULL; + return static_cast(attribute.pointer) + attribute.stride * first; + } + + template + const T* get(size_t first = 0) const { + return reinterpret_cast(data(first)); + } + + bool valid() const { return attribute.pointer != NULL; } + size_t stride() const { return attribute.stride; } + size_t components() const { return attribute.descriptor.components; } + }; + + struct View { + uf::Mesh::Input vertex; + uf::Mesh::Input index; + int32_t indirectIndex = -1; + + uf::stl::unordered_map attributes; + + const AttributeView& operator[](const uf::stl::string& name) const { + static AttributeView null{}; + if ( auto it = attributes.find(name); it != attributes.end() ) return it->second; + return null; + } + }; /* struct Bounds { pod::Vector3f min = { std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max() }; @@ -246,6 +277,10 @@ namespace uf { std::string printInstances( bool = true ) const; std::string printIndirects( bool = true ) const; + uf::Mesh::View makeView( const uf::stl::vector& wanted = {} ) const; + uf::Mesh::View makeView( size_t commandIndex, const uf::stl::vector& wanted = {} ) const; + uf::stl::vector makeViews( const uf::stl::vector& wanted = {} ) const; + inline bool hasVertex( const uf::stl::vector& descriptors ) const { return _hasV( vertex, descriptors ); } inline bool hasVertex( const uf::Mesh& mesh ) const { return _hasV( vertex, mesh.vertex ); } inline void bindVertex( const uf::stl::vector& descriptors ) { return _bindV( vertex, descriptors ); } diff --git a/engine/inc/uf/utils/tests/tests.h b/engine/inc/uf/utils/tests/tests.h new file mode 100644 index 00000000..6369e80d --- /dev/null +++ b/engine/inc/uf/utils/tests/tests.h @@ -0,0 +1,73 @@ +#include +#include +#include +#include +#include + +#if 1 || UF_DEBUG + #define EXPECT_TRUE(expr) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = ((expr));\ + test.message = ::fmt::format("EXPECT_TRUE({})", #expr);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define EXPECT_FALSE(expr) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (!(expr));\ + test.message = ::fmt::format("EXPECT_FALSE({})", #expr);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define EXPECT_EQ(a,b) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (((a) == (b)));\ + test.message = ::fmt::format("EXPECT_EQ({}, {}), ({} == {})", #a, #b, a, b);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define EXPECT_NEAR(a,b,eps) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (std::fabs((a)-(b)) <= (eps));\ + test.message = ::fmt::format("EXPECT_NEAR({}, {}), ({} == {})", #a, #b, a, b);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define EXPECT_FLOAT_EQ(a,b) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (std::fabs((a)-(b)) <= (1.0e-4f));\ + test.message = ::fmt::format("EXPECT_FLOAT_EQ({}, {}), ({} == {})", #a, #b, a, b);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define TEST(name, ...) \ + void TOKEN_PASTE(UNIT_TEST_, name)(){\ + __VA_ARGS__;\ + };\ + static uf::StaticInitialization TOKEN_PASTE(STATIC_INITIALIZATION_, __LINE__)( []{\ + uf::unitTests::tests[#name] = { false, "", TOKEN_PASTE(UNIT_TEST_, name) };\ + });\ + +#else + // no-op + #define TEST(...) +#endif + +namespace pod { + struct UnitTest { + typedef std::function function_t; + + bool passes; + uf::stl::string message; + pod::UnitTest::function_t function; + }; +} + +namespace uf { + namespace unitTests { + extern UF_API uf::stl::string current; + extern UF_API uf::stl::KeyMap tests; + + void UF_API execute(); + } +} \ No newline at end of file diff --git a/engine/src/engine/ext/ext.cpp b/engine/src/engine/ext/ext.cpp index 867351b4..94052e3e 100644 --- a/engine/src/engine/ext/ext.cpp +++ b/engine/src/engine/ext/ext.cpp @@ -48,7 +48,6 @@ bool uf::ready = false; uf::stl::vector uf::arguments; - uf::Serializer uf::config; namespace { diff --git a/engine/src/engine/ext/player/behavior.cpp b/engine/src/engine/ext/player/behavior.cpp index c721cc18..0b603baf 100644 --- a/engine/src/engine/ext/player/behavior.cpp +++ b/engine/src/engine/ext/player/behavior.cpp @@ -25,7 +25,9 @@ UF_BEHAVIOR_TRAITS_CPP(ext::PlayerBehavior, ticks = true, renders = false, multi void ext::PlayerBehavior::initialize( uf::Object& self ) { auto& transform = this->getComponent>(); +#if UF_USE_REACTPHYSICS auto& collider = this->getComponent(); +#endif auto& metadata = this->getComponent(); auto& metadataJson = this->getComponent(); @@ -130,9 +132,13 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { auto& cameraTransform = camera.getTransform(); auto& transform = this->getComponent>(); - auto& physics = this->getComponent(); auto& scene = uf::scene::getCurrentScene(); +#if UF_USE_REACTPHYSICS + auto& physics = this->getComponent(); auto& collider = this->getComponent(); +#else + auto& physics = this->getComponent(); +#endif auto& metadata = this->getComponent(); auto& metadataJson = this->getComponent(); @@ -269,11 +275,12 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { } #endif + bool wasFloored = stats.floored; stats.menu = metadata.system.menu; stats.noclipped = metadata.system.noclipped; stats.floored = stats.noclipped; - - { + if ( !stats.floored ) { + #if UF_USE_REACTPHYSICS float t = -1; uf::Object* hit = NULL; pod::Vector3f center = transform.position + metadata.movement.floored.feet; @@ -282,6 +289,17 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( metadata.movement.floored.print ) UF_MSG_DEBUG("Floored: {} | {}", hit->getName(), t); stats.floored = true; } + #else + pod::Vector3f origin = transform.position + metadata.movement.floored.feet; + pod::Vector3f direction = metadata.movement.floored.floor; + pod::RayQuery query = uf::physics::impl::rayCast( pod::Ray{origin, direction}, physics, 1.0f ); + + if ( query.hit ) { + UF_MSG_DEBUG("{}: {} | {}", query.contact.penetration, uf::string::toString(*query.body->object), uf::vector::toString(physics.velocity)); + stats.floored = true; + // if ( physics.velocity.y < 0.0f ) physics.velocity.y = 0.0f; + } + #endif } #if 0 TIMER(0.25, keys.use ) { @@ -328,7 +346,9 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { } #endif +#if UF_USE_REACTPHYSICS if ( collider.stats.gravity == pod::Vector3f{0,0,0} ) stats.noclipped = true; +#endif { speed.rotate = metadata.movement.rotate * uf::physics::time::delta; @@ -343,7 +363,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { speed.run *= 1.5; } if ( !stats.floored || stats.noclipped ) speed.friction = 1; - if ( stats.noclipped ) physics.linear.velocity = {}; + if ( stats.noclipped ) physics.velocity = {}; } if ( keys.running ) speed.move = speed.run; else if ( keys.walk ) speed.move = speed.walk; @@ -403,10 +423,13 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { translator = cameraTransform; translator.forward.y = 0; translator.forward = uf::vector::normalize( translator.forward ); - } else if ( stats.noclipped || collider.stats.gravity == pod::Vector3f{0,0,0} ){ + } + #if UF_USE_REACTPHYSICS + else if ( stats.noclipped || collider.stats.gravity == pod::Vector3f{0,0,0} ){ translator.forward.y += cameraTransform.forward.y; translator.forward = uf::vector::normalize( translator.forward ); } + #endif if ( metadata.system.control ) { // noclip handler @@ -430,16 +453,16 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( keys.left ^ keys.right ) target += translator.right * (keys.right ? 1 : -1); target = uf::vector::normalize( target ); - physics.linear.velocity *= { speed.friction, 1, speed.friction }; + physics.velocity *= { speed.friction, 1, speed.friction }; stats.walking = (keys.forward ^ keys.backwards) || (keys.left ^ keys.right); if ( stats.walking ) { float factor = stats.floored ? 1.0f : speed.air; if ( stats.noclipped ) { - physics.linear.velocity += target * speed.move; + physics.velocity += target * speed.move; } else { - physics.linear.velocity += target * std::clamp( speed.move * factor - uf::vector::dot( physics.linear.velocity, target ), 0.0f, speed.move * 10 * uf::physics::time::delta ); + physics.velocity += target * std::clamp( speed.move * factor - uf::vector::dot( physics.velocity, target ), 0.0f, speed.move * 10 * uf::physics::time::delta ); } auto dot = uf::vector::dot( transform.forward, target ); @@ -449,14 +472,18 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { auto axis = transform.up; float angle = uf::vector::signedAngle( transform.forward, target, axis ) * uf::physics::time::delta * 4; // speed.rotate; + #if UF_USE_REACTPHYSICS if ( collider.body ) uf::physics::impl::applyRotation( collider, axis, angle ); else + #else + if ( physics.object ) uf::physics::impl::applyRotation( physics, axis, angle ); else + #endif uf::transform::rotate( transform, axis, angle ); } } if ( !stats.floored ) stats.walking = false; } TIMER(0.0625, stats.floored && keys.jump && !stats.noclipped ) { - physics.linear.velocity += translator.up * metadata.movement.jump; + physics.velocity += translator.up * metadata.movement.jump; } if ( stats.floored && keys.jump && stats.noclipped ) transform.position += translator.up * metadata.movement.jump * uf::physics::time::delta * 4.0f; if ( keys.crouch ) { @@ -500,7 +527,11 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( metadata.camera.invert.x ) lookDelta.x *= -1; metadata.camera.limit.current.x += lookDelta.x; if ( metadata.camera.limit.current.x != metadata.camera.limit.current.x || ( metadata.camera.limit.current.x < metadata.camera.limit.max.x && metadata.camera.limit.current.x > metadata.camera.limit.min.x ) ) { + #if UF_USE_REACTPHYSICS if ( collider.body ) uf::physics::impl::applyRotation( collider, transform.up, lookDelta.x ); else + #else + if ( physics.object ) uf::physics::impl::applyRotation( physics, transform.up, lookDelta.x ); else + #endif uf::transform::rotate( transform, transform.up, lookDelta.x ); } else metadata.camera.limit.current.x -= lookDelta.x; } @@ -508,13 +539,19 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( metadata.camera.invert.y ) lookDelta.y *= -1; metadata.camera.limit.current.y += lookDelta.y; if ( metadata.camera.limit.current.y != metadata.camera.limit.current.y || ( metadata.camera.limit.current.y < metadata.camera.limit.max.y && metadata.camera.limit.current.y > metadata.camera.limit.min.y ) ) { + #if UF_USE_REACTPHYSICS // if ( collider.body && !collider.shared ) uf::physics::impl::applyRotation( collider, cameraTransform.right, lookDelta.y ); else + #endif uf::transform::rotate( cameraTransform, cameraTransform.right, lookDelta.y ); } else metadata.camera.limit.current.y -= lookDelta.y; } } else if ( metadata.system.control ) { if ( keys.lookRight ^ keys.lookLeft ) { + #if UF_USE_REACTPHYSICS if ( collider.body ) uf::physics::impl::applyRotation( collider, transform.up, speed.rotate * (keys.lookRight ? 1 : -1) ); else + #else + if ( physics.object ) uf::physics::impl::applyRotation( physics, transform.up, speed.rotate * (keys.lookRight ? 1 : -1) ); else + #endif uf::transform::rotate( transform, transform.up, speed.rotate * (keys.lookRight ? 1 : -1) ); } if ( keys.lookUp ^ keys.lookDown ) { @@ -529,15 +566,22 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { // cameraTransform.position = uf::quaternion::rotate( rotation, cameraTransform.position - transform.position ); } if ( keys.lookUp ^ keys.lookDown ) { + #if 0 // if ( collider.body && !collider.shared ) uf::physics::impl::applyRotation( collider, cameraTransform.right, lookDelta.y ); else + #endif float direction = keys.lookUp ? 1 : -1; if ( metadata.camera.invert.y ) direction *= -1; uf::transform::rotate( cameraTransform, cameraTransform.right, speed.rotate * direction ); } } { - if ( collider.body ) uf::physics::impl::setVelocity( collider, physics.linear.velocity ); else - transform.position += physics.linear.velocity * uf::physics::time::delta; + #if UF_USE_REACTPHYSICS + if ( collider.body ) uf::physics::impl::setVelocity( collider, physics.velocity ); else + #else + if ( physics.object ) uf::physics::impl::setVelocity( physics, physics.velocity ); else + #endif + transform.position += physics.velocity * uf::physics::time::delta; + // if ( uf::vector::magnitude( physics.velocity ) > 1.0e-6 ) UF_MSG_DEBUG("Velocity: {}", uf::vector::toString( physics.velocity )); } diff --git a/engine/src/engine/ext/region/chunk/behavior.cpp b/engine/src/engine/ext/region/chunk/behavior.cpp index fa5a0890..78849834 100644 --- a/engine/src/engine/ext/region/chunk/behavior.cpp +++ b/engine/src/engine/ext/region/chunk/behavior.cpp @@ -29,7 +29,9 @@ namespace { auto& metadataJson = this->getComponent(); auto& mesh = this->getComponent(); + #if 0 auto& collider = this->getComponent(); + #endif auto& graphic = this->getComponent(); auto& texture = graphic.material.textures.emplace_back(); @@ -67,6 +69,7 @@ namespace { uf::graph::convert( self ); } + #if 0 { auto& phyziks = metadataJson["physics"]; @@ -77,8 +80,8 @@ namespace { collider.stats.gravity = uf::vector::decode( phyziks["gravity"], collider.stats.gravity ); uf::physics::impl::create( self, mesh, !phyziks["static"].as(true) ); - } + #endif } } void ext::RegionChunkBehavior::initialize( uf::Object& self ) { diff --git a/engine/src/engine/ext/scene/behavior.cpp b/engine/src/engine/ext/scene/behavior.cpp index 7435184f..57db8d2e 100644 --- a/engine/src/engine/ext/scene/behavior.cpp +++ b/engine/src/engine/ext/scene/behavior.cpp @@ -24,7 +24,6 @@ #include -#include #include #include diff --git a/engine/src/engine/graph/graph.cpp b/engine/src/engine/graph/graph.cpp index d56ad14f..7c5adff4 100644 --- a/engine/src/engine/graph/graph.cpp +++ b/engine/src/engine/graph/graph.cpp @@ -1347,11 +1347,17 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent ) pod::Vector3f center = (max + min) * 0.5f; pod::Vector3f corner = uf::vector::abs(max - min) * 0.5f; + + UF_MSG_DEBUG("{}: Center: {} | Corner: {}", node.name, uf::vector::toString( center ), uf::vector::toString( corner )); + UF_MSG_DEBUG("{}: Min: {} | Max: {}", node.name, uf::vector::toString( min ), uf::vector::toString( max )); - metadataJson["physics"]["center"] = uf::vector::encode( center ); - metadataJson["physics"]["corner"] = uf::vector::encode( corner ); + if ( ext::json::isNull( metadataJson["physics"]["center"] ) ) metadataJson["physics"]["center"] = uf::vector::encode( center ); + if ( ext::json::isNull( metadataJson["physics"]["corner"] ) ) metadataJson["physics"]["corner"] = uf::vector::encode( corner ); + if ( ext::json::isNull( metadataJson["physics"]["min"] ) ) metadataJson["physics"]["min"] = uf::vector::encode( min ); + if ( ext::json::isNull( metadataJson["physics"]["max"] ) ) metadataJson["physics"]["max"] = uf::vector::encode( max ); } #if !UF_GRAPH_EXTENDED + #if UF_USE_REACTPHYSICS if ( type == "mesh" ) { auto& collider = entity.getComponent(); collider.stats.mass = phyziks["mass"].as(collider.stats.mass); @@ -1362,6 +1368,12 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent ) uf::physics::impl::create( entity.as(), mesh, !phyziks["static"].as(true) ); } + #else + if ( type == "mesh" ) { + auto phyziks["mass"].as(collider.stats.mass); + uf::physics::impl::create( entity.as(), mesh, mass ); + } + #endif #endif } } @@ -1917,10 +1929,11 @@ void uf::graph::reload( pod::Graph& graph, pod::Node& node ) { // bind mesh to physics state // to-do: figure out why the mesh just suddenly breaks when re-streamed in dreamcast (could just be the version of reactphysics) { + #if UF_USE_REACTPHYSICS auto phyziks = tag["physics"]; if ( !ext::json::isObject( phyziks ) ) phyziks = metadataJson["physics"]; else metadataJson["physics"] = phyziks; - + if ( ext::json::isObject( phyziks ) ) { uf::stl::string type = phyziks["type"].as(); if ( type == "mesh" ) { @@ -1939,6 +1952,24 @@ void uf::graph::reload( pod::Graph& graph, pod::Node& node ) { uf::physics::impl::create( entity, mesh, !phyziks["static"].as(true) ); } } + #else + auto phyziks = tag["physics"]; + if ( !ext::json::isObject( phyziks ) ) phyziks = metadataJson["physics"]; + else metadataJson["physics"] = phyziks; + + if ( ext::json::isObject( phyziks ) ) { + uf::stl::string type = phyziks["type"].as(); + if ( type == "mesh" ) { + bool exists = entity.hasComponent(); + if ( exists ) { + uf::physics::impl::destroy( entity ); + } + + auto mass = phyziks["mass"].as(0.0f); + uf::physics::impl::create( entity, mesh, mass ); + } + } + #endif } } void uf::graph::reload( pod::Graph& graph ) { diff --git a/engine/src/engine/object/behavior.cpp b/engine/src/engine/object/behavior.cpp index 9026b6ac..c2e91f91 100644 --- a/engine/src/engine/object/behavior.cpp +++ b/engine/src/engine/object/behavior.cpp @@ -1,7 +1,6 @@ #include #include #include -#include #include #include #include @@ -10,7 +9,6 @@ #include #include #include -#include #include #include @@ -132,27 +130,64 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) { }); if ( ext::json::isObject(metadataJson["physics"]) ) { + auto& metadataJsonPhysics = metadataJson["physics"]; + #if UF_USE_REACTPHYSICS auto& collider = this->getComponent(); - collider.stats.flags = metadataJson["physics"]["flags"].as(collider.stats.flags); - collider.stats.mass = metadataJson["physics"]["mass"].as(collider.stats.mass); - collider.stats.restitution = metadataJson["physics"]["restitution"].as(collider.stats.restitution); - collider.stats.friction = metadataJson["physics"]["friction"].as(collider.stats.friction); - collider.stats.inertia = uf::vector::decode( metadataJson["physics"]["inertia"], collider.stats.inertia ); - collider.stats.gravity = uf::vector::decode( metadataJson["physics"]["gravity"], collider.stats.gravity ); + collider.stats.flags = metadataJsonPhysics["flags"].as(collider.stats.flags); + collider.stats.mass = metadataJsonPhysics["mass"].as(collider.stats.mass); + collider.stats.restitution = metadataJsonPhysics["restitution"].as(collider.stats.restitution); + collider.stats.friction = metadataJsonPhysics["friction"].as(collider.stats.friction); + collider.stats.inertia = uf::vector::decode( metadataJsonPhysics["inertia"], collider.stats.inertia ); + collider.stats.gravity = uf::vector::decode( metadataJsonPhysics["gravity"], collider.stats.gravity ); - if ( metadataJson["physics"]["type"].as() == "bounding box" ) { - pod::Vector3f center = uf::vector::decode( metadataJson["physics"]["center"], pod::Vector3f{} ); - pod::Vector3f corner = uf::vector::decode( metadataJson["physics"]["corner"], pod::Vector3f{0.5, 0.5, 0.5} ); + if ( metadataJsonPhysics["type"].as() == "bounding box" ) { + pod::Vector3f center = uf::vector::decode( metadataJsonPhysics["center"], pod::Vector3f{} ); + pod::Vector3f corner = uf::vector::decode( metadataJsonPhysics["corner"], pod::Vector3f{0.5, 0.5, 0.5} ); - if ( metadataJson["physics"]["recenter"].as(true) ) collider.transform.position = (center - transform.position); + if ( metadataJsonPhysics["recenter"].as(true) ) collider.transform.position = (center - transform.position); uf::physics::impl::create( *this, corner ); - } else if ( metadataJson["physics"]["type"].as() == "capsule" ) { - float radius = metadataJson["physics"]["radius"].as(); - float height = metadataJson["physics"]["height"].as(); + } else if ( metadataJsonPhysics["type"].as() == "capsule" ) { + float radius = metadataJsonPhysics["radius"].as(); + float height = metadataJsonPhysics["height"].as(); uf::physics::impl::create( *this, radius, height ); } + #else + auto type = metadataJsonPhysics["type"].as(); + float mass = metadataJsonPhysics["mass"].as(); + + if ( type == "bounding box" || type == "aabb" ) { + /* + pod::Vector3f center = uf::vector::decode( metadataJsonPhysics["center"], pod::Vector3f{} ); + pod::Vector3f corner = uf::vector::decode( metadataJsonPhysics["corner"], pod::Vector3f{0.5, 0.5, 0.5} ); + uf::physics::impl::create( self, pod::AABB{ center - corner, center + corner }, mass ); + */ + pod::Vector3f min = uf::vector::decode( metadataJsonPhysics["min"], pod::Vector3f{-0.5f, -0.5f, -0.5f} ); + pod::Vector3f max = uf::vector::decode( metadataJsonPhysics["max"], pod::Vector3f{0.5f, 0.5f, 0.5f} ); + uf::physics::impl::create( self, pod::AABB{ .min = min, .max = max }, mass ); + } else if ( type == "plane" ) { + pod::Vector3f direction = uf::vector::decode( metadataJsonPhysics["direction"], pod::Vector3f{} ); + float offset = metadataJsonPhysics["offset"].as(); + + uf::physics::impl::create( self, pod::Plane{ direction, offset }, mass ); + } else if ( type == "sphere" ) { + float radius = metadataJsonPhysics["radius"].as(); + + uf::physics::impl::create( self, pod::Sphere{ radius }, mass ); + } else if ( type == "capsule" ) { + float radius = metadataJsonPhysics["radius"].as(); + float height = metadataJsonPhysics["height"].as(); + + uf::physics::impl::create( self, pod::Capsule{ radius, height * 0.5f }, mass ); + } + + if ( this->getName() == "Player" && this->hasComponent() ) { + auto& body = this->getComponent(); + body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; + body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; + } + #endif } UF_BEHAVIOR_METADATA_BIND_SERIALIZER_HOOKS(metadata, metadataJson); @@ -195,11 +230,13 @@ void uf::ObjectBehavior::destroy( uf::Object& self ) { atlas.clear(); // this->deleteComponent(); } + #if UF_USE_REACTPHYSICS if ( this->hasComponent() ) { auto& collider = this->getComponent(); uf::physics::impl::detach( collider ); // this->deleteComponent(); } + #endif if ( this->hasComponent() ) { auto& renderMode = this->getComponent(); if ( uf::renderer::settings::experimental::registerRenderMode ) { @@ -218,14 +255,6 @@ void uf::ObjectBehavior::destroy( uf::Object& self ) { // this->deleteComponent(); } } -/* - if ( this->hasComponent() ) { - uf::graph::destroy( this->getComponent() ); - } - if ( this->hasComponent() ) { - uf::physics::impl::destroy( self ); - } -*/ #if UF_ENTITY_OBJECT_UNIFIED for ( uf::Entity* kv : this->getChildren() ) kv->destroy(); diff --git a/engine/src/engine/object/object.cpp b/engine/src/engine/object/object.cpp index f605b54b..0344c047 100644 --- a/engine/src/engine/object/object.cpp +++ b/engine/src/engine/object/object.cpp @@ -346,13 +346,15 @@ bool uf::Object::load( const uf::Serializer& _json ) { } } // Set movement + #if UF_USE_REACTPHYSICS if ( ext::json::isObject( json["physics"] ) && !this->hasComponent() ) { auto& physics = this->getComponent(); - physics.linear.velocity = uf::vector::decode( json["physics"]["linear"]["velocity"], physics.linear.velocity ); - physics.linear.acceleration = uf::vector::decode( json["physics"]["linear"]["acceleration"], physics.linear.acceleration ); - physics.rotational.velocity = uf::vector::decode( json["physics"]["rotational"]["velocity"], physics.rotational.velocity ); - physics.rotational.acceleration = uf::vector::decode( json["physics"]["rotational"]["acceleration"], physics.rotational.acceleration ); + physics.velocity = uf::vector::decode( json["physics"]["linear"]["velocity"], physics.velocity ); + physics.acceleration = uf::vector::decode( json["physics"]["linear"]["acceleration"], physics.acceleration ); + physics.angularVelocity = uf::vector::decode( json["physics"]["rotational"]["velocity"], physics.angularVelocity ); + physics.angularAcceleration = uf::vector::decode( json["physics"]["rotational"]["acceleration"], physics.angularAcceleration ); } + #endif // Load assets this->loadAssets( json ); diff --git a/engine/src/engine/scene/scene.cpp b/engine/src/engine/scene/scene.cpp index 887e4286..5fbdcc66 100644 --- a/engine/src/engine/scene/scene.cpp +++ b/engine/src/engine/scene/scene.cpp @@ -211,9 +211,11 @@ void uf::scene::unloadScene() { if ( current->hasComponent() ) { uf::graph::destroy( current->getComponent() ); } + #if 0 if ( current->hasComponent() ) { uf::physics::impl::destroy( *current ); } + #endif // mark rendermodes as disabled immediately auto graph = current->getGraph(true); diff --git a/engine/src/ext/lua/usertypes/physics.cpp b/engine/src/ext/lua/usertypes/physics.cpp index 8328e0f4..f257b559 100644 --- a/engine/src/ext/lua/usertypes/physics.cpp +++ b/engine/src/ext/lua/usertypes/physics.cpp @@ -1,14 +1,14 @@ #include -#if UF_USE_LUA +#if UF_USE_LUA && UF_USE_REACTPHYSICS #include namespace binds { bool hasBody( pod::Physics& self ) { return self.body; } - pod::Vector3f& linearVelocity( pod::Physics& self ) { return self.linear.velocity; } - pod::Quaternion<>& rotationalVelocity( pod::Physics& self ) { return self.rotational.velocity; } + pod::Vector3f& linearVelocity( pod::Physics& self ) { return self.velocity; } + pod::Quaternion<>& rotationalVelocity( pod::Physics& self ) { return self.angularVelocity; } - void setLinearVelocity( pod::Physics& self, const pod::Vector3f& v ) { self.linear.velocity = v; } - void setRotationalVelocity( pod::Physics& self, const pod::Quaternion<>& v ) { self.rotational.velocity = v; } + void setLinearVelocity( pod::Physics& self, const pod::Vector3f& v ) { self.velocity = v; } + void setRotationalVelocity( pod::Physics& self, const pod::Quaternion<>& v ) { self.angularVelocity = v; } void enableGravity( pod::PhysicsState& state, bool s ) { if ( !state.body ) return; diff --git a/engine/src/ext/reactphysics/reactphysics.cpp b/engine/src/ext/reactphysics/reactphysics.cpp index f290e9ab..5f5053e0 100644 --- a/engine/src/ext/reactphysics/reactphysics.cpp +++ b/engine/src/ext/reactphysics/reactphysics.cpp @@ -16,138 +16,97 @@ namespace { rp3d::PhysicsWorld* world; // i was wrong to assume that RP3D handles deleting these per the documentation + // these should be userdatas / allocated using our mempool instead to avoid fragmentation uf::stl::unordered_map> triangleParts; reactphysics3d::TriangleMesh* createTriangleMesh( const uf::Mesh& mesh, const uf::Object& object ) { auto* rMesh = ::common.createTriangleMesh(); auto& parts = ::triangleParts[object.getUid()]; - uf::Mesh::Input vertexInput = mesh.vertex; - uf::Mesh::Input indexInput = mesh.index; + auto views = mesh.makeViews({"position", "normal"}); - uf::Mesh::Attribute vertexAttribute = mesh.vertex.attributes.front(); - uf::Mesh::Attribute normalAttribute = mesh.vertex.attributes.front(); - uf::Mesh::Attribute indexAttribute = mesh.index.attributes.front(); + if ( views.empty() ) return rMesh; - for ( auto& attribute : mesh.vertex.attributes ) { - if ( attribute.descriptor.name == "position" ) vertexAttribute = attribute; - if ( attribute.descriptor.name == "normal" ) normalAttribute = attribute; - } - UF_ASSERT( vertexAttribute.descriptor.name == "position" ); + for ( auto& view : views ) { + if ( !view.vertex.count || !view.index.count ) continue; - rp3d::TriangleVertexArray::IndexDataType indexType = rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE; - rp3d::TriangleVertexArray::VertexDataType vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE; - rp3d::TriangleVertexArray::NormalDataType normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE; - switch ( mesh.index.size ) { - case sizeof(uint16_t): indexType = rp3d::TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE; break; - case sizeof(uint32_t): indexType = rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE; break; - default: UF_EXCEPTION("unsupported index type: {}", mesh.index.size); break; - } - switch ( vertexAttribute.descriptor.type ) { - case uf::renderer::enums::Type::USHORT: - case uf::renderer::enums::Type::SHORT: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_SHORT_TYPE; break; - case uf::renderer::enums::Type::FLOAT: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE; break; - #if UF_USE_FLOAT16 - case uf::renderer::enums::Type::HALF: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT16_TYPE; break; - #endif - #if UF_USE_BFLOAT16 - case uf::renderer::enums::Type::BFLOAT: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_BFLOAT16_TYPE; break; - #endif - default: UF_EXCEPTION("unsupported vertex type: {}", vertexAttribute.descriptor.type); break; - } - switch ( normalAttribute.descriptor.type ) { - case uf::renderer::enums::Type::USHORT: - case uf::renderer::enums::Type::SHORT: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_SHORT_TYPE; break; - case uf::renderer::enums::Type::FLOAT: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE; break; - #if UF_USE_FLOAT16 - case uf::renderer::enums::Type::HALF: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_FLOAT16_TYPE; break; - #endif - #if UF_USE_BFLOAT16 - case uf::renderer::enums::Type::BFLOAT: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_BFLOAT16_TYPE; break; - #endif - default: UF_EXCEPTION("unsupported normal type: {}", normalAttribute.descriptor.type); break; - } + rp3d::TriangleVertexArray* part = NULL; - if ( mesh.indirect.count ) { - for ( auto i = 0; i < mesh.indirect.count; ++i ) { - vertexInput = mesh.remapVertexInput( i ); - indexInput = mesh.remapIndexInput( i ); - - // skip if culled - if ( vertexInput.count == 0 || indexInput.count == 0 ) continue; + auto& indices = view["index"]; + auto& positions = view["position"]; + auto& normals = view["normal"]; - if ( normalAttribute.descriptor.name == "normal" ) { - auto* part = new rp3d::TriangleVertexArray( - vertexInput.count, - (const uint8_t*) (vertexAttribute.pointer) + vertexAttribute.stride * vertexInput.first, - vertexAttribute.stride, - - (const uint8_t*) (normalAttribute.pointer) + normalAttribute.stride * vertexInput.first, - normalAttribute.stride, + UF_ASSERT( positions.valid() ); - indexInput.count / 3, - (const uint8_t*) (indexAttribute.pointer) + indexAttribute.stride * indexInput.first, - indexAttribute.stride * 3, - - vertexType, - normalType, - indexType - ); - parts.emplace_back(part); - rMesh->addSubpart(part); - } else { - auto* part = new rp3d::TriangleVertexArray( - vertexInput.count, - (const uint8_t*) (vertexAttribute.pointer) + vertexAttribute.stride * vertexInput.first, - vertexAttribute.stride, - - indexInput.count / 3, - (const uint8_t*) (indexAttribute.pointer) + indexAttribute.stride * indexInput.first, - indexAttribute.stride * 3, - - vertexType, - indexType - ); - parts.emplace_back(part); - rMesh->addSubpart(part); - } + // deduce types + rp3d::TriangleVertexArray::IndexDataType indexType = rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE; + rp3d::TriangleVertexArray::VertexDataType vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE; + rp3d::TriangleVertexArray::NormalDataType normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE; + switch ( mesh.index.size ) { + case sizeof(uint16_t): indexType = rp3d::TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE; break; + case sizeof(uint32_t): indexType = rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE; break; + default: UF_EXCEPTION("unsupported index type: {}", mesh.index.size); break; + } + switch ( positions.attribute.descriptor.type ) { + case uf::renderer::enums::Type::USHORT: + case uf::renderer::enums::Type::SHORT: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_SHORT_TYPE; break; + case uf::renderer::enums::Type::FLOAT: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE; break; + #if UF_USE_FLOAT16 + case uf::renderer::enums::Type::HALF: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT16_TYPE; break; + #endif + #if UF_USE_BFLOAT16 + case uf::renderer::enums::Type::BFLOAT: vertexType = rp3d::TriangleVertexArray::VertexDataType::VERTEX_BFLOAT16_TYPE; break; + #endif + default: UF_EXCEPTION("unsupported vertex type: {}", positions.attribute.descriptor.type); break; + } + switch ( normals.attribute.descriptor.type ) { + case uf::renderer::enums::Type::USHORT: + case uf::renderer::enums::Type::SHORT: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_SHORT_TYPE; break; + case uf::renderer::enums::Type::FLOAT: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE; break; + #if UF_USE_FLOAT16 + case uf::renderer::enums::Type::HALF: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_FLOAT16_TYPE; break; + #endif + #if UF_USE_BFLOAT16 + case uf::renderer::enums::Type::BFLOAT: normalType = rp3d::TriangleVertexArray::NormalDataType::NORMAL_BFLOAT16_TYPE; break; + #endif + default: UF_EXCEPTION("unsupported normal type: {}", normals.attribute.descriptor.type); break; } - } else if ( vertexInput.count > 0 && indexInput.count > 0 ) { - if ( normalAttribute.descriptor.name == "normal" ) { - auto* part = new rp3d::TriangleVertexArray( - vertexInput.count, - (const uint8_t*) (vertexAttribute.pointer) + vertexAttribute.stride * vertexInput.first, - vertexAttribute.stride, - - (const uint8_t*) (normalAttribute.pointer) + normalAttribute.stride * vertexInput.first, - normalAttribute.stride, - indexInput.count / 3, - (const uint8_t*) (indexAttribute.pointer) + indexAttribute.stride * indexInput.first, - indexAttribute.stride * 3, + // has normals + if ( normals.valid() ) { + part = new rp3d::TriangleVertexArray( + view.vertex.count, + positions.data(view.vertex.first), + positions.stride(), + + normals.data(view.vertex.first), + normals.stride(), + + view.index.count / 3, + indices.data(view.index.first), + indices.stride() * 3, vertexType, normalType, indexType ); - parts.emplace_back(part); - rMesh->addSubpart(part); } else { - auto* part = new rp3d::TriangleVertexArray( - vertexInput.count, - (const uint8_t*) (vertexAttribute.pointer) + vertexAttribute.stride * vertexInput.first, - vertexAttribute.stride, - - indexInput.count / 3, - (const uint8_t*) (indexAttribute.pointer) + indexAttribute.stride * indexInput.first, - indexAttribute.stride * 3, + part = new rp3d::TriangleVertexArray( + view.vertex.count, + positions.data(view.vertex.first), + positions.stride(), + + view.index.count / 3, + indices.data(view.index.first), + indices.stride() * 3, vertexType, indexType ); - parts.emplace_back(part); - rMesh->addSubpart(part); } + + parts.emplace_back(part); + rMesh->addSubpart(part); } return rMesh; @@ -426,6 +385,8 @@ pod::PhysicsState& ext::reactphysics::create( uf::Object& object ) { state.transform.reference = &object.getComponent>(); state.shared = ext::reactphysics::shared; + UF_MSG_DEBUG("Created physics state: {}", uf::string::toString( object )); + return state; } @@ -507,6 +468,8 @@ pod::PhysicsState& ext::reactphysics::create( uf::Object& object, const uf::Mesh state.stats.mass = 0; ext::reactphysics::attach( state ); + UF_MSG_DEBUG("Created physics state (mesh): {}", uf::string::toString( object )); + return state; } // collider for boundingbox @@ -514,6 +477,8 @@ pod::PhysicsState& ext::reactphysics::create( uf::Object& object, const pod::Vec auto& state = ext::reactphysics::create( object ); state.shape = ::common.createBoxShape( rp3d::Vector3( abs(extent.x), abs(extent.y), abs(extent.z) ) ); ext::reactphysics::attach( state ); + + UF_MSG_DEBUG("Created physics state (box): {}", uf::string::toString( object )); return state; } @@ -522,6 +487,8 @@ pod::PhysicsState& ext::reactphysics::create( uf::Object& object, float radius, auto& state = ext::reactphysics::create( object ); state.shape = ::common.createCapsuleShape( radius, height ); ext::reactphysics::attach( state ); + + UF_MSG_DEBUG("Created physics state (capsule): {} | {}, {}", uf::string::toString( object ), radius, height); return state; } @@ -549,8 +516,8 @@ void ext::reactphysics::syncTo( ext::reactphysics::WorldState& world ) { if ( state.shared ) { if ( !ext::reactphysics::interpolate ) body->setTransform(::convert(state.transform)); - body->setLinearVelocity( ::convert(state.linear.velocity) ); - body->setAngularVelocity( ::convertQ(state.rotational.velocity) ); + body->setLinearVelocity( ::convert(state.velocity) ); + body->setAngularVelocity( ::convertQ(state.angularVelocity) ); } // apply per-object gravities float mass = body->getMass(); @@ -629,11 +596,11 @@ void ext::reactphysics::syncFrom( ext::reactphysics::WorldState& world, float in */ state.internal.current.transform = ::convert( body->getTransform() ); - state.internal.current.linear.velocity = ::convert( body->getLinearVelocity() ); - state.internal.current.rotational.velocity = ::convertQ( body->getAngularVelocity() ); + state.internal.current.velocity = ::convert( body->getLinearVelocity() ); + state.internal.current.angularVelocity = ::convertQ( body->getAngularVelocity() ); - physics.linear.velocity = state.internal.current.linear.velocity; - physics.rotational.velocity = state.internal.current.rotational.velocity; + physics.velocity = state.internal.current.velocity; + physics.angularVelocity = state.internal.current.angularVelocity; if ( !ext::reactphysics::interpolate ) { transform.position = state.internal.current.transform.position; @@ -650,8 +617,8 @@ void ext::reactphysics::syncFrom( ext::reactphysics::WorldState& world, float in transform.orientation = uf::quaternion::slerp( state.internal.previous.transform.orientation, state.internal.current.transform.orientation, interp); if ( state.transform.reference ) transform.position -= state.transform.position; - // physics.linear.velocity = uf::vector::lerp( state.internal.previous.linear.velocity, state.internal.current.linear.velocity, interp ); - // physics.rotational.velocity = uf::quaternion::slerp( state.internal.previous.rotational.velocity, state.internal.current.rotational.velocity, interp ); + // physics.velocity = uf::vector::lerp( state.internal.previous.velocity, state.internal.current.velocity, interp ); + // physics.angularVelocity = uf::quaternion::slerp( state.internal.previous.angularVelocity, state.internal.current.angularVelocity, interp ); } } } @@ -688,7 +655,7 @@ void ext::reactphysics::setVelocity( pod::PhysicsState& state, const pod::Vector if ( !state.body ) return; if ( state.shared ) { auto& physics = state.object->getComponent(); - physics.linear.velocity = v; + physics.velocity = v; // return; } state.body->setLinearVelocity( ::convert(v) ); @@ -698,7 +665,7 @@ void ext::reactphysics::applyVelocity( pod::PhysicsState& state, const pod::Vect if ( state.shared ) { auto& physics = state.object->getComponent(); - physics.linear.velocity += v; + physics.velocity += v; // return; } state.body->setLinearVelocity( state.body->getLinearVelocity() + ::convert(v) ); diff --git a/engine/src/ext/xatlas/xatlas.cpp b/engine/src/ext/xatlas/xatlas.cpp index 7cf1f536..d7e25e8d 100644 --- a/engine/src/ext/xatlas/xatlas.cpp +++ b/engine/src/ext/xatlas/xatlas.cpp @@ -62,79 +62,53 @@ size_t ext::xatlas::unwrapExperimental( pod::Graph& graph ) { source = mesh; source.updateDescriptor(); - uf::Mesh::Input vertexInput = mesh.vertex; + for (auto& view : source.makeViews({"position", "uv", "st", "index"})) { + auto pos = view["position"]; + auto uv = view["uv"]; + auto st = view["st"]; + auto idx = view["index"]; - uf::Mesh::Attribute positionAttribute; - uf::Mesh::Attribute uvAttribute; - uf::Mesh::Attribute stAttribute; - - for ( auto& attribute : mesh.vertex.attributes ) { - if ( attribute.descriptor.name == "position" ) positionAttribute = attribute; - else if ( attribute.descriptor.name == "uv" ) uvAttribute = attribute; - else if ( attribute.descriptor.name == "st" ) stAttribute = attribute; - } - UF_ASSERT( positionAttribute.descriptor.name == "position" && uvAttribute.descriptor.name == "uv" && stAttribute.descriptor.name == "st" ); - - if ( mesh.index.count ) { - uf::Mesh::Input indexInput = mesh.index; - uf::Mesh::Attribute indexAttribute = mesh.index.attributes.front(); + // Must have positions + indices + if (!pos.valid() || !idx.valid()) continue; + if (view.vertex.count == 0 || view.index.count == 0) continue; + // Choose xatlas index format ::xatlas::IndexFormat indexType = ::xatlas::IndexFormat::UInt32; - switch ( mesh.index.size ) { + switch (idx.attribute.descriptor.size) { case sizeof(uint16_t): indexType = ::xatlas::IndexFormat::UInt16; break; case sizeof(uint32_t): indexType = ::xatlas::IndexFormat::UInt32; break; default: UF_EXCEPTION("unsupported index type"); break; } - if ( mesh.indirect.count ) { - auto& primitives = /*graph.storage*/storage.primitives[name]; - pod::DrawCommand* drawCommands = (pod::DrawCommand*) mesh.getBuffer(mesh.indirect).data(); - - for ( auto i = 0; i < mesh.indirect.count; ++i ) { - size_t atlasID = drawCommands[i].auxID; - - vertexInput = mesh.remapVertexInput( i ); - indexInput = mesh.remapIndexInput( i ); - - auto& atlas = atlases[atlasID]; - auto& entry = atlas.entries.emplace_back(); - entry.index = index; - entry.commandID = i; - - auto& decl = entry.decl; - - decl.vertexPositionData = static_cast(positionAttribute.pointer) + positionAttribute.stride * vertexInput.first; - decl.vertexPositionStride = positionAttribute.stride; - - decl.vertexUvData = static_cast(uvAttribute.pointer) + uvAttribute.stride * vertexInput.first; - decl.vertexUvStride = uvAttribute.stride; - - decl.vertexCount = vertexInput.count; - - decl.indexCount = indexInput.count; - decl.indexData = static_cast(indexAttribute.pointer) + indexAttribute.stride * indexInput.first; - decl.indexFormat = indexType; - } - } else { - size_t atlasID = 0; - auto& atlas = atlases[atlasID]; - auto& entry = atlas.entries.emplace_back(); - entry.index = index; - - auto& decl = entry.decl; - decl.vertexPositionData = static_cast(positionAttribute.pointer) + positionAttribute.stride * vertexInput.first; - decl.vertexPositionStride = positionAttribute.stride; - - decl.vertexUvData = static_cast(uvAttribute.pointer) + uvAttribute.stride * vertexInput.first; - decl.vertexUvStride = uvAttribute.stride; - - decl.vertexCount = vertexInput.count; - - decl.indexCount = indexInput.count; - decl.indexData = static_cast(indexAttribute.pointer) + indexAttribute.stride * indexInput.first; - decl.indexFormat = indexType; + // Get atlas target from indirect auxID or default 0 + size_t atlasID = 0; + if ( 0 <= view.indirectIndex ) { + auto* drawCommand = (pod::DrawCommand*) mesh.getBuffer(mesh.indirect).data(); + atlasID = drawCommand[view.indirectIndex].auxID; } - } else UF_EXCEPTION("to-do: not require indices for meshes"); + + auto& atlas = atlases[atlasID]; + auto& entry = atlas.entries.emplace_back(); + entry.index = index; + + auto& decl = entry.decl; + decl.vertexPositionData = pos.data(view.vertex.first); + decl.vertexPositionStride = pos.stride(); + + if ( uv.valid() ) { + decl.vertexUvData = uv.data(view.vertex.first); + decl.vertexUvStride = uv.stride(); + } else if ( st.valid() ) { + decl.vertexUvData = st.data(view.vertex.first); + decl.vertexUvStride = st.stride(); + } + + decl.vertexCount = view.vertex.count; + + decl.indexCount = view.index.count; + decl.indexData = idx.data(view.index.first); + decl.indexFormat = indexType; + } } ::xatlas::ChartOptions chartOptions{}; diff --git a/engine/src/utils/math/collision/boundingbox.cpp b/engine/src/utils/math/collision/boundingbox.cpp deleted file mode 100644 index 26af5eb0..00000000 --- a/engine/src/utils/math/collision/boundingbox.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#if UF_USE_UNUSED -#include - -uf::BoundingBox::BoundingBox( const pod::Vector3& origin, const pod::Vector3& corner ) { - this->m_origin = origin; - this->m_corner = corner; -} -const pod::Vector3& uf::BoundingBox::getOrigin() const { - return this->m_origin; -} -const pod::Vector3& uf::BoundingBox::getCorner() const { - return this->m_corner; -} -void uf::BoundingBox::setOrigin( const pod::Vector3& origin ) { - this->m_origin = origin; -} -void uf::BoundingBox::setCorner( const pod::Vector3& corner ) { - this->m_corner = corner; -} -pod::Vector3 uf::BoundingBox::min() const { - pod::Vector3f position = this->getPosition() + this->m_origin; - // const pod::Vector3f& position = this->m_origin; - return position - this->m_corner; -} -pod::Vector3 uf::BoundingBox::max() const { - pod::Vector3f position = this->getPosition() + this->m_origin; - // const pod::Vector3f& position = this->m_origin; - return position + this->m_corner; -} -pod::Vector3 uf::BoundingBox::closest( const pod::Vector3f& point ) const { - pod::Vector3f position = this->getPosition() + this->m_origin; - // const pod::Vector3f& position = this->m_origin; - - float distance = 9E9; - pod::Vector3f vector = point; - pod::Vector3f min = position - this->m_corner; - pod::Vector3f max = position + this->m_corner; - - float test = 0; - if ( (test = fabs(min.x - point.x)) < distance ) { - distance = test; - vector.x = min.x; - } - if ( (test = fabs(max.x - point.x)) < distance ) { - distance = test; - vector.x = max.x; - } - - if ( (test = fabs(min.y - point.y)) < distance ) { - distance = test; - vector.y = min.y; - } - if ( (test = fabs(max.y - point.y)) < distance ) { - distance = test; - vector.y = max.y; - } - - if ( (test = fabs(min.z - point.z)) < distance ) { - distance = test; - vector.z = min.z; - } - if ( (test = fabs(max.z - point.z)) < distance ) { - distance = test; - vector.z = max.z; - } - - return vector; -} -uf::stl::string uf::BoundingBox::type() const { return "BoundingBox"; } -pod::Vector3* uf::BoundingBox::expand() const { - pod::Vector3* raw = new pod::Vector3[8]; - raw[0] = pod::Vector3{ this->m_corner.x, this->m_corner.y, this->m_corner.z}; - raw[1] = pod::Vector3{ this->m_corner.x, this->m_corner.y, -this->m_corner.z}; - raw[2] = pod::Vector3{ -this->m_corner.x, this->m_corner.y, -this->m_corner.z}; - raw[3] = pod::Vector3{ -this->m_corner.x, this->m_corner.y, this->m_corner.z}; - raw[4] = pod::Vector3{ -this->m_corner.x, -this->m_corner.y, this->m_corner.z}; - raw[5] = pod::Vector3{ this->m_corner.x, -this->m_corner.y, this->m_corner.z}; - raw[6] = pod::Vector3{ this->m_corner.x, -this->m_corner.y, -this->m_corner.z}; - raw[7] = pod::Vector3{ -this->m_corner.x, -this->m_corner.y, this->m_corner.z}; - - pod::Vector3f position = this->getPosition() + this->m_origin; - // const pod::Vector3f& position = this->m_origin; - for ( uint i = 0; i < 8; i++ ) raw[0] += position; - - return raw; -} -pod::Vector3 uf::BoundingBox::support( const pod::Vector3& direction ) const { - pod::Vector3f position = this->getPosition() + this->m_origin; - // const pod::Vector3f& position = this->m_origin; - pod::Vector3 res = { - position.x + fabs(this->m_corner.x) * (direction.x > 0 ? 1 : -1), - position.y + fabs(this->m_corner.y) * (direction.y > 0 ? 1 : -1), - position.z + fabs(this->m_corner.z) * (direction.z > 0 ? 1 : -1), - }; - return res; -} - -pod::Collider::Manifold uf::BoundingBox::intersects( const uf::BoundingBox& b ) const { - const uf::BoundingBox& a = *this; - pod::Collider::Manifold manifold(a, b); - - BoundingBox difference(a.min() - b.max() + a.m_corner + b.m_corner, (a.m_corner + b.m_corner) * 2.0f); - - pod::Vector3f min = difference.m_origin - this->m_corner; - pod::Vector3f max = difference.m_origin + this->m_corner; - manifold.colliding = min.x <= 0 && max.x >= 0 && min.y <= 0 && max.y >= 0; - - if ( !manifold.colliding ) return manifold; - - pod::Vector3f penetration = difference.closest( {0, 0, 0} ); - manifold.depth = uf::vector::norm( penetration ); - manifold.normal = penetration / manifold.depth; - -/* - float a_left = position_a.x - a.m_corner.x; - float a_right = position_a.x + a.m_corner.x; - float a_bottom = position_a.y - a.m_corner.y; - float a_top = position_a.y + a.m_corner.y; - float a_back = position_a.z - a.m_corner.z; - float a_front = position_a.z + a.m_corner.z; - - - float b_left = position_b.x - b.m_corner.x; - float b_right = position_b.x + b.m_corner.x; - float b_bottom = position_b.y - b.m_corner.y; - float b_top = position_b.y + b.m_corner.y; - float b_back = position_b.z - b.m_corner.z; - float b_front = position_b.z + b.m_corner.z; - - manifold.depth = 9E9; - auto test = [&]( const pod::Vector3& axis, float minA, float maxA, float minB, float maxB )->bool{ - float axisLSqr = uf::vector::dot(axis, axis); - if ( axisLSqr < 1E-8f ) return true; - - float d0 = maxB - minA; - float d1 = maxA - minB; - if ( d0 < 0 || d1 < 0 ) return false; - float overlap = d0 < d1 ? d0 : -d1; - pod::Vector3 sep = axis * ( overlap / axisLSqr ); - float sepLSqr = uf::vector::dot( sep, sep ); - if ( sepLSqr < manifold.depth ) { - manifold.normal = sep; - manifold.depth = sepLSqr; - } - return true; - }; - - if ( !test( {1, 0, 0}, a_left, a_right, b_left, b_right ) ) return manifold; - if ( !test( {0, 1, 0}, a_bottom, a_top, b_bottom, b_top ) ) return manifold; - if ( !test( {0, 0, 1}, a_back, a_front, b_back, b_front ) ) return manifold; - - manifold.normal = uf::vector::normalize( manifold.normal ); - manifold.depth = sqrt( manifold.depth ); -// manifold.depth = manifold.depth; - manifold.colliding = true; -*/ - return manifold; -} -#endif \ No newline at end of file diff --git a/engine/src/utils/math/collision/gjk.cpp b/engine/src/utils/math/collision/gjk.cpp deleted file mode 100644 index f12c0e32..00000000 --- a/engine/src/utils/math/collision/gjk.cpp +++ /dev/null @@ -1,249 +0,0 @@ -#if UF_USE_UNUSED -#include - -/* -pod::Simplex::Simplex( const pod::Vector3& b, const pod::Vector3& c, const pod::Vector3& d ) { - this->set(b, c, d); -} -void pod::Simplex::add( const pod::Vector3& a ) { - this->d = this->c; - this->c = this->b; - this->b = a; -} -void pod::Simplex::set( const pod::Vector3& b, const pod::Vector3& c, const pod::Vector3& d ) { - this->b = b; - this->c = c; - this->d = d; - if ( b != pod::Vector3{} ) this->size++; - if ( c != pod::Vector3{} ) this->size++; - if ( d != pod::Vector3{} ) this->size++; -} -*/ -pod::Collider::~Collider(){} -uf::stl::string pod::Collider::type() const { return ""; } -pod::Collider::Manifold pod::Collider::intersects( const pod::Collider& y ) const { - const pod::Collider& x = *this; - - pod::Simplex::SupportPoint a; - pod::Simplex simplex; - pod::Vector3 direction = pod::Vector3{ 1.0f, 0.0f, 0.0f }; - - pod::Collider::Manifold manifold(x, y); - float dot = -0.1; - uint16_t iterations = 0; - uint16_t iterations_cap = 25; - while ( iterations++ < iterations_cap ) { - direction = uf::vector::normalize(direction); - - a = pod::Simplex::SupportPoint{}; - a.a = x.support(direction); - a.b = y.support(-direction); - a.v = a.a - a.b; - - if ( (dot = a.dot(direction)) < 0 ) { - return manifold; - } - if( simplex.size == 0 ) { - simplex.b = a; - - direction = uf::vector::multiply(direction, -1); - simplex.size = 1; - continue; - } - if ( simplex.size == 1 ) { - direction = uf::vector::cross(a.cross(simplex.b), a.v); - simplex.c = simplex.b; - simplex.b = a; - simplex.size = 2; - continue; - } - if ( simplex.size == 2 ) { - pod::Vector3 ao = a * -1; - pod::Vector3 ab = simplex.b - a; - pod::Vector3 ac = simplex.c - a; - pod::Vector3 abc = uf::vector::cross(ab, ac); - pod::Vector3 abP = uf::vector::cross(ab, abc); - if ( uf::vector::dot(abP, ao) > 0 ) { - simplex.c = simplex.b; - simplex.b = a; - - direction = uf::vector::cross(ab, ao); - continue; - } - pod::Vector3 acP = uf::vector::cross(abc, ac); - if ( uf::vector::dot(acP, ao) > 0 ) { - simplex.b = a; - - direction = uf::vector::cross(ac, ao); - continue; - } - if ( uf::vector::dot(abc, ao) > 0 ) { - simplex.d = simplex.c; - simplex.c = simplex.b; - simplex.b = a; - - direction = abc; - continue; - } - simplex.d = simplex.b; - simplex.b = a; - - direction = uf::vector::multiply(abc, -1); - simplex.size = 3; - continue; - } - if ( simplex.size == 3 ) { - pod::Vector3 ao = a * -1; - pod::Vector3 ab = simplex.b - a; - pod::Vector3 ac = simplex.c - a; - pod::Vector3 abc = uf::vector::cross(ab, ac); - pod::Vector3 ad, acd, adb; - if ( uf::vector::dot(abc, ao) > 0 ) { - goto check_face; - } - ad = simplex.d - a; - acd = uf::vector::cross(ac, ad); - if ( uf::vector::dot(acd, ao) > 0 ) { - simplex.b = simplex.c; - simplex.c = simplex.d; - - ab = ac; - ac = ad; - abc = acd; - - goto check_face; - } - goto EPA; - - check_face: - pod::Vector3 abP = uf::vector::cross(ab, abc); - if ( uf::vector::dot(abP, ao) > 0 ) { - simplex.c = simplex.b; - simplex.b = a; - - direction = uf::vector::cross(uf::vector::cross(ab, ao), ab); - simplex.size = 2; - continue; - } - pod::Vector3 acP = uf::vector::cross(abc, ac); - if ( uf::vector::dot(acP, ao) > 0 ) { - simplex.b = a; - direction = uf::vector::cross(uf::vector::cross(ac, ao), ac); - simplex.size = 2; - continue; - } - simplex.d = simplex.c; - simplex.c = simplex.b; - simplex.b = a; - direction = abc; - simplex.size = 3; - continue; - } - } - return manifold; - -EPA: - struct Triangle { - pod::Simplex::SupportPoint points[3]; - pod::Vector3f normal; - Triangle( const pod::Simplex::SupportPoint& a, const pod::Simplex::SupportPoint& b, const pod::Simplex::SupportPoint& c ) { - points[0] = a; - points[1] = b; - points[2] = c; - normal = uf::vector::normalize( uf::vector::cross( b.v - a.v, c.v - a.v ) ); - } - }; - struct Edge { - pod::Simplex::SupportPoint points[2]; - Edge( const pod::Simplex::SupportPoint& a, const pod::Simplex::SupportPoint& b ) { - points[0] = a; - points[1] = b; - } - }; - - iterations = 0; - manifold.colliding = true; - manifold.depth = 0.0f; - manifold.normal = { 0, 0, 0 }; - - uf::stl::vector triangles; - uf::stl::vector edges; - - triangles.emplace_back( a, simplex.b, simplex.c ); - triangles.emplace_back( a, simplex.b, simplex.d ); - triangles.emplace_back( a, simplex.c, simplex.d ); - triangles.emplace_back( simplex.b, simplex.c, simplex.d ); - - auto addEdge = [&]( const pod::Simplex::SupportPoint& a, const pod::Simplex::SupportPoint& b ) { - for ( auto it = edges.begin(); it != edges.end(); ++it ) { - if( it->points[0]== b && it->points[1]== a ) { - edges.erase(it); - return; - } - } - edges.emplace_back( a, b ); - }; - - while ( iterations++ < iterations_cap ) { - // find closest triangle to origin - struct { - uf::stl::vector::iterator it; - float distance = 9E9; - pod::Simplex::SupportPoint support; - } closest = { triangles.begin() }; - - for(auto it = triangles.begin(); it != triangles.end(); it++) { - float distance = fabs( uf::vector::dot(it->normal, it->points[0].v) ); - if( distance < closest.distance ) { - closest.distance = distance; - closest.it = it; - } - } - { - closest.support.a = x.support(closest.it->normal); - closest.support.b = y.support(-closest.it->normal); - closest.support.v = closest.support.a - closest.support.b; - } - - manifold.normal = -closest.it->normal; - manifold.depth = uf::vector::dot( closest.it->normal, closest.support.v ); - if( manifold.depth - closest.distance < 0.00001f ) { - manifold.depth = uf::vector::dot( closest.it->normal, closest.it->points[0].v ); - return manifold; - } - - for(auto it = triangles.begin(); it != triangles.end();) { - // can this face be 'seen' by closest.support? - if( uf::vector::dot( it->normal, (closest.support.v - it->points[0].v) ) > 0) { - addEdge( it->points[0], it->points[1] ); - addEdge( it->points[1], it->points[2] ); - addEdge( it->points[2], it->points[0] ); - it = triangles.erase(it); - continue; - } - it++; - } - - // create new triangles from the edges in the edge list - for(auto it = edges.begin(); it != edges.end(); it++) - triangles.emplace_back( closest.support, it->points[0], it->points[1] ); - - edges.clear(); - } - return manifold; -} - -pod::Vector3f pod::Collider::getPosition() const { - return this->m_transform.reference ? this->m_transform.reference->position : this->m_transform.position; -// return uf::transform::flatten( this->m_transform.reference ? *this->m_transform.reference : this->m_transform ).position; -} -pod::Transform<>& pod::Collider::getTransform() { - return this->m_transform; -} -const pod::Transform<>& pod::Collider::getTransform() const { - return this->m_transform; -} -void pod::Collider::setTransform( const pod::Transform<>& transform ) { - this->m_transform = transform; -} -#endif \ No newline at end of file diff --git a/engine/src/utils/math/collision/mesh.cpp b/engine/src/utils/math/collision/mesh.cpp deleted file mode 100644 index 743bc6c1..00000000 --- a/engine/src/utils/math/collision/mesh.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#if UF_USE_UNUSED -#include - -uf::MeshCollider::MeshCollider( const pod::Transform<>& transform, const uf::stl::vector& positions ) : m_positions(positions) { - this->setTransform(transform); -} -uf::stl::string UF_API uf::MeshCollider::type() const { return "Mesh"; } - -uf::stl::vector& uf::MeshCollider::getPositions() { - return this->m_positions; -} - -const uf::stl::vector& uf::MeshCollider::getPositions() const { - return this->m_positions; -} - -void uf::MeshCollider::setPositions( const uf::stl::vector& positions ) { - this->m_positions = positions; -} - -pod::Vector3* uf::MeshCollider::expand() const { - return (pod::Vector3*) &this->m_positions[0]; -} -pod::Vector3 uf::MeshCollider::support( const pod::Vector3& direction ) const { - size_t len = this->m_positions.size(); - pod::Vector3* points = this->expand(); - pod::Matrix4f model = uf::transform::model( this->m_transform ); - - struct { - size_t i = 0; - float dot = 0; - } best; - - for ( size_t i = 0; i < len; ++i ) { - float dot = uf::vector::dot( uf::matrix::multiply( model, points[i] ), direction); - // float dot = uf::vector::dot( points[i], direction); - if ( i == 0 || dot > best.dot ) { - best.i = i; - best.dot = dot; - } - } - return points[best.i]; -} -#endif \ No newline at end of file diff --git a/engine/src/utils/math/collision/modular.cpp b/engine/src/utils/math/collision/modular.cpp deleted file mode 100644 index 0539c934..00000000 --- a/engine/src/utils/math/collision/modular.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#if UF_USE_UNUSED -#include - -uf::ModularCollider::ModularCollider( uint len, pod::Vector3* container, bool should_free, const uf::ModularCollider::function_expand_t& expand, const uf::ModularCollider::function_support_t& support ) { - this->m_len = len; - this->m_container = container; - this->m_should_free = should_free; - this->m_function_expand = expand; - this->m_function_support = support; -} -uf::ModularCollider::~ModularCollider() { - if ( this->m_container != NULL && this->m_should_free ) delete[] this->m_container; -} -uf::stl::string uf::ModularCollider::type() const { return "Modular"; } -void uf::ModularCollider::setExpand( const uf::ModularCollider::function_expand_t& expand ) { - this->m_function_expand = expand; -} -void uf::ModularCollider::setSupport( const uf::ModularCollider::function_support_t& support ) { - this->m_function_support = support; -} - -pod::Vector3* uf::ModularCollider::getContainer() { - return this->m_container; -} -uint uf::ModularCollider::getSize() const { - return this->m_len; -} -void uf::ModularCollider::setContainer( pod::Vector3* container, uint len ) { - this->m_container = container; - this->m_len = len; -} - -pod::Vector3* uf::ModularCollider::expand() const { - return this->m_function_expand ? this->m_function_expand() : this->m_container; -} -pod::Vector3 uf::ModularCollider::support( const pod::Vector3& direction ) const { - if ( this->m_function_support ) return this->m_function_support(direction); - - pod::Vector3* points = this->expand(); - uint len = this->m_len; - - struct { - size_t i = 0; - float dot = 0; - } best; - - for ( size_t i = 0; i < len; ++i ) { - // float dot = uf::vector::dot( uf::matrix::multiply( model, points[i] ), direction); - float dot = uf::vector::dot( points[i], direction); - if ( i == 0 || dot > best.dot ) { - best.i = i; - best.dot = dot; - } - } - return points[best.i]; -} -#endif \ No newline at end of file diff --git a/engine/src/utils/math/collision/sphere.cpp b/engine/src/utils/math/collision/sphere.cpp deleted file mode 100644 index 222fd38c..00000000 --- a/engine/src/utils/math/collision/sphere.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#if UF_USE_UNUSED -#include - -uf::SphereCollider::SphereCollider( float r, const pod::Vector3& origin ) { - this->m_radius = r; - this->m_origin = origin; -} -uf::stl::string uf::SphereCollider::type() const { return "Sphere"; } -float uf::SphereCollider::getRadius() const { - return this->m_radius; -} -const pod::Vector3& uf::SphereCollider::getOrigin() const { - return this->m_origin; -} - -void uf::SphereCollider::setRadius( float r ) { - this->m_radius = r; -} -void uf::SphereCollider::setOrigin( const pod::Vector3& origin ) { - this->m_origin = origin; -} - -pod::Vector3* uf::SphereCollider::expand() const { - return NULL; -} -pod::Vector3 uf::SphereCollider::support( const pod::Vector3& direction ) const { - pod::Vector3f position = this->getPosition() + this->m_origin; -// const pod::Vector3f& position = this->m_origin; - return position + direction * (this->m_radius/uf::vector::magnitude(direction)); -} -pod::Collider::Manifold uf::SphereCollider::intersects( const uf::SphereCollider& b ) const { - const uf::SphereCollider& a = *this; - pod::Collider::Manifold manifold(a, b); - - pod::Vector3f position_a = a.getPosition() + a.m_origin; - pod::Vector3f position_b = b.getPosition() + b.m_origin; - // const pod::Vector3f& position_a = a.m_origin; - // const pod::Vector3f& position_b = b.m_origin; - - float distanceSquared = uf::vector::distanceSquared(position_a, position_b); - float sum = a.m_radius + b.m_radius; - float sumSquared = sum * sum; - - if ( distanceSquared >= sumSquared ) return manifold; - - manifold.depth = fabs(b.m_radius - a.m_radius); - manifold.normal = position_b - position_a; - manifold.colliding = true; - return manifold; -} -#endif \ No newline at end of file diff --git a/engine/src/utils/math/physics.cpp b/engine/src/utils/math/physics.cpp index 73060317..c074e98b 100644 --- a/engine/src/utils/math/physics.cpp +++ b/engine/src/utils/math/physics.cpp @@ -31,7 +31,7 @@ void uf::physics::tick( uf::Object& scene ) { if ( uf::physics::time::delta > uf::physics::time::clamp ) { uf::physics::time::delta = uf::physics::time::clamp; } - uf::physics::impl::tick( scene ); + uf::physics::impl::tick( scene, uf::physics::time::delta ); } void uf::physics::terminate( ) { return uf::physics::terminate( uf::scene::getCurrentScene() ); diff --git a/engine/src/utils/math/physics/aabb.inl b/engine/src/utils/math/physics/aabb.inl new file mode 100644 index 00000000..3f81d836 --- /dev/null +++ b/engine/src/utils/math/physics/aabb.inl @@ -0,0 +1,264 @@ +namespace { + bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps ) { + return (a.min.x < b.max.x - eps && a.max.x > b.min.x + eps) && + (a.min.y < b.max.y - eps && a.max.y > b.min.y + eps) && + (a.min.z < b.max.z - eps && a.max.z > b.min.z + eps); + } + + pod::AABB computeSegmentAABB( const pod::Vector3f& p1, const pod::Vector3f p2, float r ) { + return { + uf::vector::min( p1, p2 ) - r, + uf::vector::max( p1, p2 ) + r, + }; + } + + pod::Vector3f closestPointOnAABB(const pod::Vector3f& p, const pod::AABB& box) { + return { + std::max(box.min.x, std::min(p.x, box.max.x)), + std::max(box.min.y, std::min(p.y, box.max.y)), + std::max(box.min.z, std::min(p.z, box.max.z)) + }; + } + + std::pair getCapsuleSegment( const pod::RigidBody& body ) { + const auto& capsule = body.collider.u.capsule; + const pod::Vector3f up = uf::quaternion::rotate( body.transform->orientation, pod::Vector3f{0,1,0} ); + + // segment defines the cylinder axis only (ignore spherical ends) + auto p1 = body.transform->position + up * capsule.halfHeight; + auto p2 = body.transform->position - up * capsule.halfHeight; + return { p1, p2 }; + } + + pod::AABB computeAABB( const pod::RigidBody& body ) { + switch ( body.collider.type ) { + case pod::ShapeType::AABB: { + return { + body.transform->position + body.collider.u.aabb.min, + body.transform->position + body.collider.u.aabb.max, + }; + } break; + case pod::ShapeType::SPHERE: { + return { + body.transform->position - body.collider.u.sphere.radius, + body.transform->position + body.collider.u.sphere.radius, + }; + } break; + case pod::ShapeType::CAPSULE: { + auto [ p1, p2 ] = ::getCapsuleSegment( body ); + return ::computeSegmentAABB( p1, p2, body.collider.u.capsule.radius ); + } break; + case pod::ShapeType::MESH: { + if ( body.collider.u.mesh.bvh && !body.collider.u.mesh.bvh->nodes.empty() ) + return { + body.transform->position + body.collider.u.mesh.bvh->nodes[0].bounds.min, + body.transform->position + body.collider.u.mesh.bvh->nodes[0].bounds.max, + }; + } break; + default: { + } break; + } + + return {}; + } + + pod::AABB computeTriangleAABB( const pod::Triangle& tri ) { + return { + { + std::min({tri.points[0].x, tri.points[1].x, tri.points[2].x}), + std::min({tri.points[0].y, tri.points[1].y, tri.points[2].y}), + std::min({tri.points[0].z, tri.points[1].z, tri.points[2].z}), + }, + { + std::max({tri.points[0].x, tri.points[1].x, tri.points[2].x}), + std::max({tri.points[0].y, tri.points[1].y, tri.points[2].y}), + std::max({tri.points[0].z, tri.points[1].z, tri.points[2].z}), + }, + }; + } + + float triAabbDistanceSq( const pod::Triangle& tri, const pod::AABB& box ) { + float minDistSq = FLT_MAX; + for ( auto i = 0; i < 3; ++i ) { + auto cp = ::closestPointOnAABB( tri.points[i], box ); + auto d = tri.points[i] - cp; + minDistSq = std::min( minDistSq, uf::vector::dot( d, d ) ); + } + return minDistSq; + } + + bool triAabbOverlap( const pod::Triangle& tri, const pod::AABB& box, float eps ) { + // compute box center and half extents + auto c = ( box.min + box.max ) * 0.5f; + auto e = ( box.max - box.min ) * 0.5f; + + // move triangle into box's local space + auto v0 = tri.points[0] - c; + auto v1 = tri.points[1] - c; + auto v2 = tri.points[2] - c; + + // triangle edges + auto f0 = v1 - v0; + auto f1 = v2 - v1; + auto f2 = v0 - v2; + + // sAT: test the 9 edge cross axes + auto axisTest = [&]( const pod::Vector3f& axis ) { + if ( uf::vector::magnitude(axis) < eps ) return true; + + auto a = uf::vector::normalize( axis ); + + float p0 = uf::vector::dot( v0, a ); + float p1 = uf::vector::dot( v1, a ); + float p2 = uf::vector::dot( v2, a ); + + float r = e.x * fabs(a.x) + e.y * fabs(a.y) + e.z * fabs(a.z); + + float minP = std::min({p0, p1, p2}); + float maxP = std::max({p0, p1, p2}); + + return !(minP > r || maxP < -r); + }; + + if ( !axisTest( {0, -f0.z, f0.y} ) ) return false; + if ( !axisTest( {0, -f1.z, f1.y} ) ) return false; + if ( !axisTest( {0, -f2.z, f2.y} ) ) return false; + if ( !axisTest( {f0.z, 0, -f0.x} ) ) return false; + if ( !axisTest( {f1.z, 0, -f1.x} ) ) return false; + if ( !axisTest( {f2.z, 0, -f2.x} ) ) return false; + if ( !axisTest( {-f0.y, f0.x, 0} ) ) return false; + if ( !axisTest( {-f1.y, f1.x, 0} ) ) return false; + if ( !axisTest( {-f2.y, f2.x, 0} ) ) return false; + + // test AABB face axes + for ( auto i = 0; i < 3; ++i ) { + float minVal = std::min({v0[i], v1[i], v2[i]}); + float maxVal = std::max({v0[i], v1[i], v2[i]}); + if ( minVal > e[i] || maxVal < -e[i] ) return false; + } + + // test triangle normal axis + auto n = uf::vector::cross( f0, f1 ); + float d0 = uf::vector::dot( v0, n ); + float r = e.x * fabs(n.x) + e.y * fabs(n.y) + e.z * fabs(n.z); + if ( fabs(d0) > r ) return false; + + return true; + } + + pod::AABB mergeAabb( const pod::AABB& a, const pod::AABB& b ) { + return { + uf::vector::min( a.min, b.min ), + uf::vector::max( a.max, b.max ), + }; + } + + pod::AABB transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform ) { + auto inv = uf::transform::inverse( transform ); + + // transform all 8 corners + pod::Vector3f corners[8] = { + {box.min.x, box.min.y, box.min.z}, + {box.max.x, box.min.y, box.min.z}, + {box.min.x, box.max.y, box.min.z}, + {box.max.x, box.max.y, box.min.z}, + {box.min.x, box.min.y, box.max.z}, + {box.max.x, box.min.y, box.max.z}, + {box.min.x, box.max.y, box.max.z}, + {box.max.x, box.max.y, box.max.z}, + }; + + pod::AABB out = { + { FLT_MAX, FLT_MAX, FLT_MAX }, + { -FLT_MAX, -FLT_MAX, -FLT_MAX }, + }; + + for ( auto i = 0; i < 8; ++i ) { + auto local = uf::transform::apply( inv, corners[i] ); + out.min = uf::vector::min( out.min, local ); + out.max = uf::vector::max( out.max, local ); + } + return out; + } + + pod::Vector3f aabbCenter( const pod::AABB& aabb ) { + return ( aabb.min + aabb.max ) * 0.5f; + } + + bool aabbAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES(AABB, AABB); + + const pod::AABB& A = a.bounds; + const pod::AABB& B = b.bounds; + + if (!aabbOverlap(A, B)) return false; + + // Calculate overlap extents + float overlaps[3] = { + std::min(A.max.x, B.max.x) - std::max(A.min.x, B.min.x), + std::min(A.max.y, B.max.y) - std::max(A.min.y, B.min.y), + std::min(A.max.z, B.max.z) - std::max(A.min.z, B.min.z) + }; + + // Determine collision axis = smallest overlap + int axis = -1; + float minOverlap = FLT_MAX; + for (int i = 0; i < 3; ++i) { + if (overlaps[i] < minOverlap) { + minOverlap = overlaps[i]; + axis = i; + } + } + + pod::Vector3f delta = b.transform->position - a.transform->position; + pod::Vector3f normal{0,0,0}; + normal[axis] = (delta[axis] < 0 ? -1.0f : 1.0f); + + // Build manifold contacts: overlap region corners on the separating axis + float xMin = std::max(A.min.x, B.min.x); + float xMax = std::min(A.max.x, B.max.x); + float yMin = std::max(A.min.y, B.min.y); + float yMax = std::min(A.max.y, B.max.y); + float zMin = std::max(A.min.z, B.min.z); + float zMax = std::min(A.max.z, B.max.z); + + // On chosen axis, clamp to overlapped rectangle -> 4 potential points + if (axis == 0) { // X-axis separation, so face-on overlap in YZ plane + manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMin }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMax }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMin }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMax }, normal, minOverlap }); + } + else if (axis == 1) { // Y-axis separation, overlap in XZ plane + manifold.points.emplace_back(pod::Contact{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap }); + } + else if (axis == 2) { // Z-axis separation, overlap in XY plane + manifold.points.emplace_back(pod::Contact{ { xMin, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { xMin, yMax, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { xMax, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap }); + manifold.points.emplace_back(pod::Contact{ { xMax, yMax, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap }); + } + + return true; + } + + bool aabbSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( AABB, SPHERE ); + return ::sphereAabb( b, a, manifold, eps ); + } + bool aabbPlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( AABB, PLANE ); + return ::planeAabb( b, a, manifold, eps ); + } + bool aabbCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( AABB, CAPSULE ); + return ::capsuleAabb( b, a, manifold, eps ); + } + bool aabbMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( AABB, MESH ); + return ::meshAabb( b, a, manifold, eps ); + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/bvh.inl b/engine/src/utils/math/physics/bvh.inl new file mode 100644 index 00000000..bbdd7525 --- /dev/null +++ b/engine/src/utils/math/physics/bvh.inl @@ -0,0 +1,249 @@ +// BVH +namespace { + // collects a list of nodes that are overlapping with each other + void traverseNodePair( const pod::BVH& bvh, int leftID, int rightID, pod::BVH::pair_t& pairs ) { + const auto& left = bvh.nodes[leftID]; + const auto& right = bvh.nodes[rightID]; + + if ( !::aabbOverlap( left.bounds, right.bounds ) ) return; + + if ( left.count > 0 && right.count > 0 ) { + for ( auto i = 0; i < left.count; ++i ) + for ( auto j = 0; j < right.count; ++j ) { + int a = bvh.indices[left.start + i]; + int b = bvh.indices[right.start + j]; + pairs.emplace_back(std::pair{a, b}); + } + return; + } + + if ( left.count == 0 ) { + ::traverseNodePair( bvh, left.left, rightID, pairs ); + ::traverseNodePair( bvh, left.right, rightID, pairs ); + } else if ( right.count == 0 ) { + ::traverseNodePair( bvh, leftID, right.left, pairs ); + ::traverseNodePair( bvh, leftID, right.right, pairs ); + } + } + // collects a list of nodes from each BVH that are overlapping with each other (for mesh v mesh) + void traverseNodePair( const pod::BVH& a, int nodeA, const pod::BVH& b, int nodeB, pod::BVH::pair_t& pairs ) { + const auto& nA = a.nodes[nodeA]; + const auto& nB = b.nodes[nodeB]; + + if ( !::aabbOverlap( nA.bounds, nB.bounds ) ) return; + + if ( nA.count > 0 && nB.count > 0 ) { + for ( auto i = 0; i < nA.count; i++ ) + for ( auto j = 0; j < nB.count; j++ ) + pairs.emplace_back(a.indices[nA.start+i], b.indices[nB.start+j]); + return; + } + + if ( nA.count == 0 ) { + ::traverseNodePair( a, nA.left, b , nodeB, pairs ); + ::traverseNodePair( a, nA.right, b , nodeB, pairs ); + } else { + ::traverseNodePair( a, nodeA, b, nB.left, pairs ); + ::traverseNodePair( a, nodeA, b, nB.right, pairs ); + } + } + + void traverseBVH( const pod::BVH& bvh, int nodeIdx, pod::BVH::pair_t& pairs ) { + const auto& node = bvh.nodes[nodeIdx]; + + if ( node.count > 0 ) { + for ( auto i = 0; i < node.count; ++i ) { + for ( auto j = i + 1; j < node.count; ++j ) { + int a = bvh.indices[node.start + i]; + int b = bvh.indices[node.start + j]; + pairs.emplace_back(std::pair{a, b}); + } + } + return; + } + + // recurse children + ::traverseNodePair( bvh, node.left, node.right, pairs ); + } + + void queryOverlaps( const pod::BVH& bvh, pod::BVH::pair_t& outPairs ) { + if ( bvh.nodes.empty() ) return; + ::traverseBVH( bvh, 0, outPairs ); + } + + int buildBVHNode( pod::BVH& bvh, const uf::stl::vector& bounds, int start, int end, int capacity = 2 ) { + pod::BVH::Node node{}; + node.left = -1; + node.right = -1; + node.start = start; + node.count = 0; + node.bounds = bounds[bvh.indices[start]]; + + // compute bounds of this node + for ( auto i = start + 1; i < end; ++i) node.bounds = ::mergeAabb( node.bounds, bounds[bvh.indices[i]] ); + + int count = end - start; + if ( count <= capacity ) { + // leaf + node.start = start; + node.count = count; + int index = (int) bvh.nodes.size(); + bvh.nodes.emplace_back(node); + return index; + } + + // choose split axis by largest extent + auto extent = node.bounds.max - node.bounds.min; + int axis = (extent.x > extent.y && extent.x > extent.z) ? 0 : (extent.y > extent.z ? 1 : 2); + + // sort indices by centroid along axis + std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](size_t a, size_t b) { + float ca = ::aabbCenter( bounds[a] )[axis]; + float cb = ::aabbCenter( bounds[b] )[axis]; + return ca < cb; + }); + + int mid = ( start + end ) / 2; + int index = (int) bvh.nodes.size(); + bvh.nodes.emplace_back( node ); // insert now, gets filled later + + node.left = ::buildBVHNode( bvh, bounds, start, mid, capacity ); + node.right = ::buildBVHNode( bvh, bounds, mid, end, capacity ); + bvh.nodes[index] = node; + return index; + } + + void buildBroadphaseBVH( pod::BVH& bvh, const uf::stl::vector& bodies, int capacity = 2 ) { + bvh.indices.clear(); + bvh.nodes.clear(); + bvh.indices.reserve(bodies.size()); + + // stores bounds + uf::stl::vector bounds; + bounds.reserve(bodies.size()); + + // populate initial indices and bounds + for ( auto i = 0; i < bodies.size(); ++i ) { + bounds.emplace_back(::computeAABB( *bodies[i] )); + bvh.indices.emplace_back(i); // i => bodies[i] + } + + // recursively build BVH from indices + ::buildBVHNode( bvh, bounds, 0, bodies.size(), capacity ); + } + + void buildMeshBVH( pod::BVH& bvh, const uf::Mesh& mesh, int capacity = 4 ) { + int triangles = mesh.index.count / 3; + + bvh.indices.clear(); + bvh.nodes.clear(); + bvh.indices.reserve( triangles ); + + // stores bounds + uf::stl::vector bounds; + bounds.reserve( triangles ); + + auto views = mesh.makeViews({"position"}); + UF_ASSERT( !views.empty() ); + + // populate initial indices and bounds + for ( auto& view : views ) { + auto& indices = view["index"]; + auto& positions = view["position"]; + + auto tris = view.index.count / 3; + for ( auto triIndexID = 0; triIndexID < tris; ++triIndexID ) { + auto aabb = ::computeTriangleAABB( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID ); + auto triID = triIndexID + (view.index.first / 3); + + bounds.emplace_back(aabb); + bvh.indices.emplace_back(triID); // triID => mesh.index.buffer[triID * 3]; + } + } + UF_MSG_DEBUG("Built BVH with {} triangles from mesh with {} triangles ({} draw commands)", bounds.size(), triangles, views.size()); + + // recursively build BVH from indices + ::buildBVHNode( bvh, bounds, 0, triangles, capacity ); + } +} + +namespace { + // query a BVH with an AABB via a stack + void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices ) { + if ( bvh.nodes.empty() ) return; + + uf::stl::stack stack; + stack.push(0); + + while ( !stack.empty() ) { + int idx = stack.top(); stack.pop(); + auto& node = bvh.nodes[idx]; + if ( !::aabbOverlap( bounds, node.bounds ) ) continue; + + if ( node.count > 0 ) { + for ( auto i = 0; i < node.count; ++i) indices.emplace_back(bvh.indices[node.start + i]); + } else { + stack.push(node.left); + stack.push(node.right); + } + } + } + void queryBVH( const pod::BVH& bvh, const pod::RigidBody& body, uf::stl::vector& indices ) { + return ::queryBVH( bvh, body.bounds, indices ); + } + + // query a BVH with an AABB via recursion + void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices, int nodeID ) { + const auto& node = bvh.nodes[nodeID]; + if ( !::aabbOverlap( node.bounds, bounds ) ) return; + + if ( node.count > 0 ) { + for ( auto i = 0; i < node.count; ++i ) indices.emplace_back(bvh.indices[node.start + i]); + return; + } + + // recurse + ::queryBVH( bvh, bounds, indices, node.left ); + ::queryBVH( bvh, bounds, indices, node.right ); + } + + // query a BVH with a ray via a stack + void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, float maxDist ) { + if ( bvh.nodes.empty() ) return; + + uf::stl::stack stack; + stack.push(0); + + while ( !stack.empty() ) { + int idx = stack.top(); stack.pop(); + const auto& node = bvh.nodes[idx]; + + float tMin, tMax; + if ( !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) continue; + if ( tMin > maxDist ) continue; + + if ( node.count > 0 ) { + for ( auto i = 0; i < node.count; ++i) indices.emplace_back(bvh.indices[node.start + i]); + } else { + stack.push(node.left); + stack.push(node.right); + } + } + } + // query a BVH with a ray via recursion + void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, int nodeID, float maxDist ) { + const auto& node = bvh.nodes[nodeID]; + float tMin, tMax; + if ( !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) return; + if ( tMin > maxDist ) return; + + if ( node.count > 0 ) { + for ( auto i = 0; i < node.count; ++i ) indices.emplace_back(bvh.indices[node.start + i]); + return; + } + + // recurse + ::queryBVH( bvh, ray, indices, node.left, maxDist ); + ::queryBVH( bvh, ray, indices, node.right, maxDist ); + } +} diff --git a/engine/src/utils/math/physics/capsule.inl b/engine/src/utils/math/physics/capsule.inl new file mode 100644 index 00000000..3b0ae4b0 --- /dev/null +++ b/engine/src/utils/math/physics/capsule.inl @@ -0,0 +1,109 @@ +namespace { + bool capsuleCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( CAPSULE, CAPSULE ); + + auto [ A1, A2 ] = ::getCapsuleSegment( a ); + auto [ B1, B2 ] = ::getCapsuleSegment( b ); + + auto [ pA, pB ] = ::closestSegmentSegment( A1, A2, B1, B2 ); + float r = a.collider.u.capsule.radius + b.collider.u.capsule.radius; + + auto delta = pB - pA; + float dist2 = uf::vector::dot(delta, delta); + if ( dist2 > r * r ) return false; + float dist = std::sqrt( dist2 ); + + auto contact = ( pA + pB ) * 0.5f; + auto normal = ::normalizeDelta( delta, dist, eps ); + float penetration = r - dist; + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool capsuleAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( CAPSULE, AABB ); + const auto& capsule = a; + const auto& box = b; + + auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); + float r = capsule.collider.u.capsule.radius; + + auto closestPoint = ::closestPointSegmentAabb( p1, p2, box.bounds ); + auto closestSegment = ::closestPointOnSegment( closestPoint, p1, p2 ); + + auto delta = closestPoint - closestSegment; + float dist2 = uf::vector::magnitude( delta ); + if ( dist2 > r * r ) return false; + float dist = std::sqrt( dist2 ); + + float penetration = r - dist; + auto normal = ( dist > eps ) ? ( delta / dist ) : pod::Vector3f{0,1,0}; + auto contact = closestSegment + normal * ( r - penetration * 0.5f ); + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool capsulePlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( CAPSULE, PLANE ); + const auto& capsule = a; + const auto& plane = b; + + const auto& normal = plane.collider.u.plane.normal; + float o = plane.collider.u.plane.offset; + + auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); + float r = capsule.collider.u.capsule.radius; + + // the "foot" is just whichever end of the capsule is closest to the normal + auto foot = ( uf::vector::dot( normal, p1 ) < uf::vector::dot( normal, p2 ) ) ? p1 : p2; + float dist = uf::vector::dot( normal, foot ) - o; + if ( dist > r ) return false; + + auto contact = foot - (normal * r); + float penetration = r - dist; + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + + // build up to 4 contact points sampled around foot circle + #if 0 + auto tangent1 = uf::vector::normalize( uf::vector::cross( normal, pod::Vector3f{1,0,0} ) ); + if ( uf::vector::magnitude( tangent1 ) < EPS(1e-6f) ) tangent1 = uf::vector::normalize( uf::vector::cross( normal, pod::Vector3f{0,1,0} ) ); + auto tangent2 = uf::vector::cross( normal, tangent1 ); + + // four directions around circle + manifold.points.emplace_back(pod::Contact{ foot + tangent1 * r - normal * d, normal, penetration }); + manifold.points.emplace_back(pod::Contact{ foot - tangent1 * r - normal * d, normal, penetration }); + manifold.points.emplace_back(pod::Contact{ foot + tangent2 * r - normal * d, normal, penetration }); + manifold.points.emplace_back(pod::Contact{ foot - tangent2 * r - normal * d, normal, penetration }); + #endif + return true; + } + bool capsuleSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( CAPSULE, SPHERE ); + const auto& capsule = a; + const auto& sphere = b; + + auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); + + const pod::Vector3f sphereCenter = sphere.transform->position; + float r = capsule.collider.u.capsule.radius + sphere.collider.u.sphere.radius; + + // closest point on capsule segment to sphere center + auto closest = ::closestPointOnSegment( sphereCenter, p1, p2 ); + + auto delta = sphereCenter - closest; + float dist2 = uf::vector::magnitude( delta ); + if ( dist2 > r * r ) return false; + float dist = std::sqrt( dist2 ); + + float penetration = r - dist; + auto normal = ::normalizeDelta( delta, dist, eps ); + auto contact = closest + normal * (capsule.collider.u.capsule.radius - penetration * 0.5f ); + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool capsuleMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( CAPSULE, MESH ); + return meshCapsule( b, a, manifold, eps ); + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/epa.inl b/engine/src/utils/math/physics/epa.inl new file mode 100644 index 00000000..af52d923 --- /dev/null +++ b/engine/src/utils/math/physics/epa.inl @@ -0,0 +1,135 @@ +namespace { + void addOrRemoveBorder( uf::stl::vector>& edges, std::pair e) { + // look for reversed edge + auto it = std::find_if( edges.begin(), edges.end(), [&]( auto& ex ) { + return ( ex.first.p == e.second.p && ex.second.p == e.first.p ); + }); + + if ( it != edges.end() ) edges.erase(it); // internal edge cancels out + else edges.emplace_back(e); // keep border edge + } + + bool isValidSimplex( const pod::Simplex& s ) { + if ( s.pts.size() < 4 ) return false; + + const auto& A = s.pts[0].p; + const auto& B = s.pts[1].p; + const auto& C = s.pts[2].p; + const auto& D = s.pts[3].p; + + return fabs( uf::vector::dot( (B - A), uf::vector::cross(C - A, D - A) ) ) > EPS(1e-6f); + } + + pod::Face makeFace( const pod::SupportPoint& a, const pod::SupportPoint& b, const pod::SupportPoint& c ) { + pod::Face face{ a, b, c }; + + face.normal = uf::vector::normalize( uf::vector::cross( b.p - a.p, c.p - a.p ) ); + face.distance = uf::vector::dot( face.normal, a.p ); + + if ( face.distance < 0 ) { + std::swap( face.b, face.c ); + face.normal = -face.normal; + face.distance = -face.distance; + } + return face; + }; + + uf::stl::vector initialPolytope( const pod::Simplex& s ) { + UF_ASSERT( s.pts.size() == 4 ); + + const auto& A = s.pts[0]; + const auto& B = s.pts[1]; + const auto& C = s.pts[2]; + const auto& D = s.pts[3]; + + return { + ::makeFace( A, B, C ), + ::makeFace( A, C, D ), + ::makeFace( A, D, B ), + ::makeFace( B, D, C ) + }; + } + + void expandPolytope( uf::stl::vector& faces, const pod::SupportPoint& p ) { + uf::stl::vector remove; + uf::stl::vector> borders; + + // find faces visible to point + for ( auto i = 0; i < faces.size(); ++i ) { + if ( uf::vector::dot( faces[i].normal, p.p - faces[i].a.p ) > 0) { + remove.emplace_back(i); + ::addOrRemoveBorder( borders, { faces[i].a, faces[i].b } ); + ::addOrRemoveBorder( borders, { faces[i].b, faces[i].c } ); + ::addOrRemoveBorder( borders, { faces[i].c, faces[i].a } ); + } + } + + // remove visible faces + for ( auto i = (int) remove.size() - 1; i >= 0; --i ) { + int idx = remove[i]; + faces[idx] = faces.back(); + faces.pop_back(); + } + + // stitch new faces from border edges to new point + for ( auto& e : borders ) { + faces.emplace_back(::makeFace( e.first, e.second, p )); + } + } + + pod::Contact epa( const pod::RigidBody& A, const pod::RigidBody& B, const pod::Simplex& simplex, int maxIterations = 64, float eps = EPS(1.0e-4f) ) { + UF_ASSERT( ::isValidSimplex(simplex) ); + + auto faces = ::initialPolytope(simplex); + if ( faces.empty() ) return {}; + + for ( auto it = 0; it < maxIterations; ++it ) { + // find closest face + int idx = -1; + float minDist = FLT_MAX; + for ( int i = 0; i < faces.size(); ++i ) { + if ( faces[i].distance < minDist ) { + minDist = faces[i].distance; + idx = i; + } + } + + UF_ASSERT( idx != -1 ); + auto& f = faces[idx]; + + // new support + auto sp = ::supportMinkowskiDetailed( A, B, f.normal ); + float d = uf::vector::dot( sp.p, f.normal ); + + // convergence check + if ( fabs(d - f.distance) < eps || it == maxIterations - 1 ) { + // project origin onto triangle (in Minkowski space) + auto bary = ::computeBarycentric({}, f.a.p, f.b.p, f.c.p, true); + + // use barycentrics to blend real-body points + auto pA = ( f.a.pA * bary.x ) + ( f.b.pA * bary.y ) + ( f.c.pA * bary.z ); + auto pB = ( f.a.pB * bary.x ) + ( f.b.pB * bary.y ) + ( f.c.pB * bary.z ); + + auto contact = ( pA + pB ) * 0.5f; + auto normal = f.normal; + float penetration = uf::vector::dot( (pB - pA), normal ); + + // flip normal + if ( penetration < 0.0f ) { + f.normal = -f.normal; + penetration = -penetration; + } + + // discard invalid contact + if ( !std::isfinite(contact.x) || !std::isfinite(contact.y) || !std::isfinite(contact.z) || penetration <= 0.0f ) { + return {}; + } + + return { contact, normal, penetration }; + } + + ::expandPolytope(faces, sp); + } + return {}; + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/gjk.inl b/engine/src/utils/math/physics/gjk.inl new file mode 100644 index 00000000..69064cf9 --- /dev/null +++ b/engine/src/utils/math/physics/gjk.inl @@ -0,0 +1,198 @@ +namespace { + pod::Vector3f support( const pod::RigidBody& body, const pod::Vector3f& dir ) { + switch ( body.collider.type ) { + case pod::ShapeType::SPHERE: { + return body.transform->position + uf::vector::normalize( dir ) * body.collider.u.sphere.radius; + } break; + case pod::ShapeType::AABB: { + return { + ( dir.x >= 0.0f ) ? body.bounds.max.x : body.bounds.min.x, + ( dir.y >= 0.0f ) ? body.bounds.max.y : body.bounds.min.y, + ( dir.z >= 0.0f ) ? body.bounds.max.z : body.bounds.min.z + }; + } break; + case pod::ShapeType::CAPSULE: { + auto up = uf::quaternion::rotate( body.transform->orientation, pod::Vector3f{0,1,0} ); + auto p1 = body.transform->position + up * body.collider.u.capsule.halfHeight; + auto p2 = body.transform->position - up * body.collider.u.capsule.halfHeight; + // get closest end + auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2; + + return end + uf::vector::normalize( dir ) * body.collider.u.capsule.radius; + } + // to-do: mesh + default: { + + } break; + } + return {}; + } + + pod::Vector3f supportMinkowski( const pod::RigidBody& A, const pod::RigidBody& B, const pod::Vector3f& dir ) { + return ::support( A, dir ) - ::support( B, -dir ); + } + + pod::SupportPoint supportMinkowskiDetailed( const pod::RigidBody& A, const pod::RigidBody& B, const pod::Vector3f& dir ) { + auto pA = ::support( A, dir ); + auto pB = ::support( B, -dir ); + return { pA - pB, pA, pB }; + } + + bool updateSimplex( pod::Simplex& s, pod::Vector3f& dir ) { + switch (s.pts.size()) { + case 2: { + // points + auto& A = s.pts.back(); // newest + auto& B = s.pts.front(); + // edges + auto AB = B.p - A.p; + auto AO = -A.p; + + if (uf::vector::dot(AB, AO) > 0) { + dir = uf::vector::cross( uf::vector::cross( AB, AO ), AB ); + } else { + s.pts = {A}; + dir = AO; + } + return false; + } + case 3: { + // points + auto& A = s.pts[2]; // newest + auto& B = s.pts[1]; + auto& C = s.pts[0]; + + // edges + auto AB = B.p - A.p; + auto AC = C.p - A.p; + auto AO = -A.p; + + auto ABC = uf::vector::cross( AB, AC ); + if ( uf::vector::dot( uf::vector::cross( AB, ABC ), AO ) > 0 ) { + s.pts = { A, B }; + return ::updateSimplex( s, dir ); + } + if ( uf::vector::dot( uf::vector::cross( ABC, AC ), AO ) > 0 ) { + s.pts = { A, C }; + return ::updateSimplex( s, dir ); + } + + if ( uf::vector::dot( ABC, AO ) > 0 ) dir = ABC; + else { s.pts = { A, C, B }; dir = -ABC; } + return false; + } + case 4: { + // points + auto& A = s.pts[3]; // newest + auto& B = s.pts[2]; + auto& C = s.pts[1]; + auto& D = s.pts[0]; + // line to origin + auto AO = -A.p; + // faces + auto ABC = uf::vector::cross( B.p - A.p, C.p - A.p ); + if ( uf::vector::dot( ABC, AO ) > 0 ) { + s.pts = { A, B, C }; + return ::updateSimplex( s, dir ); + } + + auto ACD = uf::vector::cross( C.p - A.p, D.p - A.p ); + if ( uf::vector::dot( ACD, AO ) > 0 ) { + s.pts = { A, C, D }; + return ::updateSimplex( s, dir ); + } + + auto ADB = uf::vector::cross( D.p - A.p, B.p - A.p ); + if ( uf::vector::dot( ADB, AO ) > 0 ) { + s.pts = { A, D, B }; + return ::updateSimplex( s, dir ); + } + + // origin inside tetrahedron + return true; + } + } + return false; + } + + bool isDegenerate( const pod::Simplex& s, const pod::SupportPoint& newPt, float eps = EPS(1.0e-6f) ) { + // compare to existing + for ( auto& sp : s.pts ) { + if ( uf::vector::magnitude( sp.p - newPt.p ) < eps ) return true; // duplicate point + } + // if simplex already has 3 points, check area + if ( s.pts.size() >= 2 ) { + auto& A = s.pts[0].p; + auto& B = s.pts[1].p; + auto& C = newPt.p; + + float area = uf::vector::magnitude( uf::vector::cross( B - A, C - A ) ); + if ( area < eps ) return true; // collinear with previous + } + if ( s.pts.size() >= 3 ) { + auto& A = s.pts[0].p; + auto& B = s.pts[1].p; + auto& C = s.pts[2].p; + auto& D = newPt.p; + + float volume = fabs( uf::vector::dot( D - A, uf::vector::cross( B - A, C - A ) ) ); + if ( volume < eps ) return true; // coplanar with triangle + } + return false; + } + + bool gjk( const pod::RigidBody& a, const pod::RigidBody& b, pod::Simplex& simplex, int maxIterations = 20, float eps = EPS(1e-6f) ) { + auto dir = b.transform->position - a.transform->position; + if ( uf::vector::magnitude(dir) < eps ) dir = {1,0,0}; // fallback direction + + // initial condition + simplex.pts.clear(); + simplex.pts.emplace_back(::supportMinkowskiDetailed( a, b, dir )); + dir = -simplex.pts[0].p; + + for ( auto it = 0; it < maxIterations; ++it ) { + auto newPt = ::supportMinkowskiDetailed( a, b, dir ); + if ( uf::vector::dot( newPt.p, dir ) < 0 ) return false; // didn't pass origin, no collision + // would invalidate the simplex + if ( ::isDegenerate( simplex, newPt ) ) { + #if 1 + // nudge direction with a small orthogonal component + if ( fabs(dir.x) < fabs(dir.y) && fabs(dir.x) < fabs(dir.z) ) dir = uf::vector::normalize( pod::Vector3f{1,0,0} + dir * 0.01f ); + else if ( fabs(dir.y) < fabs(dir.z) ) dir = uf::vector::normalize( pod::Vector3f{0,1,0} + dir * 0.01f ); + else dir = uf::vector::normalize( pod::Vector3f{0,0,1} + dir * 0.01f ); + #else + // choose an alternate probe + static pod::Vector3f fallbackDirs[3] = { {1,0,0}, {0,1,0}, {0,0,1} }; + dir = fallbackDirs[ (it + simplex.pts.size()) % 3 ]; + #endif + continue; // try again + } + // add new point to simplex + simplex.pts.emplace_back( newPt ); + // update + if ( ::updateSimplex(simplex, dir) ) return true; // simplex contains origin, finished + } + + #if 1 + return false; + #else + // if overlap detected but simplex ended at triangle, fix it, as EPA requires a tetrahedron: + if ( simplex.pts.size() == 3 ) { + // points + auto& A0 = simplex.pts[0].p; + auto& B0 = simplex.pts[1].p; + auto& C0 = simplex.pts[2].p; + // triangle normal + auto normal = uf::vector::normalize( uf::vector::cross( B0 - A0, C0 - A0 ) ); + + // try support in +normal + auto extra = ::supportMinkowskiDetailed( a, b, normal ); + float vol = fabs( uf::vector::dot( extra.p - A0, uf::vector::cross( B0 - A0, C0 - A0 ) ) ); + if ( vol < eps ) extra = ::supportMinkowskiDetailed( a, b, -normal ); // if still coplanar, try -normal + simplex.pts.emplace_back(extra); // force tetrahedron + } + + return !simplex.pts.empty(); + #endif + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl new file mode 100644 index 00000000..5c3485b2 --- /dev/null +++ b/engine/src/utils/math/physics/helpers.inl @@ -0,0 +1,260 @@ +// forward declare +namespace { + bool aabbAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool aabbSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool aabbPlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool aabbCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool aabbMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + + bool sphereSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool sphereAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool spherePlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool sphereCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool sphereMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + + bool planeAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool planeSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool planePlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool planeCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool planeMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + + bool capsuleCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool capsuleAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool capsulePlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool capsuleSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool capsuleMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + + bool meshAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool meshSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool meshPlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool meshCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool meshMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + + bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS(1.0e-6f) ); + pod::AABB computeTriangleAABB( const pod::Triangle& tri ); + + void reduceContacts( pod::Manifold& manifold ); + + void traverseNodePair( const pod::BVH& bvh, int leftID, int rightID, pod::BVH::pair_t& pairs ); + void traverseNodePair( const pod::BVH& a, int nodeA, const pod::BVH& b, int nodeB, pod::BVH::pair_t& out ); + + void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices ); + void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices, int nodeID ); + void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, int nodeID, float maxDist = FLT_MAX ); + void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, float maxDist = FLT_MAX ); +} + +namespace { + // create ID from pointers + uint64_t makePairKey( const pod::RigidBody& a, const pod::RigidBody& b ) { + auto idA = reinterpret_cast(&a); + auto idB = reinterpret_cast(&b); + if ( idA > idB ) std::swap(idA, idB); // ensure consistent order + return (idA << 32) ^ idB; + } + + // normalizes the delta between two bodies / contacts by the distance (as it was already computed) if non-zero + // a lot of collider v colliders use this semantic + pod::Vector3f normalizeDelta( const pod::Vector3f& delta, float dist, float eps = EPS(1.0e-6), const pod::Vector3f& fallback = pod::Vector3f{0,1,0} ) { + return ( dist > eps ) ? delta / dist : fallback; + } + + pod::Vector3f computeTangent( const pod::Vector3f& normal ) { + pod::Vector3f up = ( std::fabs(normal.y) < 0.999f ) ? pod::Vector3f{0,1,0} : pod::Vector3f{1,0,0}; // pick a vector not parallel to normal + pod::Vector3f tangent = uf::vector::normalize( uf::vector::cross( up, normal ) ); + return tangent; + } + pod::Vector3f closestPointOnSegment( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b ) { + pod::Vector3f ab = b - a; + float t = uf::vector::dot(p - a, ab) / uf::vector::dot(ab, ab); + t = std::max(0.0f, std::min(1.0f, t)); // clamp( t, 0, 1 ) + return a + ab * t; + } + + std::pair closestSegmentSegment( const pod::Vector3f& A, const pod::Vector3f& B, const pod::Vector3f& C, const pod::Vector3f& D, float eps = EPS(1e-6f) ) { + auto u = B - A; + auto v = D - C; + auto w = A - C; + + float a = uf::vector::dot( u, u ); + float b = uf::vector::dot( u, v ); + float c = uf::vector::dot( v, v ); + float d = uf::vector::dot( u, w ); + float e = uf::vector::dot( v, w ); + + float Dd = a*c - b*b; + + float sc, sN, sD = Dd; + float tc, tN, tD = Dd; + + if ( Dd < eps ) { + sN = 0.0f; + sD = 1.0f; + tN = e; + tD = c; + } else { + sN = (b*e - c*d); + tN = (a*e - b*d); + if (sN < 0) { sN = 0; tN = e; tD = c; } + else if (sN > sD) { sN = sD; tN = e + b; tD = c; } + } + + if ( tN < 0 ) { + tN = 0; + if (-d < 0) sN = 0; + else if (-d > a) sN = sD; + else { sN = -d; sD = a; } + } else if ( tN > tD ) { + tN = tD; + if ((-d + b) < 0) sN = 0; + else if ((-d + b) > a) sN = sD; + else { sN = (-d + b); sD = a; } + } + + sc = ( fabs(sN) < eps ) ? 0 : ( sN / sD ); + tc = ( fabs(tN) < eps ) ? 0 : ( tN / tD ); + + return { A + u * sc, C + v * tc }; + } + + pod::Vector3f closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box, float eps = EPS(1e-6f) ) { + // AABB center and half extents + auto c = ( box.min + box.max ) * 0.5f; + auto e = ( box.max - box.min ) * 0.5f; + + // direction of line segment + auto d = p2 - p1; + float len2 = uf::vector::dot( d, d ); + float t = 0.0f; + + if ( len2 > eps ) { + // parametric closest t from box center + t = uf::vector::dot( c - p1, d ) / len2; + t = std::max(0.0f, std::min(1.0f, t)); + } + + // closest point on segment to box center + auto segClosest = p1 + d * t; + + // clamp this point into AABB + return { + std::max(box.min.x, std::min(segClosest.x, box.max.x)), + std::max(box.min.y, std::min(segClosest.y, box.max.y)), + std::max(box.min.z, std::min(segClosest.z, box.max.z)), + }; + } + + pod::Vector3f computeBarycentric( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, bool clamps = false ) { + // edges + auto ab = b - a; + auto ac = c - a; + auto ao = p - a; + + // compute barycentric coords wrt triangle + float d1 = uf::vector::dot(ab, ao); + float d2 = uf::vector::dot(ac, ao); + float d00 = uf::vector::dot(ab, ab); + float d01 = uf::vector::dot(ab, ac); + float d11 = uf::vector::dot(ac, ac); + + float denom = d00 * d11 - d01 * d01; + + float v = (d11*d1 - d01*d2) / denom; + float w = (d00*d2 - d01*d1) / denom; + float u = 1.0f - v - w; + + // clamp to triangle + if ( clamps && (u < 0 || v < 0 || w < 0) ) { + // if projected point is outside, snap to edges or vertices + // check against AB + float t = std::clamp(uf::vector::dot( ao, ab ) / d00, 0.0f, 1.0f); + auto pAB = a + (ab * t); + float distAB = uf::vector::dot( pAB, pAB ); + + // check against AC + float t2 = std::clamp(uf::vector::dot( ao, ac ) / d11, 0.0f, 1.0f); + auto pAC = a + (ac * t2); + float distAC = uf::vector::dot( pAC, pAC ); + + // check against BC + auto bc = c - b; + float d22 = uf::vector::dot( bc, bc ); + float t3 = std::clamp(uf::vector::dot( -b, bc ) / d22, 0.0f, 1.0f); + auto pBC = b + ( bc * t3 ); + float distBC = uf::vector::dot( pBC, pBC ); + + // pick closest edge/vertex + if ( distAB <= distAC && distAB <= distBC ) return { 1.0f - t, t, 0.0f }; + if ( distAC <= distBC ) return { 1.0f - t2, 0.0f, t2 }; + return { 0.0f, 1.0f - t3, t3 }; + } + + return { u, v, w }; + } + // made a huge mess of probably mixing up arguments................ + pod::Vector3f computeBarycentric(const pod::Vector3f& p, const pod::Triangle& tri, bool clamps = false ) { + return ::computeBarycentric( p, tri.points[0], tri.points[1], tri.points[2], clamps ); + } + pod::Vector3f interpolateWithBarycentric( const pod::Vector3f& bary, const pod::Vector3f a, const pod::Vector3f b, const pod::Vector3f c ) { + return a * bary.x + b * bary.y + c * bary.z; + } + pod::Vector3f interpolateWithBarycentric( const pod::Vector3f& bary, const pod::Vector3f points[3] ) { + return ::interpolateWithBarycentric( bary, points[0], points[1], points[2] ); + } + + pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) { + auto bary = ::computeBarycentric( p, a, b, c ); + return ::interpolateWithBarycentric( bary, a, b, c ); + } + pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri ) { + return ::closestPointOnTriangle( p, tri.points[0], tri.points[1], tri.points[2] ); + } + + + pod::Vector3f orientNormalToAB( const pod::RigidBody& a, const pod::RigidBody& b, pod::Vector3f n ) { + return uf::vector::normalize( uf::vector::dot( n, b.transform->position - a.transform->position ) < 0.0f ? -n : n ); + } + + float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri ) { + // segment vs plane + auto n = uf::vector::normalize( uf::vector::cross( tri.points[1] - tri.points[0], tri.points[2]-tri.points[0] ) ); + float d0 = uf::vector::dot( p0 - tri.points[0], n ); + float d1 = uf::vector::dot( p1 - tri.points[0], n ); + + // intersects plane + if ( ( d0 * d1 ) <= 0 ) { + float t = d0 / ( d0-d1 ); + auto p = p0 + ( p1 - p0 ) * t; + auto q = ::closestPointOnTriangle( p, tri ); + outSeg = p; outTri = q; + return uf::vector::magnitude( p - q ); + } + + // otherwise check endpoints against triangle + auto q0 = ::closestPointOnTriangle( p0, tri ); + auto q1 = ::closestPointOnTriangle( p1, tri ); + float d0sq = uf::vector::magnitude( p0 - q0 ); + float d1sq = uf::vector::magnitude( p1 - q1 ); + + if ( d0sq < d1sq ) { outSeg = p0; outTri = q0; return d0sq; } + else { outSeg = p1; outTri = q1; return d1sq; } + } + + bool triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b, float eps = EPS(1e-6f) ) { + auto boxA = ::computeTriangleAABB( a ); + auto boxB = ::computeTriangleAABB( b ); + + if ( !aabbOverlap( boxA, boxB ) ) return false; + + // check vertices of a inside b or vice versa + for ( auto i = 0; i < 3; i++ ) { + auto q = ::closestPointOnTriangle( a.points[i], b ); + if ( uf::vector::magnitude( q - a.points[i] ) < eps ) return true; + } + for ( auto i = 0; i < 3; i++ ) { + auto q = ::closestPointOnTriangle( b.points[i], a ); + if ( uf::vector::magnitude( q - b.points[i] ) < eps ) return true; + } + return false; + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp new file mode 100644 index 00000000..904561ac --- /dev/null +++ b/engine/src/utils/math/physics/impl.cpp @@ -0,0 +1,410 @@ +#if !UF_USE_REACTPHYSICS +#include +#include +#include +#include + +namespace { + bool warmupSolver = false; + bool blockContactSolver = false; + bool useGjk = false; + bool fixedStep = true; + int substeps = 0; + + int solverIterations = 20; + float baumgarteCorrectionPercent = 0.4f; + float baumgarteCorrectionSlop = 0.01f; + uf::stl::unordered_map manifoldsCache; +} + +#define EPS(x) x // 1.0e-6f +#define ASSERT_COLLIDER_TYPES( A, B ) UF_ASSERT( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B ); +#define UF_PHYSICS_TEST 0 + +#include "helpers.inl" +#include "aabb.inl" +#include "sphere.inl" +#include "plane.inl" +#include "capsule.inl" +#include "mesh.inl" +#include "ray.inl" +#include "bvh.inl" +#include "gjk.inl" +#include "epa.inl" +#include "integration.inl" + +// unused +float uf::physics::impl::timescale = 1.0f / 60.0f; +bool uf::physics::impl::interpolate = false; +bool uf::physics::impl::shared = false; +bool uf::physics::impl::globalStorage = false; + +// Bindings +void uf::physics::impl::initialize() { + uf::physics::impl::initialize( uf::scene::getCurrentScene() ); +} +void uf::physics::impl::initialize( uf::Object& object ) { + uf::physics::impl::initialize( object.getComponent() ); +} +void uf::physics::impl::initialize( pod::World& world ) { + // to-do: any additional initialization (since it's not needed right now) +} +void uf::physics::impl::tick( float dt ) { + uf::physics::impl::tick( uf::scene::getCurrentScene(), dt ); +} +void uf::physics::impl::tick( uf::Object& object, float dt ) { + uf::physics::impl::tick( object.getComponent(), dt ); +} +void uf::physics::impl::tick( pod::World& world, float dt ) { + if ( !::fixedStep ) { + + if ( ::substeps > 0 ) uf::physics::impl::substep( world, dt, ::substeps ); + else uf::physics::impl::step( world, dt ); + + return; + } + + static float accumulator = 0; + accumulator += dt; + while ( accumulator >= uf::physics::impl::timescale ) { + if ( ::substeps > 0 ) uf::physics::impl::substep( world, uf::physics::impl::timescale, ::substeps ); + else uf::physics::impl::step( world, uf::physics::impl::timescale ); + accumulator -= uf::physics::impl::timescale; + } +} +void uf::physics::impl::terminate() { + uf::physics::impl::terminate( uf::scene::getCurrentScene() ); +} +void uf::physics::impl::terminate( uf::Object& object ) { + uf::physics::impl::terminate( object.getComponent() ); +} +void uf::physics::impl::terminate( pod::World& world ) { + world.bodies.clear(); +} + +// Implementation +void uf::physics::impl::substep( pod::World& world, float dt, int substeps ) { + float h = dt / substeps; + for ( auto i=0; i < substeps; ++i) { + uf::physics::impl::step( world, h ); + } +} +void uf::physics::impl::step( pod::World& world, float dt ) { + auto& bodies = world.bodies; + auto& bvh = world.bvh; + + if ( bodies.empty() ) return; + + for ( auto* body : bodies ) { + ::integrate( *body, dt ); + } + + uf::stl::vector manifolds; + uf::stl::vector> pairs; + + // create BVH + ::buildBroadphaseBVH(bvh, bodies); + // query for overlaps + ::queryOverlaps( bvh, pairs ); + // iterate overlaps + for ( auto& [ia, ib] : pairs ) { + auto& a = *bodies[ia]; + auto& b = *bodies[ib]; + + pod::Manifold manifold; + if ( ::generateContacts( a, b, manifold, dt ) ) { + // bodies with meshes already reorient the normal to the triangle's center + // do not do it for meshes because it'll reorient to the mesh's origin + if ( a.collider.type != pod::ShapeType::MESH && b.collider.type != pod::ShapeType::MESH ) { + for ( auto& c : manifold.points ) c.normal = ::orientNormalToAB( a, b, c.normal ); + } + // retrieve accumulated impulses + if ( ::warmupSolver ) ::retrieveContacts( manifold, ::manifoldsCache[::makePairKey( a, b )] ); + // keep four most important contacts + ::reduceContacts( manifold ); + #if 0 + UF_MSG_DEBUG("body a={}, body b={}, manifold size={}", uf::vector::toString(a.transform->position), uf::vector::toString(b.transform->position), manifold.points.size()); + for ( auto& contact : manifold.points ) { + UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( contact.point ), uf::vector::toString( contact.normal ), contact.penetration ); + } + #endif + + // store manifold + manifolds.emplace_back(manifold); + } + } + + // pass manifolds to solver + ::solveContacts( manifolds, dt ); + + if ( ::warmupSolver ) { + // update cache + for ( auto& manifold : manifolds ) { + ::manifoldsCache[::makePairKey( *manifold.a, *manifold.b )] = manifold; + } + + // prune if too old / empty + for ( auto& [ key, manifold ] : manifoldsCache ) { + // prune manifolds that are X frames old + for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) { + if ( it->lifetime > 3 ) manifold.points.erase(it); + else ++it; + } + // empty manifold, kill it + if ( manifold.points.empty() ) manifoldsCache.erase(key); + } + } + + // recompute bounds for further queries + for ( auto* body : bodies ) { + body->bounds = ::computeAABB( *body ); + } +} + +void uf::physics::impl::setMass( pod::RigidBody& body, float mass ) { + body.mass = mass; + body.inverseMass = 1.0f / mass; + setInertia( body ); +} +void uf::physics::impl::setInertia( pod::RigidBody& body ) { + if ( body.isStatic || body.mass <= 0 ) { + body.inverseInertiaTensor = {}; + return; + } + + // to-do: make this a flag or something + if ( body.collider.type == pod::ShapeType::CAPSULE ) { + body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; + body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; + + return; + } + + switch ( body.collider.type ) { + case pod::ShapeType::AABB: { + pod::Vector3f extents = (body.collider.u.aabb.max - body.collider.u.aabb.min); + extents *= extents; // square it; + + body.inertiaTensor = extents * (body.mass / 12.0f); + // to-do: add overloaded inverse order arithmetic operators + //body.inverseInertiaTensor = 1.0f / body.inertiaTensor; + body.inverseInertiaTensor = { 1.0f / body.inertiaTensor.x, 1.0f / body.inertiaTensor.y, 1.0f / body.inertiaTensor.z }; + } break; + case pod::ShapeType::SPHERE: { + float I = 0.4f * body.mass * body.collider.u.sphere.radius * body.collider.u.sphere.radius; + float invI = 1.0f / I; + body.inertiaTensor = { I, I, I }; + body.inverseInertiaTensor = { invI, invI, invI }; + } break; + case pod::ShapeType::CAPSULE: { + float r = body.collider.u.capsule.radius; + float h = body.collider.u.capsule.halfHeight * 2.0f; // full cyl height + float m = body.mass; + + float Ixx = 0.25f * m * r * r + (1.0f/12.0f) * m * h * h; + float Iyy = 0.5f * m * r * r; + float Izz = Ixx; + + body.inertiaTensor = { Ixx, Iyy, Izz }; + body.inverseInertiaTensor = { 1.0f/Ixx, 1.0f/Iyy, 1.0f/Izz }; + } break; + + // to-do: add others + default: { + } break; + } +} +void uf::physics::impl::applyForce( pod::RigidBody& body, const pod::Vector3f& force ) { + if ( body.isStatic ) return; + body.forceAccumulator += force; +} +void uf::physics::impl::applyForceAtPoint( pod::RigidBody body, const pod::Vector3f& force, const pod::Vector3f& point ) { + if ( body.isStatic ) return; + // linear force + body.forceAccumulator += force; + // angular force + pod::Vector3f r = point - body.transform->position; + body.torqueAccumulator += uf::vector::cross( r, force ); +} +void uf::physics::impl::applyImpulse( pod::RigidBody& body, const pod::Vector3f& impulse ) { + if ( body.isStatic ) return; + body.velocity += impulse * body.inverseMass; +} +void uf::physics::impl::applyTorque( pod::RigidBody& body, const pod::Vector3f& torque ) { + if ( body.isStatic ) return; + body.torqueAccumulator += torque; +} +void uf::physics::impl::setVelocity( pod::RigidBody& body, const pod::Vector3f& v ) { + body.velocity = v; +} +void uf::physics::impl::applyRotation( pod::RigidBody& body, const pod::Quaternion<>& q ) { + uf::transform::rotate( *body.transform, q ); +} +void uf::physics::impl::applyRotation( pod::RigidBody& body, const pod::Vector3f& axis, float angle ) { + applyRotation( body, uf::quaternion::axisAngle( axis, angle ) ); +} + +// body creation +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, float mass ) { + // bind to component + pod::RigidBody& body = object.getComponent(); + // initial initialization + body.world = &world; + body.object = &object; + body.transform = &object.getComponent>(); + body.mass = mass; + body.inverseMass = 1.0f / mass; + body.isStatic = mass == 0.0f; + + // insert into world + world.bodies.emplace_back(&body); + + return body; +} +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass ) { + auto& body = create( world, object, mass ); + body.collider.type = pod::ShapeType::AABB; + body.collider.u.aabb = aabb; + body.bounds = ::computeAABB( body ); + setInertia( body ); + UF_MSG_DEBUG("Creating body of type: {} | mass: {} | min: {} | max: {}", "AABB", mass, uf::vector::toString(aabb.min), uf::vector::toString(aabb.max) ); + return body; +} +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Sphere& sphere, float mass ) { + auto& body = create( world, object, mass ); + body.collider.type = pod::ShapeType::SPHERE; + body.collider.u.sphere = sphere; + body.bounds = ::computeAABB( body ); + setInertia( body ); + UF_MSG_DEBUG("Creating body of type: {} | mass: {} | radius: {}", "SPHERE", mass, sphere.radius ); + return body; +} +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Plane& plane, float mass ) { + auto& body = create( world, object, mass ); + body.collider.type = pod::ShapeType::PLANE; + body.collider.u.plane = plane; + body.bounds = ::computeAABB( body ); + setInertia( body ); + UF_MSG_DEBUG("Creating body of type: {} | mass: {} | normal: {} | offset: {}", "PLANE", mass, uf::vector::toString( plane.normal ), plane.offset ); + return body; +} +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Capsule& capsule, float mass ) { + auto& body = create( world, object, mass ); + body.collider.type = pod::ShapeType::CAPSULE; + body.collider.u.capsule = capsule; + body.material.restitution = 0.001f; + body.bounds = ::computeAABB( body ); + setInertia( body ); + UF_MSG_DEBUG("Creating body of type: {} | mass: {} | radius: {} | height: {}", "CAPSULE", mass, capsule.radius, capsule.halfHeight * 2.0f ); + return body; +} +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const uf::Mesh& mesh, float mass ) { + auto& body = create( world, object, mass ); + body.collider.type = pod::ShapeType::MESH; + body.collider.u.mesh.mesh = &mesh; + body.collider.u.mesh.bvh = new pod::BVH; + + ::buildMeshBVH( *body.collider.u.mesh.bvh, mesh ); + + UF_MSG_DEBUG("Built mesh BVH: nodes={} indices={}", body.collider.u.mesh.bvh->nodes.size(), body.collider.u.mesh.bvh->indices.size()); + + body.bounds = ::computeAABB( body ); + setInertia( body ); + UF_MSG_DEBUG("Creating body of type: {} | mass: {} ", "MESH", mass ); + return body; +} + +pod::RigidBody& uf::physics::impl::create( uf::Object& object, float mass ) { + // bind to scene + // auto& root = object.getRootParent<>(); // in the event a scene is being initialized that is not the root scene, use the root parent instead + // auto& world = root.getComponent(); + auto& scene = uf::scene::getCurrentScene(); + auto& world = scene.getComponent(); + return create( world, object, mass ); + +} +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::AABB& aabb, float mass ) { + auto& scene = uf::scene::getCurrentScene(); + auto& world = scene.getComponent(); + return create( world, object, aabb, mass ); +} +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Sphere& sphere, float mass ) { + auto& scene = uf::scene::getCurrentScene(); + auto& world = scene.getComponent(); + return create( world, object, sphere, mass ); +} +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Plane& plane, float mass ) { + auto& scene = uf::scene::getCurrentScene(); + auto& world = scene.getComponent(); + return create( world, object, plane, mass ); +} +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Capsule& capsule, float mass ) { + auto& scene = uf::scene::getCurrentScene(); + auto& world = scene.getComponent(); + return create( world, object, capsule, mass ); +} +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const uf::Mesh& mesh, float mass ) { + auto& scene = uf::scene::getCurrentScene(); + auto& world = scene.getComponent(); + return create( world, object, mesh, mass ); +} + +void uf::physics::impl::destroy( uf::Object& object ) { + if ( !object.hasComponent() ) return; + + return destroy( object.getComponent() ); +} +void uf::physics::impl::destroy( pod::RigidBody& body ) { + auto& world = *body.world; + // remove from world + for ( auto it = world.bodies.begin(); it != world.bodies.end(); ++it ) { + if ( (*it)->object != body.object ) continue; + world.bodies.erase(it); + break; + } + + // remove any pointered collider data + if ( body.collider.type == pod::ShapeType::MESH ) { + if ( body.collider.u.mesh.bvh ) delete body.collider.u.mesh.bvh; + } +} + +pod::RayQuery uf::physics::impl::rayCast( const pod::Ray& ray, const pod::RigidBody& body, float maxDistance ) { + return rayCast( ray, *body.world, &body, maxDistance ); +} +pod::RayQuery uf::physics::impl::rayCast( const pod::Ray& ray, const pod::World& world, float maxDistance ) { + return rayCast( ray, world, NULL, maxDistance ); +} +pod::RayQuery uf::physics::impl::rayCast( const pod::Ray& ray, const pod::World& world, const pod::RigidBody* body, float maxDistance ) { + pod::RayQuery rayHit; + rayHit.contact.penetration = maxDistance; + + auto& bvh = world.bvh; + auto& bodies = world.bodies; + +#if 1 + uf::stl::vector candidates; + ::queryBVH( bvh, ray, candidates ); + + for ( auto i : candidates ) { +#else + for ( auto i = 0; i < bodies.size(); ++i ) { +#endif + auto* b = bodies[i]; + if ( body == b ) continue; + switch ( b->collider.type ) { + case pod::ShapeType::AABB: rayAabb( ray, *b, rayHit ); break; + case pod::ShapeType::SPHERE: raySphere( ray, *b, rayHit ); break; + case pod::ShapeType::PLANE: rayPlane( ray, *b, rayHit ); break; + case pod::ShapeType::CAPSULE: rayCapsule( ray, *b, rayHit ); break; + case pod::ShapeType::MESH: rayMesh( ray, *b, rayHit ); break; + } + } + + return rayHit; +} + +#if UF_PHYSICS_TEST + #include "tests.inl" +#endif +#endif \ No newline at end of file diff --git a/engine/src/utils/math/physics/integration.inl b/engine/src/utils/math/physics/integration.inl new file mode 100644 index 00000000..69192a10 --- /dev/null +++ b/engine/src/utils/math/physics/integration.inl @@ -0,0 +1,308 @@ +namespace { + float computeEffectiveMass( pod::RigidBody& a, pod::RigidBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& n ) { + float inverseMass = 0.0f; + if ( !a.isStatic ) inverseMass += a.inverseMass; + if ( !b.isStatic ) inverseMass += b.inverseMass; + + float angularTermA = 0.0f; + float angularTermB = 0.0f; + + if ( !a.isStatic ) { + pod::Vector3f crossA = uf::vector::cross(rA, n); + angularTermA = uf::vector::dot(uf::vector::cross(crossA * a.inverseInertiaTensor, rA), n); + } + if ( !b.isStatic ) { + pod::Vector3f crossB = uf::vector::cross(rB, n); + angularTermB = uf::vector::dot(uf::vector::cross(crossB * b.inverseInertiaTensor, rB), n); + } + + float result = inverseMass + angularTermA + angularTermB; + if (result < EPS(1e-8f)) result = 1.0f; // prevent divide by zero + return result; + } + + void applyImpulseTo( pod::RigidBody& a, pod::RigidBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) { + if ( !a.isStatic ) { + a.velocity -= impulse * a.inverseMass; + a.angularVelocity -= (uf::vector::cross(rA, impulse)) * a.inverseInertiaTensor; + } + if ( !b.isStatic ) { + b.velocity += impulse * b.inverseMass; + b.angularVelocity += (uf::vector::cross(rB, impulse)) * b.inverseInertiaTensor; + } + } + + void applyRollingResistance( pod::RigidBody& body, float dt ) { + if ( body.isStatic ) return; + + float rollingFriction = 0.02f; // to-do: derive from material + float angularSpeed = uf::vector::magnitude( body.angularVelocity ); + if ( angularSpeed < EPS(1.0e-6f) ) return; + + body.angularVelocity += body.angularVelocity * body.mass * -rollingFriction * dt; + // body.angularVelocity *= -rollingFriction * dt; + } + + void bindManifold( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt = 0 ) { + manifold.a = &a; + manifold.b = &b; + manifold.dt = dt; + manifold.points.clear(); + } + + bool generateContactsGjk( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + ::bindManifold( a, b, manifold, dt ); + + pod::Simplex simplex; + + if ( !::gjk(a,b,simplex) ) return false; + + auto result = ::epa( a, b, simplex ); + + manifold.points.clear(); + manifold.points.emplace_back(result); + return true; + } + + bool generateContacts( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + if ( ::useGjk ) return generateContactsGjk( a, b, manifold, dt ); + ::bindManifold( a, b, manifold, dt ); + + #define CHECK_CONTACT( A, B, fun )\ + if ( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B ) return fun( a, b, manifold ); + + CHECK_CONTACT( AABB, AABB, aabbAabb ); + CHECK_CONTACT( AABB, SPHERE, aabbSphere ); + CHECK_CONTACT( AABB, PLANE, aabbPlane ); + CHECK_CONTACT( AABB, CAPSULE, aabbCapsule ); + CHECK_CONTACT( AABB, MESH, aabbMesh ); + + CHECK_CONTACT( SPHERE, AABB, sphereAabb ); + CHECK_CONTACT( SPHERE, SPHERE, sphereSphere ); + CHECK_CONTACT( SPHERE, PLANE, spherePlane ); + CHECK_CONTACT( SPHERE, CAPSULE, sphereCapsule ); + CHECK_CONTACT( SPHERE, MESH, sphereMesh ); + + CHECK_CONTACT( PLANE, AABB, planeAabb ); + CHECK_CONTACT( PLANE, SPHERE, planeSphere ); + CHECK_CONTACT( PLANE, PLANE, planePlane ); + CHECK_CONTACT( PLANE, CAPSULE, planeCapsule ); + CHECK_CONTACT( PLANE, MESH, planeMesh ); + + CHECK_CONTACT( CAPSULE, AABB, capsuleAabb ); + CHECK_CONTACT( CAPSULE, SPHERE, capsuleSphere ); + CHECK_CONTACT( CAPSULE, PLANE, capsulePlane ); + CHECK_CONTACT( CAPSULE, CAPSULE, capsuleCapsule ); + CHECK_CONTACT( CAPSULE, MESH, capsuleMesh ); + + CHECK_CONTACT( MESH, AABB, meshAabb ); + CHECK_CONTACT( MESH, SPHERE, meshSphere ); + CHECK_CONTACT( MESH, PLANE, meshPlane ); + CHECK_CONTACT( MESH, CAPSULE, meshCapsule ); + CHECK_CONTACT( MESH, MESH, meshMesh ); + + return false; + } + + bool similarContact( const pod::Contact& a, const pod::Contact& b, float eps = 1.0e-3f, float distSq = 0.95f ) { + return uf::vector::distanceSquared(a.point, b.point) < eps && uf::vector::dot(a.normal, b.normal) > distSq; + } + + void reduceContacts( pod::Manifold& manifold ) { + if ( manifold.points.size() <= 4 ) return; + + uf::stl::vector reduced; + for ( auto& c : manifold.points ) { + bool merged = false; + for ( auto& r : reduced ) { + if ( !::similarContact( c, r ) ) continue; + // merge, pick deeper penetration + if (c.penetration > r.penetration) r = c; + merged = true; + break; + } + if ( !merged ) reduced.emplace_back(c); + } + + // keep only deepest + farthest up to 4 + std::sort(reduced.begin(), reduced.end(), [](auto& a, auto& b){ return a.penetration > b.penetration; }); + if (reduced.size() > 4) reduced.resize(4); + + manifold.points = reduced; + } + + void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float decay = 0.35f ) { + for ( auto& c : current.points ) { + for ( auto& p : previous.points ) { + if ( !::similarContact( c, p ) ) continue; + c.accumulatedNormalImpulse = p.accumulatedNormalImpulse * decay; + c.accumulatedTangentImpulse = p.accumulatedTangentImpulse * decay; + c.lifetime = p.lifetime + 1; + break; + } + } + } + + void warmupContacts( pod::RigidBody& a, pod::RigidBody& b, const pod::Contact& c, float dt ) { + if ( !c.lifetime ) return; // too new + + // build relative offsets + pod::Vector3f rA = c.point - a.transform->position; + pod::Vector3f rB = c.point - b.transform->position; + + // normal impulse + pod::Vector3f Pn = c.normal * c.accumulatedNormalImpulse; + ::applyImpulseTo( a, b, rA, rB, Pn ); + + // tangent basis + pod::Vector3f Pt = c.tangent * c.accumulatedTangentImpulse; + ::applyImpulseTo( a, b, rA, rB, Pt ); + + UF_MSG_DEBUG("Warming, Pn={}, Pt={}, lifetime={}", uf::vector::toString(Pn), uf::vector::toString(Pt), c.lifetime ); + } + + // baumgarte position correction + void positionCorrection( pod::RigidBody& a, pod::RigidBody& b, const pod::Contact& contact ) { + float correctionMagnitude = std::max(contact.penetration - ::baumgarteCorrectionSlop, 0.0f) / (a.inverseMass + b.inverseMass) * ::baumgarteCorrectionPercent; + pod::Vector3f correction = contact.normal * correctionMagnitude; + + if ( !a.isStatic ) a.transform->position -= correction * a.inverseMass; + if ( !b.isStatic ) b.transform->position += correction * b.inverseMass; + } + + void resolveContact( pod::RigidBody& a, pod::RigidBody& b, pod::Contact& contact, float dt ) { + // relative positions from centers to contact point + pod::Vector3f rA = contact.point - a.transform->position; + pod::Vector3f rB = contact.point - b.transform->position; + + // relative velocity at contact + pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA); + pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB); + pod::Vector3f rv = vB - vA; + + // normal contact velocity + float velAlongNormal = uf::vector::dot(rv, contact.normal); + float velTolerance = 0; // -1.0e3f; + if ( velAlongNormal > velTolerance ) return; // if separating, no impulse + + // compute restitution (bounce) + float e = std::min(a.material.restitution, b.material.restitution); + + // nullify restitution if velocity is small enough + if ( fabs(velAlongNormal) < 1.0f) e = 0.0f; + + // effective inverse mass along normal + float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal); + + // normal impulse scalar + float jn = -(1.0f + e) * velAlongNormal; + jn /= invMassN; + if ( ::warmupSolver ) { + float jnOld = contact.accumulatedNormalImpulse; + float jnNew = std::max(0.0f, jnOld + jn); + float jnDelta = jnNew - jnOld; + contact.accumulatedNormalImpulse = jnNew; + jn = jnDelta; + } + + pod::Vector3f normalImpulse = contact.normal * jn; + ::applyImpulseTo(a, b, rA, rB, normalImpulse); + + // tangent direction + pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal); + float tangentMag = uf::vector::magnitude(tangent); + if (tangentMag > EPS(1e-6f)) { + tangent /= tangentMag; + + // effective mass along tangent + float invMassT = ::computeEffectiveMass(a, b, rA, rB, tangent); + + // tangential relative velocity + float vt = uf::vector::dot(rv, tangent); + + // required tangential impulse to cancel tangent velocity + float jt = -vt / invMassT; + + // friction coefficients + float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction); + float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction); + + if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally + + if ( ::warmupSolver ) { + float maxFriction = mu_s * contact.accumulatedNormalImpulse; + float jtOld = contact.accumulatedTangentImpulse; + float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction)); + float jtDelta = jtNew - jtOld; + contact.accumulatedTangentImpulse = jtNew; + contact.tangent = tangent; + jt = jtDelta; + } + + pod::Vector3f frictionImpulse = tangent * jt; + ::applyImpulseTo(a, b, rA, rB, frictionImpulse); + } + + ::positionCorrection(a, b, contact); + } + + void solveContacts( uf::stl::vector& manifolds, float dt ) { + if ( ::warmupSolver ) { + for ( auto& m : manifolds ) { + for ( auto& c : m.points ) { + ::warmupContacts(*m.a, *m.b, c, dt); + } + } + } + + for ( auto i = 0; i < ::solverIterations; ++i ) { + for ( auto& m : manifolds ) { + #if 0 + if ( ::blockContactSolver ) { + //::solveManifoldBlockN( m, dt ); + ::solveManifoldBlock2x2( m, dt ); + } else { + for ( auto& c : m.points ) { + ::resolveContact(*m.a, *m.b, c, dt); + } + } + #endif + for ( auto& c : m.points ) ::resolveContact(*m.a, *m.b, c, dt); + } + } + } + + void integrate( pod::RigidBody& body, float dt ) { + // only integrate dynamic bodies + if ( body.isStatic || body.mass == 0 ) return; + + auto& world = *body.world; + + // linear integration + pod::Vector3f acceleration = body.forceAccumulator * body.inverseMass; + acceleration += world.gravity; // apply gravity + body.velocity += acceleration * dt; + + auto previous = body.transform->position; + body.transform->position += body.velocity * dt; + + // angular integration + body.angularVelocity += body.torqueAccumulator * body.inverseInertiaTensor * dt; + + // update orientation + if ( uf::vector::magnitude( body.angularVelocity ) > EPS(1.0e-8f) ) { + pod::Quaternion<> dq = uf::quaternion::axisAngle(uf::vector::normalize(body.angularVelocity), uf::vector::magnitude(body.angularVelocity)*dt); + uf::transform::rotate( *body.transform, dq ); + } + + // reset accumulators + body.forceAccumulator = {}; + body.torqueAccumulator = {}; + + // update AABB + body.bounds = ::computeAABB( body ); + + // apply rolling resistance + ::applyRollingResistance(body, dt); + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/mesh.inl b/engine/src/utils/math/physics/mesh.inl new file mode 100644 index 00000000..3a7ed90f --- /dev/null +++ b/engine/src/utils/math/physics/mesh.inl @@ -0,0 +1,303 @@ +#define REORIENT_NORMALS_ON_FETCH 0 +#define REORIENT_NORMALS_ON_CONTACT 0 + +// mesh BVH +namespace { + // to-do: clean this up + uint32_t getIndex( const void* indexData, size_t indexSize, size_t idx ) { + if ( indexSize == sizeof(uint8_t) ) { + auto* ptr = reinterpret_cast(indexData); + return static_cast(ptr[idx]); + } else if ( indexSize == sizeof(uint16_t) ) { + auto* ptr = reinterpret_cast(indexData); + return static_cast(ptr[idx]); + } else if ( indexSize == sizeof(uint32_t) ) { + auto* ptr = reinterpret_cast(indexData); + return ptr[idx]; + } + UF_EXCEPTION("Unsupported index type of size {}", indexSize); + } + pod::AABB computeTriangleAABB( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) { + auto triIndexID = triID * 3; + + uint32_t i0 = ::getIndex( indexData, indexSize, triIndexID + 0 ); + uint32_t i1 = ::getIndex( indexData, indexSize, triIndexID + 1 ); + uint32_t i2 = ::getIndex( indexData, indexSize, triIndexID + 2 ); + + auto& v0 = *reinterpret_cast(reinterpret_cast(vertices) + i0 * vertexStride); + auto& v1 = *reinterpret_cast(reinterpret_cast(vertices) + i1 * vertexStride); + auto& v2 = *reinterpret_cast(reinterpret_cast(vertices) + i2 * vertexStride); + + return { + { + std::min({v0.x, v1.x, v2.x}), + std::min({v0.y, v1.y, v2.y}), + std::min({v0.z, v1.z, v2.z}), + }, + { + std::max({v0.x, v1.x, v2.x}), + std::max({v0.y, v1.y, v2.y}), + std::max({v0.z, v1.z, v2.z}), + } + }; + } + + pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ) { + auto views = mesh.makeViews({"position", "normal"}); + UF_ASSERT(!views.empty()); + + uint32_t triIndexID = triID * 3; // remap triangle ID to index ID + // find which view contains this triangle index. + const uf::Mesh::View* found = nullptr; + for ( auto& v : views ) { + if ( v.index.first <= triIndexID && triIndexID < v.index.first + v.index.count ) { + found = &v; + break; + } + } + UF_ASSERT( found ); + + pod::TriangleWithNormal tri; + + auto& positions = (*found)["position"]; + auto& normals = (*found)["normal"]; + auto& indices = (*found)["index"]; + + const void* indexBase = indices.data(found->index.first); + size_t indexSize = mesh.index.size; + + // reset back to local indices range + triIndexID -= found->index.first; + + uint32_t idxs[3]; + // to-do: just make this a macro that could have a parallel hint + for ( auto i = 0; i < 3; ++i ) idxs[i] = getIndex(indexBase, indexSize, triIndexID + i); + + { + auto* base = reinterpret_cast(positions.data(found->vertex.first)); + size_t stride = positions.stride(); + + for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast(base + idxs[i] * stride); + } + + if ( normals.valid() && false ) { + auto* base = reinterpret_cast(normals.data(found->vertex.first)); + size_t stride = normals.stride(); + for ( auto i = 0; i < 3; ++i ) tri.normals[i] = *reinterpret_cast(base + idxs[i] * stride); + } else { + auto normal = uf::vector::normalize(uf::vector::cross(tri.points[1]-tri.points[0], tri.points[2]-tri.points[0])); + for ( auto i = 0; i < 3; ++i ) tri.normals[i] = normal; + } + + return tri; + } + + // if body is a mesh, apply its transform to the triangles, else reorient the normal with respect to the body + pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::RigidBody& body ) { + auto tri = ::fetchTriangle( mesh, triID ); + if ( body.collider.type == pod::ShapeType::MESH ) { + if ( body.transform ) { + for ( auto i = 0; i < 3; ++i ) { + tri.points[i] = uf::transform::apply(*body.transform, tri.points[i]); + tri.normals[i] = uf::quaternion::rotate(body.transform->orientation, tri.normals[i]); + } + } + } + else { + #if REORIENT_NORMALS_ON_FETCH + auto triCenter = (tri.points[0] + tri.points[1] + tri.points[2]) / 3.0f; + auto delta = body.transform->position - triCenter; + for ( auto i = 0; i < 3; ++i ) { + if ( uf::vector::dot(tri.normals[i], delta) < 0.0f ) tri.normals[i] = -tri.normals[i]; + } + #endif + } + + return tri; + } +} + +// +namespace { + bool meshAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( MESH, AABB ); + + const auto& mesh = a; + const auto& aabb = b; + + const auto& meshData = *mesh.collider.u.mesh.mesh; + const auto& bvh = *mesh.collider.u.mesh.bvh; + + auto bounds = ::transformAabbToLocal( aabb.bounds, *mesh.transform ); + + uf::stl::vector candidates; + ::queryBVH(bvh, bounds, candidates); + + bool hit = false; + for ( auto triID : candidates ) { + auto tri = ::fetchTriangle( meshData, triID, aabb ); + auto closestTri = ::closestPointOnTriangle( ::aabbCenter( bounds ), tri ); + auto closestAabb = ::closestPointOnAABB( closestTri, bounds ); + + if ( !uf::vector::isValid( closestTri ) ) { + //UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] )); + //UF_MSG_DEBUG("closestTri={}, closestAabb={}", uf::vector::toString( closestTri ), uf::vector::toString( closestAabb )); + continue; + } + + auto delta = closestAabb - closestTri; // direction between points + float dist2 = uf::vector::magnitude( delta ); + float tolerance = 1.0e-3; // std::max( 1.0e-3f, uf::vector::magnitude(aabb.velocity) * manifold.dt * 0.5f ); + if ( dist2 >= tolerance ) continue; + float dist = std::sqrt( dist2 ); + + auto contact = closestTri; // ( closestAabb + closestTri ) * 0.5f; // center of points + auto normal = ::normalizeDelta( delta, dist, eps ); // uf::vector::normalize( ::interpolateWithBarycentric( tri.normals, ::computeBarycentric( contact, tri ) )); + float penetration = tolerance - dist; + + #if REORIENT_NORMALS_ON_CONTACT + if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + #endif + + contact = uf::transform::apply( *mesh.transform, contact ); + normal = uf::quaternion::rotate( mesh.transform->orientation, normal ); + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + hit = true; + } + return hit; + } + bool meshSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES(MESH, SPHERE); + + const auto& mesh = a; + const auto& sphere = b; + + const auto& meshData = *mesh.collider.u.mesh.mesh; + const auto& bvh = *mesh.collider.u.mesh.bvh; + + auto bounds = ::transformAabbToLocal( ::computeAABB( sphere ), *mesh.transform ); + + auto center = ::aabbCenter( bounds ); + float r = sphere.collider.u.sphere.radius; + + uf::stl::vector candidates; + ::queryBVH(bvh, bounds, candidates); + + bool hit = false; + for ( auto triID : candidates ) { + auto tri = ::fetchTriangle( meshData, triID, sphere ); + auto closest = ::closestPointOnTriangle( ::aabbCenter(bounds), tri ); + + if ( !uf::vector::isValid( closest ) ) { + //UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] )); + //UF_MSG_DEBUG("closest={}", uf::vector::toString( closest )); + continue; + } + + auto delta = center - closest; + float dist2 = uf::vector::magnitude(delta); + if ( dist2 > r * r ) continue; + float dist = std::sqrt( dist2 ); + + auto contact = closest; + auto normal = ::normalizeDelta( delta, dist, eps ); + float penetration = r - dist; + + #if REORIENT_NORMALS_ON_CONTACT + if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + #endif + + contact = uf::transform::apply(*mesh.transform, contact); + normal = uf::quaternion::rotate(mesh.transform->orientation, normal); + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + hit = true; + } + + return hit; + } + // to-do + bool meshPlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES(MESH, PLANE); + return false; + } + bool meshCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES(MESH, CAPSULE); + + const auto& mesh = a; + const auto& capsule = b; + + const auto& meshData = *mesh.collider.u.mesh.mesh; + const auto& bvh = *mesh.collider.u.mesh.bvh; + + // capsule line segment in world space + auto [ p1, p2 ] = ::getCapsuleSegment(b); + float r = b.collider.u.capsule.radius; + + auto bounds = ::transformAabbToLocal( ::computeSegmentAABB(p1, p2, r), *a.transform ); + + uf::stl::vector candidates; + ::queryBVH(bvh, bounds, candidates); + + // for some reason (the segment points), it's just easier to transform the triangle to world space, than compare in local then transform the contact to world + bool hit = false; + for ( auto triID : candidates ) { + auto tri = ::fetchTriangle( meshData, triID, mesh ); + + pod::Vector3f closestSeg, closestTri; + float dist2 = ::segmentTriangleDistanceSq( p1, p2, tri, closestSeg, closestTri ); + + if ( !uf::vector::isValid( closestTri ) ) { + //UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] )); + //UF_MSG_DEBUG("closestTri={}", uf::vector::toString( closestTri )); + continue; + } + + if ( dist2 > r * r ) continue; + float dist = std::sqrt( dist2 ); + auto delta = ( closestSeg - closestTri ); + + auto contact = closestTri; // ( closestSeg + closestTri ) * 0.5f; // center of points + auto normal = ::normalizeDelta( delta, dist, eps ); // uf::vector::normalize( ::interpolateWithBarycentric( tri.normals, ::computeBarycentric( contact, tri ) )); + float penetration = r - dist; + + #if REORIENT_NORMALS_ON_CONTACT + if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + #endif + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + hit = true; + } + + return hit; + } + // to-do + bool meshMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( MESH, MESH ); + + auto& bodyA = a; + auto& bvhA = *a.collider.u.mesh.bvh; + auto& meshA = *a.collider.u.mesh.mesh; + + auto& bodyB = b; + auto& meshB = *b.collider.u.mesh.mesh; + auto& bvhB = *b.collider.u.mesh.bvh; + + pod::BVH::pair_t pairs; + ::traverseNodePair(bvhA, 0, bvhB, 0, pairs); + + for (auto [idA, idB] : pairs) { + auto tA = ::fetchTriangle( meshA, idA, bodyA ); + auto tB = ::fetchTriangle( meshB, idB, bodyB ); + + // narrowphase tri-tri overlap: SAT + if ( ::triangleTriangleIntersect( tA, tB ) ) { + pod::Vector3f contact = ( tA.points[0] + tB.points[0] )*0.5f; + pod::Vector3f n = uf::vector::normalize( uf::vector::cross( tA.points[1] - tA.points[0], tA.points[2] - tA.points[0] ) ); + manifold.points.emplace_back(pod::Contact{ contact, n, 0.001f }); + } + } + return !manifold.points.empty(); + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/plane.inl b/engine/src/utils/math/physics/plane.inl new file mode 100644 index 00000000..c8b56959 --- /dev/null +++ b/engine/src/utils/math/physics/plane.inl @@ -0,0 +1,57 @@ +namespace { + bool planeAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( PLANE, AABB ); + + const auto& plane = a; + const auto& aabb = b; + + auto normal = uf::vector::normalize( plane.collider.u.plane.normal ); + float offset = plane.collider.u.plane.offset; + + auto center = (aabb.bounds.min + aabb.bounds.max) * 0.5f; // center + auto extent = (aabb.bounds.max - aabb.bounds.min) * 0.5f; // half extents + float r = fabs(extent.x * normal.x) + fabs(extent.y * normal.y) + fabs(extent.z * normal.z); // effective projection radius of box onto plane normal + + float dist = uf::vector::dot( normal, center ) - offset; + if ( dist > r ) return false; + + pod::Vector3f contact = center - normal * dist; + float penetration = r - dist; + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool planeSphere(const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps) { + ASSERT_COLLIDER_TYPES(PLANE, SPHERE); + + const auto& plane = a; + const auto& sphere = b; + + auto& normal = plane.collider.u.plane.normal; + float offset = plane.collider.u.plane.offset; + + auto center = sphere.transform->position; + float r = sphere.collider.u.sphere.radius; + + float dist = uf::vector::dot( normal, center ) - offset; + if ( dist > r ) return false; + + auto contact = center - normal * dist - normal * r; + float penetration = r - dist; + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool planePlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( PLANE, PLANE ); + return false; + } + bool planeCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( PLANE, CAPSULE ); + return capsulePlane( b, a, manifold, eps ); + } + bool planeMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( PLANE, MESH ); + return meshPlane( b, a, manifold, eps ); + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/ray.inl b/engine/src/utils/math/physics/ray.inl new file mode 100644 index 00000000..f93d3e0b --- /dev/null +++ b/engine/src/utils/math/physics/ray.inl @@ -0,0 +1,237 @@ +namespace { + bool rayTriangleIntersect( const pod::Ray& ray, const pod::Vector3f tri[3], float& t, float& u, float& v, float eps = EPS(1.0e-6f) ) { + auto edge1 = tri[1] - tri[0]; + auto edge2 = tri[2] - tri[0]; + auto h = uf::vector::cross( ray.direction, edge2 ); + float a = uf::vector::dot( edge1, h ); + + if ( fabs(a) < eps ) return false; + + float f = 1.0f / a; + auto s = ray.origin - tri[0]; + u = f * uf::vector::dot( s, h ); + if ( u < 0.0f || u > 1.0f ) return false; + + auto q = uf::vector::cross( s, edge1 ); + v = f * uf::vector::dot( ray.direction, q ); + if (v < 0.0f || u + v > 1.0f) return false; + + t = f * uf::vector::dot( edge2, q ); + return ( t > eps ); + } + + bool rayAabbIntersect( const pod::Ray& ray, const pod::AABB& box, float& tMin, float& tMax, float eps = EPS(1.0e-6f) ) { + tMin = 0.0f; + tMax = FLT_MAX; + + for ( auto i = 0; i < 3; ++i ) { + float minB = box.min[i]; + float maxB = box.max[i]; + + // inflate degenerate slab + if ( fabs(maxB - minB) < 1e-6f) { + maxB += 1e-6f; + minB -= 1e-6f; + } + + float invD = 1.0f / ray.direction[i]; + float t0 = ( minB - ray.origin[i] ) * invD; + float t1 = ( maxB - ray.origin[i] ) * invD; + + if ( invD < 0.0f ) std::swap(t0, t1); + + tMin = std::max(tMin, t0); + tMax = std::min(tMax, t1); + + if ( tMax <= tMin ) return false; + } + return true; + } + + bool rayAabb( const pod::Ray& ray, const pod::RigidBody& body, pod::RayQuery& rayHit ) { + float tMin = 0.0f; + float tMax = FLT_MAX; + + if ( !::rayAabbIntersect( ray, body.bounds, tMin, tMax ) ) return false; + + if ( tMin < rayHit.contact.penetration ) { + rayHit.hit = true; + rayHit.body = &body; + rayHit.contact.point = ray.origin + ray.direction * tMin; + rayHit.contact.penetration = tMin; + + auto local = rayHit.contact.point - (body.bounds.min + body.bounds.max) * 0.5f; + auto extents = (body.bounds.max - body.bounds.min) * 0.5f; + auto absLocal = pod::Vector3f{fabs(local.x), fabs(local.y), fabs(local.z)}; + + if ( absLocal.x > absLocal.y && absLocal.x > absLocal.z ) { + rayHit.contact.normal = { (local.x > 0 ? 1.0f : -1.0f), 0, 0 }; + } else if ( absLocal.y > absLocal.z ) { + rayHit.contact.normal = { 0, (local.y > 0 ? 1.0f : -1.0f), 0 }; + } else { + rayHit.contact.normal = { 0, 0, (local.z > 0 ? 1.0f : -1.0f) }; + } + } + return true; + } + bool raySphere( const pod::Ray& ray, const pod::RigidBody& body, pod::RayQuery& rayHit ) { + auto center = body.transform->position; + float r = body.collider.u.sphere.radius; + + auto oc = ray.origin - center; + // ray starts inside + if ( uf::vector::magnitude( oc ) < r * r ) { + rayHit.hit = true; + rayHit.body = &body; + rayHit.contact.point = ray.origin; + rayHit.contact.normal = uf::vector::normalize(ray.origin - center); + rayHit.contact.penetration = 0.0f; + return true; + } + + float a = uf::vector::dot( ray.direction, ray.direction ); + float b = 2.0f * uf::vector::dot( oc, ray.direction ); + float c = uf::vector::dot( oc, oc ) - r * r; + + float discriminant = b*b - 4*a*c; + if ( discriminant < 0.0f ) return false; + + float sqrtDisc = std::sqrt(discriminant); + float t0 = (-b - sqrtDisc) / (2*a); + float t1 = (-b + sqrtDisc) / (2*a); + + // take nearest valid hit + float t = t0; + if ( t < 0.0f ) t = t1; + if ( t < 0.0f ) return false; + + if ( t >= rayHit.contact.penetration ) return false; // further than current closest + + // record hit + rayHit.hit = true; + rayHit.body = &body; + rayHit.contact.point = ray.origin + ray.direction * t; + rayHit.contact.normal = uf::vector::normalize( rayHit.contact.point - center ); + rayHit.contact.penetration = t; + return true; + } + bool rayPlane( const pod::Ray& ray, const pod::RigidBody& body, pod::RayQuery& rayHit, float eps = EPS(1e-6f) ) { + auto& normal = body.collider.u.plane.normal; + float offset = body.collider.u.plane.offset; + + float denom = uf::vector::dot( normal, ray.direction ); + if ( fabs(denom) < eps ) return false; // parallel + + float t = (offset - uf::vector::dot( normal, ray.origin )) / denom; + if ( t < 0.0f ) return false; // behind ray start + if ( t >= rayHit.contact.penetration ) return false; // existing hit is closer + + // record hit + rayHit.hit = true; + rayHit.body = &body; + rayHit.contact.point = ray.origin + ray.direction * t; + rayHit.contact.normal = normal * ( denom < 0.0f ? 1.0f : -1.0f ); // face ray + rayHit.contact.penetration = t; + return true; + } + bool rayCapsule( const pod::Ray& ray, const pod::RigidBody& body, pod::RayQuery& rayHit ) { + auto [ p1, p2 ] = ::getCapsuleSegment( body ); + float r = body.collider.u.capsule.radius; + + auto d = p2 - p1; // segment direction + auto m = ray.origin - p1; + auto n = ray.direction; + + float md = uf::vector::dot(m, d); + float nd = uf::vector::dot(n, d); + float dd = uf::vector::dot(d, d); + + // coeffs for intersection with infinite cylinder + float a = dd - nd*nd; + float k = uf::vector::dot(m, m) - r*r; + float c = dd - md*md; + + float b = dd * uf::vector::dot(m,n) - nd*md; + float discr = b*b - a*c; + if ( discr < 0.0f ) return false; + + float t = (-b - std::sqrt(discr)) / a; // nearer hit + + if ( t < 0 || t > rayHit.contact.penetration ) return false; // clipped by range + + // check hit actually lies within segment caps + float y = md + t*nd; + if ( y < 0.0f || y > dd ) { + // check against spherical caps instead + float tSphere = FLT_MAX; + bool hit = false; + + for ( auto center : {p1, p2} ) { + auto oc = ray.origin - center; + float b = uf::vector::dot(oc, n); + float c = uf::vector::dot(oc, oc) - r*r; + float disc = b*b - c; + if ( disc >= 0 ) { + float tTmp = -b - std::sqrt(disc); + if ( tTmp >= 0 && tTmp < tSphere ) { + tSphere = tTmp; + hit = true; + } + } + } + if ( !hit || tSphere > rayHit.contact.penetration ) return false; + + t = tSphere; + } + + // register hit + if ( t < rayHit.contact.penetration ) { + rayHit.hit = true; + rayHit.body = &body; + rayHit.contact.point = ray.origin + n * t; + rayHit.contact.penetration = t; + + // normal: from capsule axis to hit point + auto closest = ::closestPointOnSegment( rayHit.contact.point, p1, p2 ); + auto normal = uf::vector::normalize( rayHit.contact.point - closest ); + rayHit.contact.normal = normal; + } + return true; + } + + bool rayMesh( const pod::Ray& ray, const pod::RigidBody& body, pod::RayQuery& rayHit ) { + const uf::Mesh& meshData = *body.collider.u.mesh.mesh; + const pod::BVH& bvh = *body.collider.u.mesh.bvh; + + pod::Ray localRay; + localRay.origin = uf::transform::applyInverse( *body.transform, ray.origin ); + localRay.direction = uf::quaternion::rotate( uf::quaternion::inverse(body.transform->orientation), ray.direction ); + + uf::stl::vector indices; + ::queryBVH( bvh, localRay, indices ); + + for ( auto triID : indices ) { + auto tri = ::fetchTriangle( meshData, triID ); + + float t, u, v; + if ( !::rayTriangleIntersect( localRay, tri.points, t, u, v ) ) continue; + if ( t >= rayHit.contact.penetration ) continue; + + auto l = localRay.origin + localRay.direction * t; + auto bary = ::computeBarycentric(l, tri.points[0], tri.points[1], tri.points[2]); + auto n = uf::vector::normalize(::interpolateWithBarycentric(bary, tri.normals)); + + // push back to world + auto p = uf::transform::apply(*body.transform, l); + n = uf::quaternion::rotate(body.transform->orientation, n); + + rayHit.hit = true; + rayHit.body = &body; + rayHit.contact.point = p; + rayHit.contact.normal = n; + rayHit.contact.penetration = t; + } + + return rayHit.hit; + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/sphere.inl b/engine/src/utils/math/physics/sphere.inl new file mode 100644 index 00000000..6618e8f5 --- /dev/null +++ b/engine/src/utils/math/physics/sphere.inl @@ -0,0 +1,66 @@ +namespace { + bool sphereSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( SPHERE, SPHERE ); + + auto delta = b.transform->position - a.transform->position; + float r = a.collider.u.sphere.radius + b.collider.u.sphere.radius; + + float dist2 = uf::vector::dot(delta, delta); + if ( dist2 > r * r ) return false; + float dist = std::sqrt( dist2 ); + + float penetration = r - dist; + auto normal = ::normalizeDelta( delta, dist, eps ); + auto contact = a.transform->position + (normal * a.collider.u.sphere.radius); + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool sphereAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( SPHERE, AABB ); + + const auto& sphere = a; + const auto& aabb = b; + + auto& center = sphere.transform->position; + float r = sphere.collider.u.sphere.radius; + + auto& bounds = aabb.bounds; + auto closest = pod::Vector3f{ + std::max(bounds.min.x, std::min(center.x, bounds.max.x)), + std::max(bounds.min.y, std::min(center.y, bounds.max.y)), + std::max(bounds.min.z, std::min(center.z, bounds.max.z)), + }; + + auto delta = center - closest; + float dist2 = uf::vector::dot(delta, delta); + if ( dist2 > r * r ) return false; + float dist = std::sqrt( dist2 ); + + float penetration = r - dist; + auto normal = ::normalizeDelta( delta, dist, eps ); + auto contact = center - normal * r; + + #if 0 + pod::Vector3f dir = center - ::aabbCenter( bounds ); + if (std::fabs(dir.x) > std::fabs(dir.y) && std::fabs(dir.x) > std::fabs(dir.z)) normal = { (dir.x > 0 ? 1 : -1), 0, 0 }; + else if (std::fabs(dir.y) > std::fabs(dir.z)) normal = { 0, (dir.y > 0 ? 1 : -1), 0 }; + else normal = { 0, 0, (dir.z > 0 ? 1 : -1) }; + #endif + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool spherePlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( SPHERE, PLANE ); + return planeSphere( b, a, manifold, eps ); + } + bool sphereCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( SPHERE, CAPSULE ); + return capsuleSphere( b, a, manifold, eps ); + } + bool sphereMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( SPHERE, MESH ); + return meshSphere( b, a, manifold, eps ); + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/tests.inl b/engine/src/utils/math/physics/tests.inl new file mode 100644 index 00000000..a2ad6d33 --- /dev/null +++ b/engine/src/utils/math/physics/tests.inl @@ -0,0 +1,815 @@ +#include + +#define FRAMERATE 60 +#define INV_FRAMERATE 1.0f / FRAMERATE + +#define PHYSICS_STEP(time) for ( int i = 0; i < time * FRAMERATE; i++ ) uf::physics::impl::step(world, INV_FRAMERATE); + +namespace { + uf::Mesh generateMesh( float size = 1 ) { + uf::Mesh mesh; + mesh.bind(); + mesh.insertVertices({ + { pod::Vector3f{-1.0f * size, 0.0f, 1.0f * size}, pod::Vector2f{0.0f, 0.0f} }, + { pod::Vector3f{-1.0f * size, 0.0f, -1.0f * size}, pod::Vector2f{0.0f, 1.0f} }, + { pod::Vector3f{ 1.0f * size, 0.0f, -1.0f * size}, pod::Vector2f{1.0f, 1.0f} }, + + { pod::Vector3f{ 1.0f * size, 0.0f, -1.0f * size}, pod::Vector2f{1.0f, 1.0f} }, + { pod::Vector3f{ 1.0f * size, 0.0f, 1.0f * size}, pod::Vector2f{1.0f, 0.0f} }, + { pod::Vector3f{-1.0f * size, 0.0f, 1.0f * size}, pod::Vector2f{0.0f, 0.0f} }, + }); + mesh.insertIndices({ + //0, 1, 2, 3, 4, 5 + 0, 2, 1, 3, 5, 4 + }); + + return mesh; + } +} + +// list of unit tests to "standardly" verify the system works, but honestly this is a mess + +TEST(SphereSphere_Collision, { + pod::World world; + uf::Object objA, objB; + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Sphere{1.0f}, 1.0f); + + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {1.5f,0,0}; // closer than sum of radii (2.0) + + pod::Manifold m; + bool collided = sphereSphere(bodyA, bodyB, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); + EXPECT_NEAR(m.points[0].penetration, 0.5f, EPS(1e-4f)); +}) + +TEST(AabbAabb_Collision, { + pod::World world; + uf::Object objA, objB; + pod::AABB box = { {-1,-1,-1}, {1,1,1} }; + + auto& bodyA = uf::physics::impl::create(world, objA, box, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, box, 1.0f); + + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {1.5f,0,0}; // overlap in x-axis + + pod::Manifold m; + bool collided = aabbAabb(bodyA, bodyB, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(RaySphere_Hit, { + pod::World world; + uf::Object obj; + auto& body = uf::physics::impl::create(world, obj, pod::Sphere{1.0f}, 1.0f); + body.transform->position = {0,0,0}; + + pod::Ray ray{ {0,0,-5}, uf::vector::normalize(pod::Vector3f{0,0,1}) }; + pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); + + EXPECT_TRUE(hit.hit); + EXPECT_NEAR(hit.contact.penetration, 4.0f, EPS(1e-4f)); +}) + +TEST(SphereSphere_NoCollision, { + pod::World world; + uf::Object objA, objB; + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Sphere{1.0f}, 1.0f); + + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {5.0f,0,0}; // too far + + pod::Manifold m; + bool collided = sphereSphere(bodyA, bodyB, m); + EXPECT_TRUE(!collided); +}) + +TEST(SphereAabb_Collision, { + pod::World world; + uf::Object objSphere, objBox; + + auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); + auto& box = uf::physics::impl::create(world, objBox, pod::AABB{{-1,-1,-1}, {1,1,1}}, 1.0f); + + sphere.transform->position = {0.5f, 0.0f, 0.0f}; // overlapping inside box + box.transform->position = {0,0,0}; + + pod::Manifold m; + bool collided = sphereAabb(sphere, box, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); + EXPECT_TRUE(m.points[0].penetration > 0.0f); +}) + +TEST(SpherePlane_Collision, { + pod::World world; + uf::Object objSphere, objPlane; + + auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + + // Place sphere so it's intersecting the plane + sphere.transform->position = {0,0.5f,0}; + + pod::Manifold m; + bool collided = planeSphere(plane, sphere, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); + EXPECT_NEAR(m.points[0].penetration, 0.5f, EPS(1e-4f)); +}) + +TEST(SpherePlane_NoCollision, { + pod::World world; + uf::Object objSphere, objPlane; + + auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + + sphere.transform->position = {0, 5.0f, 0}; // clearly above + pod::Manifold m; + bool collided = planeSphere(plane, sphere, m); + EXPECT_TRUE(!collided); +}) + +TEST(CapsuleCapsule_Collision, { + pod::World world; + uf::Object objA, objB; + auto& capA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& capB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f, 1.0f}, 1.0f); + + capA.transform->position = {0,0,0}; + capB.transform->position = {0.8f,0,0}; // slight overlap + + pod::Manifold m; + bool collided = capsuleCapsule(capA, capB, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); + EXPECT_TRUE(m.points[0].penetration > 0.0f); +}) + +TEST(RayAabb_Miss, { + pod::World world; + uf::Object obj; + auto& box = uf::physics::impl::create(world, obj, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + box.transform->position = {0,0,0}; + + pod::Ray ray{{5,5,5}, uf::vector::normalize(pod::Vector3f{1,0,0})}; + auto hit = uf::physics::impl::rayCast(ray, world, 100.0f); + EXPECT_TRUE(!hit.hit); +}) + +// GJK shouldn't be used for sphere sphere +#if 0 +TEST(Gjk_SphereSphereOverlap, { + pod::World world; + uf::Object objA, objB; + auto& a = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& b = uf::physics::impl::create(world, objB, pod::Sphere{1.0f}, 1.0f); + + a.transform->position = {0,0,0}; + b.transform->position = {1.5f,0,0}; + + pod::Simplex simplex; + bool inside = gjk(a, b, simplex); + EXPECT_TRUE(inside); + auto contact = epa(a, b, simplex); + EXPECT_TRUE(contact.penetration > 0.0f); +}) +#endif + +TEST(PhysicsStep_Gravity, { + pod::World world; + uf::Object obj; + auto& body = uf::physics::impl::create(world, obj, pod::Sphere{1.0f}, 1.0f); + body.transform->position = {0, 10, 0}; + body.velocity = {0,0,0}; + + PHYSICS_STEP(1) + + EXPECT_NEAR(body.transform->position.y, 10.0f - world.gravity.y, 0.05f); +}) + +TEST(PhysicsStep_SpherePlane_Bounce, { + pod::World world; + uf::Object objSphere, objPlane; + + auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0}, 0.0f}, 0.0f); + + sphere.transform->position = {0, 2, 0}; + sphere.material.restitution = 1.0f; + + PHYSICS_STEP(1) + + // After bouncing, sphere should be near plane surface, not sinking below + EXPECT_TRUE(sphere.transform->position.y >= 0.9f); + EXPECT_TRUE(fabs(sphere.velocity.y) < 10.0f); // should have reversed sign at least once +}) + +TEST(PhysicsStep_AabbStacking, { + pod::World world; + uf::Object bottomObj, fallingObj; + + auto& bottom = uf::physics::impl::create(world, bottomObj, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f); + bottom.transform->position = {0,0,0}; + + auto& falling = uf::physics::impl::create(world, fallingObj, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + falling.transform->position = {0,5,0}; + + PHYSICS_STEP(5); + + // After time, falling cube should rest on top of static one + EXPECT_TRUE(falling.transform->position.y > 1.9f); + EXPECT_TRUE(fabs(falling.velocity.y) < 0.1f); +}) + +TEST(PhysicsStep_SphereSphere_HeadOn, { + pod::World world; + uf::Object objA, objB; + + auto& A = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& B = uf::physics::impl::create(world, objB, pod::Sphere{1.0f}, 1.0f); + + A.transform->position = {-5,0,0}; + B.transform->position = { 5,0,0}; + A.velocity = { 5,0,0}; + B.velocity = {-5,0,0}; + + PHYSICS_STEP(5); + + // Expect velocities swapped (perfect elastic bounce with equal masses) + EXPECT_TRUE(A.velocity.x < 0.0f); + EXPECT_TRUE(B.velocity.x > 0.0f); +}) + +TEST(PhysicsStep_RaycastDynamic, { + pod::World world; + uf::Object obj; + world.gravity = {}; + auto& body = uf::physics::impl::create(world, obj, pod::Sphere{1.0f}, 1.0f); + body.transform->position = {0,0,0}; + body.velocity = {0,0,10}; + + PHYSICS_STEP(1); + + pod::Ray ray{ {0,0,-5}, {0,0,1} }; + pod::RayQuery q = uf::physics::impl::rayCast(ray, world, 100.0f); + EXPECT_TRUE(q.hit); + EXPECT_TRUE(fabs(q.contact.point.z - 10.0f) <= 1.0f); // near where it moved +}) + +TEST(SphereSphere_TouchingButNotOverlapping, { + pod::World world; + uf::Object objA, objB; + auto& a = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& b = uf::physics::impl::create(world, objB, pod::Sphere{1.0f}, 1.0f); + a.transform->position = {0,0,0}; + b.transform->position = {2.0f,0,0}; // exactly touching + + pod::Manifold m; + bool collided = sphereSphere(a, b, m); + + EXPECT_TRUE(collided); // should count as a collision… + EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS(1e-6f)); +}) + +TEST(RaySphere_OriginInside, { + pod::World world; + uf::Object obj; + auto& body = uf::physics::impl::create(world, obj, pod::Sphere{2.0f}, 1.0f); + body.transform->position = {0,0,0}; + + pod::Ray ray{ {0,0,0}, {1,0,0} }; // starts inside + auto q = uf::physics::impl::rayCast(ray, world, 100.0f); + + EXPECT_TRUE(q.hit); + EXPECT_NEAR(q.contact.penetration, 0.0f, EPS(1e-6f)); +}) + +TEST(PhysicsStep_StaticFriction_Holds, { + pod::World world; + uf::Object objSphere, objPlane; + + auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0}, 0.0f}, 0.0f); + + world.gravity = {0,-9.81f,0}; + sphere.transform->position = {0,1.0f,0}; + sphere.material.staticFriction = 2.0f; // stronger grip to cover solver slop + + // Apply smaller force (well below μ_s * N) + uf::physics::impl::applyForce(sphere, {2,0,0}); + + PHYSICS_STEP(1); + + EXPECT_NEAR(sphere.transform->position.x, 0.0f, 0.05f); // allow tiny error +}) + +TEST(PhysicsStep_StaticFriction_Slips, { + pod::World world; + uf::Object objSphere, objPlane; + + auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0}, 0.0f}, 0.0f); + + world.gravity = {0,-9.81f,0}; + sphere.transform->position = {0,1.0f,0}; + sphere.material.staticFriction = 1.0f; + + uf::physics::impl::applyForce(sphere, {15,0,0}); // Above limit + + PHYSICS_STEP(1); + + EXPECT_TRUE(fabs(sphere.transform->position.x) > 0.1f); // It should slide +}) + +TEST(CapsulePlane_Slope_StaticHold, { + pod::World world; + uf::Object objCap, objPlane; + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,1},0.0f}, 0.0f); + + // Place capsule on slope + cap.transform->position = {0,2,0}; + cap.material.staticFriction = 2.0f; // More than tan(45) = 1 + cap.material.dynamicFriction = 1.0f; + + PHYSICS_STEP(5); + + EXPECT_NEAR(cap.transform->position.z, 0.0f, 0.2f); // Held in place + EXPECT_NEAR(cap.velocity.z, 0.0f, 0.05f); +}) + +TEST(CapsulePlane_Slope_Slip, { + pod::World world; + uf::Object objCap, objPlane; + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,1},0.0f}, 0.0f); + + cap.transform->position = {0,2,0}; + cap.material.staticFriction = 0.5f; // Less than tan(45) => must slip + cap.material.dynamicFriction = 0.5f; + + PHYSICS_STEP(5); + + EXPECT_TRUE(fabs(cap.transform->position.z) > 1.0f); // Should have slid downhill +}) + +TEST(CapsulePlane_RestingContact, { + pod::World world; + uf::Object objCap, objPlane; + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0}, 0.0f}, 0.0f); + + cap.transform->position = {0, 1.5f, 0}; // halfHeight=1, radius=0.5, so "foot" at y=0 + cap.velocity = {0,0,0}; + + PHYSICS_STEP(1); + + // Capsule should rest on the floor at y=1.5 + EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.05f); + EXPECT_NEAR(cap.velocity.y, 0.0f, 0.05f); // no jitter +}) + +TEST(CapsuleAabb_RestingContact, { + pod::World world; + uf::Object objCap, objBox; + + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& floor = uf::physics::impl::create(world, objBox, pod::AABB{{-5, -1, -5},{5, 0, 5}}, 0.0f); + + cap.transform->position = {0, 1.5f, 0}; + cap.velocity = {0,0,0}; + + PHYSICS_STEP(2); + + EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.05f); + EXPECT_NEAR(cap.velocity.y, 0.0f, 0.05f); +}) + +TEST(CapsulePlane_Settling, { + pod::World world; + uf::Object objCap, objPlane; + + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + + cap.transform->position = {0, 2.0f, 0}; // slightly above + cap.velocity = {0,0,0}; + + PHYSICS_STEP(3); + + EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.05f); +}) + +TEST(CapsulePlane_SlopeStaticFriction, { + pod::World world; + uf::Object objCap, objPlane; + + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& slope = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,1},0.0f}, 0.0f); // 45° slope + + cap.transform->position = {0, 3.0f, 0}; + cap.material.staticFriction = 2.0f; + cap.material.dynamicFriction = 1.0f; + + PHYSICS_STEP(4); + + // Should not slide much if static friction is strong + EXPECT_NEAR(cap.transform->position.z, 0.0f, 0.1f); +}) + +TEST(CapsuleAabb_StepEdge, { + pod::World world; + uf::Object objCap, objBox; + + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& floor = uf::physics::impl::create(world, objBox, pod::AABB{{0,-1,-5},{5,0,5}}, 0.0f); + + cap.transform->position = {0.25f, 1.5f, 0}; // Capsule foot half on, half off + + PHYSICS_STEP(4); + + // Should not spaz out or fall through + EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.1f); +}) + +TEST(Diagnostic_CapsuleGrounding, { + pod::World world; + uf::Object objCap, objFloor; + + // Capsule: radius 0.5, half-height 1.0 (total height 2.0 + end caps) + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + + // Test toggle: try both AABB floors and Plane floors. + bool usePlane = true; + auto& floor = usePlane ? uf::physics::impl::create(world, objFloor, pod::Plane{{0,1,0}, 0.0f}, 0.0f) : uf::physics::impl::create(world, objFloor, pod::AABB{{-5,-1,-5},{5,0,5}}, 0.0f); + + cap.transform->position = {0, 3, 0}; // start a little above floor + cap.velocity = {0,0,0}; + + PHYSICS_STEP(2); + + // Final resting state: should be near Y=1.5 (halfHeight + radius) + EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.1f); +}) + +TEST(CapsulePlane_ContactNormal, { + pod::World world; + uf::Object objCap, objPlane; + + auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + + cap.transform->position = {0,1.5f,0}; + pod::Manifold m; + bool collided = capsulePlane(cap, plane, m); + + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); + if ( !m.points.empty() ) { + EXPECT_NEAR(m.points[0].normal.y, 1.0f, 1e-3f); // Normal should point UP + } else { + EXPECT_NEAR(1.0f, 0.0f, 0.0f); + } +}) + +TEST(AabbPlane_RestingNoSink, { + pod::World world; + uf::Object playerObj, groundObj; + + auto& player = uf::physics::impl::create(world, playerObj, pod::AABB{{-0.5f,-1.0f,-0.5f},{0.5f,0.0f,0.5f}}, 1.0f); + auto& ground = uf::physics::impl::create(world, groundObj, pod::Plane{{0,1,0},0}, 0.0f); + + player.transform->position = {0, 2.0f, 0}; + + PHYSICS_STEP(5); + + // Expect the player to remain on ground at y=0.0 without sinking further + EXPECT_NEAR(player.transform->position.y, 0.0f, 0.05f); + EXPECT_NEAR(player.velocity.y, 0.0f, 0.05f); +}) + +TEST(CapsuleSphere_Collision, { + pod::World world; + uf::Object capObj, sphObj; + auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); + + cap.transform->position = {0,0,0}; + sph.transform->position = {0,0.5f,0}; // overlap + + pod::Manifold m; + bool collided = capsuleSphere(cap, sph, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(CapsuleSphere_NoCollision, { + pod::World world; + uf::Object capObj, sphObj; + auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); + + cap.transform->position = {0,0,0}; + sph.transform->position = {0,5,0}; // too far + + pod::Manifold m; + bool collided = capsuleSphere(cap, sph, m); + EXPECT_TRUE(!collided); +}) + + +TEST(AabbSphere_Collision, { + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); + + box.transform->position = {0,0,0}; + sphere.transform->position = {0.75f,0,0}; // Intersecting into box + + pod::Manifold m; + bool collided = aabbSphere(box,sphere,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(AabbSphere_NoCollision, { + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); + + box.transform->position = {0,0,0}; + sphere.transform->position = {5,0,0}; // too far away + + pod::Manifold m; + bool collided = aabbSphere(box,sphere,m); + EXPECT_TRUE(!collided); +}) + + +TEST(AabbPlane_Collision, { + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + + box.transform->position = {0,0.5f,0}; // half interpenetrating plane + + pod::Manifold m; + bool collided = aabbPlane(box,plane,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(AabbPlane_NoCollision, { + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + + box.transform->position = {0,5,0}; // clearly above + + pod::Manifold m; + bool collided = aabbPlane(box,plane,m); + EXPECT_TRUE(!collided); +}) + + +TEST(AabbCapsule_Collision, { + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + + box.transform->position = {0,0,0}; + cap.transform->position = {0,0.5f,0}; // partially overlapping + + pod::Manifold m; + bool collided = aabbCapsule(box,cap,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(AabbCapsule_NoCollision, { + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + + box.transform->position = {0,0,0}; + cap.transform->position = {0,5,0}; + + pod::Manifold m; + bool collided = aabbCapsule(box,cap,m); + EXPECT_TRUE(!collided); +}) + + +TEST(SphereCapsule_Collision, { + pod::World world; + uf::Object objA, objB; + auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + + sphere.transform->position = {0,0.0f,0}; + cap.transform->position = {0,0.25f,0}; + + pod::Manifold m; + bool collided = sphereCapsule(sphere,cap,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(SphereCapsule_NoCollision, { + pod::World world; + uf::Object objA, objB; + auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + + sphere.transform->position = {0,5,0}; + cap.transform->position = {0,0,0}; + + pod::Manifold m; + bool collided = sphereCapsule(sphere,cap,m); + EXPECT_TRUE(!collided); +}) + +TEST(PlanePlane_NoCollision, { + pod::World world; + uf::Object objA, objB; + auto& planeA= uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& planeB= uf::physics::impl::create(world, objB, pod::Plane{{0,0,1},0.0f}, 0.0f); + + pod::Manifold m; + bool collided = planePlane(planeA,planeB,m); + EXPECT_TRUE(!collided); // always false in your engine +}) + + +TEST(PlaneCapsule_Collision, { + pod::World world; + uf::Object objA, objB; + auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + + cap.transform->position = {0,0.25f,0}; // foot intersecting + + pod::Manifold m; + bool collided = planeCapsule(plane,cap,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(PlaneCapsule_NoCollision, { + pod::World world; + uf::Object objA, objB; + auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + + cap.transform->position = {0,5,0}; // far above + + pod::Manifold m; + bool collided = planeCapsule(plane,cap,m); + EXPECT_TRUE(!collided); +}) +TEST(MeshSphere_Collision, { + pod::World world; + uf::Object objMesh, objSphere; + + // Create mesh body (a plane on Y=0, size=1) + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); // static mesh + meshBody.transform->position = {0,0,0}; + + // Sphere just above plane, radius 1, intersects + auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{2.0f}, 1.0f); + sphereBody.transform->position = {0,0.5f,0}; // half below plane (since plane is at y=0) + + pod::Manifold m; + bool collided = meshSphere(meshBody, sphereBody, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); + if ( !m.points.empty() ) EXPECT_TRUE(m.points[0].penetration > 0.0f); +}) + +TEST(MeshSphere_NoCollision, { + pod::World world; + uf::Object objMesh, objSphere; + + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; + + auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{0.5f}, 1.0f); + sphereBody.transform->position = {0,5.0f,0}; // far above plane + + pod::Manifold m; + bool collided = meshSphere(meshBody, sphereBody, m); + EXPECT_FALSE(collided); +}) + +TEST(MeshAabb_Collision, { + pod::World world; + uf::Object objMesh, objBox; + + auto mesh = ::generateMesh(2.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; + + pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; + auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); + boxBody.transform->position = {0,0.25f,0}; // overlaps plane + + pod::Manifold m; + bool collided = meshAabb(meshBody, boxBody, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); +}) + +TEST(MeshAabb_NoCollision, { + pod::World world; + uf::Object objMesh, objBox; + + auto mesh = ::generateMesh(2.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; + + pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; + auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); + boxBody.transform->position = {0,5.0f,0}; // above plane, no overlap + + pod::Manifold m; + bool collided = meshAabb(meshBody, boxBody, m); + EXPECT_FALSE(collided); +}) + +TEST(RayMesh_Hit, { + pod::World world; + uf::Object objMesh; + + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; + + pod::Ray ray{ {0,1,0}, {0,-1,0} }; // from above, pointing down + pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); + + EXPECT_TRUE(hit.hit); + EXPECT_TRUE(hit.contact.penetration > 0.0f); +}) + +TEST(RayMesh_Miss, { + pod::World world; + uf::Object objMesh; + + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; + + pod::Ray ray{ {0,2,0}, {1,0,0} }; // parallel, goes sideways + pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); + + EXPECT_FALSE(hit.hit); +}) + +TEST(MeshMesh_Collision, { + pod::World world; + uf::Object objA, objB; + + auto mesh = ::generateMesh(1.0f); + + auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); + auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); + + meshA.transform->position = {0,0,0}; + meshB.transform->position = {0,0,0}; // same location + + pod::Manifold m; + bool collided = meshMesh(meshA, meshB, m); + EXPECT_TRUE(collided); +}) + +TEST(MeshMesh_NoCollision, { + pod::World world; + uf::Object objA, objB; + + auto mesh = ::generateMesh(1.0f); + + auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); + auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); + + meshA.transform->position = {0,0,0}; + meshB.transform->position = {0,10.0f,0}; // too far apart + + pod::Manifold m; + bool collided = meshMesh(meshA, meshB, m); + EXPECT_FALSE(collided); +}) \ No newline at end of file diff --git a/engine/src/utils/mesh/mesh.cpp b/engine/src/utils/mesh/mesh.cpp index 12421700..28b28494 100644 --- a/engine/src/utils/mesh/mesh.cpp +++ b/engine/src/utils/mesh/mesh.cpp @@ -362,6 +362,57 @@ std::string uf::Mesh::printIndirects( bool full ) const { return str.str(); } +uf::Mesh::View uf::Mesh::makeView( const uf::stl::vector& wanted ) const { + uf::Mesh::View view; + view.vertex = vertex; + view.index = index; + + if ( wanted.size() ) { + for ( auto& attr : vertex.attributes ) { + if ( std::find(wanted.begin(), wanted.end(), attr.descriptor.name ) == wanted.end() ) continue; + view.attributes[attr.descriptor.name] = { attr }; + } + } else { + for ( auto& attr : vertex.attributes ) view.attributes[attr.descriptor.name] = { attr }; + } + + if ( !index.attributes.empty() ) { + view.attributes["index"] = { index.attributes.front() }; + } + + return view; +} +uf::Mesh::View uf::Mesh::makeView( size_t i, const uf::stl::vector& wanted ) const { + uf::Mesh::View view; + view.vertex = remapVertexInput(i); + view.index = remapIndexInput(i); + view.indirectIndex = i; + + if ( wanted.size() ) { + for (auto& attr : vertex.attributes) { + if ( std::find(wanted.begin(), wanted.end(), attr.descriptor.name ) == wanted.end() ) continue; + view.attributes[attr.descriptor.name] = { attr }; + } + } else { + for ( auto& attr : vertex.attributes ) view.attributes[attr.descriptor.name] = { attr }; + } + + if ( !index.attributes.empty() ) { + view.attributes["index"] = { index.attributes.front() }; + } + + return view; +} +uf::stl::vector uf::Mesh::makeViews( const uf::stl::vector& wanted ) const { + uf::stl::vector views; + if ( indirect.count > 0 ) { + for ( auto i = 0; i < indirect.count; i++ ) views.emplace_back(makeView(i, wanted)); + } else { + views.emplace_back( makeView(wanted) ); + } + return views; +} + uf::Mesh::Input uf::Mesh::remapInput( const uf::Mesh::Input& input, size_t i ) const { uf::Mesh::Input res = input; UF_ASSERT( &input == &vertex || &input == &index ); diff --git a/engine/src/utils/tests/tests.cpp b/engine/src/utils/tests/tests.cpp new file mode 100644 index 00000000..f5227f61 --- /dev/null +++ b/engine/src/utils/tests/tests.cpp @@ -0,0 +1,28 @@ +#include + + +uf::stl::string uf::unitTests::current; +uf::stl::KeyMap uf::unitTests::tests; + +void uf::unitTests::execute() { + auto testCount = uf::unitTests::tests.keys.size(); + + if ( !testCount ) return; + + UF_MSG_DEBUG("Running {} tests...", testCount); + for ( auto& name : uf::unitTests::tests.keys ) { + uf::unitTests::current = name; + uf::unitTests::tests[name].function(); + } + UF_MSG_DEBUG(""); + int pass = 0; + int fail = 0; + for ( auto& name : uf::unitTests::tests.keys ) { + auto& test = uf::unitTests::tests[name]; + auto& value = test.passes ? pass : fail; + ++value; + UF_MSG_DEBUG("[{}]: [{}]: {}", test.passes ? "PASS" : "FAIL", name, test.message); + } + UF_MSG_DEBUG("{} / {} tests passed.", pass, testCount ); + UF_MSG_DEBUG("{} / {} tests failed.", fail, testCount ); +} \ No newline at end of file diff --git a/ext/main.cpp b/ext/main.cpp index f88edd76..ca871ddf 100644 --- a/ext/main.cpp +++ b/ext/main.cpp @@ -1,8 +1,11 @@ #include #include -void EXT_API ext::initialize() { +// perform unit tests +#include +void EXT_API ext::initialize() { + uf::unitTests::execute(); } void EXT_API ext::tick() {