enowars5-service-stldoctor

stlfile.c (9164B)

#include "stlfile.h"

static const char wsset[] = " \r\n\t";
static const struct {
	int code;
	const char *str;
} kwmap[] = {
	{ KW_SOLID_BEGIN, "solid"        },
	{ KW_SOLID_END,   "endsolid"     },
	{ KW_LOOP_BEGIN,  "outer loop"   },
	{ KW_LOOP_END,    "endloop"      },
	{ KW_FACET_BEGIN, "facet normal" },
	{ KW_FACET_END,   "endfacet"     },
	{ KW_VERTEX,      "vertex"       },
};

void
stack_init(struct stack *stack)
{
	stack->cap = 10;
	stack->data = checkp(malloc(sizeof(int) * stack->cap));
	stack->count = 0;
}

void
stack_push(struct stack *stack, int v)
{
	if (stack->count == stack->cap) {
		stack->cap *= 2;
		stack->data = realloc(stack->data, sizeof(int) * stack->cap);
		checkp(stack->data);
	}

	stack->data[stack->count] = v;
	stack->count++;
}

int
stack_pop(struct stack *stack)
{
	if (stack->count == 0)
		return -1;

	stack->count--;
	return stack->data[stack->count];
}

void
stack_free(struct stack *stack)
{
	free(stack->data);
}

int
stack_ind(struct stack *stack, int search)
{
	int i;
	for (i = 0; i < stack->count; i++)
		if (stack->data[i] == search)
			return i;
	return -1;
}

int
stack_top_eq(struct stack *stack, int cmp)
{
	if (!stack->count) return 0;
	return stack->data[stack->count-1] == cmp;
}

int
isws(char c)
{
	return c && strchr(wsset, c);
}

char*
skipws(char *p)
{
	for (; isws(*p); p++);
	return p;
}

char*
consume_arg(char **start, char **end)
{
	char *c, *tmp;

	*start = skipws(*start);
	if (!*start) return NULL;
	for (c = *start; *c && !isws(*c); c++);
	tmp = *start;
	*start = c + (*c ? 1 : 0);
	*end = c;
	return tmp;
}

int
consume_keyword(char **start)
{
	char *bp;
	int i, len;

	bp = skipws(*start);

	for (i = 0; i < ARRSIZE(kwmap); i++) {
		len = strlen(kwmap[i].str);
		if (!strncmp(kwmap[i].str, bp, len) && (!bp[len] || isws(bp[len]))) {
			*start = bp + len + (bp[len] ? 1 : 0);
			return kwmap[i].code;
		}
	}

	return KW_UNKNOWN;
}

