path: root/2d/softbody/softbody_1/damped.cpp

#include "damped.h"
#include "undamped.h"
#include "../../../shared_cpp/Renderer2d.h"
#include "../../../shared_cpp/types.h"
#include "../../../shared_cpp/WebglContext.h"
#include "../../../shared_cpp/mathlib.h"
#include "../../../shared_cpp/MainLoop.h"
#include <cstdio>
#include <cmath>
#include <emscripten/html5.h>
#include <unistd.h>
#include <pthread.h>
#include <cmath>
#include <cfloat>

namespace Damped {
    struct DampedSpringWeight {
        Mesh2d shape;
        float32 radius;
        float32 mass = 1.f;

        void load(Renderer2d* renderer, float32 inRadius, Vector4 startColor, Vector4 endColor);
        void update(float32 dtSeconds);
        void render(Renderer2d* renderer);
        void unload();
    };

	enum DampedSpringType {
		None = 0,
		Overdamped = 1,
		Underdamped = 2,
		Critical = 3
	};

    struct DampedSpring {
        DampedSpringWeight* weight;

        Mesh2d shape;

        Vertex2d* vertices = NULL;
        int32 numSegments = 0;
        int32 numVertices = 0;

        // Initialization variables
		float32 initialDisplacement = 0.f;
        float32 k = 4;        // DampedSpring Constant, in N / m (Hooke's Law)
        float32 c = 1.f;      // Viscous damping coefficient (Damping force)

		// Discovered during initialization
		DampedSpringType type = DampedSpringType::None;
        float32 R = 2.f;
        float32 gamma = 6.2;
        float32 discriminant = 0.f;
        float32 omega1       = 0.f;
		float32 c1 = 0.f;
		float32 c2 = 0.f;
		float32 r1 = 0.f;
		float32 r2 = 0.f;

        // Update variables
        float32 displacement = 0.f;
        float32 timeElapsed  = 0.f;
        

        void load(Renderer2d* renderer, DampedSpringWeight* inWieight, float32 length, float32 loopRadius, float32 initialDisplacement, float32 inK, float32 inC);
        void update(float32 dtSeconds);
        void render(Renderer2d* renderer);
        void unload();
    };

    DampedInitVariables initVariables;
    void setInitVariables(DampedInitVariables newVariables) { initVariables = newVariables; }
    DampedInitVariables getInitVariables() { return initVariables; }

    WebglContext* context;
    Renderer2d renderer;
    MainLoop mainLoop;
    DampedSpringWeight weight;
    DampedSpring spring;

    EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
    EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
    void load();
    void update(float32 deltaTimeSeconds, void* userData);
    void unload();

    void init(WebglContext* inContext) {
        context = inContext;
        emscripten_set_click_callback("#gl_canvas_play_damped", NULL, false, onPlayClicked);
        emscripten_set_click_callback("#gl_canvas_stop_damped", NULL, false, onStopClicked);
    }

    void load() {
        context->init("#gl_canvas_damped");

        renderer.load(context);

        weight.load(&renderer, initVariables.mass, Vector4 { 55.f, 235.f, 35.f, 255.f }, Vector4 { 235.f, 5.f, 235.f, 255.f });
        spring.load(&renderer, &weight, initVariables.springLength, 16.f, initVariables.initialDisplacement, initVariables.k, initVariables.c);

        mainLoop.run(update);
    }

    void update(float32 deltaTimeSeconds, void* userData) {
        // -- Update
        spring.update(deltaTimeSeconds);
        weight.update(deltaTimeSeconds);
	
        // -- Render
        renderer.render();
        weight.render(&renderer);
        spring.render(&renderer);
    }

    void unload() {
        mainLoop.stop();
        renderer.unload();
        weight.unload();
        spring.unload();
        context->destroy();
    }

