Working softbody simulation

1 files changed, 106 insertions, 138 deletions

diff --git a/2d/softbody/softbody_2/SpringRectangle.h b/2d/softbody/softbody_2/SpringRectangle.h
index e2cdf60..2965efa 100644
--- a/2d/softbody/softbody_2/SpringRectangle.h
+++ b/2d/softbody/softbody_2/SpringRectangle.h
@@ -7,51 +7,47 @@ struct PointMassUpdateData {
 	Vector2 restingPosition;  // Position is in world coordinates
 	Vector2 currentPosition;  // Position is in world coordinates
 	Vector2 velocity;
+	Vector2 acceleration;
 	Vector2 force;
 	bool isHovered = false;
 
 	PointMassUpdateData* neighbors[4];
 };
 
-EM_BOOL onMouseMove(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData);
-EM_BOOL onMouseDown(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData);
-EM_BOOL onMouseUp(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData);
-
 struct SoftbodyRectangle {
 	// User defined
 	float32 width = 200;
 	float32 height = 200;
-	int32 springDensity = 10;
-	float32 k = 3.f; // in N /m
-	float32 c = 3.f;
-	float32 jointMassKg = 1.f;
+	int32 springDensity = 16;
+	float32 k = 10000000.f; // in N /m
+	float32 c = 9000.f;
+	float32 jointMassKg = 10.f;
+	float32 floorPosition = 0;
 
 	// Calculated before runtime
     Vector2 springDimensions;
 
 	// Runtime data
-	float32 totalTimeSeconds = 0.f;
 	PointMassUpdateData* updateData = NULL;
-	bool hasLastPositionChanged = false;
-	Vector2 lastMousePosition;
-    PointMassUpdateData* draggedVertex = NULL;
 
 	// Render data
 	Mesh2d mesh;
 	Mesh2d pointsMesh;
+	Mesh2d floorMesh;
 	Vertex2d* vertices = NULL;
 	Vertex2d* pointsVertices = NULL;
 
 	void load(Renderer2d* renderer) {
-		Vector2 position = Vector2(800 / 2 - width / 2, 600 / 2 - height / 2);
+	    auto defaultPosition = Vector2(800 / 2 - width / 2, 400);
 		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 PointMassUpdateData[numVertices];
+		pointsVertices = new Vertex2d[numVertices];
 		auto indices = new GLuint[numIndices];
 
-		// Load a square with the desired density
+		// -- Load a square with the desired density
 		int32 vIdx = 0;
 		int32 iIdx = 0;
 		float32 inverseDensity = 1.f / springDensity;
@@ -59,24 +55,26 @@ 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 + position.x;
-				vpos.y = vpos.y * height + position.y;
+				vpos.x = vpos.x * width + defaultPosition.x;
+				vpos.y = vpos.y * height + defaultPosition.y;
 				vertices[vIdx] = { vpos, Vector4(1, 0, 0, 1) };
 
 				updateData[vIdx].index = vIdx;
-			    updateData[vIdx].restingPosition = vpos;
+				updateData[vIdx].restingPosition = vpos;
 				updateData[vIdx].currentPosition = vpos;
 				updateData[vIdx].force = Vector2(0, 0);
 				updateData[vIdx].velocity = Vector2(0, 0);
+				updateData[vIdx].acceleration = Vector2(0, 0);
 
-				if (x != 0)                 updateData[vIdx].neighbors[0] = &updateData[vIdx - 1]; // Left
+				if (x != springDensity - 1) updateData[vIdx].neighbors[0] = &updateData[vIdx + 1]; // Right
 				else                        updateData[vIdx].neighbors[0] = NULL;
-				if (x != springDensity - 1) updateData[vIdx].neighbors[1] = &updateData[vIdx + 1]; // Right
+				if (y != springDensity - 1) updateData[vIdx].neighbors[1] = &updateData[vIdx + springDensity]; // Bottom
 				else                        updateData[vIdx].neighbors[1] = NULL;
-				if (y != 0)                 updateData[vIdx].neighbors[2] = &updateData[vIdx - springDensity]; // Top
+				if (x != 0)                 updateData[vIdx].neighbors[2] = &updateData[vIdx - 1]; // Left
 				else                        updateData[vIdx].neighbors[2] = NULL;
-				if (y != springDensity - 1) updateData[vIdx].neighbors[3] = &updateData[vIdx + springDensity]; // Bottom
+				if (y != 0)                 updateData[vIdx].neighbors[3] = &updateData[vIdx - springDensity]; // Top
 				else                        updateData[vIdx].neighbors[3] = NULL;
+				
 
 				if (y != springDensity - 1 && x != springDensity - 1) {
 					indices[iIdx++] = vIdx;
@@ -87,132 +85,129 @@ struct SoftbodyRectangle {
 					indices[iIdx++] = vIdx + 1;
 				}
 
+				pointsVertices[vIdx].position = vpos;
+				pointsVertices[vIdx].color = Vector4(0, 0, 0, 1);
+				
 				vIdx++;
 			}
 		}
 
 		mesh.load(vertices, numVertices, indices, numIndices, renderer, GL_DYNAMIC_DRAW);
-
-	    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.load(pointsVertices, numVertices, renderer, GL_DYNAMIC_DRAW);
-		
 		delete [] indices;
 
-		// Setup callbacks
-		emscripten_set_mousemove_callback("#gl_canvas", this, false, onMouseMove);
-		emscripten_set_mousedown_callback("#gl_canvas", this, false, onMouseDown);
-		emscripten_set_mouseup_callback("#gl_canvas", this, false, onMouseUp);
+		// -- 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 };
+		floorVertices[1] = { Vector4(floorDimensions.x, floorPosition, 0, 1), floorColor };
+		floorVertices[2] = { Vector4(0, floorPosition - floorDimensions.y, 0, 1), floorColor };
+		floorVertices[3] = { Vector4(0, floorPosition - floorDimensions.y, 0, 1), floorColor };
+		floorVertices[4] = { Vector4(floorDimensions.x, floorPosition - floorDimensions.y, 0, 1), floorColor };
+		floorVertices[5] = { Vector4(floorDimensions.x, floorPosition, 0, 1), floorColor };
+		floorMesh.load(floorVertices, 6, renderer);
 	}
 
 	Vector2 getForceBetweenPointMasses(PointMassUpdateData* first, PointMassUpdateData* second) {
 		auto relativeVelocity = second->velocity - first->velocity;
-		auto restLength = (first->restingPosition - second->restingPosition).length();
+		auto restLength = (second->restingPosition - first->restingPosition).length();
 		auto relativePosition = second->currentPosition - first->currentPosition;
 		auto currentLength = relativePosition.length();
 		auto positionDir = relativePosition.normalize();
-		auto dotProduct = positionDir.dot(relativeVelocity);
+		auto velDotProduct = positionDir.dot(relativeVelocity);
+		auto accelDotProduct = positionDir.dot(second->acceleration - first->acceleration);
 	    float32 springForce = k * (currentLength - restLength);
-        float32 dampingForce = c * dotProduct;
-        float32 totalForce = springForce + dampingForce;
-		
+        float32 dampingForce = c * velDotProduct;
+		float32 accelerationForce = jointMassKg * accelDotProduct;
+        float32 totalForce = accelerationForce + springForce + dampingForce;
+
         return positionDir * totalForce;
 	}
 
 	void update(float32 dtSeconds) {
-		totalTimeSeconds += dtSeconds;
-
-		for (int32 v = 0; v < pointsMesh.numVertices; v++) {
+	    for (int32 v = 0; v < pointsMesh.numVertices; v++) {
 			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;
+			// -- 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++) {
+				auto neighbor = pointMass->neighbors[n];
+				if (neighbor == NULL) continue;
 
-				    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);
-					}
-				}
+				auto forceBetween = getForceBetweenPointMasses(pointMass, neighbor);
+				pointMass->force = pointMass->force + forceBetween;
+				neighbor->force = neighbor->force- forceBetween;
+			}
+		}
 
