Stellar: 2D Physics Engine

Table of Contents

  1. Introduction
    • Overview
    • Prerequisites
    • What We’ll Build
  2. Understanding the Fundamentals
    • Vector Mathematics
    • Physics Concepts
  3. Basic Structure and Vector Operations
    • Vector Implementation
    • Essential Vector Operations
  4. Core Components
    • Body Definition
    • Shape Types
    • World Configuration

1. Introduction

Overview

Building a physics engine is an exciting journey into the world of game development and simulation. This tutorial will guide you through creating a 2D physics engine from scratch, explaining every component in detail. We’ll cover everything from basic vector mathematics to complex collision resolution.

Prerequisites

To follow this tutorial, you should have:

  • Intermediate knowledge of C++
  • Basic understanding of vector mathematics
  • Familiarity with fundamental physics concepts
  • A C++ development environment set up

What We’ll Build

We’re going to create a complete 2D physics engine with the following features:

  • Rigid body dynamics
  • Circle and AABB (Axis-Aligned Bounding Box) collision detection
  • Spatial partitioning for efficient collision detection
  • Impulse-based collision resolution
  • Gravity and force application
  • Position and velocity integration

2. Understanding the Fundamentals

Vector Mathematics

At the heart of our physics engine lies vector mathematics. We’ll be working extensively with 2D vectors for:

  • Position representation
  • Velocity calculations
  • Force applications
  • Collision detection and response

Physics Concepts

Before diving into the code, let’s understand the key physics concepts we’ll be implementing:

  • Newton’s laws of motion
  • Linear momentum
  • Impulse forces
  • Coefficient of restitution
  • Friction
  • Conservation of energy

3. Basic Structure and Vector Operations

Vector Implementation

Let’s start with our fundamental building block - the 2D vector structure:

struct vec2 {
    double x;
    double y;
};

This simple structure represents a point or vector in 2D space. We’ll build several operations around this basic type.

Essential Vector Operations

Let’s implement the core vector operations we’ll need throughout our engine:

// Vector addition
vec2 vec2_add(const vec2 &a, const vec2 &b) {
    return vec2{a.x + b.x, a.y + b.y};
}

// Vector subtraction
vec2 vec2_sub(const vec2 &a, const vec2 &b) {
    return vec2{a.x - b.x, a.y - b.y};
}

// Scalar multiplication
vec2 vec2_mul(const vec2 &a, double scalar) {
    return vec2{a.x * scalar, a.y * scalar};
}

// Dot product
double vec2_dot(const vec2 &a, const vec2 &b) {
    return a.x * b.x + a.y * b.y;
}

// Cross product (2D version returns scalar)
double vec2_cross(const vec2 &a, const vec2 &b) {
    return a.x * b.y - b.x * a.y;
}

Each of these operations serves a specific purpose:

  • vec2_add: Used for combining vectors, such as adding velocity to position
  • vec2_sub: Used for finding direction vectors between points
  • vec2_mul: Used for scaling vectors (e.g., applying force)
  • vec2_dot: Used for projection calculations and finding angles
  • vec2_cross: Used for determining rotational direction and area calculations

Additional utility functions:

// Vector normalization
void vec2_normalize(vec2 &a) {
    double length = sqrt(a.x * a.x + a.y * a.y);
    if (length > 0) {
        a.x /= length;
        a.y /= length;
    }
}

// Vector length calculation
double vec2_length(const vec2 &a) {
    return sqrt(a.x * a.x + a.y * a.y);
}

// Squared length (optimization for comparisons)
double vec2_length_squared(const vec2 &a) {
    return a.x * a.x + a.y * a.y;
}

// Vector rotation
void vec2_rotate(vec2 &a, const double &theta) {
    double original_x = a.x;
    a.x = original_x * cos(theta) - a.y * sin(theta);
    a.y = original_x * sin(theta) + a.y * cos(theta);
}

These utility functions are crucial for:

  • Normalizing vectors for direction calculations
  • Finding distances between objects
  • Optimizing calculations using squared lengths
  • Rotating vectors for angular motion

4. Core Components

Body Definition

The core of our physics engine is the Body structure, which represents any physical object in our simulation:

struct Body {
    vec2 position, velocity, acceleration, forceAccumulator;
    double mass, inverseMass, damping, angle;
    bool isStatic, isActive;
    
    enum ShapeType {
        SHAPE_CIRCLE,
        SHAPE_AABB,
    } shapeType;
    
    union {
        Circle circle;
        AABB aabb;
    } shape;
};

Let’s break down each component:

  • Position properties:
    • position: Current position in 2D space
    • angle: Rotation angle (in radians)
  • Motion properties:
    • velocity: Linear velocity
    • acceleration: Current acceleration
    • forceAccumulator: Sum of all forces acting on the body
  • Physical properties:
    • mass: Body mass
    • inverseMass: Precalculated 1/mass (0 for static bodies)
    • damping: Velocity damping factor
  • State flags:
    • isStatic: Whether the body can move
    • isActive: Whether the body participates in simulation
  • Shape information:
    • shapeType: Enum indicating circle or AABB
    • shape: Union of possible shape types

Shape Types

We support two basic shape types:

struct AABB {
    vec2 min, max;
};

struct Circle {
    vec2 center;
    double radius;
};

AABB (Axis-Aligned Bounding Box):

  • Defined by minimum and maximum points
  • Simple and efficient collision detection
  • Limited to rectangular shapes aligned with axes

Circle:

  • Defined by center point and radius
  • Uniform collision detection in all directions
  • Perfect for round objects

5. World Configuration and Management

World Structure

The physics world is our simulation container that manages all bodies and their interactions. Let’s examine its implementation:

struct WorldConfig {
    vec2 gravity;
    double maxDeltaTime;
    AABB bounds;

    WorldConfig() {
        gravity.x = 0.0;
        gravity.y = -9.81;  // Default Earth gravity
        maxDeltaTime = 0.016;  // ~60 FPS
        bounds.min = {-100.0, -100.0};
        bounds.max = {100.0, 100.0};
    }
};

struct World {
    vector<Body> bodies;
    WorldConfig config;
    int activeBodyCount{0};
    bool isPaused{false};
    vector<int> queryResultIndices;
    Grid grid;  // Spatial partitioning grid
};

