summaryrefslogtreecommitdiff
path: root/physics.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'physics.cpp')
-rw-r--r--physics.cpp274
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);
+ }
+}