Doing undamped simulation correctly

5 files changed, 168 insertions, 66 deletions

diff --git a/2d/softbody/softbody_1/dist/output.js b/2d/softbody/softbody_1/dist/output.js
index 9de60fb..9992b73 100644
--- a/2d/softbody/softbody_1/dist/output.js
+++ b/2d/softbody/softbody_1/dist/output.js
@@ -2676,6 +2676,31 @@ var ASM_CONSTS = {
       GL.postDrawHandleClientVertexAttribBindings();
     }
 
+  function _glDrawElements(mode, count, type, indices) {
+      var buf;
+      if (!GLctx.currentElementArrayBufferBinding) {
+        var size = GL.calcBufLength(1, type, 0, count);
+        buf = GL.getTempIndexBuffer(size);
+        GLctx.bindBuffer(0x8893 /*GL_ELEMENT_ARRAY_BUFFER*/, buf);
+        GLctx.bufferSubData(0x8893 /*GL_ELEMENT_ARRAY_BUFFER*/,
+                                 0,
+                                 HEAPU8.subarray(indices, indices + size));
+        // the index is now 0
+        indices = 0;
+      }
+  
+      // bind any client-side buffers
+      GL.preDrawHandleClientVertexAttribBindings(count);
+  
+      GLctx.drawElements(mode, count, type, indices);
+  
+      GL.postDrawHandleClientVertexAttribBindings(count);
+  
+      if (!GLctx.currentElementArrayBufferBinding) {
+        GLctx.bindBuffer(0x8893 /*GL_ELEMENT_ARRAY_BUFFER*/, null);
+      }
+    }
+
   function _glEnable(x0) { GLctx['enable'](x0) }
 
   function _glEnableVertexAttribArray(index) {
@@ -3059,6 +3084,7 @@ var asmLibraryArg = {
   "glDepthFunc": _glDepthFunc,
   "glDepthMask": _glDepthMask,
   "glDrawArrays": _glDrawArrays,
+  "glDrawElements": _glDrawElements,
   "glEnable": _glEnable,
   "glEnableVertexAttribArray": _glEnableVertexAttribArray,
   "glGenBuffers": _glGenBuffers,
diff --git a/2d/softbody/softbody_1/dist/output.wasm b/2d/softbody/softbody_1/dist/output.wasm
index 4c82a10..b034adc 100755
--- a/2d/softbody/softbody_1/dist/output.wasm
+++ b/2d/softbody/softbody_1/dist/output.wasm
diff --git a/2d/softbody/softbody_1/undamped.cpp b/2d/softbody/softbody_1/undamped.cpp
index b041a6f..68e0636 100644
--- a/2d/softbody/softbody_1/undamped.cpp
+++ b/2d/softbody/softbody_1/undamped.cpp
@@ -36,13 +36,11 @@ namespace Undamped {
 
         // -- Spring initial variables
         float32 k = 4; // Spring Constant, in N / m
-        float32 initialDisplacement = 3.f;
-        float32 initialVelocity = 0.f;
 
         // -- Spring runtime variables
-        float32 angularVelocity = 0.f;
-        float32 displacement = 0.f;
-        float32 timeElapsed = 0.f;
+		float32 force = 0.f;
+        float32 velocity = 0.f;
+        float32 position = 0.f;
 
         void load(Renderer2d* renderer, SpringWeight* inWieight, float32 length, float32 inInitialDisplacement, float32 inK, float32 loopRadius);
         void update(float32 dtSeconds);
@@ -106,6 +104,7 @@ namespace Undamped {
     void SpringWeight::load(Renderer2d* renderer, float32 inMass, Vector4 startColor, Vector4 endColor) {
         mass = inMass;
         radius = mass * 16.f;
+		if (radius > 42) radius = 42.f;
         const int32 numSegments = 96;
         const float32 radiansPerSegment = (2.f * PI) / static_cast<float>(numSegments);
         const int32 numVertices = numSegments * 3;
@@ -149,14 +148,12 @@ namespace Undamped {
         shape.unload();
     }
 
-    void Spring::load(Renderer2d* renderer, SpringWeight* inWeight, float32 length, float32 inInitialDisplacement, float32 inK, float32 loopRadius) {
+    void Spring::load(Renderer2d* renderer, SpringWeight* inWeight, float32 length, float32 initialDisplacement, float32 inK, float32 loopRadius) {
         weight = inWeight;
-        initialDisplacement = inInitialDisplacement;
-		displacement = 0;
+	    position = initialDisplacement;
         k = inK;
-
-        angularVelocity = sqrtf(k / weight->mass);
-        timeElapsed = 0.f;
+		force = -k * initialDisplacement;
+        velocity = sqrtf(k / weight->mass);
         
         const int32 verticesPerSegment = 6;
         numSegments = 256;
@@ -170,12 +167,19 @@ namespace Undamped {
         const float32 offset = 0.25f;
 
         int32 vidx = 0;
+		float32 dx = position;
         for (int pidx = 0; pidx < numSegments; pidx++) {
             float32 y1 = lengthIncrement * pidx;
             float32 x1 = loopWidth * sinf(frequency * y1 + offset);
 
             float32 y2 = y1 + lengthIncrement;
             float32 x2 = loopWidth * sinf(frequency * y2 + offset);
+
+			float32 y1Offset = dx * (1.f - pidx / static_cast<float32>(numSegments));
+            float32 y2Offset = dx * (1.f - (pidx + 1) / static_cast<float32>(numSegments));
+
+			y1 += y1Offset;
+			y2 += y2Offset;
         
             vertices[vidx++].position = Vector2(x1, y1);
             vertices[vidx++].position = Vector2(x1, y2);
@@ -188,15 +192,16 @@ namespace Undamped {
         shape.load(vertices, numVertices, renderer, GL_DYNAMIC_DRAW);
         shape.model = Mat4x4().translateByVec2(Vector2(400, 300));
 
-        weight->shape.model = shape.model.translateByVec2(Vector2(0, -weight->radius));
+        weight->shape.model = shape.model.translateByVec2(Vector2(0, -weight->radius + dx));
     }
 
     void Spring::update(float32 dtSeconds) {
-        timeElapsed += dtSeconds;
-        float32 lastDisplacement = displacement;
-        displacement = initialDisplacement * cosf(angularVelocity * timeElapsed - initialVelocity);
-        float32 dx = displacement - lastDisplacement;
+		float32 lastPosition = position;
+		force = -k * position;
+	    velocity = velocity + (force * dtSeconds) / weight->mass;
+		position = position + velocity * dtSeconds;
 
+		float32 dx = position - lastPosition;
         int32 vidx = 0;
         for (int pidx = 0; pidx < numSegments; pidx++) {
             float32 y1Offset = dx * (1.f - pidx / static_cast<float32>(numSegments));
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;
 		}
 	}
diff --git a/2d/softbody/softbody_2/dist/output.wasm b/2d/softbody/softbody_2/dist/output.wasm
index cd26200..85d82ce 100755
--- a/2d/softbody/softbody_2/dist/output.wasm
+++ b/2d/softbody/softbody_2/dist/output.wasm