Key components:

  • WorldConfig: Configuration parameters
    • gravity: Global gravity vector
    • maxDeltaTime: Maximum time step (prevents instability)
    • bounds: World boundaries
  • World: Main simulation container
    • bodies: Vector of all physics bodies
    • activeBodyCount: Number of non-static bodies
    • grid: Spatial partitioning system

World Management Functions

void initWorld(World &world, const WorldConfig config = WorldConfig{}) {
    world.bodies.clear();
    world.config = config;
    world.activeBodyCount = 0;
    world.isPaused = false;
    world.queryResultIndices.reserve(100);
    
    initGrid(world.grid, config.bounds, 10.0);
}

void clearWorld(World &world) {
    world.bodies.clear();
    world.activeBodyCount = 0;
    world.queryResultIndices.clear();
}

Adding and removing bodies:

int addBody(World &world, Body &body) {
    world.bodies.push_back(body);
    if (body.isActive)
        world.activeBodyCount++;
    return static_cast<int>(world.bodies.size() - 1);
}

void removeBody(World &world, int bodyIndex) {
    if (bodyIndex < 0 || bodyIndex >= world.bodies.size())
        return;
    if (world.bodies[bodyIndex].isActive) {
        world.activeBodyCount--;
    }
    if (bodyIndex < world.bodies.size() - 1) {
        world.bodies[bodyIndex] = world.bodies.back();
    }
    world.bodies.pop_back();
}

Helper Functions for Body Creation

int addCircle(World &world, vec2 position, double radius, double mass) {
    Body body{};
    body.position = position;
    body.velocity = {0, 0};
    body.acceleration = {0, 0};
    body.mass = mass;
    body.inverseMass = mass > 0.0 ? 1.0 / mass : 0.0;
    body.damping = 0.99;
    body.isStatic = mass <= 0;
    body.isActive = true;
    body.angle = 0.0;
    body.forceAccumulator = {0, 0};

    body.shapeType = Body::SHAPE_CIRCLE;
    body.shape.circle = {position, radius};

    return addBody(world, body);
}

int addBox(World &world, vec2 position, vec2 size, double mass) {
    Body body{};
    body.position = position;
    body.velocity = {0, 0};
    body.acceleration = {0, 0};
    body.mass = mass;
    body.inverseMass = mass > 0 ? 1.0 / mass : 0;
    body.damping = 0.99;
    body.isStatic = mass <= 0;
    body.isActive = true;
    body.angle = 0;

    body.shapeType = Body::SHAPE_AABB;
    body.shape.aabb = {
        {position.x - size.x / 2, position.y - size.y / 2},
        {position.x + size.x / 2, position.y + size.y / 2}
    };

    return addBody(world, body);
}

6. Force Application and Integration

Force Management

Forces are fundamental to physics simulation. We implement a force accumulator pattern:

void applyForce(Body &body, const vec2 &force) {
    if (body.isStatic || !body.isActive) {
        return;
    }
    body.forceAccumulator = vec2_add(body.forceAccumulator, force);
}

void applyImpulse(Body &body, const vec2 &impulse) {
    if (body.isStatic || !body.isActive) {
        return;
    }
    vec2 deltaVelocity = vec2_mul(impulse, body.inverseMass);
    body.velocity = vec2_add(body.velocity, deltaVelocity);
}

void clearForces(Body &body) {
    body.forceAccumulator = {0.0, 0.0};
}

Gravity Application

const vec2 GRAVITY{0.0, -9.81};

void applyGravity(Body &body) {
    if (body.isStatic || !body.isActive)
        return;
    vec2 gravityForce = vec2_mul(GRAVITY, body.mass);
    applyForce(body, gravityForce);
}

Physics Integration

We use semi-implicit Euler integration for updating positions and velocities:

void integrateLinearMotion(Body &body, double dt) {
    if (body.isStatic || !body.isActive)
        return;

    if (dt <= 0.0)
        return;

    // Calculate acceleration from accumulated forces
    body.acceleration = vec2_mul(body.forceAccumulator, body.inverseMass);
    
    // Update velocity
    vec2 deltaVelocity = vec2_mul(body.acceleration, dt);
    body.velocity = vec2_add(body.velocity, deltaVelocity);
    
    // Apply damping
    body.velocity = vec2_mul(body.velocity, pow(body.damping, dt));
    
    // Update position
    vec2 deltaP = vec2_add(
        vec2_mul(body.velocity, dt),
        vec2_mul(body.acceleration, 0.5 * dt * dt)
    );
    body.position = vec2_add(body.position, deltaP);
    
    // Update shape positions
    if (body.shapeType == Body::SHAPE_CIRCLE) {
        body.shape.circle.center = body.position;
    } else {
        vec2 halfSize = vec2_mul(
            vec2_sub(body.shape.aabb.max, body.shape.aabb.min),
            0.5
        );
        body.shape.aabb.min = vec2_sub(body.position, halfSize);
        body.shape.aabb.max = vec2_add(body.position, halfSize);
    }

    clearForces(body);
}

The integration process:

  1. Calculate acceleration from accumulated forces
  2. Update velocity using current acceleration
  3. Apply velocity damping to simulate air resistance
  4. Update position using velocity and acceleration
  5. Update shape-specific properties
  6. Clear accumulated forces for next frame

World Stepping

The main simulation step function:

void stepWorld(World &world, double dt) {
    if (world.isPaused)
        return;
        
    // Clamp dt to prevent instability
    dt = min(dt, world.config.maxDeltaTime);
    
    // Update physics
    updatePhysics(world, dt);
    
    // Update spatial partitioning
    updateBroadPhase(world.grid, world.bodies);
}

void updatePhysics(World &world, double dt) {
    for (auto &body : world.bodies) {
        if (body.isActive) {
            applyGravity(body);
            integrateLinearMotion(body, dt);
        }
    }
}

Utility Functions

double calculateArea(const Body &body) {
    if (body.shapeType == Body::SHAPE_CIRCLE) {
        return M_PI * body.shape.circle.radius * body.shape.circle.radius;
    } else {
        vec2 size = vec2_sub(body.shape.aabb.max, body.shape.aabb.min);
        return size.x * size.y;
    }
}

double calculateMOI(const Body &body) {
    if (body.shapeType == Body::SHAPE_CIRCLE) {
        return 0.5 * body.mass * body.shape.circle.radius * 
               body.shape.circle.radius;
    } else {
        vec2 size = vec2_sub(body.shape.aabb.max, body.shape.aabb.min);
        return body.mass * (size.x * size.x + size.y * size.y) / 12;
    }
}