    void DampedSpringWeight::load(Renderer2d* renderer, float32 inMass, Vector4 startColor, Vector4 endColor) {
        mass = inMass;
        radius = mass * 16.f;
        const int32 numSegments = 96;
        const float32 radiansPerSegment = (2.f * PI) / static_cast<float>(numSegments);
        const int32 numVertices = numSegments * 3;

        float32 t = 0.f;
        float32 tIncrement = 1.f / (numSegments / 2.f);
        startColor = startColor.toNormalizedColor();
        endColor = endColor.toNormalizedColor();

        Vertex2d vertices[numVertices];
        for (int idx = 0; idx < numSegments; idx++) {
            int vIdx = idx * 3;
        
            Vector4 color = lerp(startColor, endColor, t);
            if (idx >= numSegments / 2) {
                t -= tIncrement; 
            } else {
                t += tIncrement;
            }
        
            vertices[vIdx].color = color;
            vertices[vIdx].position = Vector2 { radius * cosf(radiansPerSegment * idx), radius * sinf(radiansPerSegment * idx) };
            vertices[vIdx + 1].color = color;
            vertices[vIdx + 1].position = Vector2 { 0.f, 0.f };
            vertices[vIdx + 2].color = color;
            vertices[vIdx + 2].position = Vector2 { radius * cosf(radiansPerSegment * (idx + 1)), radius * sinf(radiansPerSegment * (idx + 1)) };
        }

        shape.load(vertices, numVertices, renderer);
    }

    void DampedSpringWeight::update(float32 dtSeconds) {
    
    }

    void DampedSpringWeight::render(Renderer2d* renderer) {
        shape.render(renderer);
    }

    void DampedSpringWeight::unload() {
        shape.unload();
    }

    const float32 epsilon = 0.0001f;

    void DampedSpring::load(Renderer2d* renderer, DampedSpringWeight* inWeight, float32 length, float32 loopRadius, float32 inInitialDisplacement, float32 inK, float32 inC) {
		initialDisplacement = inInitialDisplacement;
        weight = inWeight;
		k = inK;
		c = inC;

		// Set the quadratic terms to something that we're familiar with.
		float32 A = weight->mass;
		float32 B = c;
		float32 C = k;
		

		discriminant = B * B - (4 * A * C);
		timeElapsed = 0;
		displacement = 0;

        if (discriminant < epsilon && discriminant > -epsilon) {
			// Real repeated root (~ zero): Critically damped motion
			printf("Critically damped motion.\n");
			
			type = DampedSpringType::Critical;

			r1 = -B / (2 * A);
			r2 = r1;

			// Solve for the initial displacement.
			c2 = (initialDisplacement - r1) / (r2);
			c2 = r2 * c2;
			c1 = r1;
			printf("Equation: %f e^(%f t) + %f t e^(%f t)\n", c1, r1, c2, r2);
        }
        else if (discriminant > 0) {
			// Two real roots (greater than zero): Overdamped motion
			printf("Overdamped motion.\n");
			
			type = DampedSpringType::Overdamped;

			// Discover the two treal roots. Both of these WILL be negative.
			float32 sqrtDis = sqrtf(discriminant);
			r1 = (-B + sqrtDis) / (2 * A);
			r2 = (-B - sqrtDis) / (2 * A);

			// Now, solve for c1 and c2 given the initial displacement. The equation is:
			// x(0) = r1 * c1 * e^(r1 * t) + r2 * c2 * e^(r2 * t)   ->
			// x(0) = r1 * c1 + r2 * c2                             ->
			// initialDisplacement  = r1 * c1 + r2 * c2;            -> if abs(r1) > abs(r2) => c1 = 1, else c2 = 1
			if (r1 < r2) {
				c2 = (initialDisplacement - r1) / (r2);
				c2 = r2 * c2;
				c1 = r1;
			}
			else {
				c1 = (initialDisplacement - r2) / (r1);
				c1 = r1 * c1;
				c2 = r2;
			}

			printf("Equation: %f e^(%f t) + %f e^(%f t)\n", c1, r1, c2, r2);
        }
        else {
			// Complex pair (less than zero): Underdamped motion
			printf("Underdamped motion.\n");
			
			type = DampedSpringType::Underdamped;
			float32 sqrtDis = sqrtf(-discriminant);
            omega1 = sqrtf(sqrtDis) / (2.f * A); // Note that we negate the discriminant to get the REAL positive part.

			r1 = (-B + sqrtDis) / (2 * A);
			r2 = (-B - sqrtDis) / (2 * A);

			// Now, solve for c1 and c2 given the initial displacement. The equation is:
			// x(0) = r1 * c1 * e^(r1 * t) + r2 * c2 * e^(r2 * t)   ->
			// x(0) = r1 * c1 + r2 * c2                             ->
			// initialDisplacement  = r1 * c1 + r2 * c2;            -> if abs(r1) > abs(r2) => c1 = 1, else c2 = 1
			if (r1 < r2) {
				c2 = (initialDisplacement - r1) / (r2);
				c2 = r2 * c2;
				c1 = r1;
			}
			else {
				c1 = (initialDisplacement - r2) / (r1);
				c1 = r1 * c1;
				c2 = r2;
			}

			// At this point, we have:
			// x(t) = c1 cos(omega1 t) + c2 cos(omega1 t)     ->
			// x(t) = A cos(omega1 t - Φ)                     ->
			// Now we need to solve for A and Φ for our initial displacement, so:
			// initialDisplacement = c1
			R = sqrtf(c1 * c1 + c2 * c2);
			if (initialDisplacement < 0) {
				R = -R;
			}
			gamma = atanf(c2 / c1);

			printf("Equation: %f e^(-(%f / %f) t) * cos(%f t - %f)\n", R, c, weight->mass, omega1, gamma);
        }

        const int32 verticesPerSegment = 6;
        numSegments = 256;
        numVertices = numSegments * verticesPerSegment;
        vertices = new Vertex2d[numVertices];

        float32 lengthIncrement = length / static_cast<float32>(numSegments);

        const float32 frequency = 0.25f;
        const float32 loopWidth = 20.f;
        const float32 offset = 0.25f;

        int32 vidx = 0;
        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);
        
