work on physics

src/decor/decor_object.c

@@ -82,7 +82,7 @@ void decorObjectUpdate(struct DecorObject* decorObject) {
         soundPlayerUpdatePosition(decorObject->playingSound, &decorObject->rigidBody.transform.position);
     }
 
-    if (decorObject->rigidBody.flags & (RigidBodyIsTouchingPortal | RigidBodyWasTouchingPortal)) {
+    if (decorObject->rigidBody.flags & (RigidBodyIsTouchingPortalA | RigidBodyWasTouchingPortalA | RigidBodyIsTouchingPortalB | RigidBodyWasTouchingPortalB)) {
         dynamicSceneSetFlags(decorObject->dynamicId, DYNAMIC_SCENE_OBJECT_FLAGS_TOUCHING_PORTAL);
     } else {
         dynamicSceneClearFlags(decorObject->dynamicId, DYNAMIC_SCENE_OBJECT_FLAGS_TOUCHING_PORTAL);

src/physics/collision_object.c

@@ -66,8 +66,10 @@ void collisionObjectCollideWithQuad(struct CollisionObject* object, struct Colli
         &result
     );
 
-    if (collisionSceneIsTouchingPortal(&result.contactA, &result.normal)) {
-        object->body->flags |= RigidBodyIsTouchingPortal;
+    int touchingPortals = collisionSceneIsTouchingPortal(&result.contactA, &result.normal);
+
+    if (touchingPortals) {
+        object->body->flags |= touchingPortals;
         return;
     }
 
@@ -123,19 +125,24 @@ void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct
     }
 
     struct EpaResult result;
+    struct Vector3 objectEnd = object->body->transform.position;
+
     if (!epaSolveSwept(
         &simplex, 
         quad, minkowsiSumAgainstQuad, 
         &sweptObject, minkowsiSumAgainstSweptObject,
         objectPrevPos,
-        &object->body->transform.position,
+        &objectEnd,
         &result
     )) {
+        collisionObjectCollideWithQuad(object, quadObject, contactSolver);
         return;
     }
 
-    if (collisionSceneIsTouchingPortal(&result.contactA, &result.normal)) {
-        object->body->flags |= RigidBodyIsTouchingPortal;
+    int touchingPortals = collisionSceneIsTouchingPortal(&result.contactA, &result.normal);
+
+    if (touchingPortals) {
+        object->body->flags |= touchingPortals;
         return;
     }
 
@@ -145,6 +152,8 @@ void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct
         return;
     }
 
+    object->body->transform.position = objectEnd;
+
     contact->friction = MAX(object->collider->friction, quadObject->collider->friction);
     contact->restitution = MIN(object->collider->bounce, quadObject->collider->bounce);
 

src/physics/collision_scene.c

@@ -5,6 +5,7 @@
 #include "epa.h"
 #include "contact_solver.h"
 #include "../util/memory.h"
+#include "../scene/portal.h"
 
 struct CollisionScene gCollisionScene;
 
@@ -168,7 +169,7 @@ int collisionSceneIsTouchingPortal(struct Vector3* contactPoint, struct Vector3*
 
     for (int i = 0; i < 2; ++i) {
         if (collisionSceneIsTouchingSinglePortal(contactPoint, contactNormal, gCollisionScene.portalTransforms[i], i)) {
-            return 1;
+            return RigidBodyIsTouchingPortalA << i;
         }
     }
 
@@ -548,10 +549,22 @@ void collisionSceneCollideDynamicPairs(struct CollisionScene* collisionScene) {
     stackMallocFree(dynamicBroadphase.edges);
 }
 
+int collisionSceneObjectIsTouchingPortal(struct CollisionObject* object, int portalIndex) {
+    struct Simplex simplex;
+    struct Vector3 direction;
+    quatMultVector(&gCollisionScene.portalTransforms[portalIndex]->rotation, &gRight, &direction);
+    return gjkCheckForOverlap(&simplex, 
+        object, minkowsiSumAgainstObject,
+        gCollisionScene.portalTransforms[portalIndex], minkowsiSumAgainstPortal,
+        &direction
+    );   
+}
+
 void collisionSceneUpdateDynamics() {
     for (unsigned i = 0; i < gCollisionScene.dynamicObjectCount; ++i) {
         // added back in by contactSolverRemoveUnusedContacts if there are actually contacts
         gCollisionScene.dynamicObjects[i]->flags &= ~COLLISION_OBJECT_HAS_CONTACTS;
+        // gCollisionScene.dynamicObjects[i]->flags |= COLLISION_OBJECT_HAS_CONTACTS;
     }
 
 	contactSolverRemoveUnusedContacts(&gContactSolver);
@@ -562,6 +575,18 @@ void collisionSceneUpdateDynamics() {
             continue;
         }
 
+        if (object->body->flags & (RigidBodyIsTouchingPortalA | RigidBodyWasTouchingPortalA)) {
+            if (collisionSceneObjectIsTouchingPortal(object, 0)) {
+                object->body->flags |= RigidBodyIsTouchingPortalA;
+            }
+        }
+
+        if (object->body->flags & (RigidBodyIsTouchingPortalB | RigidBodyWasTouchingPortalB)) {
+            if (collisionSceneObjectIsTouchingPortal(object, 1)) {
+                object->body->flags |= RigidBodyIsTouchingPortalB;
+            }
+        }
+
         if (object->flags & COLLISION_OBJECT_HAS_CONTACTS || !collisionObjectIsActive(object)) {
             collisionObjectCollideWithScene(object, &gCollisionScene, &gContactSolver);
         } else {
@@ -573,7 +598,6 @@ void collisionSceneUpdateDynamics() {
             box3DUnion(&sweptBB, &object->boundingBox, &sweptBB);
 
             collisionObjectCollideWithSceneSwept(object, &prevPos, &sweptBB, &gCollisionScene, &gContactSolver);
-            collisionObjectUpdateBB(object);
 
             struct ContactManifold* manifold = contactSolverNextManifold(&gContactSolver, object, NULL);
 
@@ -594,8 +618,11 @@ void collisionSceneUpdateDynamics() {
 
                 if (velocityDot < 0.0f) {
                     vector3AddScaled(&object->body->velocity, &contact->normal, (1 + contact->restitution) * -velocityDot, &object->body->velocity);
+                    vector3AddScaled(&object->body->transform.position, &contact->normal, -0.01f, &object->body->transform.position);
                 }
             }
+
+            collisionObjectUpdateBB(object);
         }
     }
 

src/physics/epa.c

@@ -519,13 +519,19 @@ int epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum ob
             goto error;
         }
 
+
         result->penetration = 0;
 
         struct Vector3 planePos;
         vector3Scale(&raycastDir, &planePos, distance);
+        float moveOffset = vector3DistSqrd(bStart, bEnd);
+        
+        if (distance * distance > moveOffset + 0.1f) {
+            goto error;
+        }
+
         // move the swept object back to the first point of contact
         vector3Add(bEnd, &planePos, bEnd);
-
         epaCalculateContact(simplex, closestFace, &planePos, result);
     }
 

src/physics/rigid_body.c

@@ -53,7 +53,7 @@ void rigidBodyUpdate(struct RigidBody* rigidBody) {
     vector3AddScaled(&rigidBody->transform.position, &rigidBody->velocity, FIXED_DELTA_TIME, &rigidBody->transform.position);
     quatApplyAngularVelocity(&rigidBody->transform.rotation, &rigidBody->angularVelocity, FIXED_DELTA_TIME, &rigidBody->transform.rotation);
 
-    vector3Scale(&rigidBody->velocity, &rigidBody->velocity, ENERGY_SCALE_PER_STEP);
+    // vector3Scale(&rigidBody->velocity, &rigidBody->velocity, ENERGY_SCALE_PER_STEP);
     vector3Scale(&rigidBody->angularVelocity, &rigidBody->angularVelocity, ENERGY_SCALE_PER_STEP);
 }
 
@@ -87,13 +87,21 @@ int rigidBodyCheckPortals(struct RigidBody* rigidBody) {
 
     enum RigidBodyFlags newFlags = 0;
 
-    if (rigidBody->flags & RigidBodyIsTouchingPortal) {
-        newFlags |= RigidBodyWasTouchingPortal;
+    if (rigidBody->flags & RigidBodyIsTouchingPortalA) {
+        newFlags |= RigidBodyWasTouchingPortalA;
+    }
+
+    if (rigidBody->flags & RigidBodyIsTouchingPortalB) {
+        newFlags |= RigidBodyWasTouchingPortalB;
     }
 
     int result = 0;
 
     for (int i = 0; i < 2; ++i) {
+        if (!((RigidBodyIsTouchingPortalA << i) & rigidBody->flags) || !((RigidBodyWasTouchingPortalA << i) & rigidBody->flags)) {
+            continue;
+        }
+
         transformPointInverseNoScale(gCollisionScene.portalTransforms[i], &rigidBody->transform.position, &localPoint);
 
         int mask = (1 << i);
@@ -126,20 +134,18 @@ int rigidBodyCheckPortals(struct RigidBody* rigidBody) {
             continue;
         }
 
-        struct Vector3 scaledPoint;
-
-        scaledPoint.x = localPoint.x * (1.0f / PORTAL_X_RADIUS);
-        scaledPoint.y = localPoint.y;
-        scaledPoint.z = 0.0f;
-
-        if (vector3MagSqrd(&scaledPoint) > 1.0f) {
-            continue;
-        }
-
         struct Transform* otherPortal = gCollisionScene.portalTransforms[1 - i];
         rigidBodyTeleport(rigidBody, gCollisionScene.portalTransforms[i], otherPortal, gCollisionScene.portalRooms[1 - i]);
 
+        float speedSqrd = vector3MagSqrd(&rigidBody->velocity);
+
+        if (speedSqrd > MAX_PORTAL_SPEED * MAX_PORTAL_SPEED) {
+            vector3Normalize(&rigidBody->velocity, &rigidBody->velocity);
+            vector3Scale(&rigidBody->velocity, &rigidBody->velocity, MAX_PORTAL_SPEED);
+        }
+
         newFlags |= RigidBodyFlagsCrossedPortal0 << i;
+        newFlags |= RigidBodyIsTouchingPortalA << (1 - i);
         result = i + 1;
     }
 
@@ -149,8 +155,10 @@ int rigidBodyCheckPortals(struct RigidBody* rigidBody) {
         RigidBodyFlagsPortalsInactive |
         RigidBodyFlagsCrossedPortal0 |
         RigidBodyFlagsCrossedPortal1 |
-        RigidBodyIsTouchingPortal |
-        RigidBodyWasTouchingPortal
+        RigidBodyIsTouchingPortalA |
+        RigidBodyIsTouchingPortalB |
+        RigidBodyWasTouchingPortalA |
+        RigidBodyWasTouchingPortalB
     );
     rigidBody->flags |= newFlags;
 

src/physics/rigid_body.h

@@ -7,6 +7,8 @@
 
 #define RIGID_BODY_NO_ROOM  0xFFFF
 
+#define MAX_PORTAL_SPEED (1000.0f / 64.0f)
+
 enum RigidBodyFlags {
     RigidBodyFlagsInFrontPortal0 = (1 << 0),
     RigidBodyFlagsInFrontPortal1 = (1 << 1),
@@ -14,17 +16,19 @@ enum RigidBodyFlags {
     RigidBodyFlagsCrossedPortal0 = (1 << 3),
     RigidBodyFlagsCrossedPortal1 = (1 << 4),
     RigidBodyFlagsGrabbable = (1 << 5),
-    RigidBodyIsTouchingPortal = (1 << 6),
-    RigidBodyWasTouchingPortal = (1 << 7),
+    RigidBodyIsTouchingPortalA = (1 << 6),
+    RigidBodyIsTouchingPortalB = (1 << 7),
+    RigidBodyWasTouchingPortalA = (1 << 8),
+    RigidBodyWasTouchingPortalB = (1 << 9),
 
-    RigidBodyIsKinematic = (1 << 8),
-    RigidBodyIsSleeping = (1 << 9),
+    RigidBodyIsKinematic = (1 << 10),
+    RigidBodyIsSleeping = (1 << 11),
     // for kinematic bodies that should generate 
     // contacts with other kinematic bodies
-    RigidBodyGenerateContacts = (1 << 10),
+    RigidBodyGenerateContacts = (1 << 12),
 
-    RigidBodyFizzled = (1 << 11),
-    RigidBodyDisableGravity = (1 << 12),
+    RigidBodyFizzled = (1 << 13),
+    RigidBodyDisableGravity = (1 << 14),
 };
 
 struct RigidBody {

src/scene/portal.c

@@ -538,3 +538,44 @@ void portalCheckForHoles(struct Portal* portals) {
     }
     
 }
+
+int minkowsiSumAgainstPortal(void* data, struct Vector3* direction, struct Vector3* output) {
+    struct Transform* transform = (struct Transform*)data;
+    struct Vector3 localDir;
+    struct Quaternion inverseRotation;
+
+    quatConjugate(&transform->rotation, &inverseRotation);
+    quatMultVector(&inverseRotation, direction, &localDir);
+
+    float maxDot = vector3Dot(&gPortalOutline[0], &localDir);
+    int maxDotIndex = 0;
+
+    for (int i = 0; i < 9; ++i) {
+        if (maxDot < 0.0f) {
+            maxDotIndex = (maxDotIndex + 4) & 0x7;
+            maxDot = vector3Dot(&gPortalOutline[maxDotIndex], &localDir);
+        } else {
+            int prevIndex = (i - 1) & 0x7;
+            int nextIndex = (i + 1) & 0x7;
+
+            float prevDot = vector3Dot(&gPortalOutline[prevIndex], &localDir);
+            float nextDot = vector3Dot(&gPortalOutline[nextIndex], &localDir);
+
+            if (prevDot > maxDot) {
+                maxDotIndex = prevIndex;
+                maxDot = prevDot;
+            } else if (nextDot > maxDot) {
+                maxDotIndex = nextIndex;
+                maxDot = nextDot;
+            } else {
+                break;
+            }
+        }
+    }
+
+    quatMultVector(&transform->rotation, &gPortalOutline[maxDotIndex], output);
+    vector3Scale(output, output, 1.2f / SCENE_SCALE);
+    vector3Add(output, &transform->position, output);
+
+    return 1 << maxDotIndex;
+}

src/scene/portal.h

@@ -65,4 +65,7 @@ void portalRender(struct Portal* portal, struct Portal* otherPortal, struct Rend
 int portalAttachToSurface(struct Portal* portal, struct PortalSurface* surface, int surfaceIndex, struct Transform* portalAt);
 void portalCheckForHoles(struct Portal* portals);
 
+// data should be of type struct Transform
+int minkowsiSumAgainstPortal(void* data, struct Vector3* direction, struct Vector3* output);
+
 #endif