AABB getShapeBoundingBox(const Body &body) {
    if (body.shapeType == Body::SHAPE_CIRCLE) {
        return {
            {body.position.x - body.shape.circle.radius,
             body.position.y - body.shape.circle.radius},
            {body.position.x + body.shape.circle.radius,
             body.position.y + body.shape.circle.radius}
        };
    }
    return body.shape.aabb;
}

7. Spatial Partitioning System

Understanding Spatial Partitioning

Before diving into the code, let’s understand why spatial partitioning is crucial:

  • In a physics simulation, checking collisions between every pair of objects (O(n²)) becomes extremely inefficient as the number of objects grows
  • Spatial partitioning divides the world into a grid of cells
  • Objects are mapped to grid cells based on their position
  • Only objects in the same or neighboring cells need to be checked for collisions
  • This reduces collision checks from O(n²) to O(n) in most cases

Grid Implementation

struct GridCell {
    vector<int> bodyIndex;  // Stores indices of bodies in this cell
};

struct Grid {
    vector<GridCell> cells;  // 1D array representing 2D grid
    double cellSize;        // Size of each cell
    int numRows, numCols;   // Grid dimensions
    vec2 worldMin, worldMax; // World boundaries
};

Let’s examine each component:

  • GridCell: Contains indices of bodies that overlap this cell
  • Grid: Main grid structure
    • cells: 1D vector representing 2D grid for cache efficiency
    • cellSize: Determines granularity of spatial partitioning
    • numRows/numCols: Grid dimensions
    • worldMin/worldMax: World boundaries for grid mapping

Grid Initialization and Management

void initGrid(Grid &grid, AABB &worldBound, double cellSize) {
    // Set grid boundaries
    grid.worldMin = worldBound.min;
    grid.worldMax = worldBound.max;
    grid.cellSize = cellSize;

    // Calculate grid dimensions
    vec2 worldSize = vec2_sub(worldBound.max, worldBound.min);
    grid.numRows = static_cast<int>(worldSize.x / cellSize);
    grid.numCols = static_cast<int>(worldSize.y / cellSize);
    
    // Initialize cells
    grid.cells.resize(grid.numRows * grid.numCols);
}

This function:

  1. Sets the grid boundaries based on world bounds
  2. Sets the cell size (important for performance tuning)
  3. Calculates how many rows and columns we need
  4. Resizes the cells vector to accommodate all cells

Grid Operations

Converting 2D grid coordinates to 1D array index:

int getCellIndex(const Grid &grid, int row, int col) {
    return row * grid.numCols + col;
}

Converting world position to grid coordinates:

void getGridCell(const Grid &grid, const vec2 &position, int &row, int &col) {
    // Calculate relative position from world minimum
    vec2 relativePos = vec2_sub(position, grid.worldMin);
    
    // Convert to grid coordinates
    col = static_cast<int>(relativePos.x / grid.cellSize);
    row = static_cast<int>(relativePos.y / grid.cellSize);
    
    // Clamp to grid boundaries
    col = max(0, min(col, grid.numCols - 1));
    row = max(0, min(row, grid.numRows - 1));
}

This function:

  1. Calculates position relative to grid origin (worldMin)
  2. Divides by cell size to get grid coordinates
  3. Clamps values to ensure they’re within grid bounds

Inserting Bodies into the Grid

void insertBodyIntoGrid(Grid &grid, int bodyIndex, const Body &body) {
    // Get body's bounding box
    AABB bounds = getShapeBoundingBox(body);

    // Calculate grid cells that contain the body
    int startRow, startCol, endRow, endCol;
    getGridCell(grid, bounds.min, startRow, startCol);
    getGridCell(grid, bounds.max, endRow, endCol);

    // Insert body index into all overlapping cells
    for (int row = startRow; row <= endRow; ++row) {
        for (int col = startCol; col <= endCol; ++col) {
            int cellIndex = getCellIndex(grid, row, col);
            grid.cells[cellIndex].bodyIndex.push_back(bodyIndex);
        }
    }
}

This function:

  1. Gets the body’s AABB (bounding box)
  2. Determines which grid cells the AABB overlaps
  3. Adds the body’s index to all overlapping cells
  4. A body can be in multiple cells if it spans cell boundaries

Updating the Broad Phase

void updateBroadPhase(Grid &grid, const vector<Body> &bodies) {
    // Clear previous frame's data
    clearGrid(grid);
    
    // Insert active bodies into grid
    for (size_t i = 0; i < bodies.size(); ++i) {
        if (bodies[i].isActive) {
            insertBodyIntoGrid(grid, i, bodies[i]);
        }
    }
}

8. Collision Detection System

Broad Phase Collision Detection

struct BroadPhaseCollisionPair {
    int bodyA;
    int bodyB;
};

vector<BroadPhaseCollisionPair> getPotentialCollisionPairs(const Grid &grid) {
    vector<BroadPhaseCollisionPair> pairs;
    
    // Helper function to create unique pair key
    auto createPairKey = [](int id1, int id2) -> uint64_t {
        if (id1 > id2)
            swap(id1, id2);
        return (static_cast<uint64_t>(id1) << 32) | static_cast<uint64_t>(id2);
    };
    
    // Track pairs we've already checked
    unordered_set<uint64_t> addedPairs;

    // Check each cell
    for (int row = 0; row < grid.numRows; ++row) {
        for (int col = 0; col < grid.numCols; ++col) {
            const auto &currentCell = grid.cells[getCellIndex(grid, row, col)];

            // Check pairs within the same cell
            for (size_t i = 0; i < currentCell.bodyIndex.size(); ++i) {
                for (size_t j = i + 1; j < currentCell.bodyIndex.size(); ++j) {
                    int id1 = currentCell.bodyIndex[i];
                    int id2 = currentCell.bodyIndex[j];
                    uint64_t pairKey = createPairKey(id1, id2);

                    // Add only if we haven't seen this pair before
                    if (addedPairs.insert(pairKey).second) {
                        pairs.push_back({id1, id2});
                    }
                }
            }

            // Direction vectors for neighboring cells
            const int dx[] = {1, 1, 0, -1};
            const int dy[] = {0, 1, 1, 1};

            // Check neighboring cells
            for (int dir = 0; dir < 4; ++dir) {
                int newCol = col + dx[dir];
                int newRow = row + dy[dir];

                // Skip if neighbor is out of bounds
                if (newCol >= 0 && newCol < grid.numCols && 
                    newRow >= 0 && newRow < grid.numRows) {
                    
                    const auto &adjacentCell = 
                        grid.cells[getCellIndex(grid, newRow, newCol)];

                    // Check pairs between current cell and neighbor
                    for (int id1 : currentCell.bodyIndex) {
                        for (int id2 : adjacentCell.bodyIndex) {
                            uint64_t pairKey = createPairKey(id1, id2);

                            if (addedPairs.insert(pairKey).second) {
                                pairs.push_back({id1, id2});
                            }
                        }
                    }
                }
            }
        }
    }

    return pairs;
}

