avocado-old/packages/physics/dummy/world.js

import * as I from 'immutable';

import {arrayUnique} from '@avocado/core';
import {Rectangle, QuadTree, Vector} from '@avocado/math';

import {Body} from './body';

export class World {

  constructor() {
    this.bodies = [];
    this.entities = new Map();
    this.quadTree = new QuadTree();
    this.quadTreeNodes = new Map();
  }

  addBody(body) {
    this.bodies.push(body);
    // Add to quad tree.
    this.addQuadTreeNodes(body);
    body.on('positionChanged', () => {
      this.removeQuadTreeNodes(body);
      this.addQuadTreeNodes(body);
    });
  }

  addQuadTreeNodes(body) {
    // 4 points.
    const aabb = body.aabb;
    const width = aabb[2] - .0001;
    const height = aabb[3] - .0001;
    const upperLeft = Rectangle.position(aabb);
    const upperRight = Vector.add(upperLeft, [width, 0]);
    const lowerLeft = Vector.add(upperLeft, [0, height]);
    const lowerRight = Vector.add(upperLeft, [width, height]);
    const points = [
      upperLeft,
      upperRight,
      lowerLeft,
      lowerRight,
    ];
    const nodes = points.map((point) => [...point, body]);
    this.quadTreeNodes = this.quadTreeNodes.set(body, nodes);
    for (const node of nodes) {
      this.quadTree.add(node);
    }
  }

  createBodyForEntity(entity) {
    const body = new Body(entity.shape);
    body.position = entity.position;
    this.entities.set(body, entity);
    return body;
  }

  removeBody(body) {
    const index = this.bodies.indexOf(body);
    if (-1 === index) {
      return;
    }
    this.entities.delete(body);
    this.removeQuadTreeNodes(body);
    this.bodies.splice(index, 1);
  }

  removeQuadTreeNodes(body) {
    const nodes = this.quadTreeNodes.get(body);
    if (!nodes) {
      return;
    }
    for (const node of nodes) {
      this.quadTree.remove(node);
    }
  }

  tick(elapsed) {
    // Apply.
    for (const entity of this.entities.values()) {
      const body = entity.body;
      const translation = Vector.add(
        Vector.scale(body.impulse, elapsed),
        body.force,
      );
      body.position = Vector.add(body.position, translation);
    }
    // Contact checks.
    const allContacts = new Map();
    const checkedSoFar = new Map();
    for (const entity of this.entities.values()) {
      const body = entity.body;
      // Find bodies in AABB.
      const aabb = body.aabb;
      let otherBodies = this.quadTree.search(aabb).map((node) => {
        return node.data[2];
      });
      // Not self.
      otherBodies = otherBodies.filter((otherBody) => {
        return otherBody !== body;
      });
      // Uniques only.
      otherBodies = arrayUnique(otherBodies);
      // TODO: full collision check
      const checkSet = new Set();
      checkedSoFar.set(entity, checkSet);
      for (const otherBody of otherBodies) {
        const otherEntity = this.entities.get(otherBody);
        if (otherEntity) {
          // Only one check per pair.
          const otherCheckSet = checkedSoFar.get(otherEntity);
          if (otherCheckSet) {
            if (otherCheckSet.has(entity)) {
              continue;
            }
          }
            // Self contacts.
          let contacts = allContacts.get(entity);
          if (!contacts) {
            contacts = I.Set();
          }
          contacts = contacts.add(otherEntity);
          allContacts.set(entity, contacts);
          // Other contacts.
          let otherContacts = allContacts.get(otherEntity);
          if (!otherContacts) {
            otherContacts = I.Set();
          }
          otherContacts = otherContacts.add(entity);
          allContacts.set(otherEntity, otherContacts);
          // Mark as checked.
          checkSet.add(otherEntity);
        }
      }
      if (!allContacts.get(entity)) {
        allContacts.set(entity, I.Set());
      }
    }
    // Report collisions.
    for (const entity of this.entities.values()) {
      const oldContacts = entity.body.contacts;
      const newContacts = allContacts.get(entity);
      for (const contact of newContacts.values()) {
        if (!oldContacts.has(contact)) {
          entity.emit('contactStart', contact);
        }
      }
      for (const contact of oldContacts.values()) {
        if (!newContacts.has(contact)) {
          entity.emit('contactEnd', contact);
        }
      }
      entity.body.contacts = newContacts;
    }
    // Super rudimentary resolving.
    for (const entity of this.entities.values()) {
      const body = entity.body;
      // Contact? Undo impulse.
      const contacts = body.contacts;
      if (contacts.size > 0) {
        const translation = Vector.add(
          Vector.scale(body.impulse, elapsed),
          body.force,
        );
        body.position = Vector.sub(body.position, translation);
        // Check all contacts and see if impulse vector can be fixed.
        for (const axe of [0, 1]) {
          const anyContact = false;
          const potentialTranslation = [0, 0];
          potentialTranslation[axe] = translation[axe];
          body.position = Vector.add(body.position, potentialTranslation);
          for (const contact of contacts) {
            const contactBody = contact.body;
            if (Rectangle.isTouching(body.aabb, contactBody.aabb)) {
              anyContact = true;
              break;
            }
          }
          if (anyContact) {
            body.position = Vector.sub(body.position, potentialTranslation);
          }
        }
      }
    }
    // Drop transforms.
    for (const {body} of this.entities.values()) {
      body.force = [0, 0];
      body.impulse = [0, 0];
    }
    // Propagate position updates.
    for (const entity of this.entities.values()) {
      entity.position = entity.body.position;
    }
  }

}