--- /dev/null
+#include <stdio.h>
+#include <stdarg.h>
+#include <assert.h>
+#include <string>
+#include <map>
+#include <stack>
+#include "mparser.h"
+#include "machine.h"
+
+enum ValueType { VAL_NUM, VAL_VEC, VAL_STR };
+struct Value {
+ ValueType type;
+ std::string s;
+ float v[4];
+};
+
+struct ParserState;
+
+typedef bool (*Func)(ParserState*);
+
+struct FuncDesc {
+ Func func;
+ int arity;
+};
+
+struct ParserState {
+ FILE *fp;
+ const char *fname;
+ int nline, nerrors;
+ Machine *mcn;
+
+ int nextc;
+ int savedc;
+
+ std::stack<Value> val;
+ std::map<std::string, Value> var;
+ std::map<std::string, FuncDesc> func;
+
+ std::map<std::string, Gear*> gears;
+ std::vector<Gear*> cur_gear;
+ Motor *cur_motor;
+};
+
+#define MAX_ID_LEN 31
+
+static bool machine(ParserState *ps);
+static bool expect(ParserState *ps, char c);
+static bool statement(ParserState *ps);
+static bool expression(ParserState *ps);
+static bool term(ParserState *ps);
+static bool vector(ParserState *ps);
+static bool function(ParserState *ps, const char *name);
+
+static char *get_ident(ParserState *ps, char *buf, int bsz);
+static bool get_number(ParserState *ps, float *num);
+static char *get_string(ParserState *ps, char *buf, int bsz);
+
+static bool nextchar(ParserState *ps);
+//static void putback(ParserState *ps, char c);
+
+static void errmsg(ParserState *ps, const char *fmt, ...);
+static void expected(ParserState *ps, const char *str);
+
+static void set_var(ParserState *ps, const char *name, const Value &value);
+static bool have_var(ParserState *ps, const char *name);
+static const Value &get_var(ParserState *ps, const char *name);
+
+static void set_func(ParserState *ps, const char *name, Func func, int nargs = 0);
+static bool have_func(ParserState *ps, const char *name);
+static Func get_func(ParserState *ps, const char *name);
+static int get_func_arity(ParserState *ps, const char *name);
+
+static Gear *begin_gear(ParserState *ps);
+static bool end_gear(ParserState *ps);
+static Gear *this_gear(ParserState *ps);
+static Gear *find_gear(ParserState *ps, const char *name);
+static void update_gear_vars(ParserState *ps, Gear *gear);
+static bool set_gear_var(Gear *gear, const char *name, const Value &val);
+
+static void update_motor_vars(ParserState *ps, Motor *motor);
+static bool set_motor_var(ParserState *ps, Motor *motor, const char *name, const Value &val);
+
+// built-in functions
+static bool func_adjacent(ParserState *ps);
+static bool func_coaxial(ParserState *ps);
+
+
+bool parse_machine(Machine *mcn, const char *fname)
+{
+ FILE *fp = fopen(fname, "rb");
+ if(!fp) return false;
+
+ ParserState pstate;
+ pstate.fname = fname;
+ pstate.nline = 1;
+ pstate.nerrors = 0;
+ pstate.fp = fp;
+ pstate.mcn = mcn;
+ pstate.nextc = 0;
+ pstate.savedc = -1;
+ pstate.cur_motor = 0;
+
+ // init built-in function table
+ set_func(&pstate, "adjacent", func_adjacent, 2);
+ set_func(&pstate, "coaxial", func_coaxial, 2);
+
+ nextchar(&pstate);
+ bool res = machine(&pstate);
+ fclose(fp);
+ return res;
+}
+
+static bool machine(ParserState *ps)
+{
+ char tok[MAX_ID_LEN + 1];
+ if(strcmp(get_ident(ps, tok, sizeof tok), "machine") != 0) {
+ expected(ps, "machine");
+ return false;
+ }
+ expect(ps, '{');
+
+ while(ps->nextc != '}') {
+ if(!statement(ps)) {
+ return false;
+ }
+ }
+ expect(ps, '}');
+ return true;
+}
+
+static bool expect(ParserState *ps, char c)
+{
+ if(c != ps->nextc) {
+ char buf[2] = {0, 0};
+ buf[0] = c;
+ expected(ps, buf);
+ return false;
+ }
+ nextchar(ps);
+ return true;
+}
+
+static bool statement(ParserState *ps)
+{
+ char id[MAX_ID_LEN + 1];
+ if(!get_ident(ps, id, sizeof id)) {
+ return false;
+ }
+
+ if(strcmp(id, "gear") == 0) {
+ if(!expect(ps, '{')) {
+ return false;
+ }
+
+ Gear *gear = begin_gear(ps);
+ if(!gear) return false;
+
+ update_gear_vars(ps, gear);
+
+ while(ps->nextc != '}') {
+ if(!statement(ps)) {
+ return false;
+ }
+ }
+ expect(ps, '}');
+
+ if(!end_gear(ps)) {
+ return false;
+ }
+ return true;
+ }
+
+ if(strcmp(id, "motor") == 0) {
+ if(!expect(ps, '{')) {
+ return false;
+ }
+ if(ps->cur_motor) {
+ errmsg(ps, "nested motors not allowed");
+ return false;
+ }
+
+ ps->cur_motor = new Motor;
+ update_motor_vars(ps, ps->cur_motor);
+
+ while(ps->nextc != '}') {
+ if(!statement(ps)) {
+ return false;
+ }
+ }
+ expect(ps, '}');
+
+ ps->mcn->add_motor(ps->cur_motor->drive, ps->cur_motor->speed);
+ delete ps->cur_motor;
+ ps->cur_motor = 0;
+ return true;
+ }
+
+ if(ps->nextc == '=') {
+ expect(ps, '=');
+ if(!expression(ps)) {
+ return false;
+ }
+ expect(ps, ';');
+
+ set_var(ps, id, ps->val.top());
+ ps->val.pop();
+ return true;
+ }
+
+ expected(ps, "expression");
+ return false;
+}
+
+static bool expression(ParserState *ps)
+{
+ return term(ps); // TODO expand
+}
+
+static bool term(ParserState *ps)
+{
+ if(ps->nextc == '+') {
+ expect(ps, '+');
+ return expression(ps);
+ }
+ if(ps->nextc == '-') {
+ expect(ps, '-');
+ if(!expression(ps)) {
+ return false;
+ }
+
+ Value &v = ps->val.top();
+ switch(v.type) {
+ case VAL_VEC:
+ v.v[3] = -v.v[3];
+ v.v[2] = -v.v[2];
+ v.v[1] = -v.v[1];
+ case VAL_NUM:
+ v.v[0] = -v.v[0];
+ break;
+
+ default:
+ expected(ps, "number or vector after an unary -");
+ return false;
+ }
+ return true;
+ }
+ if(isalpha(ps->nextc)) {
+ char buf[MAX_ID_LEN];
+ if(!get_ident(ps, buf, sizeof buf)) {
+ return false;
+ }
+
+ if(ps->nextc == '(') {
+ if(!function(ps, buf)) {
+ return false;
+ }
+ } else if(have_var(ps, buf)) {
+ ps->val.push(get_var(ps, buf));
+ } else {
+ errmsg(ps, "unknown identifier: %s", buf);
+ return false;
+ }
+ return true;
+ }
+ if(isdigit(ps->nextc)) {
+ float num;
+ if(!get_number(ps, &num)) {
+ return false;
+ }
+
+ Value v;
+ v.type = VAL_NUM;
+ v.v[0] = num;
+ ps->val.push(v);
+ return true;
+ }
+ if(ps->nextc == '[') {
+ return vector(ps);
+ }
+ if(ps->nextc == '"') {
+ char buf[MAX_ID_LEN];
+ if(!get_string(ps, buf, sizeof buf)) {
+ return false;
+ }
+
+ Value v;
+ v.type = VAL_STR;
+ v.s = buf;
+ ps->val.push(v);
+ return true;
+ }
+
+ expected(ps, "term");
+ return false;
+}
+
+static bool vector(ParserState *ps)
+{
+ int nelem;
+ if(!expect(ps, '[') ||
+ !expression(ps) ||
+ !expect(ps, ',') ||
+ !expression(ps) ||
+ !expect(ps, ',') ||
+ !expression(ps)) {
+ return false;
+ }
+
+ if(ps->nextc == ']') {
+ expect(ps, ']');
+ nelem = 3;
+ } else {
+ if(!expect(ps, ',') || !expression(ps) || !expect(ps, ']')) {
+ return false;
+ }
+ nelem = 4;
+ }
+
+ Value vec;
+ vec.type = VAL_VEC;
+ vec.v[3] = 1.0f;
+
+ for(int i=0; i<nelem; i++) {
+ const Value &tmp = ps->val.top();
+ if(tmp.type != VAL_NUM) {
+ expected(ps, "numbers as vector elements");
+ return false;
+ }
+ vec.v[nelem - i - 1] = tmp.v[0];
+ ps->val.pop();
+ }
+
+ ps->val.push(vec);
+ return true;
+}
+
+static bool function(ParserState *ps, const char *name)
+{
+ if(!expect(ps, '(')) {
+ return false;
+ }
+
+ if(!have_func(ps, name)) {
+ errmsg(ps, "unknown function: %s", name);
+ return false;
+ }
+
+ int nargs = 0;
+ while(ps->nextc != ')') {
+ if(nargs && !expect(ps, ',')) {
+ return false;
+ }
+ if(!expression(ps)) {
+ return false;
+ }
+ ++nargs;
+ }
+ expect(ps, ')');
+
+ int arity = get_func_arity(ps, name);
+ if(arity != nargs) {
+ errmsg(ps, "function %s expects %d arguments", name, arity);
+ return false;
+ }
+ Func func = get_func(ps, name);
+ assert(func);
+
+ if(!func(ps)) {
+ errmsg(ps, "function %s failed", name);
+ return false;
+ }
+ return true;
+}
+
+static char *get_ident(ParserState *ps, char *buf, int bsz)
+{
+ char *ptr = buf;
+ if(!isalpha(ps->nextc)) {
+ expected(ps, "identifier");
+ return 0;
+ }
+ while(isalnum(ps->nextc) || ps->nextc == '_') {
+ if(bsz > 1) {
+ *ptr++ = ps->nextc;
+ --bsz;
+ }
+ nextchar(ps);
+ }
+ *ptr = 0;
+ printf("get_ident -> \"%s\"\n", buf);
+ return buf;
+}
+
+static bool get_number(ParserState *ps, float *num)
+{
+ char buf[256], *ptr = buf, *end = buf + sizeof buf;
+ bool found_point = false;
+
+ if(!isdigit(ps->nextc)) {
+ expected(ps, "number");
+ return false;
+ }
+
+ while(isdigit(ps->nextc) || (ps->nextc == '.' && !found_point)) {
+ if(ptr < end) {
+ *ptr++ = ps->nextc;
+ }
+ if(ps->nextc == '.') {
+ found_point = true;
+ }
+ nextchar(ps);
+ }
+ *ptr = 0;
+
+ *num = atof(buf);
+ printf("get_number -> %f\n", *num);
+ return true;
+}
+
+static char *get_string(ParserState *ps, char *buf, int bsz)
+{
+ char *ptr = buf;
+ if(!expect(ps, '"')) {
+ return 0;
+ }
+
+ while(ps->nextc != -1 && ps->nextc != '"') {
+ if(bsz > 1) {
+ *ptr++ = ps->nextc;
+ --bsz;
+ }
+ if((ps->nextc = fgetc(ps->fp)) == '\n') {
+ ++ps->nline;
+ }
+ }
+ *ptr = 0;
+
+ if(ps->nextc == -1) {
+ return 0;
+ }
+ nextchar(ps);
+
+ printf("get_string -> \"%s\"\n", buf);
+ return buf;
+}
+
+static bool skip_line(ParserState *ps)
+{
+ int c;
+ while((c = fgetc(ps->fp)) != -1 && c != '\n');
+ if(c != -1) {
+ ps->nextc = fgetc(ps->fp);
+ return true;
+ }
+ return false;
+}
+
+static bool nextchar(ParserState *ps)
+{
+ if(ps->savedc != -1) {
+ ps->nextc = ps->savedc;
+ ps->savedc = -1;
+ return true;
+ }
+
+ while((ps->nextc = fgetc(ps->fp)) != -1) {
+ if(ps->nextc == '#') {
+ if(!skip_line(ps)) {
+ return false;
+ }
+ ++ps->nline;
+ }
+ if(!isspace(ps->nextc)) {
+ break;
+ }
+ if(ps->nextc == '\n') {
+ ++ps->nline;
+ }
+ }
+
+ /*if(ps->nextc != -1) {
+ printf("DBG: nextchar -> %c\n", ps->nextc);
+ } else {
+ printf("DBG: nextchar -> EOF\n");
+ }*/
+ return ps->nextc != -1;
+}
+
+/*static void putback(ParserState *ps, char c)
+{
+ ps->savedc = ps->nextc;
+ ps->nextc = c;
+}*/
+
+static void errmsg(ParserState *ps, const char *fmt, ...)
+{
+ fprintf(stderr, "%s line %d error: ", ps->fname, ps->nline);
+
+ va_list ap;
+ va_start(ap, fmt);
+ vfprintf(stderr, fmt, ap);
+ va_end(ap);
+
+ fputc('\n', stderr);
+}
+
+static void expected(ParserState *ps, const char *str)
+{
+ errmsg(ps, "expected: %s", str);
+ ++ps->nerrors;
+}
+
+static void set_var(ParserState *ps, const char *name, const Value &value)
+{
+ ps->var[name] = value;
+
+ Gear *gear = this_gear(ps);
+ if(gear) {
+ set_gear_var(gear, name, value);
+ }
+
+ if(ps->cur_motor) {
+ set_motor_var(ps, ps->cur_motor, name, value);
+ }
+}
+
+static bool have_var(ParserState *ps, const char *name)
+{
+ return ps->var.find(name) != ps->var.end();
+}
+
+static const Value &get_var(ParserState *ps, const char *name)
+{
+ return ps->var[name];
+}
+
+static void set_func(ParserState *ps, const char *name, Func func, int nargs)
+{
+ FuncDesc fdesc;
+ fdesc.func = func;
+ fdesc.arity = nargs;
+ ps->func[name] = fdesc;
+}
+
+static bool have_func(ParserState *ps, const char *name)
+{
+ return ps->func.find(name) != ps->func.end();
+}
+
+static Func get_func(ParserState *ps, const char *name)
+{
+ return ps->func[name].func;
+}
+
+static int get_func_arity(ParserState *ps, const char *name)
+{
+ return ps->func[name].arity;
+}
+
+static void print_value(const Value &val, FILE *fp)
+{
+ switch(val.type) {
+ case VAL_NUM:
+ fprintf(fp, "%f", val.v[0]);
+ break;
+
+ case VAL_VEC:
+ fprintf(fp, "[%f %f %f %f]", val.v[0], val.v[1], val.v[2], val.v[3]);
+ break;
+
+ case VAL_STR:
+ fprintf(fp, "\"%s\"", val.s.c_str());
+ break;
+
+ default:
+ fprintf(fp, "<invalid>");
+ }
+}
+
+// Gear-specific stuff
+
+static Gear *begin_gear(ParserState *ps)
+{
+ Gear *res = new Gear;
+ ps->cur_gear.push_back(res);
+ return res;
+}
+
+static bool end_gear(ParserState *ps)
+{
+ if(ps->cur_gear.empty()) {
+ errmsg(ps, "parser error: unbalanced end_gear");
+ return false;
+ }
+ Gear *gear = ps->cur_gear.back();
+ ps->cur_gear.pop_back();
+
+ printf("DBG: end_gear: %s\n", gear->name.c_str());
+
+ if(gear->name.empty() || ps->gears[gear->name]) {
+ char buf[32];
+ sprintf(buf, "gear%04d", (int)ps->gears.size());
+ gear->name = buf;
+ }
+ printf("DBG: adding gear: %s\n", gear->name.c_str());
+ ps->gears[gear->name] = gear;
+ ps->mcn->add_gear(gear);
+
+ if(!ps->cur_gear.empty()) {
+ ps->cur_gear.back()->attach(gear);
+ }
+ return true;
+}
+
+static Gear *this_gear(ParserState *ps)
+{
+ return ps->cur_gear.empty() ? 0 : ps->cur_gear.back();
+}
+
+static Gear *find_gear(ParserState *ps, const char *name)
+{
+ // search progressively wider lexical scopes
+ std::vector<Gear*>::const_reverse_iterator it = ps->cur_gear.rbegin();
+ while(it != ps->cur_gear.rend()) {
+ Gear *g = *it++;
+ if(g->name == std::string(name)) {
+ return g;
+ }
+ }
+
+ return ps->gears[name];
+}
+
+static void update_gear_vars(ParserState *ps, Gear *gear)
+{
+ std::map<std::string, Value>::const_iterator it = ps->var.begin();
+ while(it != ps->var.end()) {
+ set_gear_var(gear, it->first.c_str(), it->second);
+ ++it;
+ }
+}
+
+#define ASSERT_TYPE(v, t) \
+ do { \
+ if((v).type != (t)) { \
+ fprintf(stderr, "type mismatch while trying to set %s to value: ", name); \
+ print_value(val, stderr); \
+ fputc('\n', stderr); \
+ return false; \
+ } \
+ } while(0)
+
+#define VVEC3(val) Vec3((val).v[0], (val).v[1], (val).v[2])
+#define VVEC4(val) Vec4((val).v[0], (val).v[1], (val).v[2], (val).v[3])
+
+static bool set_gear_var(Gear *gear, const char *name, const Value &val)
+{
+ if(strcmp(name, "name") == 0) {
+ ASSERT_TYPE(val, VAL_STR);
+ gear->name = val.s;
+
+ } else if(strcmp(name, "position") == 0) {
+ ASSERT_TYPE(val, VAL_VEC);
+ gear->pos = VVEC3(val);
+
+ } else if(strcmp(name, "plane") == 0) {
+ ASSERT_TYPE(val, VAL_VEC);
+ gear->axis = VVEC3(val);
+ gear->pdist = val.v[3];
+ printf("setting plane eqn: %f %f %f %f\n", val.v[0], val.v[1], val.v[2], val.v[3]);
+
+ } else if(strcmp(name, "thickness") == 0) {
+ ASSERT_TYPE(val, VAL_NUM);
+ gear->thickness = val.v[0];
+
+ } else if(strcmp(name, "teeth") == 0) {
+ ASSERT_TYPE(val, VAL_NUM);
+ gear->set_teeth(val.v[0]); // set teeth and recalc radius
+
+ } else if(strcmp(name, "teeth_length") == 0) {
+ ASSERT_TYPE(val, VAL_NUM);
+ gear->teeth_length = val.v[0];
+
+ } else if(strcmp(name, "pitch") == 0) {
+ ASSERT_TYPE(val, VAL_NUM);
+ gear->set_teeth(gear->nteeth, val.v[0]); // set pitch and recalc radius
+
+ } else if(strcmp(name, "color") == 0) {
+ ASSERT_TYPE(val, VAL_VEC);
+ gear->color = VVEC3(val);
+
+ } else if(strcmp(name, "roughness") == 0) {
+ ASSERT_TYPE(val, VAL_NUM);
+ gear->roughness = val.v[0];
+
+ } else {
+ return false;
+ }
+
+ return true;
+}
+
+
+// motor stuff
+
+static void update_motor_vars(ParserState *ps, Motor *motor)
+{
+ std::map<std::string, Value>::const_iterator it = ps->var.begin();
+ while(it != ps->var.end()) {
+ set_motor_var(ps, motor, it->first.c_str(), it->second);
+ ++it;
+ }
+}
+
+static bool set_motor_var(ParserState *ps, Motor *motor, const char *name, const Value &val)
+{
+ if(strcmp(name, "drive") == 0) {
+ ASSERT_TYPE(val, VAL_STR);
+ Gear *gear = ps->gears[val.s];
+ if(!gear) {
+ errmsg(ps, "undefined gear: %s", val.s.c_str());
+ return false;
+ }
+ int idx = ps->mcn->get_gear_index(gear);
+ assert(idx >= 0);
+ motor->drive = idx;
+
+ } else if(strcmp(name, "speed") == 0) {
+ ASSERT_TYPE(val, VAL_NUM);
+ motor->speed = val.v[0];
+
+ } else {
+ return false;
+ }
+ return true;
+}
+
+
+// built-in functions
+static bool func_adjacent(ParserState *ps)
+{
+ Value val_angle = ps->val.top(); ps->val.pop();
+ Value val_gear_name = ps->val.top(); ps->val.pop();
+
+ if(val_gear_name.type != VAL_STR) {
+ expected(ps, "gear name (string) as 1st arg to adjacent");
+ return false;
+ }
+ if(val_angle.type != VAL_NUM) {
+ expected(ps, "angle as 2nd arg to adjacent");
+ return false;
+ }
+
+ Gear *gthis = this_gear(ps);
+ if(!gthis) {
+ errmsg(ps, "adjacent: called outside of a gear block");
+ return false;
+ }
+
+ Gear *gother = ps->gears[val_gear_name.s];
+ if(!gother) {
+ errmsg(ps, "adjacent: gear \"%s\" not found", val_gear_name.s.c_str());
+ return false;
+ }
+
+ Mat4 xform;
+ xform.rotation(deg_to_rad(val_angle.v[0]), gother->axis);
+ Vec3 dir = xform * Vec3(0, 1, 0);
+
+ float sum_radii = gthis->radius + gother->radius;
+ float avg_teeth_len = (gthis->teeth_length + gother->teeth_length) * 0.5;
+ Vec3 pos = gother->pos + dir * (sum_radii - avg_teeth_len * 0.75);
+
+ Value res;
+ res.type = VAL_VEC;
+ res.v[0] = pos.x;
+ res.v[1] = pos.y;
+ res.v[2] = pos.z;
+ ps->val.push(res);
+ return true;
+}
+
+static bool func_coaxial(ParserState *ps)
+{
+ Value val_dist = ps->val.top(); ps->val.pop();
+ Value val_gear_name = ps->val.top(); ps->val.pop();
+
+ if(val_gear_name.type != VAL_STR) {
+ expected(ps, "gear name (string) as 1st arg to func_coaxial");
+ return false;
+ }
+ if(val_dist.type != VAL_NUM) {
+ expected(ps, "stacking distance as 2nd arg to func_coaxial");
+ return false;
+ }
+
+ Gear *gthis = this_gear(ps);
+ if(!gthis) {
+ errmsg(ps, "coaxial: called outside of a gear block");
+ return false;
+ }
+
+ Gear *gother = find_gear(ps, val_gear_name.s.c_str());
+ if(!gother) {
+ errmsg(ps, "coaxial: gear \"%s\" not found", val_gear_name.s.c_str());
+ return false;
+ }
+
+ float avg_thickness = (gthis->thickness + gother->thickness) * 0.5;
+ Vec3 pos = gother->pos + gother->axis * val_dist.v[0] * avg_thickness;
+
+ Value res;
+ res.type = VAL_VEC;
+ res.v[0] = pos.x;
+ res.v[1] = pos.y;
+ res.v[2] = pos.z;
+ ps->val.push(res);
+ return true;
+}
projects / laserbrain_demo / blobdiff