This broad phase collision detection:

  1. Examines each grid cell
  2. Checks for potential collisions between bodies in the same cell
  3. Checks neighboring cells for additional potential collisions
  4. Uses a hash set to prevent duplicate pairs
  5. Returns all potential collision pairs for detailed checking

Narrow Phase Collision Detection

First, let’s implement the basic collision checks for different shape combinations:

bool circleCircleCollision(const Circle &c1, const Circle &c2) {
    // Calculate vector between centers
    vec2 diff = vec2_sub(c1.center, c2.center);
    
    // Compare squared distances (avoid square root)
    double distSq = vec2_length_squared(diff);
    double radiusSum = c1.radius + c2.radius;
    
    return distSq <= radiusSum * radiusSum;
}

bool aabbAabbCollision(const AABB &aabb1, const AABB &aabb2) {
    // Check for overlap on each axis
    if (aabb1.max.x < aabb2.min.x || aabb1.min.x > aabb2.max.x)
        return false;
    if (aabb1.max.y < aabb2.min.y || aabb1.min.y > aabb2.max.y)
        return false;
    return true;
}

bool circleAabbCollision(const Circle &c, const AABB &aabb) {
    // Find closest point on AABB to circle center
    double closestX = max(aabb.min.x, min(c.center.x, aabb.max.x));
    double closestY = max(aabb.min.y, min(c.center.y, aabb.max.y));

    // Calculate squared distance to closest point
    double distanceX = c.center.x - closestX;
    double distanceY = c.center.y - closestY;
    double distanceSquared = distanceX * distanceX + distanceY * distanceY;

    return distanceSquared <= (c.radius * c.radius);
}

Then we combine these in a general collision check function:

bool checkCollision(const Body &bodyA, const Body &bodyB) {
    if (bodyA.shapeType == Body::SHAPE_CIRCLE && 
        bodyB.shapeType == Body::SHAPE_CIRCLE) {
        return circleCircleCollision(bodyA.shape.circle, bodyB.shape.circle);
    } 
    else if (bodyA.shapeType == Body::SHAPE_AABB && 
             bodyB.shapeType == Body::SHAPE_AABB) {
        return aabbAabbCollision(bodyA.shape.aabb, bodyB.shape.aabb);
    }
    else if (bodyA.shapeType == Body::SHAPE_CIRCLE && 
             bodyB.shapeType == Body::SHAPE_AABB) {
        return circleAabbCollision(bodyA.shape.circle, bodyB.shape.aabb);
    }
    else if (bodyA.shapeType == Body::SHAPE_AABB && 
             bodyB.shapeType == Body::SHAPE_CIRCLE) {
        return circleAabbCollision(bodyB.shape.circle, bodyA.shape.aabb);
    }
    return false;
}

Finally, we filter the potential collision pairs to find actual collisions:

vector<BroadPhaseCollisionPair> getActualCollisions(
    const vector<BroadPhaseCollisionPair> &potentialPairs,
    const vector<Body> &bodies) {
    
    vector<BroadPhaseCollisionPair> actualCollisions;
    
    for (const auto &pair : potentialPairs) {
        const Body &bodyA = bodies[pair.bodyA];
        const Body &bodyB = bodies[pair.bodyB];
        
        if (checkCollision(bodyA, bodyB)) {
            actualCollisions.push_back(pair);
        }
    }
    
    return actualCollisions;
}

9. Collision Resolution System

Understanding Collision Resolution

Before diving into the code, let’s understand the key concepts of collision resolution:

  1. Collision Response: When two bodies collide, we need to:

    • Calculate impulse forces to change their velocities
    • Prevent bodies from interpenetrating
    • Apply friction and restitution effects

  2. Impulse-Based Physics:

    • Uses instantaneous changes in velocity
    • Preserves momentum and energy (with restitution)
    • More stable than force-based methods for collisions

  3. Position Correction:

    • Prevents objects from sinking into each other
    • Maintains simulation stability
    • Uses a percentage-based correction factor

Circle-Circle Collision Resolution

void resolveCircleCircleCollision(Body &a, Body &b) {
    // Calculate collision vector
    vec2 distance = vec2_sub(b.position, a.position);
    vec2 normalVector = distance;
    vec2_normalize(normalVector);
    
    // Calculate collision tangent for friction
    vec2 tangentVector = {-1.0 * normalVector.y, normalVector.x};

    // Calculate relative velocity
    vec2 relativeVelocity = vec2_sub(b.velocity, a.velocity);
    double velocitySeperation = vec2_dot(relativeVelocity, normalVector);

    // Skip if objects are moving apart
    if (velocitySeperation > 0)
        return;

    // Calculate impulse scalar
    double impulse = -1.0 * (1 + e) / (a.inverseMass + b.inverseMass) 
                    * velocitySeperation;

    // Apply impulse along normal vector
    a.velocity = vec2_sub(a.velocity, 
                         vec2_mul(normalVector, impulse * a.inverseMass));
    b.velocity = vec2_add(b.velocity, 
                         vec2_mul(normalVector, impulse * b.inverseMass));

    // Position correction to prevent sinking
    const double k = 0.4; // position correction factor
    double totalRadius = a.shape.circle.radius + b.shape.circle.radius;
    double penetrationDepth = totalRadius - vec2_length(distance);

    if (penetrationDepth > 0) {
        vec2 correction = vec2_mul(
            normalVector,
            (k * penetrationDepth) / (a.inverseMass + b.inverseMass)
        );

        // Only move if body isn't static
        if (!a.isStatic) {
            a.position = vec2_sub(a.position, 
                                vec2_mul(correction, a.inverseMass));
        }
        if (!b.isStatic) {
            b.position = vec2_add(b.position, 
                                vec2_mul(correction, b.inverseMass));
        }
    }
}

