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 f8ea165f..1170f46d 100644 --- a/src/Physics/PhysicsWorld.cpp +++ b/src/Physics/PhysicsWorld.cpp @@ -1,4 +1,3 @@ -#include #include #include #include @@ -11,6 +10,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 +95,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 +172,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 +470,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