Silkworm/src/Silkworm.c

/* Silkworm - Verlet cloth physics in C
 *
 * Copyright (c) 2021 Evan Hemsley
 *
 * This software is provided 'as-is', without any express or implied warranty.
 * In no event will the authors be held liable for any damages arising from
 * the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software in a
 * product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 *
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 *
 * 3. This notice may not be removed or altered from any source distribution.
 *
 * Evan "cosmonaut" Hemsley <evan@moonside.games>
 *
 */

#include "Silkworm.h"

#include <stdlib.h>
#include <string.h>

typedef struct Silkworm_Color
{
	uint8_t r;
	uint8_t g;
	uint8_t b;
	uint8_t a;
} Silkworm_Color;

typedef struct Silkworm_Context
{
	Silkworm_Node** nodes;
	uint32_t nodeCount;

	Silkworm_Link** links;
	uint32_t linkCount;

	Silkworm_Cloth** cloths;
	uint32_t clothCount;
	uint32_t clothCapacity;

	uint64_t* nodeIndexStack;
	uint32_t nodeIndexStackCount;
	uint32_t nodeIndexStackCapacity;

	uint64_t* linkIndexStack;
	uint32_t linkIndexStackCount;
	uint32_t linkIndexStackCapacity;

	uint64_t* clothIndexStack;
	uint32_t clothIndexStackCount;
	uint32_t clothIndexStackCapacity;

	float gravity;
	float xBound;
	float yBound;
	uint32_t clothDensity;

	uint8_t* currentBufferAddress; /* GM doesnt let you pass more than 4 arguments with different types lol */
} Silkworm_Context;

static Silkworm_Context *context = NULL;

#define CONSTRAINT_ITERATION_COUNT 3 /* TODO: make this a parameter? */

void Silkworm_Init()
{
	context = malloc(sizeof(Silkworm_Context));

	context->nodes = NULL;
	context->nodeCount = 0;

	context->links = NULL;
	context->linkCount = 0;

	context->cloths = NULL;
	context->clothCount = 0;

	context->nodeIndexStackCapacity = 16;
	context->nodeIndexStack = malloc(sizeof(uint64_t) * context->nodeIndexStackCapacity);
	context->nodeIndexStackCount = 0;

	context->linkIndexStackCapacity = 16;
	context->linkIndexStack = malloc(sizeof(uint64_t) * context->linkIndexStackCapacity);
	context->linkIndexStackCount = 0;

	context->clothIndexStackCapacity = 16;
	context->clothIndexStack = malloc(sizeof(uint64_t) * context->clothIndexStackCapacity);
	context->clothIndexStackCount = 0;

	context->gravity = 200;
	context->xBound = 1000;
	context->yBound = 1000;
	context->clothDensity = 4;
}

static inline Silkworm_Node* LookupNode(uint64_t nodeId)
{
	return context->nodes[nodeId];
}

static inline Silkworm_Link* LookupLink(uint64_t linkId)
{
	return context->links[linkId];
}

static inline Silkworm_Cloth* LookupCloth(uint64_t clothId)
{
	return context->cloths[clothId];
}

/* we defer actual destruction to Update to avoid issues with NULL */

void Silkworm_DestroyNode(double nodeId)
{
	uint32_t i;
	Silkworm_Node* node = LookupNode((uint64_t)nodeId);

	if (node != NULL)
	{
		node->markedForDestroy = true;

		for (i = 0; i < node->linkCount; i += 1)
		{
			if (node->links[i] != NULL)
			{
				node->links[i]->markedForDestroy = true;
			}
		}
	}
}

void Silkworm_Internal_DestroyLink(Silkworm_Link* link)
{
	link->markedForDestroy = true;
}

void Silkworm_DestroyLink(double linkId)
{
	Silkworm_Internal_DestroyLink(LookupLink((uint64_t)linkId));
}

void Silkworm_Internal_DestroyCloth(Silkworm_Cloth* cloth)
{
	uint32_t i, j;

	context->cloths[cloth->id] = NULL;

	for (i = 0; i < cloth->horizontalNodeCount; i += 1)
	{
		for (j = 0; j < cloth->verticalNodeCount; j += 1)
		{
			Silkworm_DestroyNode((double)cloth->nodeIndices[i][j]);
		}
		free(cloth->nodeIndices[i]);
	}
	free(cloth->nodeIndices);
	free(cloth);

	if (context->clothIndexStackCount >= context->clothIndexStackCapacity)
	{
		context->clothIndexStackCapacity *= 2;
		context->nodeIndexStack = realloc(context->nodeIndexStack, sizeof(uint64_t) * context->nodeIndexStackCapacity);
	}

	context->clothIndexStack[context->clothIndexStackCount] = cloth->id;
	context->clothIndexStackCount += 1;
}