Let’s break down the circle-circle collision resolution:

  1. Normal Vector Calculation:

    • Calculate vector between circle centers
    • Normalize it to get collision normal
    • Calculate tangent vector for friction calculations

  2. Velocity Resolution:

    • Calculate relative velocity between bodies
    • Project velocity onto normal vector
    • Check if objects are moving apart (early exit if true)

  3. Impulse Calculation:

    • Use coefficient of restitution (e)
    • Consider inverse masses for proper impulse distribution
    • Apply impulse to both bodies’ velocities

  4. Position Correction:

    • Calculate penetration depth
    • Apply correction based on mass ratio
    • Use correction factor (k) to prevent overshooting

AABB-AABB Collision Resolution

void resolveAABBCollision(Body &a, Body &b) {
    // Calculate centers and half-sizes
    vec2 centerA = a.position;
    vec2 centerB = b.position;
    vec2 halfSizeA = vec2_mul(
        vec2_sub(a.shape.aabb.max, a.shape.aabb.min), 
        0.5
    );
    vec2 halfSizeB = vec2_mul(
        vec2_sub(b.shape.aabb.max, b.shape.aabb.min), 
        0.5
    );

    // Calculate delta between centers
    vec2 delta = vec2_sub(centerB, centerA);

    // Calculate overlap on each axis
    vec2 overlap = {
        halfSizeA.x + halfSizeB.x - fabs(delta.x),
        halfSizeA.y + halfSizeB.y - fabs(delta.y)
    };

    // Determine collision normal based on smallest overlap
    vec2 normal;
    double penetration;
    if (overlap.x > 0 && overlap.y > 0) {
        if (overlap.x < overlap.y) {
            normal = (delta.x < 0) ? 
                    (vec2){-1.0, 0.0} : (vec2){1.0, 0.0};
            penetration = overlap.x;
        } else {
            normal = (delta.y < 0) ? 
                    (vec2){0.0, -1.0} : (vec2){0.0, 1.0};
            penetration = overlap.y;
        }

        // Calculate relative velocity
        vec2 relativeVel = vec2_sub(b.velocity, a.velocity);
        double normalVel = vec2_dot(relativeVel, normal);

        // Early exit for objects moving apart
        const double MIN_VELOCITY = 0.01;
        if (normalVel > -MIN_VELOCITY) {
            // Objects moving apart or too slow - just correct position
            if (!a.isStatic) {
                a.position = vec2_sub(a.position, 
                                    vec2_mul(normal, penetration * 0.5));
            }
            if (!b.isStatic) {
                b.position = vec2_add(b.position, 
                                    vec2_mul(normal, penetration * 0.5));
            }
            return;
        }

        // Calculate impulse
        const double RESTITUTION = 0.3;
        double totalInverseMass = a.inverseMass + b.inverseMass;
        if (totalInverseMass <= 0)
            return;

        // Apply restitution based on velocity
        double restitution = vec2_length(relativeVel) < 2.0 ? 
                            0.0 : RESTITUTION;
        double j = -(1.0 + restitution) * normalVel / totalInverseMass;

        // Apply impulse
        vec2 impulse = vec2_mul(normal, j);
        if (!a.isStatic) {
            a.velocity = vec2_sub(a.velocity, 
                                vec2_mul(impulse, a.inverseMass));
        }
        if (!b.isStatic) {
            b.velocity = vec2_add(b.velocity, 
                                vec2_mul(impulse, b.inverseMass));
        }

        // Apply friction
        const double FRICTION = 0.4;
        vec2 tangent = {
            relativeVel.x - normalVel * normal.x,
            relativeVel.y - normalVel * normal.y
        };
        double tangentLen = vec2_length(tangent);
        
        if (tangentLen > MIN_VELOCITY) {
            tangent = vec2_mul(tangent, 1.0 / tangentLen);
            double jt = -vec2_dot(relativeVel, tangent) / totalInverseMass;

            // Apply friction impulse
            vec2 frictionImpulse = vec2_mul(tangent, jt * FRICTION);
            if (!a.isStatic) {
                a.velocity = vec2_sub(a.velocity, 
                                    vec2_mul(frictionImpulse, a.inverseMass));
            }
            if (!b.isStatic) {
                b.velocity = vec2_add(b.velocity, 
                                    vec2_mul(frictionImpulse, b.inverseMass));
            }
        }

        // Position correction
        const double CORRECTION_PERCENT = 0.8;
        vec2 correction = vec2_mul(
            normal,
            (penetration / totalInverseMass) * CORRECTION_PERCENT
        );

        if (!a.isStatic) {
            a.position = vec2_sub(a.position, 
                                vec2_mul(correction, a.inverseMass));
        }
        if (!b.isStatic) {
            b.position = vec2_add(b.position, 
                                vec2_mul(correction, b.inverseMass));
        }

        // Update AABB positions
        if (!a.isStatic) {
            vec2 halfSize = vec2_mul(
                vec2_sub(a.shape.aabb.max, a.shape.aabb.min), 
                0.5
            );
            a.shape.aabb.min = vec2_sub(a.position, halfSize);
            a.shape.aabb.max = vec2_add(a.position, halfSize);
        }

        if (!b.isStatic) {
            vec2 halfSize = vec2_mul(
                vec2_sub(b.shape.aabb.max, b.shape.aabb.min), 
                0.5
            );
            b.shape.aabb.min = vec2_sub(b.position, halfSize);
            b.shape.aabb.max = vec2_add(b.position, halfSize);
        }
    }
}

The AABB collision resolution process:

  1. Contact Point Determination:

    • Calculate box centers and half-sizes
    • Find overlap on each axis
    • Choose collision normal based on minimum overlap

  2. Collision Response:

    • Calculate relative velocity
    • Apply restitution based on velocity magnitude
    • Calculate and apply impulse
    • Handle friction using tangential impulse

  3. Advanced Features:

    • Velocity threshold for static friction
    • Variable restitution based on impact velocity
    • Proper handling of static objects

  4. Position Updates:

    • Correct positions to prevent sinking
    • Update AABB boundaries
    • Maintain object separation

10. Circle-AABB Collision Resolution

Understanding Circle-AABB Collisions

Circle-AABB collisions are more complex because:

  • They involve different shape types
  • Contact points need special calculation
  • Normal calculation varies based on circle position
  • Edge cases need careful handling