-				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;
-					}
+		// -- 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->force = Vector2(0, 0); // Reset the force for the next update
+
+			particleFloorCollision(pointMass, prevPos, dtSeconds);
+
+			// -- Collision detection
+			const float32 COLLISION_DISTANCE = 0.3f;
+			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;
+					float32 dotProduct = pointMass->velocity.dot(positionNormal);
+					pointMass->velocity = pointMass->velocity - positionNormal * (2 * dotProduct);
 				}
 			}
+			
+			vertices[v].position = pointMass->currentPosition;
+			pointsVertices[v].position = pointMass->currentPosition;
 		}
 
-		if (hasLastPositionChanged) hasLastPositionChanged = false;
 
+		// -- Update vertices
 		mesh.updateVertices(vertices);
 		pointsMesh.updateVertices(pointsVertices);
 	}
 
+    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 (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;
+		    ud->velocity = (ud->velocity - Vector2(0, 1) * (2 * dotProduct)) * 0.5f;
+		}
+	}
+
 	void render(Renderer2d* renderer) {
 		mesh.render(renderer);
 		pointsMesh.render(renderer, GL_POINTS);
+		floorMesh.render(renderer);
 	}
 
 	void unload() {
@@ -222,30 +217,3 @@ struct SoftbodyRectangle {
 		delete [] pointsVertices;
 	}
 };
-
-EM_BOOL onMouseMove(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData) {
-	SoftbodyRectangle* rectangle = (SoftbodyRectangle*)userData;
-	rectangle->hasLastPositionChanged = true;
-	rectangle->lastMousePosition = Vector2(static_cast<float32>(mouseEvent->targetX), static_cast<float32>(600 - mouseEvent->targetY));
-	return true;
-}
-
-EM_BOOL onMouseDown(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData) {
-	SoftbodyRectangle* rectangle = (SoftbodyRectangle*)userData;
-
-    for (int32 v = 0; v < rectangle->pointsMesh.numVertices; v++) {
-		auto pointMass = &rectangle->updateData[v];
-		if (pointMass->isHovered) {
-			rectangle->draggedVertex = pointMass;
-			break;
-		}
-	}
-
-	return true;
-}
-
-EM_BOOL onMouseUp(int eventType, const EmscriptenMouseEvent *mouseEvent, void *userData) {
-	SoftbodyRectangle* rectangle = (SoftbodyRectangle*)userData;
-	rectangle->draggedVertex = NULL;
-	return true;
-}