Actual working springy rectangle

1 files changed, 77 insertions, 52 deletions

diff --git a/2d/softbody/softbody_2/SpringRectangle.h b/2d/softbody/softbody_2/SpringRectangle.h
index 2965efa..02e5308 100644
--- a/2d/softbody/softbody_2/SpringRectangle.h
+++ b/2d/softbody/softbody_2/SpringRectangle.h
@@ -1,14 +1,19 @@
 #include "../../../shared_cpp/Renderer2d.h"
 #include "../../../shared_cpp/types.h"
 #include "../../../shared_cpp/mathlib.h"
+#include <cstdio>
+#include <math.h>
+
+Vector2 GRAVITY_ACCELERATION = Vector2(0, -100.f);
 
 struct PointMassUpdateData {
 	int32 index = 0;
-	Vector2 restingPosition;  // Position is in world coordinates
-	Vector2 currentPosition;  // Position is in world coordinates
-	Vector2 velocity;
+	Vector2 localPosition;  // Position is in world coordinates
+	Vector2 worldPosition;  // Position is in world coordinates
 	Vector2 acceleration;
+	Vector2 velocity;
 	Vector2 force;
+
 	bool isHovered = false;
 
 	PointMassUpdateData* neighbors[4];
@@ -19,16 +24,20 @@ struct SoftbodyRectangle {
 	float32 width = 200;
 	float32 height = 200;
 	int32 springDensity = 16;
-	float32 k = 10000000.f; // in N /m
-	float32 c = 9000.f;
-	float32 jointMassKg = 10.f;
-	float32 floorPosition = 0;
+	float32 k = 8000.f; // in N /m
+	float32 c = 30.f;
+	float32 jointMassKg = 1.f;
+	float32 floorPosition = 200;
 
 	// Calculated before runtime
     Vector2 springDimensions;
 
 	// Runtime data
+	Vector2 velocity;
+	Vector2 worldPosition;
 	PointMassUpdateData* updateData = NULL;
+	float32 rotationalVelocity = 0.f;
+	float32 rotation = 0.f;
 
 	// Render data
 	Mesh2d mesh;
@@ -38,7 +47,8 @@ struct SoftbodyRectangle {
 	Vertex2d* pointsVertices = NULL;
 
 	void load(Renderer2d* renderer) {
-	    auto defaultPosition = Vector2(800 / 2 - width / 2, 400);
+		velocity = Vector2(0, 0);
+	    worldPosition = Vector2(800.f / 2 - width / 2, 400.f);
 		springDimensions = Vector2(width / springDensity, height / springDensity);
 	    int32 numVertices = springDensity * springDensity; // Each subdivision is a square.
 		int32 numIndices = 6 * ((springDensity - 1) * (springDensity - 1));
@@ -55,13 +65,16 @@ struct SoftbodyRectangle {
 		for (int32 y = 0; y < springDensity; y++) {  // Rows
 			for (int32 x = 0; x < springDensity; x++) { // Columns
 			    Vector2 vpos = Vector2(x * inverseDensity - halfInv, y * inverseDensity- halfInv);
-				vpos.x = vpos.x * width + defaultPosition.x;
-				vpos.y = vpos.y * height + defaultPosition.y;
+				// Get the position in mode coordinates
+				vpos.x = vpos.x * width;
+				vpos.y = vpos.y * height;
+				updateData[vIdx].localPosition = vpos;
+				// Get the position in world coorodinates
+				vpos.x = vpos.x + worldPosition.x;
+				vpos.y = vpos.y + worldPosition.y;
 				vertices[vIdx] = { vpos, Vector4(1, 0, 0, 1) };
-
 				updateData[vIdx].index = vIdx;
-				updateData[vIdx].restingPosition = vpos;
-				updateData[vIdx].currentPosition = vpos;
+				updateData[vIdx].worldPosition = vpos;
 				updateData[vIdx].force = Vector2(0, 0);
 				updateData[vIdx].velocity = Vector2(0, 0);
 				updateData[vIdx].acceleration = Vector2(0, 0);
@@ -98,7 +111,6 @@ struct SoftbodyRectangle {
 
 		// -- Load the floor line;
 		Vector2 floorDimensions = Vector2(renderer->context->width, 8);
-	    floorPosition = 100.f;
 		Vector4 floorColor = Vector4(0.5, 0.5, 0.5, 1);
 	    Vertex2d floorVertices[6];
 	    floorVertices[0] = { Vector4(0, floorPosition, 0, 1), floorColor };
@@ -111,71 +123,91 @@ struct SoftbodyRectangle {
 	}
 
 	Vector2 getForceBetweenPointMasses(PointMassUpdateData* first, PointMassUpdateData* second) {
+		const float32 EPSILON = 0.1f;
 		auto relativeVelocity = second->velocity - first->velocity;
-		auto restLength = (second->restingPosition - first->restingPosition).length();
-		auto relativePosition = second->currentPosition - first->currentPosition;
+		auto restLength = (second->localPosition - first->localPosition).length();
+		auto relativePosition = second->worldPosition - first->worldPosition;
 		auto currentLength = relativePosition.length();
 		auto positionDir = relativePosition.normalize();
 		auto velDotProduct = positionDir.dot(relativeVelocity);
-		auto accelDotProduct = positionDir.dot(second->acceleration - first->acceleration);
-	    float32 springForce = k * (currentLength - restLength);
+		auto lengthDifference = (currentLength - restLength);
+		if (ABS(lengthDifference) < EPSILON) {
+			lengthDifference = 0;
+		}
+	    float32 springForce = k * lengthDifference;
         float32 dampingForce = c * velDotProduct;
-		float32 accelerationForce = jointMassKg * accelDotProduct;
-        float32 totalForce = accelerationForce + springForce + dampingForce;
-
+        float32 totalForce = springForce + dampingForce;
         return positionDir * totalForce;
 	}
 
 	void update(float32 dtSeconds) {
+		// -- Apply all forces
 	    for (int32 v = 0; v < pointsMesh.numVertices; v++) {
 			auto pointMass = &updateData[v];
+			pointMass->force += GRAVITY_ACCELERATION * jointMassKg;
 
 			// -- Add the forces from it's neighbors. Note that we only do the first two
 			// neighbors, which are the right and bottom neighbors.
-			for (int32 n = 0; n < 2; n++) {
+			for (int32 n = 0; n < 4; n++) {
 				auto neighbor = pointMass->neighbors[n];
 				if (neighbor == NULL) continue;
 
 				auto forceBetween = getForceBetweenPointMasses(pointMass, neighbor);
 				pointMass->force = pointMass->force + forceBetween;
-				neighbor->force = neighbor->force- forceBetween;
 			}
 		}
 
-		// -- Update the local position of each vertex.
+		// -- Calculate the rotation of the entire body
+		//rotationalVelocity = 0.1f;
+		//auto gravityDt = GRAVITY_ACCELERATION * dtSeconds;
+		//velocity = velocity + gravityDt;
+		//worldPosition = worldPosition + (velocity * dtSeconds);
+
+		// -- Euler integrate and update the local position of each vertex.
 		for (int32 v = 0; v < pointsMesh.numVertices; v++) {
 			auto pointMass = &updateData[v];
-			auto prevPos = pointMass->currentPosition;
 			
-			// -- Gravity
-			Vector2 g =  Vector2(0, -9.8 * jointMassKg) * dtSeconds;
-
 			// -- Euler integration to find the current velocity and position
-			pointMass->acceleration = (pointMass->force / jointMassKg) * dtSeconds;
-			pointMass->velocity = pointMass->velocity + pointMass->acceleration * dtSeconds + g;
-			pointMass->restingPosition = pointMass->restingPosition + g * dtSeconds;
-			pointMass->currentPosition = pointMass->currentPosition + (pointMass->velocity * dtSeconds);
+			pointMass->acceleration = pointMass->force / jointMassKg;
+			pointMass->velocity = pointMass->velocity + (pointMass->acceleration * dtSeconds);
+			pointMass->worldPosition = pointMass->worldPosition + (pointMass->velocity * dtSeconds);
+			// pointMass->restingPosition = pointMass->restingPosition.rotateAround(rotationalVelocity * dtSeconds, modelPosition);
+			// pointMass->currentPosition = pointMass->currentPosition.rotateAround(rotationalVelocity * dtSeconds, modelPosition);
+
+			vertices[v].position = pointMass->worldPosition;
+			pointsVertices[v].position = pointMass->worldPosition;
+			pointMass->force = Vector2(); // Reset the force for the next update
+		}
 
-			pointMass->force = Vector2(0, 0); // Reset the force for the next update
+		// -- Collision detection
+		for (int32 v = 0; v < pointsMesh.numVertices; v++) {
+			auto pointMass = &updateData[v];
+			auto prevPos = pointMass->worldPosition;
 
+			// -- Floor
 			particleFloorCollision(pointMass, prevPos, dtSeconds);
 
-			// -- Collision detection
+			// 
 			const float32 COLLISION_DISTANCE = 0.3f;
+			// auto localVector = pointMass->worldPosition - worldPosition;
+			// auto displacement = (pointMass->worldPosition - worldPosition) - pointMass->localPosition;
+			// if (displacement.length() >= 20.f) {
+			// 	// auto positionNormal = (pointMass->restingPosition - pointMass->currentPosition).normalize();
+			// 	// pointMass->currentPosition = pointMass->restingPosition - positionNormal * COLLISION_DISTANCE;
+			// 	// float32 dotProduct = pointMass->velocity.dot(positionNormal);
+			// 	// pointMass->velocity = pointMass->velocity - positionNormal * (2 * dotProduct);
+			// }
 			for (int32 n = 0; n < 4; n++) {
 				auto neighbor = pointMass->neighbors[n];
 				if (neighbor == NULL) continue;
 
-				if ((neighbor->currentPosition - pointMass->currentPosition).length() < COLLISION_DISTANCE) {
-					auto positionNormal = (neighbor->currentPosition - pointMass->currentPosition).normalize();
-					pointMass->currentPosition = neighbor->currentPosition - positionNormal * COLLISION_DISTANCE;
+				if ((neighbor->worldPosition - pointMass->worldPosition).length() < COLLISION_DISTANCE) {
+					auto positionNormal = (neighbor->worldPosition - pointMass->worldPosition).normalize();
+					pointMass->worldPosition = neighbor->worldPosition - positionNormal * COLLISION_DISTANCE;
 					float32 dotProduct = pointMass->velocity.dot(positionNormal);
 					pointMass->velocity = pointMass->velocity - positionNormal * (2 * dotProduct);
 				}
 			}
-			
-			vertices[v].position = pointMass->currentPosition;
-			pointsVertices[v].position = pointMass->currentPosition;
 		}
 
 
@@ -187,19 +219,12 @@ struct SoftbodyRectangle {
     void particleFloorCollision(PointMassUpdateData* ud, Vector2 prevPos, float32 dtSeconds) {
 	    // We assume that the floor is always horizontal for this simulation
 	    auto dotProduct = ud->velocity.dot(Vector2(0, 1));
-		if (dotProduct >= 0) {
-			return; // Not moving in the same direction
-		}
+		// if (dotProduct >= 0) {
+		// 	return; // Not moving in the same direction
+		// }
 
-		if (ud->currentPosition.y - floorPosition < 0.1f) {
-			// Find the point in the simulation at which we began intersecting, and then reflect.
-			Vector2 newPosition;
-			do {
-				dtSeconds = dtSeconds - 0.02f;
-			    newPosition = prevPos + ud->velocity * dtSeconds;
-			} while (newPosition.y < floorPosition);
-			
-		    ud->currentPosition = newPosition;
+		if (ud->worldPosition.y - floorPosition < 0.1f) {
+		    ud->worldPosition.y = floorPosition - 0.1f;;
 		    ud->velocity = (ud->velocity - Vector2(0, 1) * (2 * dotProduct)) * 0.5f;
 		}
 	}