int
parse_file_ascii(struct parseinfo *info, char *buf, size_t len)
{
	char *bp, *arg, *prev, *tmp, *end;
	struct stack states;
	float farg;
	int i, kw;

	stack_init(&states);

	info->type = TYPE_ASCII;
	info->loopcount = 0;

	memset(info->header, 0, 80);

	for (i = 0; i < 3; i++) {
		info->bbmin[i] = INFINITY;
		info->bbmax[i] = -INFINITY;
	}

	bp = prev = buf;
	while ((kw = consume_keyword(&bp))) {
		switch (kw) {
		case KW_SOLID_BEGIN:
			if (states.count) {
				FMT_ERR("Improper nesting, solid not top-level in STL\n");
				goto fail;
			}
			stack_push(&states, STATE_SOLID);
			tmp = bp;
			if (!consume_keyword(&bp)
					&& (arg = consume_arg(&bp, &end))) {
				info->solidname = strndup(arg, end - arg);
			} else {
				bp = tmp;
			}
			break;
		case KW_SOLID_END:
			if ((kw = stack_pop(&states)) != STATE_SOLID) {
				FMT_ERR("Improper nesting, parent: %s\n",
					kwmap[kw].str);
				goto fail;
			}
			tmp = bp;
			if (info->solidname && !consume_keyword(&bp)
					&& (arg = consume_arg(&bp, &end))) {
				if (strncmp(info->solidname, arg, end - arg)) {
					FMT_ERR("Solid names do not match!\n");
					goto fail;
				}
			} else {
				bp = tmp;
			}
			break;
		case KW_LOOP_BEGIN:
			if (!stack_top_eq(&states, STATE_FACET)) {
				FMT_ERR("Loop parent is not a facet!\n");
				goto fail;
			}
			stack_push(&states, STATE_LOOP);
			break;
		case KW_LOOP_END:
			if ((kw = stack_pop(&states)) != STATE_LOOP) {
				FMT_ERR("Improper nesting, parent: %s\n",
					kwmap[kw].str);
				goto fail;
			}
			info->loopcount++;
			break;
		case KW_FACET_BEGIN:
			if (!stack_top_eq(&states, STATE_SOLID)) {
				FMT_ERR("Facet parent is not a solid!\n");
				goto fail;
			}
			stack_push(&states, STATE_FACET);
			for (i = 0; i < 3; i++) {
				if (!(arg = consume_arg(&bp, &end))) {
					FMT_ERR("Facet with < 3 args!\n");
					goto fail;
				}
				farg = strtof(arg, &tmp);
				if (!isws(*tmp)) {
					FMT_ERR("Facet arg '%s'\n", arg);
					goto fail;
				}
			}
			break;
		case KW_FACET_END:
			if ((kw = stack_pop(&states)) != STATE_FACET) {
				FMT_ERR("Improper nesting, parent: %s\n",
					kwmap[kw].str);
				goto fail;
			}
			break;
		case KW_VERTEX:
			if (!stack_top_eq(&states, STATE_LOOP)) {
				FMT_ERR("Vertex parent is not a loop!\n");
				goto fail;
			}
			for (i = 0; i < 3; i++) {
				if (!(arg = consume_arg(&bp, &end))) {
					FMT_ERR("Vertex with < 3 args\n");
					goto fail;
				}
				farg = strtof(arg, &tmp);
				if (!isws(*tmp)) {
					FMT_ERR("Vertex arg '%s'\n", arg);
					goto fail;
				}
				info->bbmin[i] = MIN(info->bbmin[i], farg);
				info->bbmax[i] = MAX(info->bbmax[i], farg);
			}
			break;
		case KW_UNKNOWN:
			prev = skipws(prev);
			FMT_ERR("Expected keyword, got:\n%.*s...\n", 30, prev);
			goto fail;
		}
		prev = bp;
	}

	if (states.count) {
		FMT_ERR("Expected keyword, got:\n%.*s...\n", 30, bp);
		goto fail;
	}

	bp = skipws(bp);
	if (*bp) {
		FMT_ERR("Extraneous data at end of file\n");
		goto fail;
	}

	stack_free(&states);
	return OK;

fail:
	stack_free(&states);
	return FAIL;
}

int
parse_file_bin(struct parseinfo *info, char *buf, size_t len)
{
	char *bp, *end = buf + len;
	int i, k;
	float v;

	info->type = TYPE_BIN;

	if (len < 84) {
		FMT_ERR("Truncated data! (header missing)\n");
		goto fail;
	}

	memcpy(info->header, buf, 80);

	info->solidname = checkp(strndup(buf + (*buf == '#'), 80));

	bp = buf + 80;

	info->loopcount = le32toh(*(uint32_t*)bp);
	bp += 4;

	if (!info->loopcount) {
		memset(info->bbmax, 0, sizeof(float) * 3);
		memset(info->bbmin, 0, sizeof(float) * 3);
		return OK;
	}

	for (i = 0; i < 3; i++) {
		info->bbmin[i] = INFINITY;
		info->bbmax[i] = -INFINITY;
	}

	for (i = 0; i < info->loopcount; i++) {
		if (bp + 50 > end) {
			FMT_ERR("Truncated data! (loops missing)\n");
			goto fail;
		}
		for (k = 0; k < 12; k++, bp += 4) {
			v = fle32toh(*(float*)bp);
			if (v == INFINITY || v == NAN) {
				FMT_ERR("Encountered invalid float\n");
				goto fail;
			}
			if (k >= 3) {
				info->bbmin[k % 3] = MIN(info->bbmin[k % 3], v);
				info->bbmax[k % 3] = MAX(info->bbmax[k % 3], v);
			}
		}
		bp += 2;
	}

	if (bp != end) {
		FMT_ERR("Extraneous data at end of file\n");
		goto fail;
	}

	return OK;

fail:
	FREE(info->solidname);
	return FAIL;
}