void Silkworm_DestroyCloth(double clothId)
{
	Silkworm_Internal_DestroyCloth(LookupCloth((uint64_t)clothId));
}

void Silkworm_PerformDestroys()
{
	uint32_t i, j;

	for (i = 0; i < context->linkCount; i += 1)
	{
		if (context->links[i] != NULL && context->links[i]->markedForDestroy)
		{
			for (j = 0; j < context->links[i]->a->linkCount; j += 1)
			{
				if (context->links[i]->a->links[j] == context->links[i])
				{
					context->links[i]->a->links[j] = NULL;
				}
			}

			for (j = 0; j < context->links[i]->b->linkCount; j += 1)
			{
				if (context->links[i]->b->links[j] == context->links[i])
				{
					context->links[i]->b->links[j] = NULL;
				}
			}

			free(context->links[i]);
			context->links[i] = NULL;

			if (context->linkIndexStackCount >= context->linkIndexStackCapacity)
			{
				context->linkIndexStackCapacity *= 2;
				context->linkIndexStack = realloc(context->linkIndexStack, sizeof(uint64_t) * context->linkIndexStackCapacity);
			}

			context->linkIndexStack[context->linkIndexStackCount] = i;
			context->linkIndexStackCount += 1;
		}
	}

	for (i = 0; i < context->nodeCount; i += 1)
	{
		if (context->nodes[i] != NULL && context->nodes[i]->markedForDestroy)
		{
			free(context->nodes[i]->links);
			free(context->nodes[i]);
			context->nodes[i] = NULL;

			if (context->nodeIndexStackCount >= context->nodeIndexStackCapacity)
			{
				context->nodeIndexStackCapacity *= 2;
				context->nodeIndexStack = realloc(context->nodeIndexStack, sizeof(uint64_t) * context->nodeIndexStackCapacity);
			}

			context->nodeIndexStack[context->nodeIndexStackCount] = i;
			context->nodeIndexStackCount += 1;
		}
	}
}

void Silkworm_Update(double deltaTime)
{
	uint32_t i, j;
	Silkworm_Link *link;
	Silkworm_Node *node;

	float delta = (float)deltaTime;

	for (i = 0; i < CONSTRAINT_ITERATION_COUNT; i += 1)
	{
		for (j = 0; j < context->linkCount; j += 1)
		{
			link = context->links[j];

			if (link != NULL)
			{
				float diffX = link->a->position.x - link->b->position.x;
				float diffY = link->a->position.y - link->b->position.y;
				float d = (float)sqrt(diffX * diffX + diffY * diffY);

				float difference = (link->distance - d) / d;

				float translateX = diffX * 0.5f * difference;
				float translateY = diffY * 0.5f * difference;
				
				float distanceMoved = (float)sqrt(translateX * translateX + translateY * translateY);

				if (distanceMoved > link->tearThreshold)
				{
					Silkworm_DestroyLink((double)link->id);
				}
				else
				{
					if (!link->a->pinned)
					{
						link->a->position.x += translateX;
						link->a->position.y += translateY;
					}

					if (!link->b->pinned)
					{
						link->b->position.x -= translateX;
						link->b->position.y -= translateY;
					}
				}
			}
		}
	}

	for (j = 0; j < context->nodeCount; j += 1)
	{
		node = context->nodes[j];

		if (node != NULL)
		{
			if (!node->pinned)
			{
				float velocityX = (node->position.x - node->previousPosition.x) * node->friction;
				float velocityY = (node->position.y - node->previousPosition.y) * node->friction;

				float accelerationX = node->acceleration.x * delta * delta;
				float accelerationY = (node->acceleration.y + node->mass * context->gravity) * delta * delta;

				node->previousPosition.x = node->position.x;
				node->previousPosition.y = node->position.y;

				node->position.x += velocityX + accelerationX;
				node->position.y += velocityY + accelerationY;

				node->velocity.x = velocityX * delta;
				node->velocity.y = velocityY * delta;

				if (fabs(node->position.x) > context->xBound || fabs(node->position.x) > context->yBound)
				{
					Silkworm_DestroyNode((double)node->id);
				}
			}
		}
	}

	Silkworm_PerformDestroys();
}

