start on physics

1 files changed, 274 insertions, 0 deletions

diff --git a/physics.cpp b/physics.cpp
new file mode 100644
index 0000000..993b587
--- /dev/null
+++ b/physics.cpp
@@ -0,0 +1,274 @@
+#include "debugdraw.hpp"
+#include "physics.hpp"
+#include "scene.hpp"
+#include "world.hpp"
+
+extern "C" {
+#include "memory.h"
+#include "str.h"
+}
+
+#include <new>
+
+void Collider::debug_render(
+	Line_Renderer& lr,
+	const m4f& t,
+	const v3f& col
+) {
+	int i, c = face_count;
+	for (i = 0; i < c; i++) {
+		Face& f = faces[i];
+		uint16_t prev = f.verts[0];
+		v3f avg = (t * v4f(verts[prev], 1.0f)).xyz();
+		int j;
+		lr.colour(col);
+		for (j = 1; j < f.vert_count; j++) {
+			uint16_t idx = f.verts[j];
+			v3f from = (t * v4f(verts[prev], 1.0f)).xyz();
+			v3f to = (t * v4f(verts[idx], 1.0f)).xyz();
+			lr.add_line(from, to);
+			avg += to;
+			prev = idx;
+		}
+		lr.add_line(
+			(t * v4f(verts[prev], 1.0f)).xyz(),
+			(t * v4f(verts[f.verts[0]], 1.0f)).xyz()
+		);
+		avg /= (float)f.vert_count;
+		lr.colour(v3f(0.3f, 1.0f, 1.0f));
+		lr.add_arrow(avg, avg + (t * v4f(f.normal, 0.0f)).xyz());
+	}
+}
+
+Int_Collider* Collider::transform(const m4f& t, Arena* a) const {
+	int i;
+	Int_Collider* r = (Int_Collider*)arena_alloc(a, sizeof *r);
+	r->faces = (v3f*)arena_alloc(a, face_count * sizeof *r->faces);
+	r->verts = (v3f*)arena_alloc(a, vert_count * sizeof *r->verts);
+	for (i = 0; i < face_count; i++)
+		r->faces[i] = (t * v4f(faces[i].normal, 0.0f)).xyz();
+	for (i = 0; i < vert_count; i++)
+		r->verts[i] = (t * v4f(verts[i], 1.0f)).xyz();
+	return r;
+}
+
+Collider* make_box(Arena* a, const v3f& size) {
+	int i;
+	v3f he = size * 0.5f;
+	v3f* verts;
+	Collider::Face* faces;
+	Collider* c = (Collider*)arena_alloc(a, sizeof *c);
+	new(c) Collider();
+	c->vert_count = 8;
+	c->face_count = 6;
+	verts = (v3f*)arena_alloc(
+		a,
+		sizeof *c->verts * c->vert_count
+	);
+	faces = (Collider::Face*)arena_alloc(
+		a,
+		sizeof *c->faces * c->face_count
+	);
+	for (i = 0; i < c->face_count; i++) {
+		Collider::Face& f = faces[i];
+		f.vert_count = 4;
+		f.verts = (uint16_t*)arena_alloc(
+			a,
+			sizeof *f.verts * f.vert_count
+		);
+	}
+	c->verts = verts;
+	c->faces = faces;
+	verts[0] = he;
+	verts[1] = v3f( he.x,  he.y, -he.z);
+	verts[2] = v3f(-he.x,  he.y, -he.z);
+	verts[3] = v3f(-he.x,  he.y,  he.z);
+	verts[4] = v3f( he.x, -he.y,  he.z);
+	verts[5] = v3f( he.x, -he.y, -he.z);
+	verts[6] = -he;
+	verts[7] = v3f(-he.x, -he.y,  he.z);
+	faces[0].normal = v3f( 1.0f,  0.0f,  0.0f);
+	faces[0].verts[0] = 0;
+	faces[0].verts[1] = 1;
+	faces[0].verts[2] = 5;
+	faces[0].verts[3] = 4;
+	faces[1].normal = v3f(-1.0f,  0.0f,  0.0f);
+	faces[1].verts[0] = 2;
+	faces[1].verts[1] = 3;
+	faces[1].verts[2] = 7;
+	faces[1].verts[3] = 6;
+	faces[2].normal = v3f( 0.0f,  1.0f,  0.0f);
+	faces[2].verts[0] = 2;
+	faces[2].verts[1] = 3;
+	faces[2].verts[2] = 0;
+	faces[2].verts[3] = 1;
+	faces[3].normal = v3f( 0.0f, -1.0f,  0.0f);
+	faces[3].verts[0] = 6;
+	faces[3].verts[1] = 7;
+	faces[3].verts[2] = 4;
+	faces[3].verts[3] = 5;
+	faces[4].normal = v3f( 0.0f,  0.0f,  1.0f);
+	faces[4].verts[0] = 4;
+	faces[4].verts[1] = 7;
+	faces[4].verts[2] = 3;
+	faces[4].verts[3] = 0;
+	faces[5].normal = v3f( 0.0f,  0.0f, -1.0f);
+	faces[5].verts[0] = 6;
+	faces[5].verts[1] = 5;
+	faces[5].verts[2] = 1;
+	faces[5].verts[3] = 2;
+	c->name = dup_string(a, "<box>");
+	c->loader = 0;
+	{
+		v3f i, hl2 = he * he;
+		float a = 1.0f / 12.0f;
+		i.x = a * (hl2.z + hl2.y);
+		i.y = a * (hl2.x + hl2.z);
+		i.z = a * (hl2.x + hl2.y);
+		c->moment = m3f(
+			v3f(i.x,  0.0f, 0.0f),
+			v3f(0.0f, i.y,  0.0f),
+			v3f(0.0f, 0.0f, i.z)
+		).inverse();
+	}
+	return c;
+}
+
+void Rigidbody::init(
+	Collider* c,
+	const v3f& p,
+	const v4f& r,
+	float mass
+) {
+	col = c;
+	set_pos(p);
+	set_rot(r);
+	set_mass(mass);
+	force = v3f(0.0f);
+	torque = v3f(0.0f);
+	vel = v3f(0.0f);
+	avel = v3f(0.0f);
+	set_mass(mass);
+}
+
+void Rigidbody::set_pos(const v3f& p) {
+	pos = p;
+	prev_pos = p;
+}
+
+void Rigidbody::set_rot(const v4f& r) {
+	rot = r;
+	prev_rot = r;
+}
+
+void Rigidbody::set_mass(float m) {
+	if (m > 0.000001f)
+		inv_mass = 1.0f / m;
+	else
+		inv_mass = 0.0f;
+}
+
+void Rigidbody::add_force(const v3f& f) {
+	force += f;
+}
+
+void Rigidbody::add_force(const v3f& f, const v3f& p) {
+	force += f;
+	add_torque(v3f::cross(f, p));
+}
+
+void Rigidbody::add_impulse(const v3f& f) {
+	vel += f * inv_mass;
+}
+
+void Rigidbody::add_impulse(const v3f& f, const v3f& p) {
+	vel  += f * inv_mass;
+	avel += v3f::cross(f, p) * inv_mass;
+}
+
+void Rigidbody::add_torque(const v3f& t) {
+	torque += t;
+}
+
+void physics_tick(World& w, float ts) {
+	for (auto v : w.view<Rigidbody>()) {
+		auto& rb = v.get<Rigidbody>();
+		v3f accel = rb.inv_mass * rb.force;
+		v3f aaccel = rb.col->moment * rb.torque;
+		rb.prev_pos = rb.pos;
+		rb.prev_rot = rb.rot;
+		rb.vel += accel * ts;
+		rb.avel += aaccel * ts;
+		rb.pos += rb.vel * ts;
+		rb.rot = quat::normalised(rb.rot + quat::mul<false>(
+			quat::scale(v4f(rb.avel * 0.5f, 0.0f), ts),
+			rb.rot
+		));
+	}
+}
+
+bool Int_Collider::collide(const Int_Collider& other) const {
+	return false;
+}
+
+struct Collision_Engine {
+	Int_Collider* bodies;
+	Arena* arena;
+	void init(World& w, Arena* a) {
+		arena = a;
+		bodies = 0;
+		for (auto v : w.view<Transform, Rigidbody>()) {
+			auto& t = v.get<Transform>();
+			auto& r = v.get<Rigidbody>();
+			Int_Collider* b = r.col->transform(t.mat, a);
+			b->rb = &r;
+			r.colliding = 0;
+			b->next = bodies;
+			bodies = b;
+		}
+	}
+	void detect() {
+		Int_Collider* a;
+		for (a = bodies; a; a = a->next) {
+			Int_Collider* b;
+			for (b = a->next; b; b = b->next) {
+				if (a->collide(*b)) {
+					a->rb->colliding = 1;
+					b->rb->colliding = 1;
+				}
+			}
+		}
+	}
+};
+
+void physics_update(World& w, Arena* a, float ts) {
+	Collision_Engine ce;
+	ce.init(w, a);
+	ce.detect();
+	physics_tick(w, ts);
+	for (auto v : w.view<Transform, Rigidbody>()) {
+		auto& t = v.get<Transform>();
+		auto& r = v.get<Rigidbody>();
+		t.mat =
+			m4f::translate(m4f::identity(), r.pos) *
+			m4f::rotate(m4f::identity(), r.rot);
+	}
+}
+
+void physics_debug(World& w, Line_Renderer& r) {
+	for (auto v : w.view<Transform, Rigidbody>()) {
+		Transform& t = v.get<Transform>();
+		Rigidbody& rb = v.get<Rigidbody>();
+		Collider* c = rb.col;
+		v3f col = rb.colliding?
+			v3f(1.0f, 0.0f, 0.0f):
+			v3f(0.3f, 1.0f, 0.3f);
+		c->debug_render(r, t.mat, col);
+		r.colour(v3f(1.0f, 0.3f, 0.3f));
+		r.add_arrow(rb.pos, rb.pos + rb.vel);
+		r.colour(v3f(0.3f, 0.3f, 1.0f));
+		r.add_arrow(rb.pos, rb.pos + rb.avel);
+		r.colour(v3f(1.0f, 1.0f, 0.3f));
+		r.add_arrow(rb.pos, rb.pos + rb.torque);
+	}
+}