Doing undamped simulation correctly

1 files changed, 121 insertions, 50 deletions

diff --git a/2d/softbody/softbody_2/SpringRectangle.h b/2d/softbody/softbody_2/SpringRectangle.h
index 848db23..e2cdf60 100644
--- a/2d/softbody/softbody_2/SpringRectangle.h
+++ b/2d/softbody/softbody_2/SpringRectangle.h
@@ -2,17 +2,15 @@
 #include "../../../shared_cpp/types.h"
 #include "../../../shared_cpp/mathlib.h"
 
-struct SoftbodyUpdateVertexData {
-	Vector2 position;  // Position is in world coordinates, for math's sake.
-    Vector2 displacement;
-	Vector2 initialVelocity;
-	Vector2 initialDisplacement;
+struct PointMassUpdateData {
+	int32 index = 0;
+	Vector2 restingPosition;  // Position is in world coordinates
+	Vector2 currentPosition;  // Position is in world coordinates
+	Vector2 velocity;
+	Vector2 force;
 	bool isHovered = false;
 
-	SoftbodyUpdateVertexData* left;
-	SoftbodyUpdateVertexData* right;
-	SoftbodyUpdateVertexData* top;
-	SoftbodyUpdateVertexData* bottom;
+	PointMassUpdateData* neighbors[4];
 };
 
 EM_BOOL onMouseMove(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData);
@@ -20,20 +18,23 @@ EM_BOOL onMouseDown(int eventType, const EmscriptenMouseEvent *mouseEvent, void
 EM_BOOL onMouseUp(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData);
 
 struct SoftbodyRectangle {
+	// User defined
 	float32 width = 200;
 	float32 height = 200;
-	Vector2 position;
 	int32 springDensity = 10;
-	float32 springConstant = 4.f; // in N /m
-	float32 jointMassKg = 10;
-	float32 angularVelocity = 0.f;
+	float32 k = 3.f; // in N /m
+	float32 c = 3.f;
+	float32 jointMassKg = 1.f;
+
+	// Calculated before runtime
+    Vector2 springDimensions;
 
 	// Runtime data
 	float32 totalTimeSeconds = 0.f;
-	SoftbodyUpdateVertexData* updateData = NULL;
+	PointMassUpdateData* updateData = NULL;
 	bool hasLastPositionChanged = false;
 	Vector2 lastMousePosition;
-    SoftbodyUpdateVertexData* draggedVertex = NULL;
+    PointMassUpdateData* draggedVertex = NULL;
 
 	// Render data
 	Mesh2d mesh;
@@ -42,12 +43,12 @@ struct SoftbodyRectangle {
 	Vertex2d* pointsVertices = NULL;
 
 	void load(Renderer2d* renderer) {
-		position = Vector2(800 / 2 - width / 2, 600 / 2 - height / 2);
-		angularVelocity = sqrtf(springConstant / jointMassKg);
+		Vector2 position = Vector2(800 / 2 - width / 2, 600 / 2 - height / 2);
+		springDimensions = Vector2(width / springDensity, height / springDensity);
 	    int32 numVertices = springDensity * springDensity; // Each subdivision is a square.
 		int32 numIndices = 6 * ((springDensity - 1) * (springDensity - 1));
 	    vertices = new Vertex2d[numVertices];
-		updateData = new SoftbodyUpdateVertexData[numVertices];
+		updateData = new PointMassUpdateData[numVertices];
 		auto indices = new GLuint[numIndices];
 
 		// Load a square with the desired density
@@ -61,14 +62,21 @@ struct SoftbodyRectangle {
 				vpos.x = vpos.x * width + position.x;
 				vpos.y = vpos.y * height + position.y;
 				vertices[vIdx] = { vpos, Vector4(1, 0, 0, 1) };
-			    updateData[vIdx].position = vpos;
-				updateData[vIdx].initialVelocity = Vector2(randomFloatBetween(0, 1.0) * 0.1, randomFloatBetween(0, 1.0) * 0.1);
-				updateData[vIdx].initialDisplacement = Vector2(randomFloatBetween(0, 1.0) * 0.1, randomFloatBetween(0, 1.0) * 0.1);
 
-				if (x != 0) updateData[vIdx].left = &updateData[vIdx - 1];
-				if (x != springDensity - 1) updateData[vIdx].right = &updateData[vIdx + 1];
-				if (y != 0) updateData[vIdx].top = &updateData[vIdx - springDensity];
-				if (y != springDensity - 1) updateData[vIdx].bottom = &updateData[vIdx + springDensity];
+				updateData[vIdx].index = vIdx;
+			    updateData[vIdx].restingPosition = vpos;
+				updateData[vIdx].currentPosition = vpos;
+				updateData[vIdx].force = Vector2(0, 0);
+				updateData[vIdx].velocity = Vector2(0, 0);
+
+				if (x != 0)                 updateData[vIdx].neighbors[0] = &updateData[vIdx - 1]; // Left
+				else                        updateData[vIdx].neighbors[0] = NULL;
+				if (x != springDensity - 1) updateData[vIdx].neighbors[1] = &updateData[vIdx + 1]; // Right
+				else                        updateData[vIdx].neighbors[1] = NULL;
+				if (y != 0)                 updateData[vIdx].neighbors[2] = &updateData[vIdx - springDensity]; // Top
+				else                        updateData[vIdx].neighbors[2] = NULL;
+				if (y != springDensity - 1) updateData[vIdx].neighbors[3] = &updateData[vIdx + springDensity]; // Bottom
+				else                        updateData[vIdx].neighbors[3] = NULL;
 
 				if (y != springDensity - 1 && x != springDensity - 1) {
 					indices[iIdx++] = vIdx;
@@ -84,14 +92,12 @@ struct SoftbodyRectangle {
 		}
 
 		mesh.load(vertices, numVertices, indices, numIndices, renderer, GL_DYNAMIC_DRAW);
-		// mesh.model = Mat4x4().scale(Vector3(width, height, 0)).translateByVec2(position);
 
 	    pointsVertices = new Vertex2d[numVertices];
 		for (int32 v = 0; v < numVertices; v++) {
 			pointsVertices[v].position = vertices[v].position;
 			pointsVertices[v].color = Vector4(0, 0, 0, 1);
 		}
-		//pointsMesh.model = mesh.model;
 		pointsMesh.load(pointsVertices, numVertices, renderer, GL_DYNAMIC_DRAW);
 		
 		delete [] indices;
@@ -102,33 +108,98 @@ struct SoftbodyRectangle {
 		emscripten_set_mouseup_callback("#gl_canvas", this, false, onMouseUp);
 	}
 
+	Vector2 getForceBetweenPointMasses(PointMassUpdateData* first, PointMassUpdateData* second) {
+		auto relativeVelocity = second->velocity - first->velocity;
+		auto restLength = (first->restingPosition - second->restingPosition).length();
+		auto relativePosition = second->currentPosition - first->currentPosition;
+		auto currentLength = relativePosition.length();
+		auto positionDir = relativePosition.normalize();
+		auto dotProduct = positionDir.dot(relativeVelocity);
+	    float32 springForce = k * (currentLength - restLength);
+        float32 dampingForce = c * dotProduct;
+        float32 totalForce = springForce + dampingForce;
+		
+        return positionDir * totalForce;
+	}
+
 	void update(float32 dtSeconds) {
 		totalTimeSeconds += dtSeconds;
 
 		for (int32 v = 0; v < pointsMesh.numVertices; v++) {
-			auto springWeight = &updateData[v];
-			
-			//springWeight->displacement.x = springWeight->initialDisplacement.x * cosf(angularVelocity * totalTimeSeconds - springWeight->initialVelocity.x);
-			//springWeight->displacement.y = springWeight->initialDisplacement.y * cosf(angularVelocity * totalTimeSeconds - springWeight->initialVelocity.y);
-
-			//Vector2 nextPosition = springWeight->position + springWeight->displacement;
-			//vertices[v].position = nextPosition;
-			//pointsVertices[v].position = nextPosition;
-
-			if (springWeight == draggedVertex && hasLastPositionChanged) {
-				hasLastPositionChanged = false;
-				vertices[v].position = lastMousePosition;
-				pointsVertices[v].position = lastMousePosition;
+			auto pointMass = &updateData[v];
+
+			if (draggedVertex != NULL) {
+				if (pointMass == draggedVertex && hasLastPositionChanged) {
+					hasLastPositionChanged = false;
+				    Vector2 displacement = lastMousePosition - pointMass->restingPosition;
+
+					// We need to limit the new position based off of the triangle
+					if (displacement.x > springDimensions.x) {
+						displacement.x = springDimensions.x;
+					}
+					else if (displacement.x < -springDimensions.x) {
+						displacement.x = -springDimensions.x;
+					}
+
+					if (displacement.y > springDimensions.y) {
+						displacement.y = springDimensions.y;
+					}
+					else if (displacement.y < -springDimensions.y) {
+					    displacement.y = -springDimensions.y;
+					}
+
+				    pointMass->currentPosition = pointMass->restingPosition + displacement;
+					vertices[v].position = pointMass->currentPosition;
+					pointsVertices[v].position = pointMass->currentPosition;
+				}
 			}
+			else {
+				// Add the forces from it's neighbors
+				for (int32 n = 0; n < 4; n++) {
+				    auto neighbor = pointMass->neighbors[n];
+					if (neighbor == NULL) continue;
 
-			if (!draggedVertex && hasLastPositionChanged) {
-			    if ((springWeight->position - lastMousePosition).length() < 10.f) {
-					pointsVertices[v].color = Vector4(1, 1, 0, 1);
-					springWeight->isHovered = true;
+				    pointMass->force += getForceBetweenPointMasses(pointMass, neighbor);
+				}
+				
+			    pointMass->velocity = pointMass->velocity + (pointMass->force / jointMassKg) * dtSeconds;
+				pointMass->currentPosition = pointMass->currentPosition + (pointMass->velocity * dtSeconds);
+
+				const float32 COLLISION_DISTANCE = 4.f;
+			    for (int32 n = 0; n < pointsMesh.numVertices; n++) {
+					if (n == v) continue;
+				    auto neighbor = &updateData[n];
+
+					if ((neighbor->currentPosition - pointMass->currentPosition).length() < COLLISION_DISTANCE) {
+						auto positionNormal = (neighbor->currentPosition - pointMass->currentPosition).normalize();
+						pointMass->currentPosition = neighbor->currentPosition - positionNormal * COLLISION_DISTANCE;
+						float32 dotProduct = pointMass->velocity.dot(positionNormal);
+						pointMass->velocity = pointMass->velocity - positionNormal * (2 * dotProduct);
+					}
 				}
-				else {
-					pointsVertices[v].color = Vector4(0, 0, 0, 1);
-				    springWeight->isHovered = false;
+
+				vertices[v].position = pointMass->currentPosition;
+				pointsVertices[v].position = pointMass->currentPosition;
+
+				// Hovering highlights behavior
+				if (hasLastPositionChanged) {
+					if ((pointMass->currentPosition - lastMousePosition).length() < 10.f) {
+						pointsVertices[v].color = Vector4(1, 1, 0, 1);
+						pointMass->isHovered = true;
+
+						for (int32 n  = 0; n < 4; n++) {
+						    if (pointMass->neighbors[n])
+								pointsVertices[pointMass->neighbors[n]->index].color = Vector4(0, 0, 1, 1);
+						}
+					}
+					else if (pointMass->isHovered) {
+						pointsVertices[v].color = Vector4(0, 0, 0, 1);
+						for (int32 n  = 0; n < 4; n++) {
+						    if (pointMass->neighbors[n] && !pointMass->neighbors[n]->isHovered)
+								pointsVertices[pointMass->neighbors[n]->index].color = Vector4(0, 0, 0, 1);
+						}
+						pointMass->isHovered = false;
+					}
 				}
 			}
 		}
@@ -163,9 +234,9 @@ EM_BOOL onMouseDown(int eventType, const EmscriptenMouseEvent *mouseEvent, void
 	SoftbodyRectangle* rectangle = (SoftbodyRectangle*)userData;
 
     for (int32 v = 0; v < rectangle->pointsMesh.numVertices; v++) {
-		auto springWeight = &rectangle->updateData[v];
-		if (springWeight->isHovered) {
-			rectangle->draggedVertex = springWeight;
+		auto pointMass = &rectangle->updateData[v];
+		if (pointMass->isHovered) {
+			rectangle->draggedVertex = pointMass;
 			break;
 		}
 	}