gistfile1.cpp

#include "space.h"
#include "shapes.h"

void Space::add_object(const Object& obj)
{
    objects.push_back(obj);
	shapes.push_back(std::unique_ptr<Shape>(obj.shape));
}

void Space::update(float dt)
{
	accumulator += dt;
	accumulator = glm::min(accumulator, 0.3f);

	while (accumulator > timestep) {
		accumulator -= timestep;
		for (auto& object : objects) {
			object.last_transform = object.transform;
		}
		physics_step();
	}
}

void Space::render()
{
	float alpha = accumulator / timestep;
	for (const auto& object : objects) {
		Transform t = Transform::lerp(object.last_transform, object.transform, alpha);
		object.shape->render(t);
	}
}

void Space::physics_step()
{
	std::vector<ObjectPair> pairs;

	// ### Pre collision ###

	for (auto& object : objects) {

		// Reset force
		object.force = glm::vec2(0.0f);

		// If the object is not infinitely heavy apply gravity
		if (object.inv_mass != 0.0f)
			object.force = timestep * gravity / object.inv_mass;
	}

	// ### Broad phase ###

	// Update AABBs
	for (auto& object : objects) {
		object.aabb = object.shape->aabb(object.transform);
	}

	// Create pairs
	for (auto it = objects.begin(); it != objects.end(); ++it) {
		for (auto jt = it + 1; jt != objects.end(); ++jt) {
			if (it->groups & jt->groups && t_AABB_AABB(it->aabb, jt->aabb))
				pairs.push_back(ObjectPair(&*it, &*jt));
		}
	}

	// ### Narrow phase ###

	for (auto& pair : pairs) {
		Manifold m(pair.A, pair.B);

		// Test for collision (function table)
		bool collided = c_shape_shape[m.A->shape->shape_id << 2 | m.B->shape->shape_id](&m);

		if (collided) {
			glm::vec2 relative_vel = m.B->velocity - m.A->velocity;
			glm::vec2 normal = glm::normalize(m.normal);
			float normal_vel = glm::dot(relative_vel, normal);
			
			// Physics magic
			float e = glm::min(m.A->material.restitution, m.B->material.restitution);
			float j = -(1 + e) * normal_vel;
			j /= m.A->inv_mass + m.B->inv_mass;
			glm::vec2 impulse = j * normal;

			m.A->velocity -= m.A->inv_mass * impulse;
			m.B->velocity += m.B->inv_mass * impulse;

			glm::vec2 correction = normal * glm::max(m.penetration, 0.0f) / (m.A->inv_mass + m.B->inv_mass) * 0.5f;
			m.A->transform.position -= m.A->inv_mass * correction;
			m.B->transform.position += m.B->inv_mass * correction;
		}
	}

	// ### Integration ###

	for (auto& object : objects) {
		object.velocity += object.inv_mass * object.force * timestep;
		object.transform.position += object.velocity * timestep;
	}
}