double Silkworm_CreateNode(double xPosition, double yPosition, double mass, double friction, double radius, double pushFactor)
{
	Silkworm_Node* node = malloc(sizeof(Silkworm_Node));
	uint64_t id;

	if (context->nodeIndexStackCount > 0)
	{
		id = context->nodeIndexStack[context->nodeIndexStackCount - 1];
		context->nodeIndexStackCount -= 1;
	}
	else
	{
		id = context->nodeCount;

		context->nodes = realloc(context->nodes, sizeof(Silkworm_Node*) * (context->nodeCount + 1));
		context->nodeCount += 1;
	}

	context->nodes[id] = node;

	node->id = id;
	node->position.x = (float)xPosition;
	node->position.y = (float)yPosition;
	node->previousPosition.x = (float)xPosition;
	node->previousPosition.y = (float)yPosition;
	node->velocity.x = 0;
	node->velocity.y = 0;
	node->acceleration.x = 0;
	node->acceleration.y = 0;
	node->mass = (float)mass;
	node->friction = (float)friction;
	node->radius = (float)radius;
	node->pushFactor = (float)pushFactor;
	node->pinned = false;
	node->destroyable = false;
	node->markedForDestroy = false;
	node->links = NULL;
	node->linkCount = 0;

	return (double)node->id;
}

void Silkworm_NodeSetVelocity(double nodeId, double xVelocity, double yVelocity)
{
	LookupNode(nodeId)->velocity.x = (float)xVelocity;
	LookupNode(nodeId)->velocity.y = (float)yVelocity;
}

void Silkworm_NodeSetAcceleration(double nodeId, double xAcceleration, double yAcceleration)
{
	LookupNode(nodeId)->acceleration.x = (float)xAcceleration;
	LookupNode(nodeId)->acceleration.y = (float)yAcceleration;
}

void Silkworm_NodeSetDestroyable(double nodeId)
{
	LookupNode(nodeId)->destroyable = true;
}

void Silkworm_ClothNodePin(double clothId, double i, double j)
{
	Silkworm_Cloth* cloth = LookupCloth(clothId);
	context->nodes[cloth->nodeIndices[(uint64_t)i][(uint64_t)j]]->pinned = true;
}

void Silkworm_ClothNodeUnpin(double clothId, double i, double j)
{
	Silkworm_Cloth* cloth = LookupCloth(clothId);
	context->nodes[cloth->nodeIndices[(uint64_t)i][(uint64_t)j]]->pinned = false;
}

void Silkworm_ClothNodeDestroy(double clothId, double i, double j)
{
	uint32_t iterator;

	Silkworm_Cloth* cloth = LookupCloth(clothId);
	uint64_t nodeIndex = cloth->nodeIndices[(uint64_t)i][(uint64_t)j];
	Silkworm_Node* node = context->nodes[nodeIndex];
	Silkworm_DestroyNode(nodeIndex);

	/* fixme: this should be hashed */
	for (iterator = 0; iterator < cloth->triangleCount; iterator += 1)
	{
		if (cloth->triangles[iterator] != NULL)
		{
			if (cloth->triangles[iterator]->a == node || cloth->triangles[iterator]->b == node || cloth->triangles[iterator]->c == node)
			{
				free(cloth->triangles[iterator]);
				cloth->triangles[iterator] = NULL;
			}
		}
	}
}

double Silkworm_CreateLink(double aId, double bId, double distance, double tearThreshold)
{
	Silkworm_Node *nodeA = LookupNode(aId);
	Silkworm_Node *nodeB = LookupNode(bId);

	uint64_t id;

	Silkworm_Link* link = malloc(sizeof(Silkworm_Link));

	if (context->linkIndexStackCount > 0)
	{
		id = context->linkIndexStack[context->linkIndexStackCount - 1];
		context->linkIndexStackCount -= 1;
	}
	else 
	{
		id = context->linkCount;

		context->links = realloc(context->links, sizeof(Silkworm_Link*) * (context->linkCount + 1));
		context->linkCount += 1;
	}

	context->links[id] = link;

	link->id = id;
	link->a = nodeA;
	link->b = nodeB;
	link->distance = (float)distance;
	link->tearThreshold = (float)tearThreshold;
	link->markedForDestroy = false;

	link->a->links = realloc(link->a->links, sizeof(Silkworm_Link*) * (link->a->linkCount + 1));
	link->a->links[link->a->linkCount] = link;
	link->a->linkCount += 1;

	link->b->links = realloc(link->b->links, sizeof(Silkworm_Link*) * (link->b->linkCount + 1));
	link->b->links[link->b->linkCount] = link;
	link->b->linkCount += 1;

	return (double)link->id;
}

