From 097d39628a95ffe74e06fdc2eae932b32e32b05e Mon Sep 17 00:00:00 2001
From: Josip Miskovic <josipmiskovic@gmail.com>
Date: Wed, 29 Apr 2020 23:01:32 +0300
Subject: [PATCH] Initialize inertia information for new colliders

Inertia matrix has to be calculated with algorithm specific for each
shape. Without calculated matrix the physics behaves very floaty and
slowed down.
---
 src/api/l_physics_world.c     | 4 ++++
 src/modules/physics/physics.c | 8 ++++++++
 src/modules/physics/physics.h | 3 ++-
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/api/l_physics_world.c b/src/api/l_physics_world.c
index d3995295..3762c693 100644
--- a/src/api/l_physics_world.c
+++ b/src/api/l_physics_world.c
@@ -60,6 +60,7 @@ static int l_lovrWorldNewBoxCollider(lua_State* L) {
   Collider* collider = lovrColliderCreate(world, x, y, z);
   BoxShape* shape = lovrBoxShapeCreate(sx, sy, sz);
   lovrColliderAddShape(collider, shape);
+  lovrColliderInitInertia(collider, shape);
   luax_pushtype(L, Collider, collider);
   lovrRelease(Collider, collider);
   lovrRelease(Shape, shape);
@@ -76,6 +77,7 @@ static int l_lovrWorldNewCapsuleCollider(lua_State* L) {
   Collider* collider = lovrColliderCreate(world, x, y, z);
   CapsuleShape* shape = lovrCapsuleShapeCreate(radius, length);
   lovrColliderAddShape(collider, shape);
+  lovrColliderInitInertia(collider, shape);
   luax_pushtype(L, Collider, collider);
   lovrRelease(Collider, collider);
   lovrRelease(Shape, shape);
@@ -92,6 +94,7 @@ static int l_lovrWorldNewCylinderCollider(lua_State* L) {
   Collider* collider = lovrColliderCreate(world, x, y, z);
   CylinderShape* shape = lovrCylinderShapeCreate(radius, length);
   lovrColliderAddShape(collider, shape);
+  lovrColliderInitInertia(collider, shape);
   luax_pushtype(L, Collider, collider);
   lovrRelease(Collider, collider);
   lovrRelease(Shape, shape);
@@ -107,6 +110,7 @@ static int l_lovrWorldNewSphereCollider(lua_State* L) {
   Collider* collider = lovrColliderCreate(world, x, y, z);
   SphereShape* shape = lovrSphereShapeCreate(radius);
   lovrColliderAddShape(collider, shape);
+  lovrColliderInitInertia(collider, shape);
   luax_pushtype(L, Collider, collider);
   lovrRelease(Collider, collider);
   lovrRelease(Shape, shape);
diff --git a/src/modules/physics/physics.c b/src/modules/physics/physics.c
index 374a0513..b771de4d 100644
--- a/src/modules/physics/physics.c
+++ b/src/modules/physics/physics.c
@@ -332,6 +332,14 @@ void lovrColliderDestroyData(Collider* collider) {
   lovrRelease(Collider, collider);
 }
 
+void lovrColliderInitInertia(Collider* collider, Shape * shape) {
+  // compute inertia matrix for default density
+  const float density = 1.0f;
+  float cx, cy, cz, mass, inertia[6];
+  lovrShapeGetMass(shape, density, &cx, &cy, &cz, &mass, inertia);
+  lovrColliderSetMassData(collider, cx, cy, cz, mass, inertia);
+}
+
 World* lovrColliderGetWorld(Collider* collider) {
   return collider->world;
 }
diff --git a/src/modules/physics/physics.h b/src/modules/physics/physics.h
index 20f77080..fb4ce1a9 100644
--- a/src/modules/physics/physics.h
+++ b/src/modules/physics/physics.h
@@ -5,7 +5,7 @@
 
 #pragma once
 
-#define MAX_CONTACTS 4
+#define MAX_CONTACTS 10
 #define MAX_TAGS 16
 #define NO_TAG ~0u
 
@@ -111,6 +111,7 @@ Collider* lovrColliderInit(Collider* collider, World* world, float x, float y, f
 #define lovrColliderCreate(...) lovrColliderInit(lovrAlloc(Collider), __VA_ARGS__)
 void lovrColliderDestroy(void* ref);
 void lovrColliderDestroyData(Collider* collider);
+void lovrColliderInitInertia(Collider* collider, Shape * shape);
 World* lovrColliderGetWorld(Collider* collider);
 void lovrColliderAddShape(Collider* collider, Shape* shape);
 void lovrColliderRemoveShape(Collider* collider, Shape* shape);