            vertices[vidx++].position = Vector2(x1, y1);
            vertices[vidx++].position = Vector2(x1, y2);
            vertices[vidx++].position = Vector2(x2, y1);
            vertices[vidx++].position = Vector2(x2, y1);
            vertices[vidx++].position = Vector2(x1, y2);
            vertices[vidx++].position = Vector2(x2, y2);
        }

        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));
    }

    void DampedSpring::update(float32 dtSeconds) {
        timeElapsed += dtSeconds;

		float32 lastDisplacement = displacement;

		switch (type) {
		case DampedSpringType::Critical:
			displacement = c1 * pow(E, r1 * timeElapsed) + c2 * timeElapsed * pow(E, r2 * timeElapsed);
			break;
		case DampedSpringType::Overdamped:
			displacement = c1 * pow(E, r1 * timeElapsed) + c2 * pow(E, r2 * timeElapsed);
			break;
		case DampedSpringType::Underdamped:  {
			float32 exponent = -(c / (2 * weight->mass)) * timeElapsed;
            displacement = R * pow(E, exponent) * (cosf(omega1 * timeElapsed - gamma));
			break;
		}
		default:
			break;
		}

		float32 dx = displacement - lastDisplacement;
        int32 vidx = 0;
        for (int pidx = 0; pidx < numSegments; pidx++) {
            float32 y1Offset = dx * (1.f - pidx / static_cast<float32>(numSegments));
            float32 y2Offset = dx * (1.f - (pidx + 1) / static_cast<float32>(numSegments));
            vertices[vidx++].position.y += y1Offset;
            vertices[vidx++].position.y += y2Offset;
            vertices[vidx++].position.y += y1Offset;
            vertices[vidx++].position.y += y1Offset;
            vertices[vidx++].position.y += y2Offset;
            vertices[vidx++].position.y += y2Offset;
        }

        weight->shape.model = weight->shape.model.translateByVec2(Vector2(0, dx));
    }

    void DampedSpring::render(Renderer2d* renderer) {
        glBindBuffer(GL_ARRAY_BUFFER, shape.vbo);
        glBufferSubData(GL_ARRAY_BUFFER, 0, numVertices * sizeof(Vertex2d), &vertices[0]);

        shape.render(renderer);
    }

    void DampedSpring::unload() {
        shape.unload();
        delete[] vertices;
    }


    EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
        printf("Play clicked\n");
    
        load();
        return true;
    }

    EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
        printf("Stop clicked\n");
        unload();
        return true;
    }
}