Commits

Peter Shinners committed 4a6c0a4

Assembling my experimental code into a library.
(Dragons)

  • Participants
  • Parent commits 7589732

Comments (0)

Files changed (4)

+CFLAGS += -std=c99
+
+#nrnr.a: nrnr.c
+
+test: nrnr.c test.c nrnr.h Makefile
+	$(CC) $(CFLAGS) -o test nrnr.c test.c
+
+#include <string.h>
+#include <alloca.h>
+#include <stdio.h> // temp
+#include "nrnr.h"
+
+
+// start is inclusive, end is exclusive
+struct range {
+    const size_t* start;
+    const size_t* end;
+};
+
+
+#ifndef MIN
+#define MIN(a,b) ((a)<(b) ? (a) : (b))
+#endif
+
+
+
+void SortIndices(size_t* array, int numPoints, const float* points, size_t axis, size_t dimensions);
+struct range BisectIndices(const size_t* array, size_t numPoints, const float* points, size_t axis, size_t dimensions, float minVal, float maxVal);
+
+
+
+struct NrNrHood* NrNrCreateNeighborhood(size_t numPoints, size_t dimensions, const float* points) {
+    // All data is allocated in a single block. Allocate and then
+    // aim some pointers into the space.
+
+    size_t allocSize = sizeof(struct NrNrHood) + sizeof(size_t) * numPoints * 6;
+    struct NrNrHood* hood = malloc(allocSize);
+    hood->numPoints = numPoints;
+    hood->dimensions = dimensions;
+    hood->points = points;
+    hood->indices = (size_t*)(hood + 1);
+    hood->nextIndices = hood->indices + numPoints * 3;
+
+    // Initialize indices with list of all points
+
+    for (size_t i = 0; i < numPoints; ++i) {
+        hood->indices[i] = i;
+    }
+    for (size_t d = 1; d < dimensions; ++d) {
+        size_t* initSrc = hood-> indices + (d - 1) * numPoints;
+        size_t* initDst = hood-> indices + d * numPoints;
+        memcpy(initDst, initSrc, sizeof(size_t) * numPoints);
+    }
+
+    // Sort indices for each dimension
+
+    for (size_t d = 0; d < dimensions; ++d) {
+        size_t* array = hood->indices + d * numPoints;
+        SortIndices(array, numPoints, points, d, dimensions);
+    }
+
+    // Find inverse mapping to sort index (point to index)
+    // This goes into a temporary set of inverse indices
+
+    size_t* inverse = malloc(sizeof(size_t) * numPoints * dimensions);
+
+    for (size_t d = 0; d < dimensions; ++d) {
+        size_t* ind = hood->indices + numPoints * d;
+        size_t* inv = inverse + numPoints * d;
+        for (size_t i = 0; i < numPoints; ++i) {
+            inv[ind[i]] = i;
+        }
+    }
+
+	printf("  invx:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu", inverse[i]);} printf("\n");
+	printf("  invy:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu", inverse[numPoints + i]);} printf("\n");
+	printf("  invz:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu", inverse[numPoints * 2 + i]);} printf("\n");
+
+    // Build next arrays that map a point from one dimension to the next
+    // Allows jumping from dim0 index N to dim1, and then to dim2, ...
+
+    for (size_t d = 0; d < dimensions; ++d) {
+        size_t* next = hood->nextIndices + numPoints * d;
+        size_t* ind = hood->indices + numPoints * d;
+        size_t* inv;
+        if (d) {
+            inv = inverse + numPoints * (d - 1);
+        } else {
+            inv = inverse + numPoints * (dimensions - 1);
+        }
+        for (size_t i = 0; i < numPoints; ++i) {
+            next[i] = inv[ind[i]];
+        }
+    }
+
+    free(inverse);
+
+    return hood;
+}
+
+
+struct NrNrHood* NrNrFreeNeighborhood(struct NrNrHood* hood) {
+    if (!hood) {
+        free(hood);
+    }
+    return NULL;
+}
+
+
+
+//size_t NrNrCountBlock(struct NrNrHood* hood, size_t most, float* minimum, float* maximum) {
+//}
+
+
+size_t NrNrSearchBlock(const struct NrNrHood* hood, size_t** results, const float* minimum, const float* maximum) {
+    // Find ranges of indices for each dimension
+
+    struct range* ranges = alloca(sizeof(ranges) * hood->dimensions);
+    for (size_t d = 0; d < hood->dimensions; ++d) {
+        size_t* array = hood->indices + hood->numPoints * d;
+        ranges[d] = BisectIndices(array, hood->numPoints, hood->points, d, hood->dimensions, minimum[d], maximum[d]);
+    }
+
+    // Find dimension with the least number of matches
+
+    size_t minAxis = 0;
+    size_t minSize = ranges[0].end - ranges[0].start;
+    for (size_t d = 0; d < hood->dimensions; ++d) {
+        size_t size = ranges[d].end - ranges[d].start;
+        if (size < minSize) {
+            minSize = size;
+            minAxis = d;
+        }
+    }
+    if (minSize == 0) {
+        *results = NULL;
+        return 0;
+    }
+
+    // Find values common to each dimension
+
+    size_t* common = (size_t*)malloc(sizeof(size_t) * minSize);
+    size_t* found = common;
+
+#if 0
+    for (size_t* minStep = ranges[minAxis].start; minStep < ranges[minAxis].end; ++minStep) {
+        unsigned d;
+        size_t* workPt = hood->indices + hood->numPoints * minAxis - minStep;
+        for (d = 1; d < hood->dimensions; ++d) {
+            size_t workAxis = minAxis + d;
+            if (workAxis >= hood->dimensions) {
+                workAxis -= hood->dimensions;
+            }
+
+            workPt = hood->nextIndices[hood->numPoints * workAxis + workPt];
+            if (workPt < ranges[workAxis].start || workPt > ranges[workAxis].end) {
+                break;
+            }
+        }
+        if (d == hood->dimensions) {
+            *found = *minStep;
+            ++found;
+        }
+    }
+#endif
+
+    // Handle return values
+
+    size_t totalFound = found - common;
+    if (!totalFound) {
+        free(common);
+        *results = NULL;
+        return 0;
+    }
+    if (totalFound < minSize) {
+        common = realloc(common, sizeof(size_t) * totalFound);
+    }
+
+    // realloc common to the smaller, found size
+    *results = common;
+    return totalFound;
+}
+
+
+
+size_t* NrNrFreeResults(size_t* results) {
+    if (!results) {
+        free(results);
+    }
+    return NULL;
+}
+
+
+
+void SortIndices(size_t* array, int numPoints, const float* points, size_t axis, size_t dimensions) {
+    if (numPoints < 2) {
+        return;
+    }
+
+    // Insertion sort for small ranges
+    if (numPoints < 16) {
+        for (size_t i = 1; i < numPoints; ++i) {
+            size_t point = array[i];
+            float position = points[point * dimensions + axis];
+            size_t j;
+            for (j = i; j > 0 && points[array[j - 1] * dimensions + axis] > position; --j) {
+                array[j] = array[j - 1];
+            }
+            array[j] = point;
+        }
+        return;
+    }
+
+    // Quicksort (recursive) for most spans
+    float position = points[array[numPoints / 2] * dimensions + axis];
+    size_t *left = array;
+    size_t *right = array + numPoints - 1;
+    while (left <= right) {
+        while (points[(*left) * dimensions + axis] < position)
+            left++;
+        while (points[(*right) * dimensions + axis] > position)
+            right--;
+        if (left <= right) {
+            size_t t = *left;
+            *left++ = *right;
+            *right-- = t;
+        }
+    }
+
+    SortIndices(array, right - array + 1, points, axis, dimensions);
+    SortIndices(left, array + numPoints - left, points, axis, dimensions);
+}
+
+
+
+struct range BisectIndices(const size_t* array, size_t numPoints, const float* points, size_t axis, size_t dimensions, float minVal, float maxVal) {
+    const size_t* fullStart = array;
+    const size_t* fullEnd = fullStart + numPoints;
+    const size_t* start = fullStart;
+    const size_t* end = fullEnd;
+    struct range range;
+
+    // Find min index
+    while (start < end) {
+        size_t dist = end - start;
+        const size_t* half = start + (dist / 2);
+        float halfValue = array[*half * 3 + axis];
+        if (minVal < halfValue) {
+            end = half;
+        } else {
+            start = half + 1;
+        }
+    }
+    range.start = start;
+
+    // Find max index
+    end = fullEnd;
+    while (start < end) {
+        size_t dist = end - start;
+        const size_t* half = start + (dist / 2);
+        float halfValue = points[*half * 3 + axis];
+        if (maxVal > halfValue) {
+            start = half + 1;
+        } else {
+            end = half;
+        }
+    }
+    range.end = end;
+
+    return range;
+}
+
+#ifndef __NRNR_H__
+#define __NRNR_H__
+
+
+#include <stdlib.h>
+
+
+struct NrNrHood {
+    size_t numPoints;
+    size_t dimensions;
+    const float* points;
+    size_t* indices;
+    size_t* nextIndices;
+};
+
+
+struct NrNrHood* NrNrCreateNeighborhood(size_t numPoints, size_t dimensions, const float* points);
+struct NrNrHood* NrNrFreeNeighborhood(struct NrNrHood* hood);
+
+//size_t NrNrCountBlock(struct NrNrHood* hood, size_t most, float* minimum, float* maximum);
+size_t NrNrSearchBlock(const struct NrNrHood* hood, size_t** results, const float* minimum, const float* maximum);
+size_t* NrNrFreeResults(size_t* results);
+
+
+#endif // __NRNR_H__
+#include <stdio.h>
+#include "nrnr.h"
+
+
+#include <sys/time.h>
+#include <time.h>
+
+
+
+float GetTime(void);
+void CheckSorting(struct NrNrHood* hood);
+void CheckNextIndices(struct NrNrHood* hood);
+
+
+
+#ifndef MIN
+#define MIN(a,b) ((a)<(b) ? (a) : (b))
+#endif
+
+
+
+int main(int _argc, char** _argv) {
+
+    float start;
+
+    // Create random points
+
+    start = GetTime();
+    srand(12345);
+    size_t numPoints = 5; //0000000;
+    float* points = (float*)malloc(sizeof(float) * 3 * numPoints);
+	for(size_t i=0; i<numPoints; i++) {
+		float* p = points + i * 3;
+		p[0] = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
+		p[1] = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
+		p[2] = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
+
+    }
+	printf("Create random points (%zu): %.3f sec\n", numPoints, GetTime() - start);
+
+    // Create neighborhood
+
+    start = GetTime();
+    struct NrNrHood* hood = NrNrCreateNeighborhood(numPoints, 3, points);
+	printf("Create neighborhood: %.3f sec\n", GetTime() - start);
+
+
+	printf("  x:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu=%.3f", hood->indices[i], points[hood->indices[i] * 3]);} printf("\n");
+	printf("  y:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu=%.3f", hood->indices[numPoints + i], points[hood->indices[numPoints + i] * 3 + 1]);} printf("\n");
+	printf("  z:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu=%.3f", hood->indices[numPoints * 2 + i], points[hood->indices[numPoints * 2 + i] * 3 + 2]);} printf("\n");
+    CheckSorting(hood);
+
+	printf(" nx:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu", hood->nextIndices[i]);} printf("\n");
+	printf(" ny:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu", hood->nextIndices[numPoints + i]);} printf("\n");
+	printf(" nz:"); for (size_t i=0; i<MIN(numPoints,10); ++i) {printf(" %zu", hood->nextIndices[numPoints * 2 + i]);} printf("\n");
+    CheckNextIndices(hood);
+
+    // Search block
+
+    start = GetTime();
+    float minPos[3] = {0.0f, 20.0f, -20.0f};
+    float maxPos[3] = {minPos[0] + 40.0, minPos[1] + 40.0, minPos[2] + 40.0};
+    size_t* results;
+    size_t count = NrNrSearchBlock(hood, &results, minPos, maxPos);
+    printf("Search block %zu: %.3f sec\n", count, GetTime() - start);
+
+    // Cleanup
+
+    start = GetTime();
+    free(points);
+    results = NrNrFreeResults(results);
+    hood = NrNrFreeNeighborhood(hood);
+    printf("Cleanup: %.3f sec\n", GetTime() - start);
+
+	return 0;
+}
+
+
+
+
+
+float GetTime(void)
+{
+	static struct timeval timeval, first_timeval;
+	gettimeofday(&timeval, 0);
+	if(first_timeval.tv_sec == 0) {
+		first_timeval = timeval;
+		return 0;
+	}
+	unsigned int s = (timeval.tv_sec - first_timeval.tv_sec) * 1000 + (timeval.tv_usec - first_timeval.tv_usec) / 1000.0;
+    return s / 1000.0f;
+}
+
+
+
+
+void CheckSorting(struct NrNrHood* hood) {
+    // Check value ordering
+    for (size_t d = 0; d < hood->dimensions; ++d) {
+        size_t* indices = hood->indices + hood->numPoints * d;
+        float val = hood->points[indices[0] * 3 + d];
+        for (size_t pt = 1; pt < hood->numPoints; ++pt) {
+            float nextVal = hood->points[indices[pt] * 3 + d];
+            if (nextVal < val) {
+                printf("Error sorting, values not in ascending order: axis=%zu, index=%zu\n", d, pt);
+                exit(1);
+            }
+            val = nextVal;
+        }
+    }
+
+    // Check repeated indices
+    for (size_t d = 0; d < hood->dimensions; ++d) {
+        size_t* indices = hood->indices + hood->numPoints * d;
+        for (size_t i = 0; i < hood->numPoints; ++i) {
+            size_t pt = indices[i];
+            if (pt >= hood->numPoints) {
+                printf("Error sorting, point number out of range: axis=%zu, index=%zu\n", d, i);
+                exit(1);
+            }
+            for (size_t j = i + 1; j < hood->numPoints; ++j) {
+                if (pt == indices[j]) {
+                    printf("Error sorting, point number used more than once: axis=%zu, index=%zu\n", d, i);
+                    exit(1);
+                }
+            }
+        }
+    }
+}
+
+
+void CheckNextIndices(struct NrNrHood* hood) {
+    // Check repeated indices
+    for (size_t d = 0; d < hood->dimensions; ++d) {
+        size_t* next = hood->nextIndices + hood->numPoints * d;
+        for (size_t i = 0; i < hood->numPoints; ++i) {
+            size_t pt = next[i];
+            if (pt >= hood->numPoints) {
+                printf("Error with nexts, point number out of range: axis=%zu, index=%zu\n", d, i);
+                exit(1);
+            }
+            for (size_t j = i + 1; j < hood->numPoints; ++j) {
+                if (pt == next[j]) {
+                    printf("Error with nexts, point number used more than once: axis=%zu, index=%zu\n", d, i);
+                    exit(1);
+                }
+            }
+        }
+    }
+
+    // Check roundtrip through dimensions ends with the same
+    for (size_t d = 1; d < hood->dimensions; ++d) {
+        size_t* next = hood->nextIndices + hood->numPoints * d;
+        size_t* indices = hood->indices + hood->numPoints * d;
+        size_t* from;
+        if (d) {
+            from = hood->indices + hood->numPoints * (d - 1);
+        } else {
+            from = hood->indices + hood->numPoints * (hood->dimensions - 1);
+        }
+        for (size_t i = 0; i < hood->numPoints; ++i) {
+            size_t pt = from[i];
+            if (pt != indices[next[i]]) {
+                printf("Error with nexts, incorrect index: axis=%zu, index=%zu\n", d, i);
+                exit(1);
+            }
+        }
+    }
+
+}