double Silkworm_CreateCloth(double xPosition, double yPosition, double horizontalNodeCount, double verticalNodeCount, double mass, double friction, double windFactor, double tearThreshold)
{
	int32_t i, j;

	Silkworm_Cloth* cloth = malloc(sizeof(Silkworm_Cloth));

	cloth->windFactor = (float)windFactor;
	cloth->horizontalNodeCount = (uint32_t) horizontalNodeCount;
	cloth->verticalNodeCount = (uint32_t) verticalNodeCount;
	cloth->nodeIndices = malloc(sizeof(uint64_t*) * cloth->horizontalNodeCount);

	uint64_t id;

	if (context->clothIndexStackCount > 0)
	{
		id = context->clothIndexStack[context->clothIndexStackCount - 1];
		context->clothIndexStackCount -= 1;

		context->cloths[id] = cloth;
	}
	else
	{
		id = context->clothCount;

		context->cloths = realloc(context->cloths, sizeof(Silkworm_Cloth*) * (context->clothCount + 1));
		context->cloths[id] = cloth;
		context->clothCount += 1;
	}

	cloth->id = id;

	for (i = 0; i < horizontalNodeCount; i += 1)
	{
		cloth->nodeIndices[i] = malloc(sizeof(uint64_t) * cloth->verticalNodeCount);

		for (j = 0; j < verticalNodeCount; j += 1)
		{
			uint64_t nodeId = (uint64_t) Silkworm_CreateNode(xPosition + i * context->clothDensity, yPosition + j * context->clothDensity, mass, friction, 1, 0.5);

			cloth->nodeIndices[i][j] = nodeId;

			if (j == 0)
			{
				Silkworm_ClothNodePin((double)cloth->id, (double)i, (double)j);
			}

			Silkworm_NodeSetDestroyable((double)nodeId);
		}
	}

	cloth->triangles = malloc(sizeof(Silkworm_Triangle*) * cloth->horizontalNodeCount * cloth->verticalNodeCount * 2);
	uint32_t triangleIndex = 0;

	for (i = 0; i < horizontalNodeCount; i += 1)
	{
		for (j = 0; j < verticalNodeCount; j += 1)
		{
			if (i + 1 < horizontalNodeCount && j + 1 < verticalNodeCount)
			{
				cloth->triangles[triangleIndex] = malloc(sizeof(Silkworm_Triangle));

				cloth->triangles[triangleIndex]->a = context->nodes[cloth->nodeIndices[i][j]];
				cloth->triangles[triangleIndex]->b = context->nodes[cloth->nodeIndices[i + 1][j]];
				cloth->triangles[triangleIndex]->c = context->nodes[cloth->nodeIndices[i][j + 1]];
				cloth->triangles[triangleIndex]->orientation = UpperLeft;

				cloth->triangles[triangleIndex]->aHorizontalIndex = i;
				cloth->triangles[triangleIndex]->aVerticalIndex = j;

				cloth->triangles[triangleIndex]->bHorizontalIndex = i + 1;
				cloth->triangles[triangleIndex]->bVerticalIndex = j;

				cloth->triangles[triangleIndex]->cHorizontalIndex = i;
				cloth->triangles[triangleIndex]->cVerticalIndex = j + 1;

				triangleIndex += 1;
			}

			if (i - 1 >= 0 && j - 1 >= 0)
			{
				cloth->triangles[triangleIndex] = malloc(sizeof(Silkworm_Triangle));

				cloth->triangles[triangleIndex]->a = context->nodes[cloth->nodeIndices[i][j]];
				cloth->triangles[triangleIndex]->b = context->nodes[cloth->nodeIndices[i - 1][j]];
				cloth->triangles[triangleIndex]->c = context->nodes[cloth->nodeIndices[i][j - 1]];
				cloth->triangles[triangleIndex]->orientation = BottomRight;

				cloth->triangles[triangleIndex]->aHorizontalIndex = i;
				cloth->triangles[triangleIndex]->aVerticalIndex = j;

				cloth->triangles[triangleIndex]->bHorizontalIndex = i - 1;
				cloth->triangles[triangleIndex]->bVerticalIndex = j;

				cloth->triangles[triangleIndex]->cHorizontalIndex = i;
				cloth->triangles[triangleIndex]->cVerticalIndex = j - 1;

				triangleIndex += 1;
			}
		}
	}

	cloth->triangleCount = triangleIndex;

	for (i = 0; i < horizontalNodeCount; i += 1)
	{
		for (j = 0; j < verticalNodeCount; j += 1)
		{
			if (i - 1 >= 0)
			{
				Silkworm_CreateLink((double)cloth->nodeIndices[i - 1][j], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold);
			}

			if (j - 1 >= 0)
			{
				Silkworm_CreateLink((double)cloth->nodeIndices[i][j - 1], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold);
			}
		}
	}

	return (double)cloth->id;
}

