bool
hs_isect_arr (struct hs *res, const struct hs *hs, const array_t *a)
{
const struct hs_vec *v = &hs->list;
array_t tmp[ARRAY_BYTES (hs->len) / sizeof (array_t)];
int pos = -1;
for (int i = 0; i < v->used; i++) {
if (!array_isect (v->elems[i], a, hs->len, tmp)) continue;
pos = i; break;
}
if (pos == -1) return false;
memset (res, 0, sizeof *res);
res->len = hs->len;
struct hs_vec *resv = &res->list;
for (int i = pos; i < v->used; i++) {
if (i == pos) vec_append (resv, xmemdup (tmp, sizeof tmp), false);
else {
array_t *isect = array_isect_a (v->elems[i], a, res->len);
if (!isect) continue;
vec_append (resv, isect, false);
}
struct hs_vec *diff = &v->diff[i], *resd = &resv->diff[resv->used - 1];
for (int j = 0; j < diff->used; j++) {
array_t *isect = array_isect_a (diff->elems[j], a, res->len);
if (!isect) continue;
vec_append (resd, isect, true);
}
}
return true;
}
static qcint_t prog_enterfunction(qc_program_t *prog, prog_section_function_t *func) {
qc_exec_stack_t st;
size_t parampos;
int32_t p;
/* back up locals */
st.localsp = vec_size(prog->localstack);
st.stmt = prog->statement;
st.function = func;
if (prog->xflags & VMXF_TRACE) {
const char *str = prog_getstring(prog, func->name);
vec_push(prog->function_stack, str);
}
#ifdef QCVM_BACKUP_STRATEGY_CALLER_VARS
if (vec_size(prog->stack))
{
prog_section_function_t *cur;
cur = prog->stack[vec_size(prog->stack)-1].function;
if (cur)
{
qcint_t *globals = &prog->globals[0] + cur->firstlocal;
vec_append(prog->localstack, cur->locals, globals);
}
}
#else
{
qcint_t *globals = &prog->globals[0] + func->firstlocal;
vec_append(prog->localstack, func->locals, globals);
}
#endif
/* copy parameters */
parampos = func->firstlocal;
for (p = 0; p < func->nargs; ++p)
{
size_t s;
for (s = 0; s < func->argsize[p]; ++s) {
prog->globals[parampos] = prog->globals[OFS_PARM0 + 3*p + s];
++parampos;
}
}
vec_push(prog->stack, st);
return func->entry;
}
uint32_t code_genstring(code_t *code, const char *str) {
size_t hash;
code_hash_entry_t existing;
if (!str)
return 0;
if (!*str) {
if (!code->string_cached_empty) {
code->string_cached_empty = vec_size(code->chars);
vec_push(code->chars, 0);
}
return code->string_cached_empty;
}
if (OPTS_OPTIMIZATION(OPTIM_OVERLAP_STRINGS)) {
hash = ((unsigned char*)str)[strlen(str)-1];
CODE_HASH_ENTER(existing) = code_util_str_htgeth(code->string_cache, str, hash);
} else {
hash = util_hthash(code->string_cache, str);
CODE_HASH_ENTER(existing) = util_htgeth(code->string_cache, str, hash);
}
if (CODE_HASH_ENTER(existing))
return CODE_HASH_LEAVE(existing);
CODE_HASH_LEAVE(existing) = vec_size(code->chars);
vec_append(code->chars, strlen(str)+1, str);
util_htseth(code->string_cache, str, hash, CODE_HASH_ENTER(existing));
return CODE_HASH_LEAVE(existing);
}
/** Generates a linear path from point p1 to point p2 */
std::vector<std::vector<double> > PathPlanner::genLinearPath(std::vector<double> &p1, std::vector<double> &p2) {
std::vector<double> vec;
std::vector<double> vec_res;
std::vector<std::vector<double> > solution_vector;
for (size_t i = 0; i < p1.size(); i++) {
vec.push_back(p2[i] - p1[i]);
vec_res.push_back(p1[i]);
}
double length(0.0);
for (size_t j = 0; j < vec.size(); j++) {
length += std::pow(vec[j], 2);
}
length = std::sqrt(length);
int num_points = (int) (length / planning_range_);
std::vector<double> vec_norm;
for (size_t j = 0; j < vec.size(); j++) {
vec_norm.push_back(vec[j] / length);
}
solution_vector.push_back(p1);
for (int j = 0; j < num_points; j++) {
for (size_t k = 0; k < vec_res.size(); k++) {
vec_res[k] += planning_range_ * vec_norm[k];
}
std::vector<double> vec_append(vec_res);
solution_vector.push_back(vec_append);
}
solution_vector.push_back(p2);
return solution_vector;
}
static void
vec_diff (struct hs_vec *dst, const array_t *isect, const struct hs_vec *src, int len)
{
for (int i = 0; i < src->used; i++) {
array_t *tmp = array_isect_a (isect, src->elems[i], len);
if (tmp) vec_append (dst, tmp, true);
}
}
void
hs_diff (struct hs *hs, const array_t *a)
{
struct hs_vec *v = &hs->list;
for (int i = 0; i < v->used; i++) {
array_t *tmp = array_isect_a (v->elems[i], a, hs->len);
if (tmp) vec_append (&v->diff[i], tmp, true);
}
}
void
hs_comp_diff (struct hs *hs)
{
struct hs_vec *v = &hs->list, new_list = {0};
for (int i = 0; i < v->used; i++) {
struct hs tmp = {hs->len}, tmp2 = {hs->len};
vec_append (&tmp.list, v->elems[i], false);
v->elems[i] = NULL;
tmp2.list = v->diff[i];
hs_minus (&tmp, &tmp2);
if (!new_list.used) new_list = tmp.list;
else {
for (int j = 0; j < tmp.list.used; j++) {
vec_append (&new_list, tmp.list.elems[j], false);
tmp.list.elems[j] = NULL;
}
hs_destroy (&tmp);
}
}
vec_destroy (v);
hs->list = new_list;
}
struct hs_vec
vec_isect_a (const struct hs_vec *a, const struct hs_vec *b, int len)
{
struct hs_vec new_list = {0};
for (int i = 0; i < a->used; i++) {
for (int j = 0; j < b->used; j++) {
array_t *isect = array_isect_a (a->elems[i], b->elems[j], len);
if (!isect) continue;
vec_append (&new_list, isect, false);
int idx = new_list.used - 1;
struct hs_vec *d = &new_list.diff[idx];
vec_diff (d, isect, &a->diff[i], len);
vec_diff (d, isect, &b->diff[j], len);
}
}
return new_list;
}
bool hs_potponed_diff_and_rewrite (const struct hs *orig_hs, struct hs *rw_hs,
const array_t *diff, const array_t *mask, const array_t *rewrite) {
const struct hs_vec *orig_v = &orig_hs->list;
struct hs_vec *rw_v = &rw_hs->list;
bool changed = false;
for (int i = 0; i < orig_v->used; i++) {
array_t *tmp = array_isect_a (orig_v->elems[i], diff, orig_hs->len);
if (!tmp) continue;
int n = array_x_count (orig_v->elems[i], mask, orig_hs->len);
int m = array_rewrite (tmp, mask, rewrite, orig_hs->len);
if (n == m) {
changed = true;
vec_append (&rw_v->diff[i], tmp, true);
} else {
free(tmp);
}
}
return changed;
}
void
hs_cmpl (struct hs *hs)
{
if (!hs->list.used) {
hs_add (hs, array_create (hs->len, BIT_X));
return;
}
struct hs_vec *v = &hs->list, new_list = {0};
for (int i = 0; i < v->used; i++) {
struct hs_vec tmp = {0};
tmp.elems = array_cmpl_a (v->elems[i], hs->len, &tmp.used);
tmp.alloc = tmp.used;
/* If complement is empty, result will be empty. */
if (!tmp.elems) {
vec_destroy (&new_list);
vec_destroy (&hs->list);
memset (&hs->list, 0, sizeof hs->list);
return;
}
tmp.diff = xcalloc (tmp.alloc, sizeof *tmp.diff);
if (v->diff) { /* NULL if called from comp_diff */
struct hs_vec *d = &v->diff[i];
for (int j = 0; j < d->used; j++)
vec_append (&tmp, d->elems[j], false);
}
if (!new_list.used) new_list = tmp;
else {
vec_isect (&new_list, &tmp, hs->len);
vec_destroy (&tmp);
}
}
vec_destroy (v);
hs->list = new_list;
}
/* Remove elems of V that are covered by another elem. V must be a diff list.
LEN is length of each array. */
static void
vec_compact (struct hs_vec *v, const array_t* mask, int len)
{
for (int i = 0; i < v->used; i++) {
for (int j = i + 1; j < v->used; j++) {
array_t *extra;
array_combine(&(v->elems[i]), &(v->elems[j]), &extra, mask, len);
if (extra) {
vec_append(v,extra,true);
}
if (v->elems[i] == NULL) {
vec_elem_free (v, i);
if (v->elems[j] == NULL) vec_elem_free (v, j);
i--;
break;
}
if (v->elems[j] == NULL) {
vec_elem_free (v, j);
j--;
continue;
}
}
}
}
void
hs_vec_append (struct hs_vec *v, array_t *a, bool diff)
{ vec_append (v, a, diff); }
void
hs_add (struct hs *hs, array_t *a)
{ vec_append (&hs->list, a, false); }
static char *opts_ini_load(const char *section, const char *name, const char *value, char **parse_file) {
char *error = NULL;
bool found = false;
/*
* undef all of these because they may still be defined like in my
* case they where.
*/
#undef GMQCC_TYPE_FLAGS
#undef GMQCC_TYPE_OPTIMIZATIONS
#undef GMQCC_TYPE_WARNS
/* deal with includes */
if (!strcmp(section, "includes")) {
static const char *include_error_beg = "failed to open file `";
static const char *include_error_end = "' for inclusion";
fs_file_t *file = fs_file_open(value, "r");
found = true;
if (!file) {
vec_append(error, strlen(include_error_beg), include_error_beg);
vec_append(error, strlen(value), value);
vec_append(error, strlen(include_error_end), include_error_end);
} else {
if (opts_ini_parse(file, &opts_ini_load, &error, parse_file) != 0)
found = false;
/* Change the file name */
mem_d(*parse_file);
*parse_file = util_strdup(value);
fs_file_close(file);
}
}
/* flags */
#define GMQCC_TYPE_FLAGS
#define GMQCC_DEFINE_FLAG(X) \
if (!strcmp(section, "flags") && !strcmp(name, #X)) { \
opts_set(opts.flags, X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* warnings */
#define GMQCC_TYPE_WARNS
#define GMQCC_DEFINE_FLAG(X) \
if (!strcmp(section, "warnings") && !strcmp(name, #X)) { \
opts_set(opts.warn, WARN_##X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* Werror-individuals */
#define GMQCC_TYPE_WARNS
#define GMQCC_DEFINE_FLAG(X) \
if (!strcmp(section, "errors") && !strcmp(name, #X)) { \
opts_set(opts.werror, WARN_##X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* optimizations */
#define GMQCC_TYPE_OPTIMIZATIONS
#define GMQCC_DEFINE_FLAG(X,Y) \
if (!strcmp(section, "optimizations") && !strcmp(name, #X)) { \
opts_set(opts.optimization, OPTIM_##X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* nothing was found ever! */
if (!found) {
if (strcmp(section, "includes") &&
strcmp(section, "flags") &&
strcmp(section, "warnings") &&
strcmp(section, "optimizations"))
{
static const char *invalid_section = "invalid_section `";
vec_append(error, strlen(invalid_section), invalid_section);
vec_append(error, strlen(section), section);
vec_push(error, '`');
} else if (strcmp(section, "includes")) {
static const char *invalid_variable = "invalid_variable `";
static const char *in_section = "` in section: `";
vec_append(error, strlen(invalid_variable), invalid_variable);
vec_append(error, strlen(name), name);
vec_append(error, strlen(in_section), in_section);
vec_append(error, strlen(section), section);
vec_push(error, '`');
} else {
static const char *expected_something = "expected something";
vec_append(error, strlen(expected_something), expected_something);
}
}
vec_push(error, '\0');
return error;
}
static size_t opts_ini_parse (
fs_file_t *filehandle,
char *(*loadhandle)(const char *, const char *, const char *, char **),
char **errorhandle,
char **parse_file
) {
size_t linesize;
size_t lineno = 1;
size_t error = 0;
char *line = NULL;
char section_data[2048] = "";
char oldname_data[2048] = "";
/* parsing and reading variables */
char *parse_beg;
char *parse_end;
char *read_name;
char *read_value;
while (fs_file_getline(&line, &linesize, filehandle) != FS_FILE_EOF) {
parse_beg = line;
/* handle BOM */
if (lineno == 1 && (
(unsigned char)parse_beg[0] == 0xEF &&
(unsigned char)parse_beg[1] == 0xBB &&
(unsigned char)parse_beg[2] == 0xBF
)
) {
parse_beg ++; /* 0xEF */
parse_beg ++; /* 0xBB */
parse_beg ++; /* 0xBF */
}
if (*(parse_beg = opts_ini_lskip(opts_ini_rstrip(parse_beg))) == ';' || *parse_beg == '#') {
/* ignore '#' is a perl extension */
} else if (*parse_beg == '[') {
/* section found */
if (*(parse_end = opts_ini_next(parse_beg + 1, ']')) == ']') {
* parse_end = '\0'; /* terminate bro */
util_strncpy(section_data, parse_beg + 1, sizeof(section_data));
section_data[sizeof(section_data) - 1] = '\0';
*oldname_data = '\0';
} else if (!error) {
/* otherwise set error to the current line number */
error = lineno;
}
} else if (*parse_beg && *parse_beg != ';') {
/* not a comment, must be a name value pair :) */
if (*(parse_end = opts_ini_next(parse_beg, '=')) != '=')
parse_end = opts_ini_next(parse_beg, ':');
if (*parse_end == '=' || *parse_end == ':') {
*parse_end = '\0'; /* terminate bro */
read_name = opts_ini_rstrip(parse_beg);
read_value = opts_ini_lskip(parse_end + 1);
if (*(parse_end = opts_ini_next(read_value, '\0')) == ';')
* parse_end = '\0';
opts_ini_rstrip(read_value);
/* valid name value pair, lets call down to handler */
util_strncpy(oldname_data, read_name, sizeof(oldname_data));
oldname_data[sizeof(oldname_data) - 1] ='\0';
if ((*errorhandle = loadhandle(section_data, read_name, read_value, parse_file)) && !error)
error = lineno;
} else if (!strcmp(section_data, "includes")) {
/* Includes are special */
if (*(parse_end = opts_ini_next(parse_beg, '=')) == '='
|| *(parse_end = opts_ini_next(parse_beg, ':')) == ':') {
static const char *invalid_include = "invalid use of include";
vec_append(*errorhandle, strlen(invalid_include), invalid_include);
error = lineno;
} else {
read_name = opts_ini_rstrip(parse_beg);
if ((*errorhandle = loadhandle(section_data, read_name, read_name, parse_file)) && !error)
error = lineno;
}
} else if (!error) {
/* otherwise set error to the current line number */
error = lineno;
}
}
lineno++;
}
mem_d(line);
return error;
}
static char *opts_ini_load(const char *section, const char *name, const char *value) {
char *error = NULL;
bool found = false;
/*
* undef all of these because they may still be defined like in my
* case they where.
*/
#undef GMQCC_TYPE_FLAGS
#undef GMQCC_TYPE_OPTIMIZATIONS
#undef GMQCC_TYPE_WARNS
/* flags */
#define GMQCC_TYPE_FLAGS
#define GMQCC_DEFINE_FLAG(X) \
if (!strcmp(section, "flags") && !strcmp(name, #X)) { \
opts_set(opts.flags, X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* warnings */
#define GMQCC_TYPE_WARNS
#define GMQCC_DEFINE_FLAG(X) \
if (!strcmp(section, "warnings") && !strcmp(name, #X)) { \
opts_set(opts.warn, WARN_##X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* Werror-individuals */
#define GMQCC_TYPE_WARNS
#define GMQCC_DEFINE_FLAG(X) \
if (!strcmp(section, "errors") && !strcmp(name, #X)) { \
opts_set(opts.werror, WARN_##X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* optimizations */
#define GMQCC_TYPE_OPTIMIZATIONS
#define GMQCC_DEFINE_FLAG(X,Y) \
if (!strcmp(section, "optimizations") && !strcmp(name, #X)) { \
opts_set(opts.optimization, OPTIM_##X, opts_ini_bool(value)); \
found = true; \
}
#include "opts.def"
/* nothing was found ever! */
if (!found) {
if (strcmp(section, "flags") &&
strcmp(section, "warnings") &&
strcmp(section, "optimizations"))
{
vec_append(error, 17, "invalid section `");
vec_append(error, strlen(section), section);
vec_push (error, '`');
vec_push (error, '\0');
} else {
vec_append(error, 18, "invalid variable `");
vec_append(error, strlen(name), name);
vec_push (error, '`');
vec_append(error, 14, " in section: `");
vec_append(error, strlen(section), section);
vec_push (error, '`');
vec_push (error, '\0');
}
}
return error;
}
