Got player button pressing working but standing on things is way unstable now

README.md

@@ -58,6 +58,5 @@ where `/home/james/Blender/blender-2.93.1-linux-x64` is the folder where blender
 ## Current TODO list
 
 Allow player to press button -- kinda done
-Cylinder raycasting
 cube dispenser
 nan in overlap

src/physics/collision_cylinder.c

@@ -179,13 +179,6 @@ int collisionCylinderRaycastCap(struct CollisionObject* cylinderObject, struct R
     return 1;
 }
 
-void collisionObjectLocalRay(struct CollisionObject* cylinderObject, struct Ray* ray, struct Ray* localRay) {
-    struct Vector3 offset;
-    vector3Sub(&ray->origin, &cylinderObject->body->transform.position, &offset);
-    basisUnRotate(&cylinderObject->body->rotationBasis, &ray->dir, &localRay->dir);
-    basisUnRotate(&cylinderObject->body->rotationBasis, &offset, &localRay->origin);
-}
-
 int collisionCylinderRaycast(struct CollisionObject* cylinderObject, struct Ray* ray, float maxDistance, struct RaycastHit* contact) {
     float rayLerp;
     float cylinderLerp;

src/physics/collision_object.c

@@ -100,6 +100,13 @@ void collisionObjectCollideTwoObjects(struct CollisionObject* a, struct Collisio
         return;
     }
 
+    if (contact->shapeA == b) {
+        struct Vector3 tmp = result.contactA;
+        result.contactA = result.contactB;
+        result.contactB = tmp;
+        vector3Negate(&result.normal, &result.normal);
+    }
+
     contact->friction = MAX(a->collider->friction, b->collider->friction);
     contact->restitution = MIN(a->collider->bounce, b->collider->bounce);
 
@@ -144,3 +151,10 @@ int minkowsiSumAgainstObject(void* data, struct Vector3* direction, struct Vecto
     vector3Add(output, &object->body->transform.position, output);
     return result;
 }
+
+void collisionObjectLocalRay(struct CollisionObject* cylinderObject, struct Ray* ray, struct Ray* localRay) {
+    struct Vector3 offset;
+    vector3Sub(&ray->origin, &cylinderObject->body->transform.position, &offset);
+    basisUnRotate(&cylinderObject->body->rotationBasis, &ray->dir, &localRay->dir);
+    basisUnRotate(&cylinderObject->body->rotationBasis, &offset, &localRay->origin);
+}

src/physics/collision_object.h

@@ -33,5 +33,6 @@ int minkowsiSumAgainstQuad(void* data, struct Vector3* direction, struct Vector3
 // data should be of type struct CollisionObject
 int minkowsiSumAgainstObject(void* data, struct Vector3* direction, struct Vector3* output);
 
+void collisionObjectLocalRay(struct CollisionObject* cylinderObject, struct Ray* ray, struct Ray* localRay);
 
 #endif

src/physics/raycasting.c

@@ -62,11 +62,8 @@ int raycastBox(struct CollisionObject* boxObject, struct Ray* ray, float maxDist
         return 0;
     }
 
-    struct Transform boxInverse;
-    transformInvert(&boxObject->body->transform, &boxInverse);
-
     struct Ray localRay;
-    rayTransform(&boxInverse, ray, &localRay);
+    collisionObjectLocalRay(boxObject, ray, &localRay);
 
     contact->distance = maxDistance;
 

src/player/player.c

@@ -12,6 +12,7 @@
 #include "../physics/config.h"
 #include "../physics/point_constraint.h"
 #include "../util/time.h"
+#include "../physics/contact_insertion.h"
 
 #define GRAB_RAYCAST_DISTANCE   2.5f
 
@@ -95,7 +96,7 @@ void playerHandleCollision(struct Player* player) {
             vector3AddScaled(
                 &player->body.transform.position, 
                 &contact->normal, 
-                (contact->shapeA == &player->collisionObject ? offset : -offset) * 0.9f, 
+                (contact->shapeA == &player->collisionObject ? offset : -offset) * 0.5f, 
                 &player->body.transform.position
             );
 
@@ -268,21 +269,50 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
 
     vector3AddScaled(&player->body.transform.position, &player->body.velocity, FIXED_DELTA_TIME, &player->body.transform.position);
 
-    playerHandleCollision(player);
-
     struct RaycastHit hit;
     struct Ray ray;
     ray.origin = player->body.transform.position;
     vector3Scale(&gUp, &ray.dir, -1.0f);
     if (collisionSceneRaycast(&gCollisionScene, player->body.currentRoom, &ray, COLLISION_LAYERS_TANGIBLE, PLAYER_HEAD_HEIGHT, 1, &hit)) {
-        vector3AddScaled(&hit.at, &gUp, PLAYER_HEAD_HEIGHT, &player->body.transform.position);
+        struct ContactManifold* collisionManifold = contactSolverGetContactManifold(&gContactSolver, &player->collisionObject, hit.object);
 
-        player->body.velocity.y = 0.0f;
+        struct EpaResult newContact;
+
+        newContact.id = 0xFFFF;
+
+        struct Vector3* playerContact;
+        struct Vector3* otherContact;
+        
+        if (collisionManifold->shapeA == &player->collisionObject) {
+            playerContact = &newContact.contactA;
+            otherContact = &newContact.contactB;
+            vector3Negate(&hit.normal, &newContact.normal);
+        } else {
+            playerContact = &newContact.contactB;
+            otherContact = &newContact.contactA;
+            newContact.normal = hit.normal;
+        }
+
+        playerContact->x = 0.0f; playerContact->y = -PLAYER_HEAD_HEIGHT; playerContact->z = 0.0f;
+        *otherContact = hit.at;
+
+        newContact.penetration = hit.distance - PLAYER_HEAD_HEIGHT;
+
+        if (hit.object && hit.object->body) {
+            transformPointInverseNoScale(&hit.object->body->transform, otherContact, otherContact);
+        }
+
+        collisionManifold->friction = MAX(player->collisionObject.collider->friction, hit.object->collider->friction);
+        collisionManifold->restitution = MIN(player->collisionObject.collider->bounce, hit.object->collider->bounce);
+
+        contactInsert(collisionManifold, &newContact);
         player->flags |= PlayerFlagsGrounded;
     } else {
         player->flags &= ~PlayerFlagsGrounded;
     }
 
+    playerHandleCollision(player);
+
     player->body.transform.rotation = player->lookTransform.rotation;    
 
     int didPassThroughPortal = rigidBodyCheckPortals(&player->body);