diff --git a/src/Silkworm.c b/src/Silkworm.c index 82c7056..0fe2135 100644 --- a/src/Silkworm.c +++ b/src/Silkworm.c @@ -174,6 +174,88 @@ static inline void NodeTriangleHashTable_Insert( } } +typedef struct LinkTriangleHashMap +{ + Silkworm_Link* key; + uint32_t* indexArray; + uint32_t indexArrayCount; +} LinkTriangleHashMap; + +typedef struct LinkTriangleHashArray +{ + LinkTriangleHashMap* elements; + uint32_t count; +} LinkTriangleHashArray; + +#define NUM_LINK_TRIANGLE_HASH_BUCKETS 1031 + +typedef struct LinkTriangleHashTable +{ + LinkTriangleHashArray buckets[NUM_LINK_TRIANGLE_HASH_BUCKETS]; +} LinkTriangleHashTable; + +static inline uint64_t LinkTriangleHashTable_GetHashCode(Silkworm_Link* key) +{ + return 97 + (uint64_t)(size_t)key; +} + +static inline uint32_t* LinkTriangleHashTable_Fetch( + LinkTriangleHashTable* table, + Silkworm_Link* key, + uint32_t* arrayCount +) { + uint32_t i; + uint64_t hashcode = LinkTriangleHashTable_GetHashCode(key); + LinkTriangleHashArray* arr = &table->buckets[hashcode % NUM_LINK_TRIANGLE_HASH_BUCKETS]; + + for (i = 0; i < arr->count; i += 1) + { + if (arr->elements[i].key == key) + { + *arrayCount = arr->elements[i].indexArrayCount; + return arr->elements[i].indexArray; + } + } + + *arrayCount = 0; + return NULL; +} + +static inline void LinkTriangleHashTable_Insert( + LinkTriangleHashTable* table, + Silkworm_Link* key, + uint32_t index +) { + uint32_t i; + uint64_t hashcode = LinkTriangleHashTable_GetHashCode(key); + LinkTriangleHashArray* arr = &table->buckets[hashcode % NUM_LINK_TRIANGLE_HASH_BUCKETS]; + bool foundKey = false; + + for (i = 0; i < arr->count; i += 1) + { + if (arr->elements[i].key == key) + { + arr->elements[i].indexArray = realloc(arr->elements[i].indexArray, sizeof(uint32_t) * (arr->elements[i].indexArrayCount + 1)); + arr->elements[i].indexArray[arr->elements[i].indexArrayCount] = index; + arr->elements[i].indexArrayCount += 1; + foundKey = true; + break; + } + } + + if (!foundKey) + { + arr->elements = realloc(arr->elements, sizeof(LinkTriangleHashMap) * (arr->count + 1)); + + arr->elements[arr->count].key = key; + arr->elements[arr->count].indexArray = malloc(sizeof(uint32_t)); + arr->elements[arr->count].indexArray[0] = index; + arr->elements[arr->count].indexArrayCount = 1; + + arr->count += 1; + } +} + typedef struct Silkworm_Cloth { uint64_t id; @@ -188,6 +270,7 @@ typedef struct Silkworm_Cloth uint32_t triangleCount; NodeTriangleHashTable nodeHash; + LinkTriangleHashTable linkHash; } Silkworm_Cloth; typedef struct Silkworm_Rope @@ -269,7 +352,7 @@ void Silkworm_Init() context->gravity = 200; context->xBound = 1000; context->yBound = 1000; - context->clothDensity = 4; + context->clothDensity = 2; } static inline Silkworm_Node* LookupNode(uint64_t nodeId) @@ -333,6 +416,15 @@ void Silkworm_Internal_DestroyCloth(Silkworm_Cloth* cloth) free(cloth->nodeHash.buckets[i].elements); } + for (i = 0; i < NUM_LINK_TRIANGLE_HASH_BUCKETS; i += 1) + { + for (j = 0; j < cloth->linkHash.buckets[i].count; j += 1) + { + free(cloth->linkHash.buckets[i].elements[j].indexArray); + } + free(cloth->linkHash.buckets[i].elements); + } + for (i = 0; i < cloth->horizontalNodeCount; i += 1) { for (j = 0; j < cloth->verticalNodeCount; j += 1) @@ -365,21 +457,46 @@ void Silkworm_PerformDestroys() for (i = 0; i < context->linkCount; i += 1) { - if (context->links[i] != NULL && context->links[i]->markedForDestroy) + Silkworm_Link* link = context->links[i]; + + if (link != NULL && context->links[i]->markedForDestroy) { - for (j = 0; j < context->links[i]->a->linkCount; j += 1) + /* find cloth to remove from relevant triangles */ + for (j = 0; j < context->clothCount; j += 1) { - if (context->links[i]->a->links[j] == context->links[i]) + Silkworm_Cloth* cloth = context->cloths[j]; + + if (cloth != NULL) { - context->links[i]->a->links[j] = NULL; + uint32_t triangleIndexCount = 0; + uint32_t* triangleIndices = LinkTriangleHashTable_Fetch(&cloth->linkHash, link, &triangleIndexCount); + + for (k = 0; k < triangleIndexCount; k += 1) + { + uint32_t triangleIndex = triangleIndices[k]; + + if (cloth->triangles[triangleIndex] != NULL) + { + free(cloth->triangles[triangleIndex]); + cloth->triangles[triangleIndex] = NULL; + } + } } } - for (j = 0; j < context->links[i]->b->linkCount; j += 1) + for (j = 0; j < link->a->linkCount; j += 1) { - if (context->links[i]->b->links[j] == context->links[i]) + if (link->a->links[j] == context->links[i]) { - context->links[i]->b->links[j] = NULL; + link->a->links[j] = NULL; + } + } + + for (j = 0; j < link->b->linkCount; j += 1) + { + if (link->b->links[j] == context->links[i]) + { + link->b->links[j] = NULL; } } @@ -647,7 +764,7 @@ double Silkworm_CreateLink(double aId, double bId, double distance, double tearT double Silkworm_CreateCloth(double xPosition, double yPosition, double horizontalNodeCount, double verticalNodeCount, double mass, double friction, double windFactor, double tearThreshold) { - int32_t i, j; + int32_t i, j, k, m; Silkworm_Cloth* cloth = malloc(sizeof(Silkworm_Cloth)); @@ -682,6 +799,12 @@ double Silkworm_CreateCloth(double xPosition, double yPosition, double horizonta cloth->nodeHash.buckets[i].count = 0; } + for (i = 0; i < NUM_LINK_TRIANGLE_HASH_BUCKETS; i += 1) + { + cloth->linkHash.buckets[i].elements = NULL; + cloth->linkHash.buckets[i].count = 0; + } + for (i = 0; i < horizontalNodeCount; i += 1) { cloth->nodeIndices[i] = malloc(sizeof(uint64_t) * cloth->verticalNodeCount); @@ -768,12 +891,48 @@ double Silkworm_CreateCloth(double xPosition, double yPosition, double horizonta { if (i - 1 >= 0) { - Silkworm_CreateLink((double)cloth->nodeIndices[i - 1][j], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold); + double linkId = Silkworm_CreateLink((double)cloth->nodeIndices[i - 1][j], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold); + + uint32_t indexArrayOneCount = 0; + uint32_t* indexArrayOne = NodeTriangleHashTable_Fetch(&cloth->nodeHash, context->nodes[cloth->nodeIndices[i - 1][j]], &indexArrayOneCount); + + uint32_t indexArrayTwoCount = 0; + uint32_t* indexArrayTwo = NodeTriangleHashTable_Fetch(&cloth->nodeHash, context->nodes[cloth->nodeIndices[i][j]], &indexArrayTwoCount); + + for (k = 0; k < indexArrayOneCount; k += 1) + { + uint32_t triangleIndex = indexArrayOne[k]; + for (m = 0; m < indexArrayTwoCount; m += 1) + { + if (indexArrayTwo[m] == triangleIndex) + { + LinkTriangleHashTable_Insert(&cloth->linkHash, context->links[(uint64_t)linkId], triangleIndex); + } + } + } } if (j - 1 >= 0) { - Silkworm_CreateLink((double)cloth->nodeIndices[i][j - 1], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold); + double linkId = Silkworm_CreateLink((double)cloth->nodeIndices[i][j - 1], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold); + + uint32_t indexArrayOneCount = 0; + uint32_t* indexArrayOne = NodeTriangleHashTable_Fetch(&cloth->nodeHash, context->nodes[cloth->nodeIndices[i][j - 1]], &indexArrayOneCount); + + uint32_t indexArrayTwoCount = 0; + uint32_t* indexArrayTwo = NodeTriangleHashTable_Fetch(&cloth->nodeHash, context->nodes[cloth->nodeIndices[i][j]], &indexArrayTwoCount); + + for (k = 0; k < indexArrayOneCount; k += 1) + { + uint32_t triangleIndex = indexArrayOne[k]; + for (m = 0; m < indexArrayTwoCount; m += 1) + { + if (indexArrayTwo[m] == triangleIndex) + { + LinkTriangleHashTable_Insert(&cloth->linkHash, context->links[(uint64_t)linkId], triangleIndex); + } + } + } } } }