hippocampaif/core_knowledge/physics_system.py

"""
Physics System — Core Knowledge of Intuitive Physics

Hardcoded priors on world dynamics — "believed, not computed."
These are NOT physics simulations; they are the brain's innate
expectations about how the physical world behaves.

Implements:
1. Gravity: Objects fall downward (constant acceleration prior)
2. Friction: Moving objects slow down without force
3. Mass: Heavier objects resist acceleration
4. Elasticity: Objects bounce on collision
5. Support: Unsupported objects fall

Critical for Breakout: ball trajectory prediction without full physics engine.

Reference: Spelke (1990), Baillargeon (1987)
Author: Algorembrant, Rembrant Oyangoren Albeos (2026)
"""

import numpy as np


class PhysicsState:
    """Physical state of an object."""
    
    __slots__ = ['position', 'velocity', 'mass', 'elasticity', 
                 'is_supported', 'radius']
    
    def __init__(self, position: np.ndarray, velocity: np.ndarray = None,
                 mass: float = 1.0, elasticity: float = 0.8, radius: float = 0.5):
        self.position = np.asarray(position, dtype=np.float64)
        self.velocity = np.zeros_like(self.position) if velocity is None else np.asarray(velocity, dtype=np.float64)
        self.mass = mass
        self.elasticity = elasticity
        self.is_supported = False
        self.radius = radius