Implementation

void resolveCircleAABBCollision(Body &circleBody, Body &aabbBody, 
                               bool circleIsA) {
    // Validate shape types
    if (circleBody.shapeType != Body::SHAPE_CIRCLE || 
        aabbBody.shapeType != Body::SHAPE_AABB) {
        return;
    }

    // 1. Find closest point on AABB to circle center
    vec2 closestPoint = {
        max(aabbBody.shape.aabb.min.x,
            min(circleBody.position.x, aabbBody.shape.aabb.max.x)),
        max(aabbBody.shape.aabb.min.y,
            min(circleBody.position.y, aabbBody.shape.aabb.max.y))
    };

    // 2. Calculate distance vector from closest point to circle center
    vec2 distance = vec2_sub(circleBody.position, closestPoint);
    double distanceMag = vec2_length(distance);

    // Skip if no collision
    if (distanceMag > circleBody.shape.circle.radius) {
        return;
    }

    // 3. Calculate normal vector
    vec2 normal;
    const double EPSILON = 0.0001;
    
    if (distanceMag < EPSILON) {
        // Circle center is inside AABB, use vector from AABB center to circle
        vec2 aabbCenter = {
            (aabbBody.shape.aabb.min.x + aabbBody.shape.aabb.max.x) * 0.5,
            (aabbBody.shape.aabb.min.y + aabbBody.shape.aabb.max.y) * 0.5
        };
        distance = vec2_sub(circleBody.position, aabbCenter);
        distanceMag = vec2_length(distance);
        
        if (distanceMag < EPSILON) {
            // Objects are at same position, use arbitrary normal
            normal = {0.0, 1.0};
        } else {
            normal = vec2_mul(distance, 1.0 / distanceMag);
        }
    } else {
        normal = vec2_mul(distance, 1.0 / distanceMag);
    }

    // 4. Calculate penetration depth
    double penetrationDepth = circleBody.shape.circle.radius - distanceMag;

    // 5. Calculate relative velocity
    vec2 relativeVel = vec2_sub(circleBody.velocity, aabbBody.velocity);
    double impactSpeed = vec2_dot(relativeVel, normal);

    // Skip if objects are moving apart
    const double MIN_VELOCITY = 0.01;
    if (impactSpeed > -MIN_VELOCITY) {
        // Objects moving apart or too slow - just correct position
        const double POSITION_CORRECTION = 0.8;
        double totalInverseMass = circleBody.inverseMass + aabbBody.inverseMass;
        if (totalInverseMass > 0) {
            vec2 correction = vec2_mul(normal, 
                (penetrationDepth / totalInverseMass) * POSITION_CORRECTION);
            
            if (!circleBody.isStatic) {
                circleBody.position = vec2_add(circleBody.position, 
                    vec2_mul(correction, circleBody.inverseMass));
            }
            if (!aabbBody.isStatic) {
                aabbBody.position = vec2_sub(aabbBody.position, 
                    vec2_mul(correction, aabbBody.inverseMass));
            }
        }
        return;
    }

    // 6. Calculate collision response
    double totalInverseMass = circleBody.inverseMass + aabbBody.inverseMass;
    if (totalInverseMass <= 0) return;

    // Apply restitution based on impact speed
    double restitution = vec2_length(relativeVel) < 2.0 ? 0.0 : e;
    double j = -(1.0 + restitution) * impactSpeed / totalInverseMass;

    // Apply impulse
    vec2 impulse = vec2_mul(normal, j);
    if (!circleBody.isStatic) {
        circleBody.velocity = vec2_add(circleBody.velocity, 
            vec2_mul(impulse, circleBody.inverseMass));
    }
    if (!aabbBody.isStatic) {
        aabbBody.velocity = vec2_sub(aabbBody.velocity, 
            vec2_mul(impulse, aabbBody.inverseMass));
    }

    // 7. Apply friction
    const double FRICTION = 0.4;
    vec2 tangent = {-normal.y, normal.x}; // Perpendicular to normal
    double tangentSpeed = vec2_dot(relativeVel, tangent);

    // Calculate and clamp friction impulse
    double frictionImpulse = -tangentSpeed / totalInverseMass;
    double maxFriction = FRICTION * fabs(j);
    frictionImpulse = max(-maxFriction, min(frictionImpulse, maxFriction));

    vec2 frictionVector = vec2_mul(tangent, frictionImpulse);
    if (!circleBody.isStatic) {
        circleBody.velocity = vec2_add(circleBody.velocity, 
            vec2_mul(frictionVector, circleBody.inverseMass));
    }
    if (!aabbBody.isStatic) {
        aabbBody.velocity = vec2_sub(aabbBody.velocity, 
            vec2_mul(frictionVector, aabbBody.inverseMass));
    }

    // 8. Update positions
    // Update circle position
    if (!circleBody.isStatic) {
        circleBody.shape.circle.center = circleBody.position;
    }

    // Update AABB position
    if (!aabbBody.isStatic) {
        vec2 halfSize = vec2_mul(
            vec2_sub(aabbBody.shape.aabb.max, aabbBody.shape.aabb.min), 
            0.5
        );
        aabbBody.shape.aabb.min = vec2_sub(aabbBody.position, halfSize);
        aabbBody.shape.aabb.max = vec2_add(aabbBody.position, halfSize);
    }
}

Key aspects of circle-AABB collision resolution:

  1. Contact Point Calculation:

    • Find closest point on AABB to circle
    • Handle special case when circle is inside AABB
    • Calculate proper normal vector for collision response

  2. Edge Cases:

    • Circle center inside AABB
    • Objects at exact same position
    • Very small penetration depths

  3. Physics Response:

    • Variable restitution based on impact speed
    • Friction calculation using tangential impulse
    • Position correction to prevent sinking

11. Complete Collision System Integration

Collision Resolution Manager

void resolveCollisions(vector<BroadPhaseCollisionPair> &collisions,
                      vector<Body> &bodies) {
    for (const auto &pair : collisions) {
        Body &bodyA = bodies[pair.bodyA];
        Body &bodyB = bodies[pair.bodyB];

        // Choose appropriate resolution method based on shape types
        if (bodyA.shapeType == Body::SHAPE_CIRCLE && 
            bodyB.shapeType == Body::SHAPE_CIRCLE) {
            resolveCircleCircleCollision(bodyA, bodyB);
        } 
        else if (bodyA.shapeType == Body::SHAPE_AABB && 
                 bodyB.shapeType == Body::SHAPE_AABB) {
            resolveAABBCollision(bodyA, bodyB);
        } 
        else {
            // Handle circle-AABB collisions
            if (bodyA.shapeType == Body::SHAPE_CIRCLE) {
                resolveCircleAABBCollision(bodyA, bodyB, true);
            } else {
                resolveCircleAABBCollision(bodyB, bodyA, false);
            }
        }
    }
}