int
parse_file(struct parseinfo *info, char *buf, size_t len, char **modelname)
{
	int status;
	const char *resp;
	char *bp;

	if (info->valid)
		free_info(info);

	if (len < 10) {
		ERR("File too small!\n");
		return FAIL;
	}

	info->filesize = len;

	/* check bin vs ascii with first keyword */
	for (bp = buf; isws(*bp); bp++);
	status = !strncmp("solid", bp, 5) && isws(bp[5])
		? parse_file_ascii(info, buf, len)
		: parse_file_bin(info, buf, len);
	if (status == FAIL) return FAIL;

	/* transfer ownership */
	info->modelname = *modelname;
	*modelname = NULL;

	info->hash = checkp(strdup(mhash(info->modelname, -1)));

	return OK;
}

int
save_info(struct parseinfo *info, FILE *f)
{
	size_t nwrote = 0;
	int i;

	nwrote += fwrite(&info->type, sizeof(int), 1, f);
	nwrote += fwrite(&info->loopcount, sizeof(int), 1, f);
	nwrote += fwrite(&info->filesize, sizeof(unsigned), 1, f);

	for (i = 0; i < 3; i++) {
		nwrote += fwrite(&info->bbmin[i], sizeof(float), 1, f);
		nwrote += fwrite(&info->bbmax[i], sizeof(float), 1, f);
	}

	nwrote += fwrite(info->header, 80, 1, f);

	if (nwrote != 10) return FAIL;

	fputstr(f, info->solidname);
	fputstr(f, info->hash);
	fputstr(f, info->modelname);

	return OK;
}

int
load_info(struct parseinfo *info, FILE *f)
{
	size_t nread = 0;
	int i;

	nread += fread(&info->type, sizeof(int), 1, f);
	nread += fread(&info->loopcount, sizeof(int), 1, f);
	nread += fread(&info->filesize, sizeof(unsigned), 1, f);

	for (i = 0; i < 3; i++) {
		nread += fread(&info->bbmin[i], sizeof(float), 1, f);
		nread += fread(&info->bbmax[i], sizeof(float), 1, f);
	}

	nread += fread(info->header, 80, 1, f);

	if (nread != 10) return FAIL;

	freadstr(f, &info->solidname);
	freadstr(f, &info->hash);
	freadstr(f, &info->modelname);

	info->valid = 1;

	return OK;
}

void
print_info(struct parseinfo *info)
{
	int i, k;

	printf(" === Model info === \n");

	printf("  File Size: %u\n", info->filesize);
	if (info->type == TYPE_BIN) {
		printf("  Header:\n");
		for (i = 0; i < 80; i += k) {
			printf("  ");
			for (k = 0; k < MIN(80 - i, 20); k++)
				printf(" %02x", (uint8_t) info->header[i+k]);
			printf(" | ");
			for (k = 0; k < MIN(80 - i, 20); k++)
				putchar(PRINTABLE(info->header[i+k]));
			printf("\n");
		}
	}
	printf("  Model ID: %s\n", info->hash);
	printf("  Model Name: %s\n", info->modelname);
	printf("  Solid Name: %s\n", info->solidname);
	printf("  Triangle Count: %i\n", info->loopcount);
	printf("  Bounding Box Size: %.2f x %.2f x %.2f\n",
			info->bbmax[0] - info->bbmin[0],
			info->bbmax[1] - info->bbmin[1],
			info->bbmax[2] - info->bbmin[2]);
	printf("  Bounding Box Origin: %.2f x %.2f x %.2f\n",
			info->bbmin[0], info->bbmin[1], info->bbmin[2]);

	printf(" ================== \n");
}

void
free_info(struct parseinfo *info)
{
	FREE(info->hash);
	FREE(info->modelname);
	FREE(info->solidname);
	info->valid = 0;
}

float fle32toh(float v)
{
	union {
		uint32_t u;
		float f;
	} conv;

	conv.f = v;
	conv.u = le32toh(conv.u);
	return conv.f;
}