class PhysicsSystem:
    """
    Innate physics engine — the brain's "believed" physics.
    
    This is NOT a real physics simulator. It's the set of innate
    expectations that infants have about how objects behave.
    Violations of these expectations create surprise signals
    (analogous to infants looking longer at impossible events).
    """
    
    def __init__(self, gravity: float = 9.8, friction: float = 0.02, dt: float = 0.016):
        """
        Args:
            gravity: Gravitational acceleration (downward).
            friction: Kinetic friction coefficient.
            dt: Time step for physics predictions.
        """
        self.gravity = gravity
        self.friction = friction
        self.dt = dt
    
    def predict_trajectory(self, state: PhysicsState, steps: int = 10,
                           bounds: tuple = None) -> list[np.ndarray]:
        """
        Predict the future trajectory of an object using intuitive physics.
        
        This is how the brain predicts where a ball will go in Breakout —
        not by solving equations, but by "feeling" the trajectory based
        on innate gravity, friction, and bounce priors.
        
        Args:
            state: Current physical state of the object.
            steps: Number of future timesteps to predict.
            bounds: Optional (min_pos, max_pos) for bounce boundaries.
            
        Returns:
            List of predicted positions.
        """
        pos = state.position.copy()
        vel = state.velocity.copy()
        trajectory = [pos.copy()]
        
        gravity_vec = np.zeros_like(pos)
        if len(pos) >= 2:
            gravity_vec[1] = self.gravity  # Downward
        
        for _ in range(steps):
            # --- GRAVITY: Objects accelerate downward ---
            if not state.is_supported:
                vel += gravity_vec * self.dt
            
            # --- FRICTION: Moving objects slow down ---
            speed = np.linalg.norm(vel)
            if speed > 0.01:
                friction_force = -self.friction * vel / speed * state.mass
                vel += friction_force * self.dt / state.mass
            
            # --- UPDATE POSITION ---
            pos = pos + vel * self.dt
            
            # --- BOUNCE at boundaries (elasticity) ---
            if bounds is not None:
                min_b, max_b = bounds
                min_b = np.asarray(min_b, dtype=np.float64)
                max_b = np.asarray(max_b, dtype=np.float64)
                for dim in range(len(pos)):
                    if pos[dim] - state.radius < min_b[dim]:
                        pos[dim] = min_b[dim] + state.radius
                        vel[dim] = -vel[dim] * state.elasticity
                    elif pos[dim] + state.radius > max_b[dim]:
                        pos[dim] = max_b[dim] - state.radius
                        vel[dim] = -vel[dim] * state.elasticity
            
            trajectory.append(pos.copy())
        
        return trajectory
    
    def check_support(self, obj_pos: np.ndarray, obj_radius: float,
                      surfaces: list[dict]) -> bool:
        """
        Check if an object is supported by a surface.
        
        Innate prior: unsupported objects fall. Infants expect this.
        
        Args:
            obj_pos: Object center position.
            surfaces: List of dicts with 'y' (surface height), 'x_min', 'x_max'.
            
        Returns:
            True if supported, False if should fall.
        """
        for surface in surfaces:
            surface_y = surface['y']
            x_min = surface.get('x_min', -float('inf'))
            x_max = surface.get('x_max', float('inf'))
            
            # Object is on this surface if:
            # 1. Object bottom is at or below surface level
            # 2. Object center x is within surface extent
            obj_bottom = obj_pos[1] + obj_radius  # y-down convention
            if (abs(obj_bottom - surface_y) < obj_radius * 0.5 and 
                x_min <= obj_pos[0] <= x_max):
                return True
        
        return False
    
    def predict_collision(self, state_a: PhysicsState, state_b: PhysicsState) -> dict:
        """
        Predict if and when two objects will collide.
        
        Innate contact principle: objects cannot pass through each other.
        
        Returns:
            Dict with 'will_collide' (bool), 'time' (float), 'position' (ndarray).
        """
        # Relative position and velocity
        rel_pos = state_b.position - state_a.position
        rel_vel = state_b.velocity - state_a.velocity
        min_dist = state_a.radius + state_b.radius
        
        # Currently overlapping?
        current_dist = np.linalg.norm(rel_pos)
        if current_dist <= min_dist:
            return {
                'will_collide': True,
                'time': 0.0,
                'position': (state_a.position + state_b.position) / 2.0
            }
        
        # Compute closest approach time via quadratic
        a_coeff = np.dot(rel_vel, rel_vel)
        if a_coeff < 1e-10:
            return {'will_collide': False, 'time': float('inf'), 'position': None}
        
        b_coeff = 2.0 * np.dot(rel_pos, rel_vel)
        c_coeff = np.dot(rel_pos, rel_pos) - min_dist**2
        
        discriminant = b_coeff**2 - 4.0 * a_coeff * c_coeff
        if discriminant < 0:
            return {'will_collide': False, 'time': float('inf'), 'position': None}
        
        t1 = (-b_coeff - np.sqrt(discriminant)) / (2.0 * a_coeff)
        t2 = (-b_coeff + np.sqrt(discriminant)) / (2.0 * a_coeff)
        
        t = t1 if t1 > 0 else t2
        if t < 0:
            return {'will_collide': False, 'time': float('inf'), 'position': None}
        
        collision_pos = state_a.position + state_a.velocity * t
        return {
            'will_collide': True,
            'time': float(t),
            'position': collision_pos
        }
    
    def resolve_collision(self, state_a: PhysicsState, 
                          state_b: PhysicsState) -> tuple[np.ndarray, np.ndarray]:
        """
        Resolve a collision between two objects using mass and elasticity priors.
        
        The brain's intuitive collision model: heavier objects push lighter ones,
        and things bounce based on material (elasticity prior).
        
        Returns:
            Tuple of (new_velocity_a, new_velocity_b).
        """
        # Normal vector
        normal = state_b.position - state_a.position
        dist = np.linalg.norm(normal)
        if dist < 1e-8:
            normal = np.array([1.0, 0.0]) if len(state_a.position) == 2 else np.array([1.0, 0.0, 0.0])
        else:
            normal = normal / dist
        
        # Relative velocity along normal
        rel_vel = state_a.velocity - state_b.velocity
        vel_normal = np.dot(rel_vel, normal)
        
        if vel_normal <= 0:
            # Objects already separating
            return state_a.velocity.copy(), state_b.velocity.copy()
        
        # Coefficient of restitution (average elasticity)
        e = (state_a.elasticity + state_b.elasticity) / 2.0
        
        # Impulse magnitude (conservation of momentum + restitution)
        j = -(1.0 + e) * vel_normal / (1.0 / state_a.mass + 1.0 / state_b.mass)
        
        new_vel_a = state_a.velocity + (j / state_a.mass) * normal
        new_vel_b = state_b.velocity - (j / state_b.mass) * normal
        
        return new_vel_a, new_vel_b
    
    def check_violation(self, expected_pos: np.ndarray, observed_pos: np.ndarray,
                        expected_exists: bool, observed_exists: bool) -> dict:
        """
        Check if a physical event violates intuitive physics expectations.
        
        This generates surprise signals — analogous to infant looking-time
        paradigms where babies look longer at "impossible" events.
        
        Returns:
            Dict with violation type and surprise magnitude.
        """
        violations = {}
        
        # Object disappeared (violates permanence + physics)
        if expected_exists and not observed_exists:
            violations['vanishing'] = 1.0
        
        # Object appeared from nowhere
        if not expected_exists and observed_exists:
            violations['spontaneous_generation'] = 0.8
        
        # Teleportation (violates continuity)
        if expected_exists and observed_exists:
            displacement = np.linalg.norm(
                np.asarray(observed_pos) - np.asarray(expected_pos)
            )
            if displacement > 10.0:  # Unreasonable jump
                violations['teleportation'] = min(1.0, displacement / 20.0)
        
        return violations