Complete Physics Update Cycle

Here’s how all the systems work together:

void stepWorld(World &world, double dt) {
    if (world.isPaused)
        return;
        
    // 1. Limit time step to prevent instability
    dt = min(dt, world.config.maxDeltaTime);
    
    // 2. Apply forces and integrate motion
    for (auto &body : world.bodies) {
        if (body.isActive) {
            // Apply gravity
            applyGravity(body);
            
            // Integrate forces
            integrateLinearMotion(body, dt);
        }
    }
    
    // 3. Update spatial partitioning
    updateBroadPhase(world.grid, world.bodies);
    
    // 4. Get potential collision pairs
    auto potentialPairs = getPotentialCollisionPairs(world.grid);
    
    // 5. Perform narrow phase collision detection
    auto actualCollisions = getActualCollisions(potentialPairs, world.bodies);
    
    // 6. Resolve collisions
    resolveCollisions(actualCollisions, world.bodies);
}

12. Performance Optimization Tips

  1. Spatial Partitioning Tuning:

    // Adjust cell size based on average object size
double optimalCellSize = averageObjectSize * 2.0;
initGrid(world.grid, worldBound, optimalCellSize);

  2. Avoiding Square Roots:

    // Use squared distances when possible
double distanceSquared = vec2_length_squared(diff);
double radiusSquared = radius * radius;
if (distanceSquared <= radiusSquared) {
    // Collision detected
}

  3. Reuse Vectors:

    // Pre-allocate vectors to avoid reallocations
world.queryResultIndices.reserve(100);
grid.cells[cellIndex].bodyIndex.reserve(10);

  4. Early Exit Conditions:

    // Skip static-static collisions
if (bodyA.isStatic && bodyB.isStatic)
    return;

// Skip if moving apart
if (relativeVelocity > 0)
    return;

13. Example Usage

Basic Setup and Initialization

int main() {
    // Initialize the physics world
    World world;
    WorldConfig config;
    
    // Configure world parameters
    config.gravity = {0.0, -9.81};  // Standard Earth gravity
    config.maxDeltaTime = 1.0/60.0; // 60 FPS simulation
    config.bounds = {
        {-50.0, -50.0},  // min bounds
        {50.0, 50.0}     // max bounds
    };
    
    initWorld(world, config);

    // Example: Create ground
    vec2 groundPos = {0.0, -45.0};
    vec2 groundSize = {100.0, 10.0};
    int groundId = addBox(world, groundPos, groundSize, 0.0); // mass = 0 makes it static

    // Example: Add some dynamic bodies
    int circle1 = addCircle(world, {0.0, 10.0}, 1.0, 1.0);
    int box1 = addBox(world, {-5.0, 15.0}, {2.0, 2.0}, 1.0);

    // Main simulation loop
    double timeStep = 1.0/60.0;
    while (/* your game loop condition */) {
        stepWorld(world, timeStep);
        // Render or process results
    }
}

Common Scenarios

1. Creating a Stack of Boxes

void createBoxStack(World& world, int numBoxes) {
    const double boxSize = 2.0;
    const double boxMass = 1.0;
    const double startX = 0.0;
    const double startY = -40.0;  // Above ground level
    
    for (int i = 0; i < numBoxes; i++) {
        vec2 position = {
            startX,
            startY + (i * boxSize * 1.1)  // Small gap between boxes
        };
        vec2 size = {boxSize, boxSize};
        addBox(world, position, size, boxMass);
    }
}

2. Creating a Pendulum

void createPendulum(World& world) {
    // Create anchor point (static box)
    vec2 anchorPos = {0.0, 10.0};
    int anchor = addBox(world, anchorPos, {1.0, 1.0}, 0.0);  // Static

    // Create pendulum bob (dynamic circle)
    vec2 bobPos = {0.0, 0.0};
    int bob = addCircle(world, bobPos, 1.0, 1.0);
    
    // Note: In a real implementation, you'd need to add constraint/joint
    // handling to connect the anchor and bob
}

3. Creating a Newton’s Cradle

void createNewtonsCradle(World& world, int numBalls) {
    const double radius = 1.0;
    const double mass = 1.0;
    const double spacing = radius * 2.1;
    const double startX = -(numBalls - 1) * spacing / 2;
    const double height = 0.0;

    for (int i = 0; i < numBalls; i++) {
        vec2 position = {
            startX + (i * spacing),
            height
        };
        int ball = addCircle(world, position, radius, mass);
        
        // Set initial velocity for first ball
        if (i == 0) {
            world.bodies[ball].velocity = {5.0, 0.0};
        }
    }
}

14. Testing and Debugging

Test Cases

struct PhysicsTest {
    string name;
    function<void(World&)> setup;
    function<bool(const World&)> verify;
};

// Example test case: Conservation of momentum
PhysicsTest testConservationOfMomentum() {
    return {
        "Conservation of Momentum Test",
        // Setup function
        [](World& world) {
            // Create two colliding bodies
            int body1 = addCircle(world, {-5.0, 0.0}, 1.0, 1.0);
            int body2 = addCircle(world, {5.0, 0.0}, 1.0, 1.0);
            
            // Set initial velocities
            world.bodies[body1].velocity = {1.0, 0.0};
            world.bodies[body2].velocity = {-1.0, 0.0};
        },
        // Verification function
        [](const World& world) {
            vec2 totalMomentum = {0.0, 0.0};
            for (const auto& body : world.bodies) {
                vec2 momentum = vec2_mul(body.velocity, body.mass);
                totalMomentum = vec2_add(totalMomentum, momentum);
            }
            double momentumMagnitude = vec2_length(totalMomentum);
            return momentumMagnitude < 0.01;  // Allow for small numerical errors
        }
    };
}

