nodes/physics.py

Part of Q1K3.

  1"""AABB-vs-block collision integrator for Q1K3 port.
  2
  3Mirrors upstream's `entity._update_physics` algorithm:
  4
  5- Cap each step to 16 units (so fast projectiles don't tunnel walls).
  6- Per axis (x, then z, then y):
  7  - Try the move along that axis.
  8  - If it collides, restore previous coordinate and zero/reflect velocity.
  9  - On the X / Z axes attempt a "step up" if the entity has a step_height
 10    and is on the ground.
 11- After the move, apply gravity and friction *outside* this function (the
 12  caller updates `entity.v` first, we only translate).
 13
 14The caller passes:
 15- ``entity``: any object with .p (Vec3), .v (Vec3), .s (Vec3 half-extents),
 16              ._on_ground (bool), ._step_height (float), ._bounciness (float),
 17              ._gravity (float), and friction coefficient ``.f``.
 18- ``world``:  exposes ``block_at(p)``, ``block_at_box(min, max)`` and an
 19              entity list filtered by group.
 20
 21Returns: list[(axis, other_entity_or_None)] of "did collide" notifications
 22the caller can dispatch to entity-specific reaction methods.
 23"""
 24
 25from __future__ import annotations
 26
 27import math
 28from typing import TYPE_CHECKING
 29
 30from simvx.core import Vec3
 31
 32if TYPE_CHECKING:  # pragma: no cover
 33    from .world import MapData
 34
 35
 36GRAVITY = -1200.0
 37
 38
 39class PhysicsBody:
 40    """Mixin for entities subject to AABB-vs-block physics.
 41
 42    Subclasses set ``self.s`` (half-extents Vec3), ``self._step_height``,
 43    ``self._bounciness``, ``self._gravity`` and ``self.f`` (friction).
 44    """
 45
 46    s: Vec3
 47    p: Vec3
 48    v: Vec3
 49    a: Vec3
 50    f: float
 51    _on_ground: bool
 52    _step_height: float
 53    _bounciness: float
 54    _gravity: float
 55    _stepped_up_at: float
 56    _check_against: int  # group id of entities to test for sphere collision
 57    _did_collide_axis: int = -1
 58    _did_collide_with: object | None = None
 59
 60    def _physics_init(self) -> None:
 61        self.s = Vec3(2, 2, 2)
 62        self.p = getattr(self, "p", Vec3())
 63        self.v = Vec3()
 64        self.a = Vec3()
 65        self.f = 0.0
 66        self._on_ground = False
 67        self._step_height = 0.0
 68        self._bounciness = 0.0
 69        self._gravity = 1.0
 70        self._stepped_up_at = 0.0
 71        self._check_against = 0  # 0 = no entity-vs-entity collision
 72
 73    def did_collide(self, axis: int) -> None:
 74        """Override in subclasses to react to wall/ground hits.
 75
 76        ``axis`` = 0 for X-axis wall, 1 for ground/ceiling, 2 for Z-axis wall.
 77        """
 78
 79    def did_collide_with_entity(self, other) -> None:
 80        """Override in subclasses to react to entity-vs-entity overlap."""
 81
 82
 83def update_physics(
 84    entity: PhysicsBody,
 85    world: MapData,
 86    enemy_list: list[PhysicsBody],
 87    friendly_list: list[PhysicsBody],
 88    dt: float,
 89    game_time: float,
 90) -> None:
 91    """Update entity velocity and position one frame; dispatch collision
 92    callbacks. Caller is responsible for setting ``entity.a`` (acceleration
 93    excluding gravity) before invoking us, gravity is added here.
 94    """
 95    # Gravity
 96    entity.a = Vec3(entity.a.x, GRAVITY * entity._gravity, entity.a.z)
 97
 98    # Integrate acceleration & friction into velocity
 99    ff = min(entity.f * dt, 1.0)
