Fix some physics problems between player and grabbed objects

README.md

@@ -84,6 +84,10 @@ Where `/home/james/Blender/blender-2.93.1-linux-x64` is the folder where Blender
 
 ## Current TODO list
 
+- [ ] fix chell animation problem
+- [ ] first portal wall
+- [ ] sound effects for ball
+--------------------------------------------------------
 - [ ] Skips audio sometimes
 - [ ] Portal not rending recursively sometimes
 - [ ] Passing into a ceiling portal can sometimes mess with the player rotation
@@ -92,7 +96,9 @@ Where `/home/james/Blender/blender-2.93.1-linux-x64` is the folder where Blender
 - [ ] Presort portal gun polygon order
 - [ ] Signage should not always be on
 - [ ] Camera shake
-- [x] Release grabbed objects when line of sight is cut
+- [x] the cube bounces the player
+- [x] break constraint after large distance
+- [x] prevent objects from pushing the player around
 - [x] Stop looping sounds betwen levels
 - [x] red light district
 - [x] burn marks

src/physics/contact_solver.c

@@ -195,7 +195,7 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 		struct RigidBody* bodyA = cs->shapeA->body;
 		struct RigidBody* bodyB = cs->shapeB->body;
 
-		if (bodyA) {
+		if (bodyA && !(bodyA->flags & RigidBodyIsKinematic)) {
 			vA = &bodyA->velocity;
 			wA = &bodyA->angularVelocity;
 		} else {
@@ -203,7 +203,7 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 			wA = NULL;
 		}
 
-		if (bodyB) {
+		if (bodyB && !(bodyB->flags & RigidBodyIsKinematic)) {
 			vB = &bodyB->velocity;
 			wB = &bodyB->angularVelocity;
 		} else {
@@ -222,11 +222,11 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
             vector3Cross(&c->contactBWorld, &cs->normal, &rbCn);
 			float nm = 0;
 			
-			if (bodyA) {
+			if (vA) {
 				nm += bodyA->massInv;
 			}
 
-			if (bodyB) {
+			if (vB) {
 				nm += bodyB->massInv;
 			}
 
@@ -234,11 +234,11 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 			tm[ 0 ] = nm;
 			tm[ 1 ] = nm;
 
-			if (bodyA) {
+			if (vA) {
 				nm += bodyA->momentOfInertiaInv * vector3MagSqrd(&raCn);
 			}
 
-			if (bodyB) {
+			if (vB) {
 				nm += bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCn);
 			}
 
@@ -251,10 +251,10 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 				struct Vector3 rbCt;
                 vector3Cross(&cs->tangentVectors[ i ], &c->contactBWorld, &rbCt);
 
-				if (bodyA) {
+				if (vA) {
 					tm[ i ] += bodyA->momentOfInertiaInv * vector3MagSqrd(&raCt);
 				}
-				if (bodyB) {
+				if (vB) {
 					tm[ i ] += bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCt);
 				}
 
@@ -276,13 +276,13 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 
             struct Vector3 w;
 
-			if (bodyA) {
+			if (vA) {
 				vector3AddScaled(vA, &P, -bodyA->massInv, vA);
 				vector3Cross(&c->contactAWorld, &P, &w);
 				vector3AddScaled(wA, &w, -bodyA->momentOfInertiaInv, wA);
 			}
 
-			if (bodyB) {
+			if (vB) {
 				vector3AddScaled(vB, &P, bodyB->massInv, vB);
 				vector3Cross(&c->contactBWorld, &P, &w);
 				vector3AddScaled(wB, &w, bodyB->momentOfInertiaInv, wB);
@@ -314,11 +314,31 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
 	}
 }
 
+#define BREAK_DISTANCE	0.5f
+
 void contactSolverIterateConstraints(struct ContactSolver* contactSolver) {
 	struct PointConstraint* curr = contactSolver->firstPointConstraint;
 
+	struct PointConstraint* prev = NULL;
+
 	while (curr) {
-		pointConstraintMoveToPoint(curr->object, &curr->targetPos, curr->maxPosImpulse);
+		if (!pointConstraintMoveToPoint(curr->object, &curr->targetPos, curr->maxPosImpulse)) {
+			struct PointConstraint* next = curr->nextConstraint;
+
+			if (prev) {
+				prev->nextConstraint = next;
+			} else {
+				contactSolver->firstPointConstraint = next;
+			}
+
+
+			curr->nextConstraint = NULL;
+			curr->object = NULL;
+
+			curr = next;
+			continue;
+		}
+
 		pointConstraintRotateTo(curr->object->body, &curr->targetRot, curr->maxRotImpulse);		
 
 		curr = curr->nextConstraint;
@@ -343,7 +363,7 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
 		struct RigidBody* bodyA = cs->shapeA->body;
 		struct RigidBody* bodyB = cs->shapeB->body;
 
-		if (bodyA) {
+		if (bodyA && !(bodyA->flags & RigidBodyIsKinematic)) {
 			vA = &bodyA->velocity;
 			wA = &bodyA->angularVelocity;
 		} else {
@@ -351,7 +371,7 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
 			wA = NULL;
 		}
 
-		if (bodyB) {
+		if (bodyB && !(bodyB->flags & RigidBodyIsKinematic)) {
 			vB = &bodyB->velocity;
 			wB = &bodyB->angularVelocity;
 		} else {

src/physics/point_constraint.c

@@ -2,12 +2,19 @@
 
 #include "../util/time.h"
 
-void pointConstraintMoveToPoint(struct CollisionObject* object, struct Vector3* worldPoint, float maxImpulse) {
+#define BREAK_CONSTRAINT_DISTANCE 2.0f
+
+int pointConstraintMoveToPoint(struct CollisionObject* object, struct Vector3* worldPoint, float maxImpulse) {
     struct RigidBody* rigidBody = object->body;
 
-    vector3Sub(worldPoint, &rigidBody->transform.position, &rigidBody->velocity);
     struct Vector3 targetVelocity;
-    vector3Scale(&rigidBody->velocity, &targetVelocity, 1.0f / FIXED_DELTA_TIME);
+    vector3Sub(worldPoint, &rigidBody->transform.position, &targetVelocity);
+
+    if (vector3MagSqrd(&targetVelocity) > BREAK_CONSTRAINT_DISTANCE * BREAK_CONSTRAINT_DISTANCE) {
+        return 0;
+    }
+
+    vector3Scale(&targetVelocity, &targetVelocity, 1.0f / FIXED_DELTA_TIME);
 
     struct ContactManifold* contact = contactSolverNextManifold(&gContactSolver, object, NULL);
 
@@ -38,6 +45,8 @@ void pointConstraintMoveToPoint(struct CollisionObject* object, struct Vector3*
     } else {
         vector3AddScaled(&rigidBody->velocity, &delta, maxImpulse / sqrtf(deltaSqrd), &rigidBody->velocity);
     }
+
+    return 1;
 }
 
 void pointConstraintRotateTo(struct RigidBody* rigidBody, struct Quaternion* worldRotation, float maxImpulse) {

src/physics/point_constraint.h

@@ -16,7 +16,7 @@ struct PointConstraint {
 void pointConstraintInit(struct PointConstraint* constraint, struct CollisionObject* object, float maxPosImpulse, float maxRotImpulse);
 void pointConstraintUpdateTarget(struct PointConstraint* constraint, struct Vector3* worldPoint, struct Quaternion* worldRotation);
 
-void pointConstraintMoveToPoint(struct CollisionObject* object, struct Vector3* worldPoint, float maxImpulse);
+int pointConstraintMoveToPoint(struct CollisionObject* object, struct Vector3* worldPoint, float maxImpulse);
 void pointConstraintRotateTo(struct RigidBody* rigidBody, struct Quaternion* worldRotation, float maxImpulse);
 
 #endif

src/player/player.c

@@ -155,15 +155,21 @@ void playerInit(struct Player* player, struct Location* startLocation, struct Ve
 #define JUMP_IMPULSE   2.7f
 
 void playerHandleCollision(struct Player* player) {
-    struct ContactManifold* contact = contactSolverNextManifold(&gContactSolver, &player->collisionObject, NULL);
+    for (struct ContactManifold* contact = contactSolverNextManifold(&gContactSolver, &player->collisionObject, NULL);
-
+        contact;
-    while (contact) {
+        contact = contactSolverNextManifold(&gContactSolver, &player->collisionObject, contact)
+    ) {
         float offset = 0.0f;
 
         for (int i = 0; i < contact->contactCount; ++i) {
             struct ContactPoint* contactPoint = &contact->contacts[i];
             offset = MIN(offset, contactPoint->penetration);
         }
+
+        if (contact->shapeA == player->grabConstraint.object || contact->shapeB == player->grabConstraint.object) {
+            // objects being grabbed by the player shouldn't push the player
+            continue;
+        }
         
         if (offset != 0.0f) {
             vector3AddScaled(
@@ -184,12 +190,6 @@ void playerHandleCollision(struct Player* player) {
         if (isColliderForBall(contact->shapeA) || isColliderForBall(contact->shapeB)) {
             playerKill(player, 0);
         }
-
-        if (contact->shapeA == player->grabConstraint.object || contact->shapeB == player->grabConstraint.object) {
-            playerSetGrabbing(player, NULL);
-        }
-
-        contact = contactSolverNextManifold(&gContactSolver, &player->collisionObject, contact);
     }
 }
 
@@ -213,6 +213,12 @@ void playerSetGrabbing(struct Player* player, struct CollisionObject* grabbing)
     }
 }
 
+void playerSignalPortalChanged(struct Player* player) {
+    if (player->grabbingThroughPortal != PLAYER_GRABBING_THROUGH_NOTHING) {
+        playerSetGrabbing(player, NULL);
+    }
+}
+
 int playerRaycastGrab(struct Player* player, struct RaycastHit* hit) {
     struct Ray ray;
 
@@ -306,11 +312,6 @@ void playerUpdateGrabbedObject(struct Player* player) {
 
         pointConstraintUpdateTarget(&player->grabConstraint, &grabPoint, &grabRotation);
     }
-
-    struct RaycastHit hit;
-    if (player->grabConstraint.object && !(playerRaycastGrab(player, &hit) || hit.object != player->grabConstraint.object)) {
-        playerSetGrabbing(player, NULL);
-    }
 }
 
 #define DEADZONE_SIZE       5
@@ -500,7 +501,11 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
         hit.object->flags |= COLLISION_OBJECT_PLAYER_STANDING;
         player->flags |= PlayerFlagsGrounded;
 
-        if (hit.object->body) {
+        if (hit.object == player->grabConstraint.object) {
+            playerSetGrabbing(player, NULL);
+        }
+
+        if (hit.object->body && (hit.object->body->flags & RigidBodyIsKinematic)) {
             player->anchoredTo = hit.object->body;
             player->lastAnchorPoint = hit.at;
             transformPointInverseNoScale(&player->anchoredTo->transform, &hit.at, &player->relativeAnchor);

src/player/player.h

@@ -52,5 +52,6 @@ void playerKill(struct Player* player, int isUnderwater);
 
 int playerIsDead(struct Player* player);
 void playerSetGrabbing(struct Player* player, struct CollisionObject* grabbing);
+void playerSignalPortalChanged(struct Player* player);
 
 #endif

src/scene/scene.c

@@ -581,6 +581,7 @@ int sceneOpenPortal(struct Scene* scene, struct Transform* at, int transformInde
 
             contactSolverCheckPortalContacts(&gContactSolver, collisionObject);
             ballBurnFilterOnPortal(&portal->transform, portalIndex);
+            playerSignalPortalChanged(&scene->player);
             return 1;
         }
     }