From 7c25020781da71bcdc2550a1979c66a2feae5502 Mon Sep 17 00:00:00 2001 From: Canberk Pitirli Date: Sun, 4 Oct 2026 20:40:25 +0300 Subject: [PATCH] feat: physics contact callbacks, trigger fix, real ray and overlap queries Pairs are collected first and resolved second with fresh poses. Triggers no longer push, dynamic pairs split the correction by inverse mass, and touching pairs are tracked across steps to send OnCollision/OnTrigger Enter/Stay/Exit to every component on both objects. Raycast walks the tree's ray query and tests the actual shapes; the overlap queries do a narrowphase and return OverlapHit. Colliders cache their rigidbody and transform keyed on a new GameObject component version. --- include/public/Core/GameObject.hpp | 13 + include/public/ECS/Component.hpp | 17 +- include/public/Physics/ColliderComponent.hpp | 28 +- include/public/Physics/CollisionDetection.hpp | 27 ++ include/public/Physics/DynamicAABBTree.hpp | 3 +- include/public/Physics/PhysicsWorld.hpp | 83 +++- include/public/Physics/RigidbodyComponent.hpp | 3 + src/Physics/ColliderComponent.cpp | 51 +- src/Physics/CollisionDetection.cpp | 237 ++++++++++ src/Physics/PhysicsWorld.cpp | 445 +++++++++++++----- src/Physics/RigidbodyComponent.cpp | 12 +- 11 files changed, 767 insertions(+), 152 deletions(-) diff --git a/include/public/Core/GameObject.hpp b/include/public/Core/GameObject.hpp index 19085363..628c2816 100644 --- a/include/public/Core/GameObject.hpp +++ b/include/public/Core/GameObject.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -76,6 +77,7 @@ namespace Sleak { if (bIsInitialized) newComponent->Initialize(); if (m_isActive && bIsInitialized) newComponent->OnEnable(); Components.add(std::move(newComponent)); + ++m_componentVersion; } /// Destroys and detaches the first component of type T, if present. @@ -87,6 +89,7 @@ namespace Sleak { if (dynamic_cast(Components[i].get()) != nullptr) { Components[i]->OnDestroy(); Components.erase(i); + ++m_componentVersion; break; } } @@ -109,6 +112,15 @@ namespace Sleak { return nullptr; } + /// Every attached component, in the order they were added. + const List>& GetComponents() const { + return Components; + } + + /// Changes whenever a component is added or removed, so callers can + /// cache GetComponent() results and know when to look again. + uint32_t GetComponentVersion() const { return m_componentVersion; } + /// True if a component of type T is attached. template bool HasComponent() { @@ -162,6 +174,7 @@ namespace Sleak { std::string m_tag = "Untagged"; List> Components; + uint32_t m_componentVersion = 0; // Hierarchy GameObject* m_parent; diff --git a/include/public/ECS/Component.hpp b/include/public/ECS/Component.hpp index a10a52ea..eacb421a 100644 --- a/include/public/ECS/Component.hpp +++ b/include/public/ECS/Component.hpp @@ -7,6 +7,7 @@ namespace Sleak { class GameObject; + namespace Physics { struct CollisionEvent; } /// Base for behavior attached to a GameObject. Owner deletes it via /// GameObject::RemoveComponent(), never directly. /// @@ -15,7 +16,9 @@ namespace Sleak { /// parameter, because GameObject::AddComponent() supplies it and /// forwards the rest. Initialize() and Update() are pure virtual; /// FixedUpdate(), LateUpdate(), OnEnable(), OnDisable(), and - /// OnDestroy() are optional. + /// OnDestroy() are optional, as are the OnCollision and OnTrigger + /// callbacks the physics world sends to every component on an object + /// whose collider starts, keeps, or stops touching another. /// /// The owning GameObject holds components in a RefPtr and destroys them /// with itself, so never delete a component directly. Reach the owner @@ -73,6 +76,18 @@ namespace Sleak { virtual void OnEnable() {} virtual void OnDisable() {} + /// Sent on the first physics step this object's collider touches + /// another solid collider. + virtual void OnCollisionEnter(const Physics::CollisionEvent&) {} + /// Sent on every later physics step the two keep touching. + virtual void OnCollisionStay(const Physics::CollisionEvent&) {} + /// Sent on the first physics step they no longer touch. + virtual void OnCollisionExit(const Physics::CollisionEvent&) {} + /// Same as OnCollisionEnter when either collider is a trigger. + virtual void OnTriggerEnter(const Physics::CollisionEvent&) {} + virtual void OnTriggerStay(const Physics::CollisionEvent&) {} + virtual void OnTriggerExit(const Physics::CollisionEvent&) {} + GameObject* GetOwner() { return owner; } diff --git a/include/public/Physics/ColliderComponent.hpp b/include/public/Physics/ColliderComponent.hpp index 0d13cfbc..732df989 100644 --- a/include/public/Physics/ColliderComponent.hpp +++ b/include/public/Physics/ColliderComponent.hpp @@ -4,10 +4,15 @@ #include #include #include +#include namespace Sleak { struct MeshData; + class Camera; + class RigidbodyComponent; + class TransformComponent; + namespace Physics { class PhysicsWorld; } /// Attaches a collision shape to a GameObject and registers it with PhysicsWorld's broadphase. /// @@ -27,7 +32,8 @@ namespace Sleak { /// API, so put triggers, terrain, and characters on distinct layers and /// a raycast can ignore the ones it does not care about. Marking a /// collider as a trigger keeps it in the broadphase while skipping - /// physical response. + /// physical response; overlaps then arrive as OnTriggerEnter/Stay/Exit + /// on the components of both objects. /// /// @code{.cpp} /// // Static level geometry @@ -81,6 +87,15 @@ namespace Sleak { /// Local shape transformed into world space by the owner's current transform. Physics::AABB GetWorldAABB() const; + /// Where the shape sits in the world: owner position plus offset, + /// and owner scale. + void GetWorldPose(Math::Vector3D& position, + Math::Vector3D& scale) const; + + /// Rigidbody on the same object, or null. Cached until the owner's + /// components change. + RigidbodyComponent* GetRigidbody() const; + // Offset from owner's transform void SetOffset(const Math::Vector3D& offset) { m_offset = offset; } Math::Vector3D GetOffset() const { return m_offset; } @@ -100,6 +115,12 @@ namespace Sleak { int GetProxyId() const { return m_proxyId; } private: + friend class Physics::PhysicsWorld; + + void RefreshCache() const; + /// Moves the owner through its transform, or a camera's position. + void TranslateOwner(const Math::Vector3D& delta); + Physics::ColliderShape m_shape; Physics::ColliderType m_type = Physics::ColliderType::AABB; Math::Vector3D m_offset; @@ -107,6 +128,11 @@ namespace Sleak { uint32_t m_mask = 0xFFFFFFFF; bool m_isTrigger = false; int m_proxyId = -1; + + mutable RigidbodyComponent* m_body = nullptr; + mutable TransformComponent* m_transform = nullptr; + mutable Camera* m_camera = nullptr; + mutable uint32_t m_cacheVersion = UINT32_MAX; }; } // namespace Sleak diff --git a/include/public/Physics/CollisionDetection.hpp b/include/public/Physics/CollisionDetection.hpp index 9efd3937..5e53e8e6 100644 --- a/include/public/Physics/CollisionDetection.hpp +++ b/include/public/Physics/CollisionDetection.hpp @@ -44,6 +44,33 @@ namespace Physics { CollisionManifold TestSphereVsTriangle(const BoundingSphere& sphere, const Vector3D& v0, const Vector3D& v1, const Vector3D& v2); + /// Ray against a world-space box. dir must be unit length; a ray starting + /// inside the box reports no hit. On a hit, t is the distance along dir + /// and normal is the surface normal facing the ray. + bool RaycastAABB(const AABB& box, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal); + /// Ray against a world-space sphere, same contract as RaycastAABB(). + bool RaycastSphere(const BoundingSphere& sphere, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal); + /// Ray against a world-space capsule, same contract as RaycastAABB(). + bool RaycastCapsule(const BoundingCapsule& capsule, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal); + /// Two-sided ray against one triangle. dir need not be unit length; t is + /// in units of dir. + bool RaycastTriangle(const Vector3D& v0, const Vector3D& v1, + const Vector3D& v2, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal); + /// Ray against a collider shape placed at pos with scale, the same way + /// TestCollision() places it. dir must be unit length. + bool RaycastShape(const ColliderShape& shape, const Vector3D& pos, + const Vector3D& scale, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal); + } // namespace Physics } // namespace Sleak diff --git a/include/public/Physics/DynamicAABBTree.hpp b/include/public/Physics/DynamicAABBTree.hpp index acbf2bd7..b1426f46 100644 --- a/include/public/Physics/DynamicAABBTree.hpp +++ b/include/public/Physics/DynamicAABBTree.hpp @@ -1,6 +1,7 @@ #ifndef _DYNAMIC_AABB_TREE_HPP_ #define _DYNAMIC_AABB_TREE_HPP_ +#include #include #include #include @@ -26,7 +27,7 @@ namespace Physics { /// Broadphase AABB tree; PhysicsWorld inserts colliders as proxies and queries overlaps against it. /// @ingroup physics - class DynamicAABBTree { + class ENGINE_API DynamicAABBTree { public: DynamicAABBTree(); ~DynamicAABBTree() = default; diff --git a/include/public/Physics/PhysicsWorld.hpp b/include/public/Physics/PhysicsWorld.hpp index fe4a15c8..26922983 100644 --- a/include/public/Physics/PhysicsWorld.hpp +++ b/include/public/Physics/PhysicsWorld.hpp @@ -1,6 +1,7 @@ #ifndef _PHYSICS_WORLD_HPP_ #define _PHYSICS_WORLD_HPP_ +#include #include #include #include @@ -21,6 +22,29 @@ namespace Sleak { CollisionManifold manifold; }; + /// One collider found by OverlapSphere() or OverlapAABB(). + /// @ingroup physics + struct OverlapHit { + ColliderComponent* collider = nullptr; + Vector3D point; + /// Points from the query shape toward the collider. + Vector3D normal; + float penetration = 0.0f; + }; + + /// What a component receives in OnCollision* and OnTrigger* callbacks. + /// Point, normal, and penetration are zero on Exit. + /// @ingroup physics + struct CollisionEvent { + /// The collider on the object receiving the callback. + ColliderComponent* self = nullptr; + ColliderComponent* other = nullptr; + Vector3D point; + /// Points from self toward other. + Vector3D normal; + float penetration = 0.0f; + }; + /// Result of a single Raycast call. /// @ingroup physics struct RayHit { @@ -53,7 +77,17 @@ namespace Sleak { /// /// Step() integrates every dynamic RigidbodyComponent using that /// body's own gravity setting, refreshes the DynamicAABBTree - /// broadphase, then finds and resolves overlapping pairs. + /// broadphase, then finds and resolves overlapping pairs. Only pairs + /// where at least one side has a non-static rigidbody are tested. + /// Two dynamic bodies split the correction by inverse mass, a + /// kinematic body pushes a dynamic one without being pushed, and a + /// trigger never pushes anything. + /// + /// After resolving, the world compares this step's touching pairs + /// with the last step's and calls OnCollisionEnter/Stay/Exit, or + /// OnTriggerEnter/Stay/Exit when either side is a trigger, on every + /// component of both objects. Pairs involving a collider that gets + /// unregistered (its object removed or destroyed) end without an Exit. /// /// The part you will use directly is the query API. Raycast(), /// SphereSweep(), OverlapSphere(), and OverlapAABB() all take an @@ -76,15 +110,15 @@ namespace Sleak { /// // Everything inside a blast radius /// auto caught = world->OverlapSphere( /// Sleak::Math::Vector3D(0.0f, 1.0f, 0.0f), 4.0f); - /// for (const auto& pair : caught) { - /// ApplyDamage(pair.b->GetOwner()); + /// for (const auto& hit : caught) { + /// ApplyDamage(hit.collider->GetOwner()); /// } /// @endcode /// /// @see ColliderComponent, RigidbodyComponent, DynamicAABBTree, - /// RayHit, SweepResult, CollisionPair + /// RayHit, SweepResult, OverlapHit, CollisionEvent /// @ingroup physics - class PhysicsWorld { + class ENGINE_API PhysicsWorld { public: PhysicsWorld() = default; ~PhysicsWorld() = default; @@ -98,23 +132,50 @@ namespace Sleak { void RegisterCollider(ColliderComponent* collider); void UnregisterCollider(ColliderComponent* collider); - /// Query API: colliders overlapping a sphere, filtered by layerMask. - std::vector OverlapSphere(const Vector3D& center, float radius, uint32_t layerMask = 0xFFFFFFFF) const; - /// Colliders overlapping an AABB, filtered by layerMask. - std::vector OverlapAABB(const AABB& aabb, uint32_t layerMask = 0xFFFFFFFF) const; + /// Query API: colliders whose shape overlaps a sphere, filtered + /// by layerMask. + std::vector OverlapSphere( + const Vector3D& center, float radius, + uint32_t layerMask = 0xFFFFFFFF) const; + /// Colliders whose shape overlaps an AABB, filtered by layerMask. + std::vector OverlapAABB( + const AABB& aabb, uint32_t layerMask = 0xFFFFFFFF) const; /// Sweeps a sphere from start along direction and returns the first collider it hits within maxDist. SweepResult SphereSweep(const Vector3D& start, const Vector3D& direction, float radius, float maxDist, uint32_t layerMask = 0xFFFFFFFF) const; - /// Casts a ray and returns the closest collider hit within maxDist. + /// Casts a ray against the collider shapes and returns the closest + /// hit within maxDist. direction need not be normalized, distance + /// is in world units, and colliders containing origin are skipped. RayHit Raycast(const Vector3D& origin, const Vector3D& direction, float maxDist, uint32_t layerMask = 0xFFFFFFFF) const; private: + struct TouchingPair { + ColliderComponent* a = nullptr; + ColliderComponent* b = nullptr; + uint64_t key = 0; + bool trigger = false; + ContactPoint contact; + }; + + std::vector OverlapShape(const ColliderShape& query, + const AABB& bounds, + uint32_t layerMask) const; + void Integrate(float dt); void UpdateBroadphase(); - void FindPairsAndResolve(); + void CollectPairs(); + void ResolvePairs(); + void DispatchEvents(); + bool RemovedDuringDispatch(const ColliderComponent* collider) const; DynamicAABBTree m_tree; std::vector m_colliders; + + std::vector m_candidates; + std::vector m_touching; + std::vector m_prevTouching; + std::vector m_removedDuringDispatch; + bool m_dispatching = false; }; } // namespace Physics diff --git a/include/public/Physics/RigidbodyComponent.hpp b/include/public/Physics/RigidbodyComponent.hpp index b07094e6..c838b26e 100644 --- a/include/public/Physics/RigidbodyComponent.hpp +++ b/include/public/Physics/RigidbodyComponent.hpp @@ -73,6 +73,9 @@ namespace Sleak { /// Pushes the body out along normal by penetration and updates ground/wall collision state. void ResolveCollision(const Math::Vector3D& normal, float penetration); + /// Records a contact along normal and cancels velocity into it, + /// without moving the body. ResolveCollision() calls this after moving. + void ApplyContactNormal(const Math::Vector3D& normal); BodyType GetBodyType() const { return m_bodyType; } void SetBodyType(BodyType type) { m_bodyType = type; } diff --git a/src/Physics/ColliderComponent.cpp b/src/Physics/ColliderComponent.cpp index 01262898..7f594c92 100644 --- a/src/Physics/ColliderComponent.cpp +++ b/src/Physics/ColliderComponent.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -84,18 +85,48 @@ void ColliderComponent::Update(float /*deltaTime*/) { // Collider shape is static relative to owner - no per-frame work needed } -Physics::AABB ColliderComponent::GetWorldAABB() const { - Math::Vector3D worldPos; - Math::Vector3D worldScale(1, 1, 1); - - auto* transform = const_cast(owner)->GetComponent(); - if (transform) { - worldPos = transform->GetWorldPosition() + m_offset; - worldScale = transform->GetWorldScale(); - } else if (auto* cam = dynamic_cast(owner)) { - worldPos = cam->GetPosition() + m_offset; +void ColliderComponent::RefreshCache() const { + if (!owner) return; + uint32_t version = owner->GetComponentVersion(); + if (version == m_cacheVersion) return; + + m_body = owner->GetComponent(); + m_transform = owner->GetComponent(); + m_camera = m_transform ? nullptr : dynamic_cast(owner); + m_cacheVersion = version; +} + +RigidbodyComponent* ColliderComponent::GetRigidbody() const { + RefreshCache(); + return m_body; +} + +void ColliderComponent::GetWorldPose(Math::Vector3D& position, + Math::Vector3D& scale) const { + RefreshCache(); + position = m_offset; + scale = Math::Vector3D(1, 1, 1); + if (m_transform) { + position = m_transform->GetWorldPosition() + m_offset; + scale = m_transform->GetWorldScale(); + } else if (m_camera) { + position = m_camera->GetPosition() + m_offset; } +} + +void ColliderComponent::TranslateOwner(const Math::Vector3D& delta) { + RefreshCache(); + if (m_transform) { + m_transform->Translate(delta); + } else if (m_camera) { + m_camera->AddPosition(delta); + } +} +Physics::AABB ColliderComponent::GetWorldAABB() const { + Math::Vector3D worldPos; + Math::Vector3D worldScale; + GetWorldPose(worldPos, worldScale); return Physics::GetWorldAABB(m_shape, worldPos, worldScale); } diff --git a/src/Physics/CollisionDetection.cpp b/src/Physics/CollisionDetection.cpp index 2f2e5124..73bc70ef 100644 --- a/src/Physics/CollisionDetection.cpp +++ b/src/Physics/CollisionDetection.cpp @@ -373,5 +373,242 @@ CollisionManifold TestCollision(const ColliderShape& shapeA, const Vector3D& pos }, shapeA, shapeB); } +/// Slab test for any ray direction length; reports the entry distance and axis. +static bool RaySlab(const AABB& box, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& tEnter, + int& enterAxis) { + float tmin = 0.0f; + float tmax = maxDist; + enterAxis = -1; + + const float o[3] = {origin.GetX(), origin.GetY(), origin.GetZ()}; + const float d[3] = {dir.GetX(), dir.GetY(), dir.GetZ()}; + const float lo[3] = {box.min.GetX(), box.min.GetY(), box.min.GetZ()}; + const float hi[3] = {box.max.GetX(), box.max.GetY(), box.max.GetZ()}; + + for (int i = 0; i < 3; ++i) { + if (std::abs(d[i]) < 1e-12f) { + if (o[i] < lo[i] || o[i] > hi[i]) return false; + continue; + } + float inv = 1.0f / d[i]; + float t1 = (lo[i] - o[i]) * inv; + float t2 = (hi[i] - o[i]) * inv; + if (t1 > t2) std::swap(t1, t2); + if (t1 > tmin) { + tmin = t1; + enterAxis = i; + } + tmax = std::min(tmax, t2); + if (tmin > tmax) return false; + } + + tEnter = tmin; + return true; +} + +bool RaycastAABB(const AABB& box, const Vector3D& origin, const Vector3D& dir, + float maxDist, float& t, Vector3D& normal) { + int axis = -1; + float tEnter = 0.0f; + if (!RaySlab(box, origin, dir, maxDist, tEnter, axis)) return false; + if (axis < 0) return false; // origin inside the box + + const float d[3] = {dir.GetX(), dir.GetY(), dir.GetZ()}; + float n[3] = {0.0f, 0.0f, 0.0f}; + n[axis] = d[axis] > 0.0f ? -1.0f : 1.0f; + + t = tEnter; + normal = Vector3D(n[0], n[1], n[2]); + return true; +} + +bool RaycastSphere(const BoundingSphere& sphere, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal) { + Vector3D m = origin - sphere.center; + float c = m.Dot(m) - sphere.radius * sphere.radius; + if (c <= 0.0f) return false; // origin inside + + float b = m.Dot(dir); + if (b > 0.0f) return false; + + float disc = b * b - c; + if (disc < 0.0f) return false; + + float hitT = -b - std::sqrt(disc); + if (hitT < 0.0f) hitT = 0.0f; + if (hitT > maxDist) return false; + + t = hitT; + if (sphere.radius > 1e-8f) { + normal = (origin + dir * hitT - sphere.center) * (1.0f / sphere.radius); + } else { + normal = dir * -1.0f; + } + return true; +} + +bool RaycastCapsule(const BoundingCapsule& capsule, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal) { + Vector3D pa = capsule.GetPointB(); + Vector3D pb = capsule.GetPointA(); + float r = capsule.radius; + + Vector3D toOrigin = origin - ClosestPointOnSegment(origin, pa, pb); + if (toOrigin.Dot(toOrigin) <= r * r) return false; // origin inside + + float best = std::numeric_limits::max(); + + Vector3D ba = pb - pa; + float baba = ba.Dot(ba); + if (baba > 1e-12f) { + Vector3D oa = origin - pa; + float bard = ba.Dot(dir); + float baoa = ba.Dot(oa); + float rdoa = dir.Dot(oa); + float oaoa = oa.Dot(oa); + float a = baba - bard * bard; + if (a > 1e-8f * baba) { + float b = baba * rdoa - baoa * bard; + float c = baba * oaoa - baoa * baoa - r * r * baba; + float h = b * b - a * c; + if (h >= 0.0f) { + float hitT = (-b - std::sqrt(h)) / a; + float y = baoa + hitT * bard; + if (hitT >= 0.0f && y > 0.0f && y < baba) best = hitT; + } + } + } + + float capT = 0.0f; + Vector3D capN; + if (RaycastSphere(BoundingSphere(pa, r), origin, dir, maxDist, capT, + capN) && + capT < best) { + best = capT; + } + if (RaycastSphere(BoundingSphere(pb, r), origin, dir, maxDist, capT, + capN) && + capT < best) { + best = capT; + } + + if (best > maxDist) return false; + + Vector3D hit = origin + dir * best; + Vector3D out = hit - ClosestPointOnSegment(hit, pa, pb); + float len = out.Magnitude(); + t = best; + normal = len > 1e-8f ? out * (1.0f / len) : dir * -1.0f; + return true; +} + +bool RaycastTriangle(const Vector3D& v0, const Vector3D& v1, const Vector3D& v2, + const Vector3D& origin, const Vector3D& dir, float maxDist, + float& t, Vector3D& normal) { + Vector3D e1 = v1 - v0; + Vector3D e2 = v2 - v0; + Vector3D p = dir.Cross(e2); + float det = e1.Dot(p); + if (std::abs(det) < 1e-12f) return false; + + float invDet = 1.0f / det; + Vector3D s = origin - v0; + float u = s.Dot(p) * invDet; + if (u < 0.0f || u > 1.0f) return false; + + Vector3D q = s.Cross(e1); + float v = dir.Dot(q) * invDet; + if (v < 0.0f || u + v > 1.0f) return false; + + float hitT = e2.Dot(q) * invDet; + if (hitT < 0.0f || hitT > maxDist) return false; + + Vector3D n = e1.Cross(e2); + if (n.Dot(dir) > 0.0f) n = n * -1.0f; + float len = n.Magnitude(); + if (len < 1e-12f) return false; + + t = hitT; + normal = n * (1.0f / len); + return true; +} + +/// Ray against a mesh, done in the mesh's local space so scale needs no copy. +static bool RaycastMesh(const TriangleMesh& mesh, const Vector3D& pos, + const Vector3D& scale, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal) { + float sx = scale.GetX(), sy = scale.GetY(), sz = scale.GetZ(); + if (std::abs(sx) < 1e-12f || std::abs(sy) < 1e-12f || std::abs(sz) < 1e-12f) + return false; + + Vector3D rel = origin - pos; + Vector3D localOrigin(rel.GetX() / sx, rel.GetY() / sy, rel.GetZ() / sz); + Vector3D localDir(dir.GetX() / sx, dir.GetY() / sy, dir.GetZ() / sz); + + // t is shared between spaces because the ray is mapped affinely + float boundsT = 0.0f; + int axis = -1; + if (!RaySlab(mesh.bounds, localOrigin, localDir, maxDist, boundsT, axis)) + return false; + + bool hit = false; + float best = maxDist; + Vector3D bestLocalN; + for (size_t i = 0; i + 2 < mesh.indices.size(); i += 3) { + uint32_t i0 = mesh.indices[i], i1 = mesh.indices[i + 1], + i2 = mesh.indices[i + 2]; + if (i0 >= mesh.vertices.size() || i1 >= mesh.vertices.size() || + i2 >= mesh.vertices.size()) + continue; + float triT = 0.0f; + Vector3D triN; + if (RaycastTriangle(mesh.vertices[i0], mesh.vertices[i1], + mesh.vertices[i2], localOrigin, localDir, best, + triT, triN)) { + best = triT; + bestLocalN = triN; + hit = true; + } + } + if (!hit) return false; + + Vector3D n(bestLocalN.GetX() / sx, bestLocalN.GetY() / sy, + bestLocalN.GetZ() / sz); + float len = n.Magnitude(); + if (len < 1e-12f) return false; + n = n * (1.0f / len); + if (n.Dot(dir) > 0.0f) n = n * -1.0f; + + t = best; + normal = n; + return true; +} + +bool RaycastShape(const ColliderShape& shape, const Vector3D& pos, + const Vector3D& scale, const Vector3D& origin, + const Vector3D& dir, float maxDist, float& t, + Vector3D& normal) { + if (auto* box = std::get_if(&shape)) { + return RaycastAABB(TransformAABB(*box, pos, scale), origin, dir, + maxDist, t, normal); + } + if (auto* sphere = std::get_if(&shape)) { + return RaycastSphere(TransformSphere(*sphere, pos, scale), origin, dir, + maxDist, t, normal); + } + if (auto* capsule = std::get_if(&shape)) { + return RaycastCapsule(TransformCapsule(*capsule, pos, scale), origin, + dir, maxDist, t, normal); + } + if (auto* mesh = std::get_if(&shape)) { + return RaycastMesh(*mesh, pos, scale, origin, dir, maxDist, t, normal); + } + return false; +} + } // namespace Physics } // namespace Sleak diff --git a/src/Physics/PhysicsWorld.cpp b/src/Physics/PhysicsWorld.cpp index 0ce5ad31..f6b77dbd 100644 --- a/src/Physics/PhysicsWorld.cpp +++ b/src/Physics/PhysicsWorld.cpp @@ -2,8 +2,6 @@ #include #include #include -#include -#include #include #include #include @@ -11,6 +9,65 @@ namespace Sleak { namespace Physics { +namespace { + +enum class ContactCallback { Enter, Stay, Exit }; + +uint64_t PairKey(int proxyA, int proxyB) { + return (static_cast(static_cast(proxyA)) << 32) | + static_cast(proxyB); +} + +bool IsMovable(const RigidbodyComponent* rb) { + return rb && rb->GetBodyType() != BodyType::Static; +} + +/// Share of the separation each body takes; zero means it does not move. +void SeparationShares(RigidbodyComponent* rbA, RigidbodyComponent* rbB, + float& shareA, float& shareB) { + bool movA = IsMovable(rbA); + bool movB = IsMovable(rbB); + shareA = shareB = 0.0f; + + if (movA && movB) { + bool dynA = rbA->GetBodyType() == BodyType::Dynamic; + bool dynB = rbB->GetBodyType() == BodyType::Dynamic; + if (dynA && dynB) { + float invA = 1.0f / rbA->GetMass(); + float invB = 1.0f / rbB->GetMass(); + shareA = invA / (invA + invB); + shareB = 1.0f - shareA; + } else if (dynA) { + shareA = 1.0f; + } else if (dynB) { + shareB = 1.0f; + } else { + shareA = shareB = 0.5f; + } + } else if (movA) { + shareA = 1.0f; + } else if (movB) { + shareB = 1.0f; + } +} + +void Invoke(Component* c, bool trigger, ContactCallback kind, + const CollisionEvent& event) { + switch (kind) { + case ContactCallback::Enter: + trigger ? c->OnTriggerEnter(event) : c->OnCollisionEnter(event); + break; + case ContactCallback::Stay: + trigger ? c->OnTriggerStay(event) : c->OnCollisionStay(event); + break; + case ContactCallback::Exit: + trigger ? c->OnTriggerExit(event) : c->OnCollisionExit(event); + break; + } +} + +} // namespace + void PhysicsWorld::RegisterCollider(ColliderComponent* collider) { if (!collider) return; @@ -37,64 +94,71 @@ void PhysicsWorld::UnregisterCollider(ColliderComponent* collider) { m_colliders.erase( std::remove(m_colliders.begin(), m_colliders.end(), collider), m_colliders.end()); + + // A callback is walking the pair lists; DispatchEvents purges them after + if (m_dispatching) { + m_removedDuringDispatch.push_back(collider); + return; + } + + auto involves = [collider](const TouchingPair& p) { + return p.a == collider || p.b == collider; + }; + m_prevTouching.erase( + std::remove_if(m_prevTouching.begin(), m_prevTouching.end(), involves), + m_prevTouching.end()); } void PhysicsWorld::Step(float dt) { if (!(dt > 0.0f) || !std::isfinite(dt)) return; + Integrate(dt); + UpdateBroadphase(); + CollectPairs(); + ResolvePairs(); + DispatchEvents(); +} + +void PhysicsWorld::Integrate(float dt) { // We need wasGrounded BEFORE clearing, so gravity doesn't apply while standing for (auto* collider : m_colliders) { - if (auto* owner = collider->GetOwner()) { - auto* rb = owner->GetComponent(); - if (!rb) continue; - - bool wasGrounded = rb->IsGrounded(); - rb->ClearCollisionState(); - - if (rb->GetBodyType() == BodyType::Dynamic) { - // Reset grounded — collision detection will re-set it if still touching ground - rb->SetGrounded(false); - - if (rb->GetUseGravity()) { - Math::Vector3D vel = rb->GetVelocity(); - - if (!wasGrounded) { - // Airborne: apply full gravity - vel = vel + rb->GetGravity() * dt; - } else { - // Grounded: apply small downward force to maintain ground contact - // This ensures collision detection keeps finding the floor - // without causing visible jitter - if (vel.GetY() <= 0.0f) { - vel = Math::Vector3D(vel.GetX(), -0.5f, vel.GetZ()); - } - // If vel.Y > 0 (jumping), don't override — let the jump happen - } - - // Clamp to terminal velocity - float termVel = rb->GetTerminalVelocity(); - if (vel.GetY() < -termVel) { - vel = Math::Vector3D(vel.GetX(), -termVel, vel.GetZ()); - } - - rb->SetVelocity(vel); - } + auto* rb = collider->GetRigidbody(); + if (!rb) continue; + + bool wasGrounded = rb->IsGrounded(); + rb->ClearCollisionState(); + + if (rb->GetBodyType() != BodyType::Dynamic) continue; - Math::Vector3D vel = rb->GetVelocity(); - Math::Vector3D delta = vel * dt; + // Collision detection sets it again while still on the ground + rb->SetGrounded(false); - auto* transform = owner->GetComponent(); - if (transform) { - transform->Translate(delta); - } else if (auto* cam = dynamic_cast(owner)) { - cam->AddPosition(delta); + if (rb->GetUseGravity()) { + Math::Vector3D vel = rb->GetVelocity(); + + if (!wasGrounded) { + // Airborne: apply full gravity + vel = vel + rb->GetGravity() * dt; + } else { + // Grounded: a small downward velocity keeps collision + // detection finding the floor without visible jitter + if (vel.GetY() <= 0.0f) { + vel = Math::Vector3D(vel.GetX(), -0.5f, vel.GetZ()); } + // If vel.Y > 0 (jumping), leave it so the jump happens } + + // Clamp to terminal velocity + float termVel = rb->GetTerminalVelocity(); + if (vel.GetY() < -termVel) { + vel = Math::Vector3D(vel.GetX(), -termVel, vel.GetZ()); + } + + rb->SetVelocity(vel); } - } - UpdateBroadphase(); - FindPairsAndResolve(); + collider->TranslateOwner(rb->GetVelocity() * dt); + } } void PhysicsWorld::UpdateBroadphase() { @@ -107,109 +171,215 @@ void PhysicsWorld::UpdateBroadphase() { } } -void PhysicsWorld::FindPairsAndResolve() { - for (size_t i = 0; i < m_colliders.size(); ++i) { - ColliderComponent* colliderA = m_colliders[i]; - if (colliderA->GetProxyId() < 0) continue; +void PhysicsWorld::CollectPairs() { + m_candidates.clear(); + + for (auto* colliderA : m_colliders) { + int proxyA = colliderA->GetProxyId(); + if (proxyA < 0) continue; + bool movableA = IsMovable(colliderA->GetRigidbody()); AABB worldA = colliderA->GetWorldAABB(); - m_tree.Query(worldA, [&](int proxyId) -> bool { - auto* colliderB = static_cast(m_tree.GetUserData(proxyId)); - if (colliderB == colliderA) return true; + m_tree.Query(worldA, [&](int proxyB) -> bool { + if (proxyB <= proxyA) return true; - // Skip duplicate pairs: only process when A < B (pointer order) - if (colliderA > colliderB) return true; + auto* colliderB = + static_cast(m_tree.GetUserData(proxyB)); + if (!movableA && !IsMovable(colliderB->GetRigidbody())) return true; - // Layer filtering - if ((colliderA->GetLayer() & colliderB->GetMask()) == 0) return true; - if ((colliderB->GetLayer() & colliderA->GetMask()) == 0) return true; + if ((colliderA->GetLayer() & colliderB->GetMask()) == 0) + return true; + if ((colliderB->GetLayer() & colliderA->GetMask()) == 0) + return true; - // Get transform data - auto* ownerA = colliderA->GetOwner(); - auto* ownerB = colliderB->GetOwner(); - if (!ownerA || !ownerB) return true; + TouchingPair pair; + pair.a = colliderA; + pair.b = colliderB; + pair.key = PairKey(proxyA, proxyB); + m_candidates.push_back(pair); + return true; + }); + } - Vector3D posA, posB; - Vector3D scaleA(1, 1, 1), scaleB(1, 1, 1); + std::sort(m_candidates.begin(), m_candidates.end(), + [](const TouchingPair& l, const TouchingPair& r) { + return l.key < r.key; + }); +} - auto* transformA = ownerA->GetComponent(); - if (transformA) { - posA = transformA->GetWorldPosition() + colliderA->GetOffset(); - scaleA = transformA->GetWorldScale(); - } else if (auto* camA = dynamic_cast(ownerA)) { - posA = camA->GetPosition() + colliderA->GetOffset(); - } +void PhysicsWorld::ResolvePairs() { + m_touching.clear(); - auto* transformB = ownerB->GetComponent(); - if (transformB) { - posB = transformB->GetWorldPosition() + colliderB->GetOffset(); - scaleB = transformB->GetWorldScale(); - } else if (auto* camB = dynamic_cast(ownerB)) { - posB = camB->GetPosition() + colliderB->GetOffset(); - } + for (TouchingPair& pair : m_candidates) { + ColliderComponent* colliderA = pair.a; + ColliderComponent* colliderB = pair.b; - CollisionManifold manifold = TestCollision( - colliderA->GetShape(), posA, scaleA, - colliderB->GetShape(), posB, scaleB); + // Fresh poses, so an earlier pair's correction is seen by this one + Vector3D posA, scaleA, posB, scaleB; + colliderA->GetWorldPose(posA, scaleA); + colliderB->GetWorldPose(posB, scaleB); - if (!manifold.hasCollision) return true; + CollisionManifold manifold = + TestCollision(colliderA->GetShape(), posA, scaleA, + colliderB->GetShape(), posB, scaleB); + if (!manifold.hasCollision) continue; - // Skip if both are triggers - if (colliderA->IsTrigger() && colliderB->IsTrigger()) return true; + pair.trigger = colliderA->IsTrigger() || colliderB->IsTrigger(); + pair.contact = manifold.contact; + m_touching.push_back(pair); - // Resolve: push rigidbodies apart - auto* rbA = ownerA->GetComponent(); - auto* rbB = ownerB->GetComponent(); + if (pair.trigger) continue; - if (rbA && rbA->GetBodyType() != BodyType::Static) { - rbA->ResolveCollision(manifold.contact.normal * -1.0f, - manifold.contact.penetration); - } - if (rbB && rbB->GetBodyType() != BodyType::Static) { - rbB->ResolveCollision(manifold.contact.normal, - manifold.contact.penetration); - } + auto* rbA = colliderA->GetRigidbody(); + auto* rbB = colliderB->GetRigidbody(); - return true; - }); + float shareA = 0.0f, shareB = 0.0f; + SeparationShares(rbA, rbB, shareA, shareB); + + const Vector3D& normal = manifold.contact.normal; + float penetration = manifold.contact.penetration; + + if (IsMovable(rbA)) { + if (shareA > 0.0f) + colliderA->TranslateOwner(normal * (-penetration * shareA)); + rbA->ApplyContactNormal(normal * -1.0f); + } + if (IsMovable(rbB)) { + if (shareB > 0.0f) + colliderB->TranslateOwner(normal * (penetration * shareB)); + rbB->ApplyContactNormal(normal); + } } } -std::vector PhysicsWorld::OverlapSphere(const Vector3D& center, float radius, uint32_t layerMask) const { - std::vector results; - BoundingSphere sphere(center, radius); - AABB queryAABB = sphere.ToAABB(); +bool PhysicsWorld::RemovedDuringDispatch( + const ColliderComponent* collider) const { + return std::find(m_removedDuringDispatch.begin(), + m_removedDuringDispatch.end(), + collider) != m_removedDuringDispatch.end(); +} - m_tree.Query(queryAABB, [&](int proxyId) -> bool { - auto* collider = static_cast(m_tree.GetUserData(proxyId)); - if ((collider->GetLayer() & layerMask) == 0) return true; +void PhysicsWorld::DispatchEvents() { + m_dispatching = true; + m_removedDuringDispatch.clear(); + + // Stops as soon as a callback removes self's object, which deletes it + auto notify = [this](ColliderComponent* self, bool trigger, + ContactCallback kind, const CollisionEvent& event) { + GameObject* owner = self->GetOwner(); + if (!owner) return; + const auto& components = owner->GetComponents(); + for (size_t i = 0; i < components.GetSize(); ++i) { + if (Component* c = components[i].get()) + Invoke(c, trigger, kind, event); + if (RemovedDuringDispatch(self)) return; + } + }; + + auto send = [&](const TouchingPair& pair, ContactCallback kind) { + if (RemovedDuringDispatch(pair.a) || RemovedDuringDispatch(pair.b)) + return; + + CollisionEvent forA; + forA.self = pair.a; + forA.other = pair.b; + CollisionEvent forB; + forB.self = pair.b; + forB.other = pair.a; + if (kind != ContactCallback::Exit) { + forA.point = forB.point = pair.contact.point; + forA.penetration = forB.penetration = pair.contact.penetration; + forA.normal = pair.contact.normal; + forB.normal = pair.contact.normal * -1.0f; + } - CollisionPair pair; - pair.b = collider; - results.push_back(pair); - return true; - }); + notify(pair.a, pair.trigger, kind, forA); + if (RemovedDuringDispatch(pair.a) || RemovedDuringDispatch(pair.b)) + return; + notify(pair.b, pair.trigger, kind, forB); + }; + + // Both lists are sorted by key, so one merge pass finds enter/stay/exit + size_t prev = 0, cur = 0; + while (prev < m_prevTouching.size() || cur < m_touching.size()) { + if (cur == m_touching.size() || + (prev < m_prevTouching.size() && + m_prevTouching[prev].key < m_touching[cur].key)) { + send(m_prevTouching[prev++], ContactCallback::Exit); + } else if (prev == m_prevTouching.size() || + m_touching[cur].key < m_prevTouching[prev].key) { + send(m_touching[cur++], ContactCallback::Enter); + } else { + const TouchingPair& before = m_prevTouching[prev++]; + const TouchingPair& now = m_touching[cur++]; + if (before.trigger != now.trigger) { + send(before, ContactCallback::Exit); + send(now, ContactCallback::Enter); + } else { + send(now, ContactCallback::Stay); + } + } + } - return results; + m_dispatching = false; + std::swap(m_prevTouching, m_touching); + m_touching.clear(); + + for (const ColliderComponent* removed : m_removedDuringDispatch) { + m_prevTouching.erase( + std::remove_if(m_prevTouching.begin(), m_prevTouching.end(), + [removed](const TouchingPair& p) { + return p.a == removed || p.b == removed; + }), + m_prevTouching.end()); + } + m_removedDuringDispatch.clear(); } -std::vector PhysicsWorld::OverlapAABB(const AABB& aabb, uint32_t layerMask) const { - std::vector results; +std::vector PhysicsWorld::OverlapShape(const ColliderShape& query, + const AABB& bounds, + uint32_t layerMask) const { + std::vector results; + const Vector3D origin(0, 0, 0); + const Vector3D unitScale(1, 1, 1); - m_tree.Query(aabb, [&](int proxyId) -> bool { - auto* collider = static_cast(m_tree.GetUserData(proxyId)); + m_tree.Query(bounds, [&](int proxyId) -> bool { + auto* collider = + static_cast(m_tree.GetUserData(proxyId)); if ((collider->GetLayer() & layerMask) == 0) return true; - CollisionPair pair; - pair.b = collider; - results.push_back(pair); + Vector3D pos, scale; + collider->GetWorldPose(pos, scale); + CollisionManifold m = TestCollision(query, origin, unitScale, + collider->GetShape(), pos, scale); + if (!m.hasCollision) return true; + + OverlapHit hit; + hit.collider = collider; + hit.point = m.contact.point; + hit.normal = m.contact.normal; + hit.penetration = m.contact.penetration; + results.push_back(hit); return true; }); return results; } +std::vector PhysicsWorld::OverlapSphere(const Vector3D& center, + float radius, + uint32_t layerMask) const { + if (!(radius >= 0.0f)) return {}; + BoundingSphere sphere(center, radius); + return OverlapShape(sphere, sphere.ToAABB(), layerMask); +} + +std::vector PhysicsWorld::OverlapAABB(const AABB& aabb, + uint32_t layerMask) const { + return OverlapShape(aabb, aabb, layerMask); +} + SweepResult PhysicsWorld::SphereSweep(const Vector3D& start, const Vector3D& direction, float radius, float maxDist, uint32_t layerMask) const { SweepResult result; @@ -299,14 +469,39 @@ SweepResult PhysicsWorld::SphereSweep(const Vector3D& start, const Vector3D& dir RayHit PhysicsWorld::Raycast(const Vector3D& origin, const Vector3D& direction, float maxDist, uint32_t layerMask) const { - // Raycast is sphere sweep with radius 0, but use direct ray-AABB for better precision - SweepResult sweep = SphereSweep(origin, direction, 0.01f, maxDist, layerMask); RayHit result; - result.hit = sweep.hit; - result.collider = sweep.collider; - result.point = sweep.point; - result.normal = sweep.normal; - result.distance = sweep.distance; + + float lenSq = direction.Dot(direction); + if (!(lenSq > 1e-12f) || !(maxDist > 0.0f)) return result; + Vector3D dir = direction * (1.0f / std::sqrt(lenSq)); + + float closest = maxDist; + + m_tree.RayCast(origin, dir, maxDist, [&](int proxyId) -> bool { + auto* collider = + static_cast(m_tree.GetUserData(proxyId)); + if ((collider->GetLayer() & layerMask) == 0) return true; + + Vector3D pos, scale; + collider->GetWorldPose(pos, scale); + + float t = 0.0f; + Vector3D normal; + if (!RaycastShape(collider->GetShape(), pos, scale, origin, dir, + closest, t, normal)) + return true; + + if (!result.hit || t < closest) { + closest = t; + result.hit = true; + result.collider = collider; + result.distance = t; + result.point = origin + dir * t; + result.normal = normal; + } + return true; + }); + return result; } diff --git a/src/Physics/RigidbodyComponent.cpp b/src/Physics/RigidbodyComponent.cpp index 6bb27ed4..026ffe63 100644 --- a/src/Physics/RigidbodyComponent.cpp +++ b/src/Physics/RigidbodyComponent.cpp @@ -29,9 +29,6 @@ void RigidbodyComponent::ClearCollisionState() { void RigidbodyComponent::ResolveCollision(const Math::Vector3D& normal, float penetration) { if (m_bodyType == BodyType::Static) return; - m_lastCollisionNormal = normal; - m_hadCollision = true; - // Position correction — push out of the colliding object Math::Vector3D correction = normal * penetration; @@ -42,6 +39,15 @@ void RigidbodyComponent::ResolveCollision(const Math::Vector3D& normal, float pe cam->AddPosition(correction); } + ApplyContactNormal(normal); +} + +void RigidbodyComponent::ApplyContactNormal(const Math::Vector3D& normal) { + if (m_bodyType == BodyType::Static) return; + + m_lastCollisionNormal = normal; + m_hadCollision = true; + if (m_bodyType != BodyType::Dynamic) return; // Classify collision: ground (normal.y > 0.7) vs wall