100    entity.v = Vec3(
101        entity.v.x + entity.a.x * dt - entity.v.x * ff,
102        entity.v.y + entity.a.y * dt,  # no horizontal-style friction on Y
103        entity.v.z + entity.a.z * dt - entity.v.z * ff,
104    )
105
106    # Pick the entity list we collide against (groups: 1 = friendly, 2 = enemy)
107    if entity._check_against == 1:
108        check_entities = friendly_list
109    elif entity._check_against == 2:
110        check_entities = enemy_list
111    else:
112        check_entities = []
113
114    # Step the move to ≤ 16 units per sub-step so fast projectiles don't tunnel.
115    move_dist = entity.v * dt
116    move_len = float(move_dist.length())
117    steps = max(1, int(math.ceil(move_len / 16.0)))
118    move_step = move_dist * (1.0 / steps)
119
120    original_step_height = entity._step_height
121
122    # Match upstream `entity.js`: each sub-step runs all three axis tests.
123    # After a collision, upstream sets `s = steps` so this is the LAST
124    # iteration but the current iteration still finishes. We replicate by
125    # using a flag.
126    s = 0
127    while s < steps:
128        lp = Vec3(entity.p.x, entity.p.y, entity.p.z)
129        entity.p = entity.p + move_step
130        last_iteration = False
131
132        # X-axis wall
133        if _collides(entity, Vec3(entity.p.x, lp.y, lp.z), world, check_entities):
134            if (
135                not entity._step_height
136                or not entity._on_ground
137                or entity.v.y > 0
138                or _collides(entity, Vec3(entity.p.x, lp.y + entity._step_height, lp.z), world, check_entities)
139            ):
140                entity.did_collide(0)
141                entity.p = Vec3(lp.x, entity.p.y, entity.p.z)
142                entity.v = Vec3(-entity.v.x * entity._bounciness, entity.v.y, entity.v.z)
143            else:
144                lp = Vec3(lp.x, lp.y + entity._step_height, lp.z)
145                entity._stepped_up_at = game_time
146            last_iteration = True
147
148        # Z-axis wall
149        if _collides(entity, Vec3(entity.p.x, lp.y, entity.p.z), world, check_entities):
150            if (
151                not entity._step_height
152                or not entity._on_ground
153                or entity.v.y > 0
154                or _collides(entity, Vec3(entity.p.x, lp.y + entity._step_height, entity.p.z), world, check_entities)
155            ):
156                entity.did_collide(2)
157                entity.p = Vec3(entity.p.x, entity.p.y, lp.z)
158                entity.v = Vec3(entity.v.x, entity.v.y, -entity.v.z * entity._bounciness)
159            else:
160                lp = Vec3(lp.x, lp.y + entity._step_height, lp.z)
161                entity._stepped_up_at = game_time
162            last_iteration = True
163
164        # Y-axis (ground/ceiling)
165        if _collides(entity, entity.p, world, check_entities):
166            entity.did_collide(1)
167            entity.p = Vec3(entity.p.x, lp.y, entity.p.z)
168            bounce = entity._bounciness if abs(entity.v.y) > 200 else 0.0
169            entity._on_ground = entity.v.y < 0 and bounce == 0.0
170            entity.v = Vec3(entity.v.x, -entity.v.y * bounce, entity.v.z)
171            last_iteration = True
172
173        entity._step_height = original_step_height
174        if last_iteration:
175            break
176        s += 1
177
178    entity._step_height = original_step_height
179
180
181def _collides(
182    entity: PhysicsBody,
183    p: Vec3,
184    world: MapData,
185    check_entities: list[PhysicsBody],
186) -> bool:
187    """Sphere-vs-entity OR AABB-vs-block test at hypothetical position *p*."""
188    # Entity-vs-entity (sphere on entity.s.y radius)
189    sy = float(entity.s.y)
190    for other in check_entities:
191        if other is entity:
192            continue
193        d = Vec3(p.x - other.p.x, p.y - other.p.y, p.z - other.p.z)
194        dist2 = float(d.x) ** 2 + float(d.y) ** 2 + float(d.z) ** 2
195        rad = sy + float(other.s.y)
196        if dist2 < rad * rad:
197            entity._step_height = 0.0
198            entity.did_collide_with_entity(other)
199            return True
200
201    # AABB-vs-block
202    half = entity.s
203    bmin = Vec3(p.x - half.x, p.y - half.y, p.z - half.z)
204    bmax = Vec3(p.x + half.x, p.y + half.y, p.z + half.z)
205    return world.block_at_box(bmin, bmax)
206
207
208def trace_los(world: MapData, a: Vec3, b: Vec3) -> bool:
209    """Step from *a* to *b* in 16-unit increments; return True if a block is hit
210    (= line of sight is blocked). Mirrors upstream `map_trace`."""
211    diff = Vec3(b.x - a.x, b.y - a.y, b.z - a.z)
212    length = math.sqrt(float(diff.x) ** 2 + float(diff.y) ** 2 + float(diff.z) ** 2)
213    if length < 1e-6:
214        return False
215    inv = 16.0 / length
216    step = Vec3(float(diff.x) * inv, float(diff.y) * inv, float(diff.z) * inv)
217    steps = int(math.ceil(length / 16.0))
218    cur = Vec3(a.x, a.y, a.z)
219    for _ in range(steps):
220        cur = cur + step
221        if world.block_at(cur):
222            return True
223    return False