1 files changed, 474 insertions, 135 deletions

diff --git a/ent.c b/ent.c
index 61c6c6a..2b23f0f 100644
--- a/ent.c
+++ b/ent.c
@@ -1,5 +1,7 @@
 #include "ent.h"
 #include "res.h"
+#include "app.h"
+#include "editor.h"
 
 typedef struct {
 	u32 gen, dense_id, next_free;
@@ -243,30 +245,11 @@ entity_system_clear(void) {
 	en.delete_queue_cnt = 0;
 	en.free_slot = ENTITY_INVALID_IDX;
 	en.next_pid = 1;
-
-
 }
 
 static void
-entity_draw_mdl(Entity *ent) {
-	Matrix T = MatrixTranslate(
-				ent->tr.pos.x,
-				ent->tr.pos.y,
-				ent->tr.pos.z);
-
-	// Matrix R = MatrixRotateXYZ((Vec3) {
-	// 			ent->tr.rot.x * DEG2RAD,
-	// 			ent->tr.rot.y * DEG2RAD,
-	// 			ent->tr.rot.z * DEG2RAD,
-	// 		});
-
-	Matrix R = QuaternionToMatrix(ent->tr.rot);
-
-	Matrix S = MatrixScale(
-				ent->tr.scale.x,
-				ent->tr.scale.y,
-				ent->tr.scale.z);
-
+entity_draw_mdl(u64 id) {
+	Entity *ent = entity_get(id);
 	if (!ent->mdl.mdl) {
 		Asset *asset = asset_get(ent->mdl.key);
 		if (!asset) {
@@ -276,108 +259,32 @@ entity_draw_mdl(Entity *ent) {
 	}
 
 	Model mdl = *ent->mdl.mdl;
-	mdl.transform = MatrixMultiply(S, MatrixMultiply(R, T));
+	mdl.transform = entity_world_matrix(id);
 
-	Color tint = ent->ed.selected ?
+	Color tint = ent->ed.selected && ed.mode == ED_MODE_SCENE ?
 	                (Color){ 100, 100, 100, 255 } :
 	                BLACK;
 
 	DrawModel(mdl, (Vec3) {0}, 1, tint);
 }
 
-// static void
-// entity_draw_col(u64 id) {
-// 	Entity *ent = entity_get(id);
-
-// 	if (!b3Body_IsValid(ent->body_id)) {
-// 		TraceLog(LOG_WARNING, "tried to draw invalid physics body");
-// 		return;
-// 	}
-
-// 	if (b3Body_GetShapes(ent->body_id, &ent->shape_id, 1) == 0) {
-// 		return;
-// 	}
-
-// 	const b3HullData *hull = b3Shape_GetHull(ent->shape_id);
-// 	const b3Vec3 *points = b3GetHullPoints(hull);
-// 	const b3HullHalfEdge *edges = b3GetHullEdges(hull);
-// 	b3WorldTransform tr = b3Body_GetTransform(ent->body_id);
-
-// 	for (int i = 0; i < hull->edgeCount; ++i) {
-// 		if (i > edges[i].twin) {
-// 			continue;
-// 		}
-
-// 		const b3HullHalfEdge *edge = &edges[i];
-// 		const b3HullHalfEdge *twin = &edges[edge->twin];
-
-// 		b3Pos world_a =
-// 			b3TransformWorldPoint(tr, points[edge->origin]);
-// 		b3Pos world_b =
-// 			b3TransformWorldPoint(tr, points[twin->origin]);
-
-// 		Color tint = GRAY;
-// 		if (FLAG_CHECK(ent->flags, BIT(EFLAG_EDITOR_SELECTED))) {
-// 			tint = LIME;
-// 		}
-
-// 		DrawLine3D(*(Vec3*)&world_a, *(Vec3*)&world_b, tint);
-// 	}
-// }
-
-// static void
-// entity_set_b3_transform(u64 id) {
-// 	Entity *ent = entity_get(id);
-
-// 	if (!b3Body_IsValid(ent->body_id)) {
-// 		return;
-// 	}
-
-// 	b3WorldTransform tr = b3Body_GetTransform(ent->body_id);
-
-// 	// if in edit mode, entity transform is authoritative
-// 	if (FLAG_CHECK(app.flags, BIT(APP_FLAG_DEBUG))) {
-// 		Quaternion eq = QuaternionFromEuler(
-// 					ent->rot.x * DEG2RAD,
-// 					ent->rot.y * DEG2RAD,
-// 					ent->rot.z * DEG2RAD);
-
-// 		Quaternion bq = {
-// 			.x = tr.q.v.x,
-// 			.y = tr.q.v.y,
-// 			.z = tr.q.v.z,
-// 			.w = tr.q.s
-// 		};
-
-// 		if (Vector3Equals(ent->pos, *(Vec3*)&tr.p) &&
-// 			QuaternionEquals(eq, bq)) {
-// 			return;
-// 		}
-
-// 		b3Quat b3rot = { .v = { eq.x, eq.y, eq.z }, .s = eq.w };
-// 		b3Body_SetTransform(ent->body_id, *(b3Pos*)&ent->pos, b3rot);
-// 	} else {
-// 		ent->pos.x = tr.p.x;
-// 		ent->pos.y = tr.p.y;
-// 		ent->pos.z = tr.p.y;
-
-// 		Vec3 euler_angles =
-// 			QuaternionToEuler((Quaternion) {
-// 				.x = tr.q.v.x,
-// 				.y = tr.q.v.y,
-// 				.z = tr.q.v.z,
-// 				.w = tr.q.s
-// 			});
-
-// 		ent->rot.x = euler_angles.x * RAD2DEG;
-// 		ent->rot.y = euler_angles.y * RAD2DEG;
-// 		ent->rot.z = euler_angles.z * RAD2DEG;
-// 	}
-// }
-
 void
 entity_system_update(void) {
 	entity_process_delete_queue();
+
+	if (physics.simulate) {
+		return;
+	}
+
+	for (u32 i = 0; i < entity_count(); ++i) {
+		u64 id = entity_id_at(i);
+		Entity *ent = entity_get(id);
+		PhysicsCmp *phys = &ent->phys;
+
+		if (phys->has && phys->body_cnt && !phys->runtime.bodies) {
+			entity_physics_build(id);
+		}
+	}
 }
 
 void
@@ -386,8 +293,14 @@ entity_system_draw_3d(void) {
 		u64 id = entity_id_at(i);
 		Entity *ent = entity_get(id);
 
+		if (app.mode == APP_MODE_EDIT &&
+			ed.mode == ED_MODE_PHYSICS &&
+			ent->pid != ed.physics_pid) {
+			continue;
+		}
+
 		if (ent->mdl.has && ent->mdl.draw) {
-			entity_draw_mdl(ent);
+			entity_draw_mdl(id);
 		}
 	}
 }
@@ -475,10 +388,75 @@ entity_delete(u64 id) {
 	en.delete_queue_cnt += 1;
 }
 
-static void
-entity_physics_clear(Entity *ent) {
-	PhysicsCmp *phys = &ent->phys;
+static void *
+physics_copy_data(const void *src, size_t size) {
+	if (!size) {
+		return NULL;
+	}
+
+	void *copy = malloc(size);
+	if (!copy) {
+		perror("physics_copy_data");
+		exit(1);
+	}
+
+	memcpy(copy, src, size);
+	return copy;
+}
+
+PhysicsCmp
+entity_physics_copy(const PhysicsCmp *src) {
+	PhysicsCmp copy = *src;
+
+	copy.runtime.bodies = NULL;
+	copy.runtime.joints = NULL;
+
+	copy.bodies = physics_copy_data(
+		src->bodies, src->body_cnt * sizeof(*src->bodies));
+	copy.joints = physics_copy_data(
+		src->joints, src->joint_cnt * sizeof(*src->joints));
+
+	for (u32 i = 0; i < src->body_cnt; ++i) {
+		const PhysicsBody *source = &src->bodies[i];
+		PhysicsBody *body = &copy.bodies[i];
+
+		body->shapes = physics_copy_data(
+			source->shapes,
+			source->shape_cnt * sizeof(*source->shapes));
+
+		for (u32 j = 0; j < body->shape_cnt; ++j) {
+			PhysicsShape *shape = &body->shapes[j];
+
+			switch (shape->tag) {
+			case PHYS_SHAPE_CONVEX_HULL:
+				shape->ConvexHull.points = physics_copy_data(
+					shape->ConvexHull.points,
+					shape->ConvexHull.point_cnt *
+					sizeof(*shape->ConvexHull.points));
+				break;
+
+			case PHYS_SHAPE_STATIC_TRIMESH:
+				shape->TriMesh.points = physics_copy_data(
+					shape->TriMesh.points,
+					shape->TriMesh.point_cnt *
+					sizeof(*shape->TriMesh.points));
+				shape->TriMesh.indices = physics_copy_data(
+					shape->TriMesh.indices,
+					shape->TriMesh.idx_cnt *
+					sizeof(*shape->TriMesh.indices));
+				break;
+
+			default:
+				break;
+			}
+		}
+	}
 
+	return copy;
+}
+
+void
+entity_physics_clear(PhysicsCmp *phys) {
 	if (phys->runtime.joints) {
 		for (int i = 0; i < phys->joint_cnt; ++i) {
 			b3JointId joint_id = phys->runtime.joints[i];
@@ -504,41 +482,400 @@ entity_physics_clear(Entity *ent) {
 	phys->runtime.bodies = NULL;
 }
 
+static void
+physics_build_shapes(u64 id, u32 body_index) {
+	Entity *ent = entity_get(id);
+	PhysicsBody *body = &ent->phys.bodies[body_index];
+	PhysicsBodyRuntime *runtime = &ent->phys.runtime.bodies[body_index];
+
+	if (!body->shape_cnt) {
+		return;
+	}
+
+	runtime->shapes = calloc(
+		body->shape_cnt, sizeof(*runtime->shapes));
+
+	if (!runtime->shapes) {
+		perror("physics shape handles allocation failed");
+		exit(EXIT_FAILURE);
+	}
+
+	Matrix entity_world = entity_world_matrix(id);
+	Quaternion entity_rot = entity_world_rot(id);
+
+	Vec3 pos = Vector3Transform(body->pos, entity_world);
+	Quaternion rot = QuaternionMultiply(entity_rot, body->rot);
+	Quaternion inverse_rot = QuaternionInvert(rot);
+
+	Matrix body_local = MatrixMultiply(
+		QuaternionToMatrix(body->rot),
+		MatrixTranslate(body->pos.x, body->pos.y, body->pos.z));
+
+	Matrix body_world = MatrixMultiply(body_local, entity_world);
+
+	for (u32 j = 0; j < body->shape_cnt; ++j) {
+		PhysicsShape *shape = &body->shapes[j];
+
+		Matrix shape_local = MatrixMultiply(
+			QuaternionToMatrix(shape->rot),
+			MatrixTranslate(
+				shape->pos.x, shape->pos.y, shape->pos.z));
+
+		Matrix world = MatrixMultiply(shape_local, body_world);
+
+		Vec3 scale = {
+			Vector3Length((Vec3){world.m0, world.m1, world.m2}),
+			Vector3Length((Vec3){world.m4, world.m5, world.m6}),
+			Vector3Length((Vec3){world.m8, world.m9, world.m10}),
+		};
+
+		Vec3 center = {world.m12, world.m13, world.m14};
+		center = Vector3RotateByQuaternion(
+			Vector3Subtract(center, pos), inverse_rot);
+
+		b3ShapeDef def = b3DefaultShapeDef();
+		def.density = shape->density;
+		def.baseMaterial.friction = shape->friction;
+		def.baseMaterial.restitution = shape->restitution;
+
+		def.filter.categoryBits = shape->category_bits |
+			BIT(PHYS_MASK_EDITOR_SELECTABLE);
+		def.filter.maskBits = shape->mask_bits |
+			BIT(PHYS_MASK_EDITOR_RAYCAST);
+
+		switch (shape->tag) {
+		case PHYS_SHAPE_BOX: {
+			Vec3 size = Vector3Multiply(shape->Box.sz, scale);
+
+			if (size.x <= 0 || size.y <= 0 || size.z <= 0) {
+				break;
+			}
+
+			b3Transform local = {
+				.p = {center.x, center.y, center.z},
+				.q = {
+					.v = {
+						shape->rot.x,
+						shape->rot.y,
+						shape->rot.z,
+					},
+					.s = shape->rot.w,
+				},
+			};
+
+			b3BoxHull hull = b3MakeTransformedBoxHull(
+				size.x * 0.5f,
+				size.y * 0.5f,
+				size.z * 0.5f,
+				local);
+
+			runtime->shapes[j] = b3CreateHullShape(
+				runtime->id, &def, &hull.base);
+			break;
+		}
+
+		case PHYS_SHAPE_SPHERE: {
+			Vec3 entity_scale = {
+				Vector3Length((Vec3){
+					entity_world.m0,
+					entity_world.m1,
+					entity_world.m2,
+				}),
+				Vector3Length((Vec3){
+					entity_world.m4,
+					entity_world.m5,
+					entity_world.m6,
+				}),
+				Vector3Length((Vec3){
+					entity_world.m8,
+					entity_world.m9,
+					entity_world.m10,
+				}),
+			};
+
+			f32 largest = fmaxf(entity_scale.x,
+				fmaxf(entity_scale.y, entity_scale.z));
+			f32 radius = shape->Sphere.radius * largest;
+
+			if (radius <= 0) {
+				break;
+			}
+
+			b3Sphere sphere = {
+				.center = {center.x, center.y, center.z},
+				.radius = radius,
+			};
+
+			runtime->shapes[j] = b3CreateSphereShape(
+				runtime->id, &def, &sphere);
+			break;
+		}
+
+		case PHYS_SHAPE_CAPSULE: {
+			f32 radius = shape->Capsule.radius *
+				fmaxf(scale.x, scale.z);
+			f32 half_height =
+				shape->Capsule.height * scale.y * 0.5f;
+
+			if (radius <= 0) {
+				break;
+			}
+
+			if (half_height == 0) {
+				b3Sphere sphere = {
+					.center = {
+						center.x,
+						center.y,
+						center.z
+					},
+					.radius = radius,
+				};
+
+				runtime->shapes[j] = b3CreateSphereShape(
+					runtime->id, &def, &sphere);
+				break;
+			}
+
+			Vec3 offset = Vector3RotateByQuaternion(
+				(Vec3){0, half_height, 0}, shape->rot);
+
+			Vec3 start = Vector3Subtract(center, offset);
+			Vec3 end = Vector3Add(center, offset);
+
+			b3Capsule capsule = {
+				.center1 = {start.x, start.y, start.z},
+				.center2 = {end.x, end.y, end.z},
+				.radius = radius,
+			};
+
+			runtime->shapes[j] = b3CreateCapsuleShape(
+				runtime->id, &def, &capsule);
+			break;
+		}
+
+		case PHYS_SHAPE_CYLINDER: {
+			f32 radius = shape->Cylinder.radius *
+				fmaxf(scale.x, scale.z);
+			f32 height = shape->Cylinder.height * scale.y;
+
+			if (radius <= 0 || height <= 0) {
+				break;
+			}
+
+			b3HullData *hull = b3CreateCylinder(
+				height, radius, -height * 0.5f, 12);
+
+			if (!hull) {
+				TraceLog(LOG_ERROR,
+					"cylinder hull creation failed");
+				break;
+			}
+
+			b3Transform local = {
+				.p = {center.x, center.y, center.z},
+				.q = {
+					.v = {
+						shape->rot.x,
+						shape->rot.y,
+						shape->rot.z,
+					},
+					.s = shape->rot.w,
+				},
+			};
+
+			runtime->shapes[j] = b3CreateTransformedHullShape(
+				runtime->id, &def, hull, local,
+				(b3Vec3){1, 1, 1});
+
+			b3DestroyHull(hull);
+			break;
+		}
+
+		default:
+			break;
+		}
+	}
+}
+
 void
-entity_clean(Entity *ent) {
-	free(ent->name);
+entity_physics_build(u64 id) {
+	Entity *ent = entity_get(id);
+	if (!ent) {
+		TraceLog(LOG_ERROR,
+			"entity_physics_build: invalid entity id");
+		return;
+	}
 
-	/* model component data */
-	free(ent->mdl.key);
+	PhysicsCmp *phys = &ent->phys;
+	entity_physics_clear(phys);
 
-	/* physics component data */
-	entity_physics_clear(ent); // runtime data
-	for (int i = 0; i < ent->phys.body_cnt; ++i) {
-		PhysicsBody *body = &ent->phys.bodies[i];
+	if (!phys->has || !phys->body_cnt) {
+		return;
+	}
+
+	phys->runtime.bodies = calloc(phys->body_cnt,
+	                        sizeof(*phys->runtime.bodies));
+
+	if (!phys->runtime.bodies) {
+		perror("entity_physics_build: allocation failed");
+		exit(1);
+	}
+
+	Matrix entity_world = entity_world_matrix(id);
+	Quaternion entity_rot = entity_world_rot(id);
+
+	phys->runtime.world = entity_world;
+	phys->runtime.world_rot = entity_rot;
+
+	for (u32 i = 0; i < phys->body_cnt; ++i) {
+		PhysicsBody *body = &phys->bodies[i];
+		PhysicsBodyRuntime *runtime = &phys->runtime.bodies[i];
+
+		runtime->entity_id = id;
+
+		Vec3 pos = Vector3Transform(body->pos, entity_world);
+		Quaternion rot = QuaternionMultiply(entity_rot, body->rot);
+
+		b3BodyDef def = b3DefaultBodyDef();
+
+		switch (body->mode) {
+		case PHYS_MODE_STATIC:
+			def.type = b3_staticBody;
+			break;
+
+		case PHYS_MODE_KINEMATIC:
+			def.type = b3_kinematicBody;
+			break;
+
+		case PHYS_MODE_DYNAMIC:
+			def.type = b3_dynamicBody;
+			break;
+		}
+
+		def.position = (b3Pos){pos.x, pos.y, pos.z};
+		def.rotation = (b3Quat){
+			.v = {rot.x, rot.y, rot.z},
+			.s = rot.w,
+		};
+
+		def.linearDamping = body->linear_damping;
+		def.angularDamping = body->angular_damping;
+		def.gravityScale = body->gravity_scale;
+		def.enableSleep = body->enable_sleep;
+		def.userData = &runtime->entity_id;
 
-		for (int j = 0; j < body->shape_cnt; ++j) {
+		runtime->id = b3CreateBody(physics.world_id, &def);
+		physics_build_shapes(id, i);
+	}
+}
+
+void
+entity_physics_free(PhysicsCmp *phys) {
+	entity_physics_clear(phys);
+
+	for (u32 i = 0; i < phys->body_cnt; ++i) {
+		PhysicsBody *body = &phys->bodies[i];
+
+		for (u32 j = 0; j < body->shape_cnt; ++j) {
 			PhysicsShape *shape = &body->shapes[j];
 
 			switch (shape->tag) {
 			case PHYS_SHAPE_CONVEX_HULL:
-			free(shape->ConvexHull.points);
-			break;
+				free(shape->ConvexHull.points);
+				break;
 
 			case PHYS_SHAPE_STATIC_TRIMESH:
-			free(shape->TriMesh.points);
-			free(shape->TriMesh.indices);
-			break;
+				free(shape->TriMesh.points);
+				free(shape->TriMesh.indices);
+				break;
 
 			default:
-			break;
+				break;
 			}
 		}
 
 		free(body->shapes);
 	}
 
-	free(ent->phys.bodies);
-	free(ent->phys.joints);
+	free(phys->bodies);
+	free(phys->joints);
+	*phys = (PhysicsCmp){0};
+}
+
+void
+entity_physics_transform_changed(u64 id) {
+	if (physics.simulate) {
+		return;
+	}
+
+	Entity *ent = entity_get(id);
+	assert(ent);
+
+	/* Children inherit this entity's transform. */
+	for (u64 child = ent->tr.first_child; child;
+		child = entity_get(child)->tr.next) {
+		entity_physics_transform_changed(child);
+	}
+
+	PhysicsCmp *phys = &ent->phys;
+	if (!phys->runtime.bodies) {
+		return;
+	}
+
+	Matrix world = entity_world_matrix(id);
+	Quaternion world_rot = entity_world_rot(id);
+
+	/* Remove world rotation, leaving scale and distortion. */
+	Matrix previous = MatrixMultiply(
+		phys->runtime.world,
+		QuaternionToMatrix(
+			QuaternionInvert(phys->runtime.world_rot)));
+
+	Matrix current = MatrixMultiply(
+		world,
+		QuaternionToMatrix(QuaternionInvert(world_rot)));
+
+	float16 before = MatrixToFloatV(previous);
+	float16 after = MatrixToFloatV(current);
+
+	/* Compare only the upper-left 3x3; translation is irrelevant. */
+	for (int column = 0; column < 3; ++column) {
+		for (int row = 0; row < 3; ++row) {
+			int index = column * 4 + row;
+
+			if (!FloatEquals(before.v[index], after.v[index])) {
+				entity_physics_clear(phys);
+				return;
+			}
+		}
+	}
+
+	/* Geometry still fits: update the existing body poses. */
+	for (u32 i = 0; i < phys->body_cnt; ++i) {
+		PhysicsBody *body = &phys->bodies[i];
+
+		Vec3 pos = Vector3Transform(body->pos, world);
+		Quaternion rot = QuaternionMultiply(world_rot, body->rot);
+
+		b3Body_SetTransform(
+			phys->runtime.bodies[i].id,
+			(b3Pos){pos.x, pos.y, pos.z},
+			(b3Quat){
+				.v = {rot.x, rot.y, rot.z},
+				.s = rot.w,
+			});
+	}
+}
+
+void
+entity_clean(Entity *ent) {
+	free(ent->name);
+
+	/* model component data */
+	free(ent->mdl.key);
+
+	/* physics component data */
+	entity_physics_free(&ent->phys);
 }
 
 Entity
@@ -552,7 +889,7 @@ entity_extract_now(u64 id) {
 
 	assert(!en.entity[dense_id].tr.first_child);
 
-	entity_physics_clear(&en.entity[dense_id]);
+	entity_physics_clear(&en.entity[dense_id].phys);
 	entity_set_parent(id, 0);
 	Entity extracted = en.entity[dense_id];
 
@@ -661,6 +998,7 @@ entity_set_parent(u64 id, u64 parent_id) {
 	entity_unlink_parent(id);
 
 	if (!parent) {
+		entity_physics_transform_changed(id);
 		return true;
 	}
 
@@ -676,6 +1014,7 @@ entity_set_parent(u64 id, u64 parent_id) {
 	}
 
 	parent->tr.first_child = id;
+	entity_physics_transform_changed(id);
 	return true;
 }