// Example test case: Energy conservation
PhysicsTest testEnergyConservation() {
    return {
        "Energy Conservation Test",
        [](World& world) {
            // Create a bouncing ball
            int ball = addCircle(world, {0.0, 10.0}, 1.0, 1.0);
            // Create ground
            int ground = addBox(world, {0.0, -10.0}, {20.0, 1.0}, 0.0);
        },
        [](const World& world) {
            double totalEnergy = 0.0;
            for (const auto& body : world.bodies) {
                if (body.isStatic) continue;
                
                // Kinetic energy
                double velocity_squared = vec2_length_squared(body.velocity);
                double kinetic = 0.5 * body.mass * velocity_squared;
                
                // Potential energy
                double height = body.position.y + 10.0;  // Relative to ground
                double potential = body.mass * 9.81 * height;
                
                totalEnergy += kinetic + potential;
            }
            static double initialEnergy = -1;
            if (initialEnergy < 0) initialEnergy = totalEnergy;
            
            // Allow for some energy loss due to restitution
            return totalEnergy <= initialEnergy;
        }
    };
}

Debugging Helpers

struct PhysicsDebugInfo {
    vector<string> messages;
    vector<vec2> points;
    vector<pair<vec2, vec2>> lines;
    vector<Circle> circles;
    vector<AABB> boxes;
};

class PhysicsDebugger {
public:
    static void drawCollisionNormal(PhysicsDebugInfo& debug, 
                                  const vec2& point, 
                                  const vec2& normal) {
        debug.points.push_back(point);
        debug.lines.push_back({
            point,
            vec2_add(point, vec2_mul(normal, 2.0))
        });
    }
    
    static void drawBody(PhysicsDebugInfo& debug, const Body& body) {
        if (body.shapeType == Body::SHAPE_CIRCLE) {
            debug.circles.push_back(body.shape.circle);
        } else {
            debug.boxes.push_back(body.shape.aabb);
        }
    }
    
    static void logCollision(PhysicsDebugInfo& debug, 
                           const Body& a, 
                           const Body& b, 
                           const vec2& point) {
        string msg = "Collision between bodies at (" + 
                    to_string(point.x) + ", " + 
                    to_string(point.y) + ")";
        debug.messages.push_back(msg);
    }
};

// Usage in collision resolution:
void resolveCollisions(vector<BroadPhaseCollisionPair>& collisions,
                      vector<Body>& bodies,
                      PhysicsDebugInfo* debug = nullptr) {
    for (const auto& pair : collisions) {
        Body& bodyA = bodies[pair.bodyA];
        Body& bodyB = bodies[pair.bodyB];
        
        if (debug) {
            PhysicsDebugger::drawBody(*debug, bodyA);
            PhysicsDebugger::drawBody(*debug, bodyB);
        }
        
        // Regular collision resolution code...
        
        if (debug) {
            PhysicsDebugger::logCollision(*debug, bodyA, bodyB, 
                                        bodyA.position);
        }
    }
}

15. Common Problems and Solutions

1. Tunneling Problems

When objects move too fast and pass through each other:

// Solution: Implement Continuous Collision Detection (CCD)
bool checkContinuousCollision(const Body& a, const Body& b, double dt) {
    // Calculate swept volumes
    vec2 aMove = vec2_mul(a.velocity, dt);
    vec2 bMove = vec2_mul(b.velocity, dt);
    
    // Expand AABBs to include motion path
    AABB aSwept = getShapeBoundingBox(a);
    AABB bSwept = getShapeBoundingBox(b);
    
    // Expand bounds based on movement
    if (aMove.x > 0) aSwept.max.x += aMove.x;
    else aSwept.min.x += aMove.x;
    if (aMove.y > 0) aSwept.max.y += aMove.y;
    else aSwept.min.y += aMove.y;
    
    // Similar for b...
    
    // Check if swept volumes intersect
    return aabbAabbCollision(aSwept, bSwept);
}

2. Stacking Stability

Objects in stacks may jitter or collapse:

// Solution: Implement position-based dynamics correction
void stabilizeStack(Body& body, const vector<Body>& contacts) {
    if (contacts.size() < 2) return;
    
    // Calculate average contact normal
    vec2 avgNormal = {0, 0};
    for (const auto& contact : contacts) {
        vec2 normal = vec2_sub(contact.position, body.position);
        vec2_normalize(normal);
        avgNormal = vec2_add(avgNormal, normal);
    }
    avgNormal = vec2_mul(avgNormal, 1.0/contacts.size());
    
    // If mostly vertical stack, dampen horizontal velocity
    if (fabs(avgNormal.y) > 0.8) {
        body.velocity.x *= 0.8;
    }
}

3. Energy Gain

Objects gaining energy due to numerical errors:

// Solution: Implement energy monitoring and correction
void monitorAndCorrectEnergy(Body& body, double& lastEnergy) {
    double currentEnergy = 0.5 * body.mass * 
                          vec2_length_squared(body.velocity);
    
    if (currentEnergy > lastEnergy * 1.01) {  // 1% tolerance
        // Scale back velocity to match previous energy
        double scale = sqrt(lastEnergy / currentEnergy);
        body.velocity = vec2_mul(body.velocity, scale);
        currentEnergy = lastEnergy;
    }
    
    lastEnergy = currentEnergy;
}

Key Takeaways

  1. Core Principles

    • Always separate broad and narrow phase collision detection
    • Use spatial partitioning for performance optimization
    • Implement proper integration methods for stability
    • Handle edge cases carefully in collision resolution
    • Consider numerical precision in calculations

  2. Performance Considerations

    • Optimize hot paths (collision detection and resolution)
    • Use appropriate data structures (grid-based spatial partitioning)
    • Implement early-out conditions where possible
    • Avoid unnecessary square root calculations
    • Pre-allocate memory for frequently used containers

  3. Stability Tips

    • Use consistent time steps
    • Implement position correction
    • Handle resting contacts properly
    • Monitor and correct energy conservation
    • Use appropriate coefficients for restitution and friction

Common Pitfalls to Avoid

  1. Mathematical Issues

    • Dividing by zero in normalizations
    • Accumulated floating-point errors
    • Missing edge cases in collision detection
    • Incorrect handling of static objects

  2. Performance Issues

    • Checking every object against every other object
    • Excessive memory allocations in tight loops
    • Not using spatial partitioning
    • Unnecessary precision in calculations

  3. Stability Issues

    • Too large time steps
    • Insufficient position correction
    • Improper handling of stacking
    • Missing rest conditions

Happy physics programming!