void Silkworm_ApplyWind(double xSpeed, double ySpeed)
{
	uint32_t i, j, k;
	Silkworm_Cloth* cloth;
	Silkworm_Node* node;

	for (i = 0; i < context->clothCount; i += 1)
	{
		if (context->cloths[i] != NULL)
		{
			cloth = context->cloths[i];

			for (j = 0; j < cloth->horizontalNodeCount; j += 1)
			{
				for (k = 0; k < cloth->verticalNodeCount; k += 1)
				{
					node = context->nodes[cloth->nodeIndices[j][k]];

					if (node != NULL && !node->pinned)
					{
						node->position.x += (float)xSpeed * 0.05f * cloth->windFactor;
						node->position.y += (float)ySpeed * 0.05f * cloth->windFactor;
					}
				}
			}
		}
	}
}

void Silkworm_SetTriangleBuffer(const char* bufferId)
{
	context->currentBufferAddress = (uint8_t*)bufferId;
}

/* pattern is x, y position then color + alpha then UV position */
double Silkworm_ClothFillTriangleBuffer(double clothId, double leftUV, double widthUV, double topUV, double heightUV)
{
	uint32_t i, triangleCount;
	uint8_t* bufferAddress = context->currentBufferAddress;
	Silkworm_Cloth* cloth = LookupCloth(clothId);

	Silkworm_Color color;
	color.r = 255;
	color.g = 255;
	color.b = 255;
	color.a = 255;

	triangleCount = 0;
	for (i = 0; i < cloth->triangleCount; i += 1)
	{
		if (cloth->triangles[i] != NULL)
		{
			if (cloth->triangles[i]->orientation == UpperLeft)
			{
				float left = (float)leftUV + (float)widthUV * ((float)cloth->triangles[i]->aHorizontalIndex / (cloth->horizontalNodeCount - 1));
				float right = (float)leftUV + (float)widthUV * ((float)cloth->triangles[i]->bHorizontalIndex / (cloth->horizontalNodeCount - 1));

				float top = (float)topUV + (float)heightUV * ((float)cloth->triangles[i]->aVerticalIndex / (cloth->verticalNodeCount - 1));
				float bottom = (float)topUV + (float)heightUV * ((float)cloth->triangles[i]->cVerticalIndex / (cloth->verticalNodeCount - 1));

				memcpy(bufferAddress, &cloth->triangles[i]->a->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &left, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &top, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->b->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &right, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &top, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->c->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &left, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &bottom, sizeof(float));
				bufferAddress += sizeof(float);

				triangleCount += 1;
			}
			else if (cloth->triangles[i]->orientation == BottomRight)
			{
				float left = (float)leftUV + (float)widthUV * ((float)cloth->triangles[i]->bHorizontalIndex / (cloth->horizontalNodeCount - 1));
				float right = (float)leftUV + (float)widthUV * ((float)cloth->triangles[i]->aHorizontalIndex / (cloth->horizontalNodeCount - 1));

				float top = (float)topUV + (float)heightUV * ((float)cloth->triangles[i]->cVerticalIndex / (cloth->verticalNodeCount - 1));
				float bottom = (float)topUV + (float)heightUV * ((float)cloth->triangles[i]->aVerticalIndex / (cloth->verticalNodeCount - 1));

				memcpy(bufferAddress, &cloth->triangles[i]->a->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &right, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &bottom, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->b->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &left, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &bottom, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->c->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &right, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &top, sizeof(float));
				bufferAddress += sizeof(float);

				triangleCount += 1;
			}
		}
	}

	return (double)triangleCount;
}

void Silkworm_ClearAll()
{
	uint32_t i;
	
	for (i = 0; i < context->clothCount; i += 1)
	{
		if (context->cloths[i] != NULL)
		{
			Silkworm_Internal_DestroyCloth(context->cloths[i]);
		}
	}

	Silkworm_PerformDestroys();
}