From 276db9c96802d7cf1426b51aad7b17797eda6115 Mon Sep 17 00:00:00 2001 From: Michel Pollet Date: Sat, 19 May 2012 14:32:04 +0100 Subject: [PATCH] reprap: Added c3, a small scene graph lib To display the hotend over the printer bed. Very very primitive Signed-off-by: Michel Pollet --- examples/board_reprap/src/c3/README.md | 55 + examples/board_reprap/src/c3/c3.c | 24 + examples/board_reprap/src/c3/c3.h | 30 + examples/board_reprap/src/c3/c3algebra.c | 1001 +++++++++++++++++ examples/board_reprap/src/c3/c3algebra.h | 214 ++++ examples/board_reprap/src/c3/c3arcball.c | 254 +++++ examples/board_reprap/src/c3/c3arcball.h | 125 ++ examples/board_reprap/src/c3/c3cairo.c | 41 + examples/board_reprap/src/c3/c3cairo.h | 41 + examples/board_reprap/src/c3/c3camera.c | 403 +++++++ examples/board_reprap/src/c3/c3camera.h | 236 ++++ examples/board_reprap/src/c3/c3geometry.c | 61 + examples/board_reprap/src/c3/c3geometry.h | 72 ++ examples/board_reprap/src/c3/c3object.c | 241 ++++ examples/board_reprap/src/c3/c3object.h | 85 ++ .../board_reprap/src/c3/c3object_driver.h | 73 ++ examples/board_reprap/src/c3/c3quaternion.c | 249 ++++ examples/board_reprap/src/c3/c3quaternion.h | 93 ++ examples/board_reprap/src/c3/c3transform.c | 63 ++ examples/board_reprap/src/c3/c3transform.h | 49 + examples/board_reprap/src/c3/c_array.h | 134 +++ examples/board_reprap/src/c3/c_utils.c | 67 ++ examples/board_reprap/src/c3/c_utils.h | 166 +++ 23 files changed, 3777 insertions(+) create mode 100644 examples/board_reprap/src/c3/README.md create mode 100644 examples/board_reprap/src/c3/c3.c create mode 100644 examples/board_reprap/src/c3/c3.h create mode 100644 examples/board_reprap/src/c3/c3algebra.c create mode 100644 examples/board_reprap/src/c3/c3algebra.h create mode 100644 examples/board_reprap/src/c3/c3arcball.c create mode 100644 examples/board_reprap/src/c3/c3arcball.h create mode 100644 examples/board_reprap/src/c3/c3cairo.c create mode 100644 examples/board_reprap/src/c3/c3cairo.h create mode 100644 examples/board_reprap/src/c3/c3camera.c create mode 100644 examples/board_reprap/src/c3/c3camera.h create mode 100644 examples/board_reprap/src/c3/c3geometry.c create mode 100644 examples/board_reprap/src/c3/c3geometry.h create mode 100644 examples/board_reprap/src/c3/c3object.c create mode 100644 examples/board_reprap/src/c3/c3object.h create mode 100644 examples/board_reprap/src/c3/c3object_driver.h create mode 100644 examples/board_reprap/src/c3/c3quaternion.c create mode 100644 examples/board_reprap/src/c3/c3quaternion.h create mode 100644 examples/board_reprap/src/c3/c3transform.c create mode 100644 examples/board_reprap/src/c3/c3transform.h create mode 100644 examples/board_reprap/src/c3/c_array.h create mode 100644 examples/board_reprap/src/c3/c_utils.c create mode 100644 examples/board_reprap/src/c3/c_utils.h diff --git a/examples/board_reprap/src/c3/README.md b/examples/board_reprap/src/c3/README.md new file mode 100644 index 0000000..b1e7090 --- /dev/null +++ b/examples/board_reprap/src/c3/README.md @@ -0,0 +1,55 @@ +libc3 - No frill 'scene' graph library in C +===== +(C) 2012 Michel Pollet + +**WARNING** This API is not your nanny. It is made to be lean, mean, efficient +with no frill, no asserts, no bounds checking, no sugar coating. + +On the other hand it's fast, reasonably clean and is a micro-fraction of the +other giganormous 'scene graphs' or 'game engine' libraries around. + +It's vaguely inspired by THREE.js funnily enough, because it allows you to +hack around and quickly get stuff on screen with the minimal amount of +effort. + +The API has various bits: +* c3algebra: C derivative of an old C++ piece of code I had lying around and that has +been present in my toolset for a long time. It gives you *vectors* (c3vec2, c3vec3, c3vec4) +and *matrices* (c3mat3, c3mat4) with various tools to manipulate them. +* c3quaternion: Quaternion implementation using c3algebra +* c3camera/c3arcball: camera manipulation, not perfect + +The data structure is as follow: +* *c3object*: + * Has a list of (sub) c3objects + * Has a list of c3transforms (ie matrices) + * Has a list of c3geometry (ie real vertices and stuff) + The object is a container for other objects, and for geometry itself. Objects don't + necessary have geometry and/or sub objects, and don't even need transforms if their + vertices are already projected. +* *c3geometry*: + * Has a 'type' (lines, quads, triangles..) + * Has a 'material' (ie color, texture... to be completed) + * Has a list of vertices + * Has a list of texture coordinates (optional) + * Has a list of vertices colors (optional) + * Has a cached copy of a vertices when it has been 'projected' +* *c3transform*: + Is just a sugar coated matrix, with an optional name. + +Dirtyness +--------- +There is a notion of 'dirtyness' in the tree, when you touch c3transform, and/remove +objects and geometry, a dirty bit is propagated up the tree of object. This tells the +rendering it needs to reproject the dirty bits and repopulate the projected vertice +cache. + +The 'dirty' bit moves both ways, when setting a dirty bit to true, it propagates upward, +when you set it to false, it propagates downward in the tree. + +"Inheritance" +------------- +There is a vague notion of inheritance for objects, where you can create more complex +ones and install a 'driver' (ie a function pointer table) that will be called to +perform various things. The skim is still evolving. + diff --git a/examples/board_reprap/src/c3/c3.c b/examples/board_reprap/src/c3/c3.c new file mode 100644 index 0000000..38fcf83 --- /dev/null +++ b/examples/board_reprap/src/c3/c3.c @@ -0,0 +1,24 @@ +/* + c3.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#include "c3.h" + diff --git a/examples/board_reprap/src/c3/c3.h b/examples/board_reprap/src/c3/c3.h new file mode 100644 index 0000000..5766c58 --- /dev/null +++ b/examples/board_reprap/src/c3/c3.h @@ -0,0 +1,30 @@ +/* + c3.h + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3_H___ +#define __C3_H___ + +#include "c3/c3object.h" +#include "c3/c3geometry.h" +#include "c3/c3transform.h" + +#endif /* __C3_H___ */ diff --git a/examples/board_reprap/src/c3/c3algebra.c b/examples/board_reprap/src/c3/c3algebra.c new file mode 100644 index 0000000..af816b1 --- /dev/null +++ b/examples/board_reprap/src/c3/c3algebra.c @@ -0,0 +1,1001 @@ +/* + c3algebra.c + + Copyright 2008-2012 Michel Pollet + + Derivative and inspiration from original C++: + Paul Rademacher & Jean-Francois DOUEG, + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + +#include +#include +#include "c3/c3algebra.h" + +#ifndef MAX +#define MAX(a,b) ((a)>(b) ? (a) : (b)) +#define MIN(a,b) ((a)<(b) ? (a) : (b)) +#endif + +/**************************************************************** + * * + * c3vec2 Member functions * + * * + ****************************************************************/ + +/******************** c3vec2 CONSTRUCTORS ********************/ + +c3vec2 c3vec2_zero() +{ + c3vec2 n = { .x = 0, .y = 0 }; + return n; +} + +c3vec2 c3vec2f(c3f x, c3f y) +{ + c3vec2 v = { .x = x, .y = y }; + return v; +} + +/******************** c3vec2 ASSIGNMENT OPERATORS ******************/ + +c3vec2 c3vec2_add(c3vec2 a, const c3vec2 v) +{ + a.n[VX] += v.n[VX]; + a.n[VY] += v.n[VY]; + return a; +} + +c3vec2 c3vec2_sub(c3vec2 a, const c3vec2 v) +{ + a.n[VX] -= v.n[VX]; + a.n[VY] -= v.n[VY]; + return a; +} + +c3vec2 c3vec2_mulf(c3vec2 a, c3f d) +{ + a.n[VX] *= d; + a.n[VY] *= d; + return a; +} + +c3vec2 c3vec2_divf(c3vec2 a, c3f d) +{ + c3f d_inv = 1.0f/d; + a.n[VX] *= d_inv; + a.n[VY] *= d_inv; + return a; +} + +/******************** c3vec2 SPECIAL FUNCTIONS ********************/ + + +c3f c3vec2_length2(const c3vec2 a) +{ + return a.n[VX]*a.n[VX] + a.n[VY]*a.n[VY]; +} + +c3f c3vec2_length(const c3vec2 a) +{ + return (c3f) sqrt(c3vec2_length2(a)); +} + +c3vec2 c3vec2_normalize(const c3vec2 a) // it is up to caller to avoid divide-by-zero +{ + return c3vec2_divf(a, c3vec2_length(a)); +} + +c3vec2 c3vec2_apply(c3vec2 a, V_FCT_PTR fct) +{ + a.n[VX] = fct(a.n[VX]); + a.n[VY] = fct(a.n[VY]); + return a; +} + + +/******************** c3vec2 FRIENDS *****************************/ + +c3vec2 c3vec2_minus(const c3vec2 a) +{ + return c3vec2f(-a.n[VX],-a.n[VY]); +} + +c3vec2 c3mat3_mulv2(const c3mat3p a, const c3vec2 v) +{ + c3vec2 av; + + av.n[VX] = a->v[0].n[VX]*v.n[VX] + a->v[0].n[VY]*v.n[VY] + a->v[0].n[VZ]; + av.n[VY] = a->v[1].n[VX]*v.n[VX] + a->v[1].n[VY]*v.n[VY] + a->v[1].n[VZ]; +// av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ]; + + return av; +} + +c3vec2 c3vec2_mulm3(const c3vec2 v, const c3mat3p a) +{ + c3mat3 t = c3mat3_transpose(a); + return c3mat3_mulv2(&t, v); +} + +c3vec3 c3mat3_mulv3(const c3mat3p a, const c3vec3 v) +{ + c3vec3 av; + + av.n[VX] = a->v[0].n[VX]*v.n[VX] + a->v[0].n[VY]*v.n[VY] + a->v[0].n[VZ]*v.n[VZ]; + av.n[VY] = a->v[1].n[VX]*v.n[VX] + a->v[1].n[VY]*v.n[VY] + a->v[1].n[VZ]*v.n[VZ]; + av.n[VZ] = a->v[2].n[VX]*v.n[VX] + a->v[2].n[VY]*v.n[VY] + a->v[2].n[VZ]*v.n[VZ]; + + return av; +} + +c3vec3 c3vec3_mulm3(const c3vec3 v, const c3mat3p a) +{ + c3mat3 t = c3mat3_transpose(a); + return c3mat3_mulv3(&t, v); +} + +c3f c3vec2_dot(const c3vec2 a, const c3vec2 b) +{ + return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY]; +} + +c3vec3 c3vec2_cross(const c3vec2 a, const c3vec2 b) +{ + return c3vec3f(0.0, 0.0, a.n[VX] * b.n[VY] - b.n[VX] * a.n[VY]); +} + +int c3vec2_equal(const c3vec2 a, const c3vec2 b) +{ + return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]); +} + + +c3vec2 c3vec2_min(const c3vec2 a, const c3vec2 b) +{ + return c3vec2f(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY])); +} + +c3vec2 c3vec2_max(const c3vec2 a, const c3vec2 b) +{ + return c3vec2f(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY])); +} + +c3vec2 c3vec2_prod(const c3vec2 a, const c3vec2 b) +{ + return c3vec2f(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY]); +} + +/**************************************************************** + * * + * c3vec3 Member functions * + * * + ****************************************************************/ + +// CONSTRUCTORS + +c3vec3 c3vec3_zero() +{ + c3vec3 n = { .x = 0, .y = 0, .z = 0 }; + return n; +} + +c3vec3 c3vec3f(c3f x, c3f y, c3f z) +{ + c3vec3 v = { .x = x, .y = y, .z = z }; + return v; +} + +c3vec3 c3vec3_vec2f(const c3vec2 v, c3f d) +{ + c3vec3 n = { .x = v.x, .y = v.y, .z = d }; + return n; +} + +c3vec3 c3vec3_vec2(const c3vec2 v) +{ + return c3vec3_vec2f(v, 1.0); +} + +c3vec3 c3vec3_vec4(const c3vec4 v) // it is up to caller to avoid divide-by-zero +{ + c3vec3 n; + n.n[VX] = v.n[VX] / v.n[VW]; + n.n[VY] = v.n[VY] / v.n[VW]; + n.n[VZ] = v.n[VZ] / v.n[VW]; + return n; +} + + +c3vec3 c3vec3_add(c3vec3 a, const c3vec3 v) +{ + a.n[VX] += v.n[VX]; + a.n[VY] += v.n[VY]; + a.n[VZ] += v.n[VZ]; + return a; +} + +c3vec3 c3vec3_sub(c3vec3 a, const c3vec3 v) +{ + a.n[VX] -= v.n[VX]; + a.n[VY] -= v.n[VY]; + a.n[VZ] -= v.n[VZ]; + return a; +} + +c3vec3 c3vec3_mulf(c3vec3 a, c3f d) +{ + a.n[VX] *= d; + a.n[VY] *= d; + a.n[VZ] *= d; + return a; +} + +c3vec3 c3vec3_divf(c3vec3 a, c3f d) +{ + c3f d_inv = 1.0f/d; + a.n[VX] *= d_inv; + a.n[VY] *= d_inv; + a.n[VZ] *= d_inv; + return a; +} + +// SPECIAL FUNCTIONS + +c3f c3vec3_length2(const c3vec3 a) +{ + return a.n[VX]*a.n[VX] + a.n[VY]*a.n[VY] + a.n[VZ]*a.n[VZ]; +} + +c3f c3vec3_length(const c3vec3 a) +{ + return (c3f) sqrt(c3vec3_length2(a)); +} + +c3vec3 c3vec3_normalize(const c3vec3 a) // it is up to caller to avoid divide-by-zero +{ + return c3vec3_divf(a, c3vec3_length(a)); +} + +c3vec3 c3vec3_homogenize(c3vec3 a) // it is up to caller to avoid divide-by-zero +{ + a.n[VX] /= a.n[VZ]; + a.n[VY] /= a.n[VZ]; + a.n[VZ] = 1.0; + return a; +} + +c3vec3 c3vec3_apply(c3vec3 a, V_FCT_PTR fct) +{ + a.n[VX] = fct(a.n[VX]); + a.n[VY] = fct(a.n[VY]); + a.n[VZ] = fct(a.n[VZ]); + return a; +} + +// FRIENDS + +c3vec3 c3vec3_minus(const c3vec3 a) +{ + return c3vec3f(-a.n[VX],-a.n[VY],-a.n[VZ]); +} + +#if later +c3vec3 operator*(const c3mat4 &a, const c3vec3 &v) +{ + return a*c3vec4(v); +} + +c3vec3 operator*(const c3vec3 &v, c3mat4 &a) +{ + return a.transpose()*v; +} +#endif + +c3f c3vec3_dot(const c3vec3 a, const c3vec3 b) +{ + return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ]; +} + +c3vec3 c3vec3_cross(const c3vec3 a, const c3vec3 b) +{ + return + c3vec3f(a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY], + a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ], + a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX]); +} + +int c3vec3_equal(const c3vec3 a, const c3vec3 b) +{ + return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ]); +} + + +c3vec3 c3vec3_min(const c3vec3 a, const c3vec3 b) +{ + return c3vec3f( + MIN(a.n[VX], b.n[VX]), + MIN(a.n[VY], b.n[VY]), + MIN(a.n[VZ], b.n[VZ])); +} + +c3vec3 c3vec3_max(const c3vec3 a, const c3vec3 b) +{ + return c3vec3f( + MAX(a.n[VX], b.n[VX]), + MAX(a.n[VY], b.n[VY]), + MAX(a.n[VZ], b.n[VZ])); +} + +c3vec3 c3vec3_prod(const c3vec3 a, const c3vec3 b) +{ + return c3vec3f(a.n[VX]*b.n[VX], a.n[VY]*b.n[VY], a.n[VZ]*b.n[VZ]); +} + +/**************************************************************** + * * + * c3vec4 Member functions * + * * + ****************************************************************/ + +// CONSTRUCTORS + +c3vec4 c3vec4_zero() +{ + c3vec4 n = { .x = 0, .y = 0, .z = 0, .w = 1.0 }; + return n; +} + +c3vec4 c3vec4f(c3f x, c3f y, c3f z, c3f w) +{ + c3vec4 n = { .x = x, .y = y, .z = z, .w = w }; + return n; +} + +c3vec4 c3vec4_vec3(const c3vec3 v) +{ + return c3vec4f(v.n[VX], v.n[VY], v.n[VZ], 1.0); +} + +c3vec4 c3vec4_vec3f(const c3vec3 v, c3f d) +{ + return c3vec4f(v.n[VX], v.n[VY], v.n[VZ], d); +} + +// ASSIGNMENT OPERATORS + +c3vec4 c3vec4_add(c3vec4 a, const c3vec4 v) +{ + a.n[VX] += v.n[VX]; + a.n[VY] += v.n[VY]; + a.n[VZ] += v.n[VZ]; + a.n[VW] += v.n[VW]; + return a; +} + +c3vec4 c3vec4_sub(c3vec4 a, const c3vec4 v) +{ + a.n[VX] -= v.n[VX]; + a.n[VY] -= v.n[VY]; + a.n[VZ] -= v.n[VZ]; + a.n[VW] -= v.n[VW]; + return a; +} + +c3vec4 c3vec4_mulf(c3vec4 a, c3f d) +{ + a.n[VX] *= d; + a.n[VY] *= d; + a.n[VZ] *= d; + a.n[VW] *= d; + return a; +} + +c3vec4 c3vec4_divf(c3vec4 a, c3f d) +{ + c3f d_inv = 1.0f/d; + a.n[VX] *= d_inv; + a.n[VY] *= d_inv; + a.n[VZ] *= d_inv; + a.n[VW] *= d_inv; + return a; +} + +// SPECIAL FUNCTIONS + +c3f c3vec4_length2(const c3vec4 a) +{ + return a.n[VX]*a.n[VX] + a.n[VY]*a.n[VY] + a.n[VZ]*a.n[VZ] + a.n[VW]*a.n[VW]; +} + +c3f c3vec4_length(const c3vec4 a) +{ + return (c3f) sqrt(c3vec4_length2(a)); +} + +c3vec4 c3vec4_normalize(c3vec4 a) // it is up to caller to avoid divide-by-zero +{ + return c3vec4_divf(a, c3vec4_length(a)); +} + +c3vec4 c3vec4_homogenize(c3vec4 a) // it is up to caller to avoid divide-by-zero +{ + a.n[VX] /= a.n[VW]; + a.n[VY] /= a.n[VW]; + a.n[VZ] /= a.n[VW]; + a.n[VW] = 1.0; + return a; +} + +c3vec4 c3vec4_apply(c3vec4 a, V_FCT_PTR fct) +{ + a.n[VX] = fct(a.n[VX]); + a.n[VY] = fct(a.n[VY]); + a.n[VZ] = fct(a.n[VZ]); + a.n[VW] = fct(a.n[VW]); + return a; +} + +c3vec4 c3mat4_mulv4(const c3mat4p a, const c3vec4 v) +{ + #define ROWCOL(i) \ + a->v[i].n[0]*v.n[VX] + \ + a->v[i].n[1]*v.n[VY] + \ + a->v[i].n[2]*v.n[VZ] + \ + a->v[i].n[3]*v.n[VW] + + return c3vec4f(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3)); + + #undef ROWCOL +} + +c3vec4 c3vec4_mulm4(const c3vec4 v, const c3mat4p a) +{ + c3mat4 m = c3mat4_transpose(a); + return c3mat4_mulv4(&m, v); +} + +c3vec3 c3mat4_mulv3(const c3mat4p a, const c3vec3 v) +{ + return c3vec3_vec4(c3mat4_mulv4(a, c3vec4_vec3(v))); +} + +c3vec4 c3vec4_minus(const c3vec4 a) +{ + return c3vec4f(-a.n[VX],-a.n[VY],-a.n[VZ],-a.n[VW]); +} + +int c3vec4_equal(const c3vec4 a, const c3vec4 b) +{ + return + (a.n[VX] == b.n[VX]) && + (a.n[VY] == b.n[VY]) && + (a.n[VZ] == b.n[VZ]) && + (a.n[VW] == b.n[VW]); +} + +c3vec4 c3vec4_min(const c3vec4 a, const c3vec4 b) +{ + return c3vec4f( + MIN(a.n[VX], b.n[VX]), + MIN(a.n[VY], b.n[VY]), + MIN(a.n[VZ], b.n[VZ]), + MIN(a.n[VW], b.n[VW])); +} + +c3vec4 c3vec4_max(const c3vec4 a, const c3vec4 b) +{ + return c3vec4f( + MAX(a.n[VX], b.n[VX]), + MAX(a.n[VY], b.n[VY]), + MAX(a.n[VZ], b.n[VZ]), + MAX(a.n[VW], b.n[VW])); +} + +c3vec4 c3vec4_prod(const c3vec4 a, const c3vec4 b) +{ + return c3vec4f( + a.n[VX] * b.n[VX], + a.n[VY] * b.n[VY], + a.n[VZ] * b.n[VZ], + a.n[VW] * b.n[VW]); +} + +/**************************************************************** + * * + * c3mat3 member functions * + * * + ****************************************************************/ + +// CONSTRUCTORS + +c3mat3 c3mat3_identity() +{ + return identity2D(); +} + +c3mat3 c3mat3_vec3(const c3vec3 v0, const c3vec3 v1, const c3vec3 v2) +{ + c3mat3 m = { .v[0] = v0, .v[1] = v1, .v[2] = v2 }; + return m; +} + +c3mat3p c3mat3_add(const c3mat3p a, const c3mat3p m) +{ + a->v[0] = c3vec3_add(a->v[0], m->v[0]); + a->v[1] = c3vec3_add(a->v[1], m->v[1]); + a->v[2] = c3vec3_add(a->v[2], m->v[2]); + return a; +} + +c3mat3p c3mat3_sub(const c3mat3p a, const c3mat3p m) +{ + a->v[0] = c3vec3_sub(a->v[0], m->v[0]); + a->v[1] = c3vec3_sub(a->v[1], m->v[1]); + a->v[2] = c3vec3_sub(a->v[2], m->v[2]); + return a; +} + +c3mat3p c3mat3_mulf(const c3mat3p a, c3f d) +{ + a->v[0] = c3vec3_mulf(a->v[0], d); + a->v[1] = c3vec3_mulf(a->v[1], d); + a->v[2] = c3vec3_mulf(a->v[2], d); + return a; +} + +c3mat3p c3mat3_divf(const c3mat3p a, c3f d) +{ + a->v[0] = c3vec3_divf(a->v[0], d); + a->v[1] = c3vec3_divf(a->v[1], d); + a->v[2] = c3vec3_divf(a->v[2], d); + return a; +} + +// SPECIAL FUNCTIONS + +c3mat3 c3mat3_transpose(const c3mat3p a) +{ + return c3mat3_vec3( + c3vec3f(a->v[0].n[0], a->v[1].n[0], a->v[2].n[0]), + c3vec3f(a->v[0].n[1], a->v[1].n[1], a->v[2].n[1]), + c3vec3f(a->v[0].n[2], a->v[1].n[2], a->v[2].n[2])); +} + +c3mat3 c3mat3_inverse(const c3mat3p m) // Gauss-Jordan elimination with partial pivoting +{ + c3mat3 a = *m; // As a evolves from original mat into identity + c3mat3 b = c3mat3_identity(); // b evolves from identity into inverse(a) + int i, j, i1; + + // Loop over cols of a from left to right, eliminating above and below diag + for (j = 0; j < 3; j++) { // Find largest pivot in column j among rows j..2 + i1 = j; // Row with largest pivot candidate + for (i = j + 1; i < 3; i++) + if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j])) + i1 = i; + + // Swap rows i1 and j in a and b to put pivot on diagonal + c3vec3 _s; + _s = a.v[i1]; a.v[i1] = a.v[j]; a.v[j] = _s; // swap(a.v[i1], a.v[j]); + _s = b.v[i1]; b.v[i1] = b.v[j]; b.v[j] = _s; //swap(b.v[i1], b.v[j]); + + // Scale row j to have a unit diagonal + if (a.v[j].n[j] == 0.) { + // VEC_ERROR("c3mat3::inverse: singular matrix; can't invert\n"); + return *m; + } + + b.v[j] = c3vec3_divf(b.v[j], a.v[j].n[j]); + a.v[j] = c3vec3_divf(a.v[j], a.v[j].n[j]); + + // Eliminate off-diagonal elems in col j of a, doing identical ops to b + for (i = 0; i < 3; i++) + if (i != j) { + b.v[i] = c3vec3_sub(b.v[i], c3vec3_mulf(b.v[j], a.v[i].n[j])); + a.v[i] = c3vec3_sub(a.v[i], c3vec3_mulf(a.v[j], a.v[i].n[j])); + } + } + + return b; +} + +c3mat3p c3mat3_apply(c3mat3p a, V_FCT_PTR fct) +{ + a->v[0] = c3vec3_apply(a->v[0], fct); + a->v[1] = c3vec3_apply(a->v[1], fct); + a->v[2] = c3vec3_apply(a->v[2], fct); + return a; +} + + +c3mat3 c3mat3_minus(const c3mat3p a) +{ + return c3mat3_vec3( + c3vec3_minus(a->v[0]), + c3vec3_minus(a->v[1]), + c3vec3_minus(a->v[2])); +} + +c3mat3 c3mat3_mul(const c3mat3p a, const c3mat3p b) +{ + #define ROWCOL(i, j) \ + a->v[i].n[0]*b->v[0].n[j] + a->v[i].n[1]*b->v[1].n[j] + a->v[i].n[2]*b->v[2].n[j] + + return c3mat3_vec3( + c3vec3f(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)), + c3vec3f(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)), + c3vec3f(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2))); + + #undef ROWCOL +} + +int c3mat3_equal(const c3mat3p a, const c3mat3p b) +{ + return + c3vec3_equal(a->v[0], b->v[0]) && + c3vec3_equal(a->v[1], b->v[1]) && + c3vec3_equal(a->v[2], b->v[2]); +} + +/**************************************************************** + * * + * c3mat4 member functions * + * * + ****************************************************************/ + +// CONSTRUCTORS + +c3mat4 c3mat4_identity() +{ + return identity3D(); +} + +c3mat4 c3mat4_vec4(const c3vec4 v0, const c3vec4 v1, const c3vec4 v2, const c3vec4 v3) +{ + c3mat4 m = { .v[0] = v0, .v[1] = v1, .v[2] = v2, .v[3] = v3 }; + return m; +} + +c3mat4 c3mat4f( + c3f a00, c3f a01, c3f a02, c3f a03, + c3f a10, c3f a11, c3f a12, c3f a13, + c3f a20, c3f a21, c3f a22, c3f a23, + c3f a30, c3f a31, c3f a32, c3f a33 ) +{ + c3mat4 m; + m.v[0] = c3vec4f(a00, a01, a01, a03); + m.v[1] = c3vec4f(a10, a11, a11, a13); + m.v[2] = c3vec4f(a20, a21, a21, a23); + m.v[3] = c3vec4f(a30, a31, a21, a33); + return m; +} + +c3mat4p c3mat4p_add(c3mat4p a, const c3mat4p m) +{ + a->v[0] = c3vec4_add(a->v[0], m->v[0]); + a->v[1] = c3vec4_add(a->v[1], m->v[1]); + a->v[2] = c3vec4_add(a->v[2], m->v[2]); + a->v[3] = c3vec4_add(a->v[3], m->v[3]); + return a; +} + +c3mat4p c3mat4p_sub(c3mat4p a, const c3mat4p m) +{ + a->v[0] = c3vec4_sub(a->v[0], m->v[0]); + a->v[1] = c3vec4_sub(a->v[1], m->v[1]); + a->v[2] = c3vec4_sub(a->v[2], m->v[2]); + a->v[3] = c3vec4_sub(a->v[3], m->v[3]); + return a; +} + +c3mat4p c3mat4p_mulf(c3mat4p a, c3f d) +{ + a->v[0] = c3vec4_mulf(a->v[0], d); + a->v[1] = c3vec4_mulf(a->v[1], d); + a->v[2] = c3vec4_mulf(a->v[2], d); + a->v[3] = c3vec4_mulf(a->v[3], d); + return a; +} + +c3mat4p c3mat4p_divf(c3mat4p a, c3f d) +{ + a->v[0] = c3vec4_divf(a->v[0], d); + a->v[1] = c3vec4_divf(a->v[1], d); + a->v[2] = c3vec4_divf(a->v[2], d); + a->v[3] = c3vec4_divf(a->v[3], d); + return a; +} + +// SPECIAL FUNCTIONS; + +c3mat4 c3mat4_transpose(const c3mat4p a) +{ + return c3mat4_vec4( + c3vec4f(a->v[0].n[0], a->v[1].n[0], a->v[2].n[0], a->v[3].n[0]), + c3vec4f(a->v[0].n[1], a->v[1].n[1], a->v[2].n[1], a->v[3].n[1]), + c3vec4f(a->v[0].n[2], a->v[1].n[2], a->v[2].n[2], a->v[3].n[2]), + c3vec4f(a->v[0].n[3], a->v[1].n[3], a->v[2].n[3], a->v[3].n[3])); +} + +c3mat4 c3mat4_inverse(const c3mat4p m) // Gauss-Jordan elimination with partial pivoting +{ + c3mat4 a = *m; // As a evolves from original mat into identity + c3mat4 b = identity3D(); // b evolves from identity into inverse(a) + int i, j, i1; + + // Loop over cols of a from left to right, eliminating above and below diag + for (j = 0; j < 4; j++) { // Find largest pivot in column j among rows j..3 + i1 = j; // Row with largest pivot candidate + for (i = j + 1; i < 4; i++) + if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j])) + i1 = i; + + // Swap rows i1 and j in a and b to put pivot on diagonal + c3vec4 _s; + _s = a.v[i1]; a.v[i1] = a.v[j]; a.v[j] = _s; // swap(a.v[i1], a.v[j]); + _s = b.v[i1]; b.v[i1] = b.v[j]; b.v[j] = _s; // swap(b.v[i1], b.v[j]); + + // Scale row j to have a unit diagonal + if (a.v[j].n[j] == 0.) { + // VEC_ERROR("c3mat4::inverse: singular matrix; can't invert\n"); + return a; + } + b.v[j] = c3vec4_divf(b.v[j], a.v[j].n[j]); + a.v[j] = c3vec4_divf(a.v[j], a.v[j].n[j]); + + // Eliminate off-diagonal elems in col j of a, doing identical ops to b + for (i = 0; i < 4; i++) + if (i != j) { + b.v[i] = c3vec4_sub(b.v[i], c3vec4_mulf(b.v[j], a.v[i].n[j])); + a.v[i] = c3vec4_sub(a.v[i], c3vec4_mulf(a.v[j], a.v[i].n[j])); + } + } + + return b; +} + +c3mat4p c3mat4p_apply(c3mat4p a, V_FCT_PTR fct) +{ + a->v[0] = c3vec4_apply(a->v[0], fct); + a->v[1] = c3vec4_apply(a->v[1], fct); + a->v[2] = c3vec4_apply(a->v[2], fct); + a->v[3] = c3vec4_apply(a->v[3], fct); + return a; +} + +c3mat4p c3mat4p_swap_rows(c3mat4p a, int i, int j) +{ + c3vec4 t; + + t = a->v[i]; + a->v[i] = a->v[j]; + a->v[j] = t; + return a; +} + +c3mat4p c3mat4p_swap_cols(c3mat4p a, int i, int j) +{ + c3f t; + + for (int k = 0; k < 4; k++) { + t = a->v[k].n[i]; + a->v[k].n[i] = a->v[k].n[j]; + a->v[k].n[j] = t; + } + return a; +} + + +// FRIENDS + +c3mat4 c3mat4_minus(const c3mat4p a) +{ + return c3mat4_vec4( + c3vec4_minus(a->v[0]), + c3vec4_minus(a->v[1]), + c3vec4_minus(a->v[2]), + c3vec4_minus(a->v[3])); +} + +c3mat4 c3mat4_add(const c3mat4p a, const c3mat4p b) +{ + return c3mat4_vec4( + c3vec4_add(a->v[0], b->v[0]), + c3vec4_add(a->v[1], b->v[1]), + c3vec4_add(a->v[2], b->v[2]), + c3vec4_add(a->v[3], b->v[3])); +} + +c3mat4 c3mat4_sub(const c3mat4p a, const c3mat4p b) +{ + return c3mat4_vec4( + c3vec4_sub(a->v[0], b->v[0]), + c3vec4_sub(a->v[1], b->v[1]), + c3vec4_sub(a->v[2], b->v[2]), + c3vec4_sub(a->v[3], b->v[3])); +} + +c3mat4 c3mat4_mul(const c3mat4p a, const c3mat4p b) +{ + #define ROWCOL(i, j) \ + a->v[i].n[0]*b->v[0].n[j] + \ + a->v[i].n[1]*b->v[1].n[j] + \ + a->v[i].n[2]*b->v[2].n[j] + \ + a->v[i].n[3]*b->v[3].n[j] + + return c3mat4_vec4( + c3vec4f(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)), + c3vec4f(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)), + c3vec4f(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)), + c3vec4f(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3)) + ); + + #undef ROWCOL +} + +c3mat4 c3mat4_mulf(const c3mat4p a, c3f d) +{ + c3mat4 r = *a; + return *c3mat4p_mulf(&r, d); +} + +c3mat4 c3mat4_divf(const c3mat4p a, c3f d) +{ + c3mat4 r = *a; + return *c3mat4p_divf(&r, d); +} + +int c3mat4_equal(const c3mat4p a, const c3mat4p b) +{ + return !memcmp(a->n, b->n, sizeof(a->n)); +#if 0 + return + c3vec4_equal(a->v[0], b->v[0]) && + c3vec4_equal(a->v[1], b->v[1]) && + c3vec4_equal(a->v[2], b->v[2]) && + c3vec4_equal(a->v[3], b->v[3]); +#endif +} + +/**************************************************************** + * * + * 2D functions and 3D functions * + * * + ****************************************************************/ + +c3mat3 identity2D() +{ + return c3mat3_vec3( + c3vec3f(1.0, 0.0, 0.0), + c3vec3f(0.0, 1.0, 0.0), + c3vec3f(0.0, 0.0, 1.0)); +} + +c3mat3 translation2D(const c3vec2 v) +{ + return c3mat3_vec3( + c3vec3f(1.0, 0.0, v.n[VX]), + c3vec3f(0.0, 1.0, v.n[VY]), + c3vec3f(0.0, 0.0, 1.0)); +} + +c3mat3 rotation2D(const c3vec2 Center, c3f angleDeg) +{ + c3f angleRad = (c3f) (angleDeg * M_PI / 180.0); + c3f c = (c3f) cos(angleRad); + c3f s = (c3f) sin(angleRad); + + return c3mat3_vec3( + c3vec3f(c, -s, Center.n[VX] * (1.0f-c) + Center.n[VY] * s), + c3vec3f(s, c, Center.n[VY] * (1.0f-c) - Center.n[VX] * s), + c3vec3f(0.0, 0.0, 1.0)); +} + +c3mat3 scaling2D(const c3vec2 scaleVector) +{ + return c3mat3_vec3( + c3vec3f(scaleVector.n[VX], 0.0, 0.0), + c3vec3f(0.0, scaleVector.n[VY], 0.0), + c3vec3f(0.0, 0.0, 1.0)); +} + +c3mat4 identity3D() +{ + return c3mat4_vec4( + c3vec4f(1.0, 0.0, 0.0, 0.0), + c3vec4f(0.0, 1.0, 0.0, 0.0), + c3vec4f(0.0, 0.0, 1.0, 0.0), + c3vec4f(0.0, 0.0, 0.0, 1.0)); +} + +c3mat4 translation3D(const c3vec3 v) +{ + return c3mat4_vec4( + c3vec4f(1.0, 0.0, 0.0, v.n[VX]), + c3vec4f(0.0, 1.0, 0.0, v.n[VY]), + c3vec4f(0.0, 0.0, 1.0, v.n[VZ]), + c3vec4f(0.0, 0.0, 0.0, 1.0)); +} + +c3mat4 rotation3D(const c3vec3 Axis, c3f angleDeg) +{ + c3f angleRad = (c3f) (angleDeg * M_PI / 180.0); + c3f c = (c3f) cos(angleRad); + c3f s = (c3f) sin(angleRad); + c3f t = 1.0f - c; + + c3vec3 axis = c3vec3_normalize(Axis); + + return c3mat4_vec4( + c3vec4f(t * axis.n[VX] * axis.n[VX] + c, + t * axis.n[VX] * axis.n[VY] - s * axis.n[VZ], + t * axis.n[VX] * axis.n[VZ] + s * axis.n[VY], + 0.0), + c3vec4f(t * axis.n[VX] * axis.n[VY] + s * axis.n[VZ], + t * axis.n[VY] * axis.n[VY] + c, + t * axis.n[VY] * axis.n[VZ] - s * axis.n[VX], + 0.0), + c3vec4f(t * axis.n[VX] * axis.n[VZ] - s * axis.n[VY], + t * axis.n[VY] * axis.n[VZ] + s * axis.n[VX], + t * axis.n[VZ] * axis.n[VZ] + c, + 0.0), + c3vec4f(0.0, 0.0, 0.0, 1.0)); +} + +c3mat4 rotation3Drad(const c3vec3 Axis, c3f angleRad) +{ + c3f c = (c3f) cos(angleRad); + c3f s = (c3f) sin(angleRad); + c3f t = 1.0f - c; + + c3vec3 axis = c3vec3_normalize(Axis); + + return c3mat4_vec4( + c3vec4f(t * axis.n[VX] * axis.n[VX] + c, + t * axis.n[VX] * axis.n[VY] - s * axis.n[VZ], + t * axis.n[VX] * axis.n[VZ] + s * axis.n[VY], + 0.0), + c3vec4f(t * axis.n[VX] * axis.n[VY] + s * axis.n[VZ], + t * axis.n[VY] * axis.n[VY] + c, + t * axis.n[VY] * axis.n[VZ] - s * axis.n[VX], + 0.0), + c3vec4f(t * axis.n[VX] * axis.n[VZ] - s * axis.n[VY], + t * axis.n[VY] * axis.n[VZ] + s * axis.n[VX], + t * axis.n[VZ] * axis.n[VZ] + c, + 0.0), + c3vec4f(0.0, 0.0, 0.0, 1.0)); +} + +c3mat4 scaling3D(const c3vec3 scaleVector) +{ + return c3mat4_vec4( + c3vec4f(scaleVector.n[VX], 0.0, 0.0, 0.0), + c3vec4f(0.0, scaleVector.n[VY], 0.0, 0.0), + c3vec4f(0.0, 0.0, scaleVector.n[VZ], 0.0), + c3vec4f(0.0, 0.0, 0.0, 1.0)); +} + +c3mat4 perspective3D(c3f d) +{ + return c3mat4_vec4( + c3vec4f(1.0f, 0.0f, 0.0f, 0.0f), + c3vec4f(0.0f, 1.0f, 0.0f, 0.0f), + c3vec4f(0.0f, 0.0f, 1.0f, 0.0f), + c3vec4f(0.0f, 0.0f, 1.0f/d, 0.0f)); +} + diff --git a/examples/board_reprap/src/c3/c3algebra.h b/examples/board_reprap/src/c3/c3algebra.h new file mode 100644 index 0000000..d2b71a1 --- /dev/null +++ b/examples/board_reprap/src/c3/c3algebra.h @@ -0,0 +1,214 @@ +/* + c3algebra.h + + Copyright 2008-2012 Michel Pollet + + Derivative and inspiration from original C++: + Paul Rademacher & Jean-Francois DOUEG, + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3ALGEBRA_H___ +#define __C3ALGEBRA_H___ + + +#ifndef M_PI +#define M_PI 3.141592654 +#endif + +enum {VX, VY, VZ, VW}; // axes +enum {PA, PB, PC, PD}; // planes +enum {RED, GREEN, BLUE, ALPHA}; // colors +enum {KA, KD, KS, ES}; // phong coefficients + +typedef float c3f; +typedef c3f (*V_FCT_PTR)(c3f); + +typedef union c3vec2 { + struct { c3f x,y; }; + c3f n[2]; +} c3vec2; + +typedef union c3vec3 { + struct { c3f x,y,z; }; + c3f n[3]; +} c3vec3; + +typedef union c3vec4 { + struct { c3f x,y,z,w; }; + c3f n[4]; +} c3vec4, * c3vec4p; + +typedef union c3mat3 { + c3vec3 v[3]; + c3f n[3*3]; +} c3mat3, * c3mat3p; + +typedef union c3mat4 { + c3vec4 v[4]; + c3f n[4*4]; +} c3mat4, * c3mat4p; + +/* + * c3vec2 related + */ + +c3vec2 c3vec2_zero(); +c3vec2 c3vec2f(c3f x, c3f y); + +c3vec2 c3vec2_add(c3vec2 a, const c3vec2 v); +c3vec2 c3vec2_sub(c3vec2 a, const c3vec2 v); +c3vec2 c3vec2_mulf(c3vec2 a, c3f d); +c3vec2 c3vec2_divf(c3vec2 a, c3f d); + +c3f c3vec2_length2(const c3vec2 a); +c3f c3vec2_length(const c3vec2 a); +c3vec2 c3vec2_normalize(const c3vec2 a); // it is up to caller to avoid divide-by-zero +c3vec2 c3vec2_apply(c3vec2 a, V_FCT_PTR fct); +c3vec2 c3vec2_minus(const c3vec2 a); +c3f c3vec2_dot(const c3vec2 a, const c3vec2 b); +c3vec2 c3vec2_min(const c3vec2 a, const c3vec2 b); +c3vec2 c3vec2_max(const c3vec2 a, const c3vec2 b); +c3vec2 c3vec2_prod(const c3vec2 a, const c3vec2 b); + +/* + * c3vec4 related + */ + +c3vec3 c3vec3_zero(); +c3vec3 c3vec3f(c3f x, c3f y, c3f z); +c3vec3 c3vec3_vec2f(const c3vec2 v, c3f d); +c3vec3 c3vec3_vec2(const c3vec2 v); +c3vec3 c3vec3_vec4(const c3vec4 v); // it is up to caller to avoid divide-by-zero + +c3vec3 c3vec3_add(const c3vec3 a, const c3vec3 v); +c3vec3 c3vec3_sub(const c3vec3 a, const c3vec3 v); +c3vec3 c3vec3_mulf(const c3vec3 a, c3f d); +c3vec3 c3vec3_divf(const c3vec3 a, c3f d); + +c3f c3vec3_length2(const c3vec3 a); +c3f c3vec3_length(const c3vec3 a); +c3vec3 c3vec3_normalize(const c3vec3 a); // it is up to caller to avoid divide-by-zero +c3vec3 c3vec3_homogenize(c3vec3 a); // it is up to caller to avoid divide-by-zero +c3vec3 c3vec3_apply(c3vec3 a, V_FCT_PTR fct); +c3vec3 c3vec3_minus(const c3vec3 a); +c3f c3vec3_dot(const c3vec3 a, const c3vec3 b); +int c3vec3_equal(const c3vec3 a, const c3vec3 b); +c3vec3 c3vec3_min(const c3vec3 a, const c3vec3 b); +c3vec3 c3vec3_max(const c3vec3 a, const c3vec3 b); +c3vec3 c3vec3_prod(const c3vec3 a, const c3vec3 b); + +c3vec3 c3vec3_cross(const c3vec3 a, const c3vec3 b); +c3vec3 c3vec2_cross(const c3vec2 a, const c3vec2 b); + +/* + * c3vec4 related + */ + +c3vec4 c3vec4_zero(); +c3vec4 c3vec4f(c3f x, c3f y, c3f z, c3f w); +c3vec4 c3vec4_vec3(const c3vec3 v); +c3vec4 c3vec4_vec3f(const c3vec3 v, c3f d); + +c3vec4 c3vec4_add(c3vec4 a, const c3vec4 v); +c3vec4 c3vec4_sub(c3vec4 a, const c3vec4 v); +c3vec4 c3vec4_mulf(c3vec4 a, c3f d); +c3vec4 c3vec4_divf(c3vec4 a, c3f d); + +c3f c3vec4_length2(const c3vec4 a); +c3f c3vec4_length(const c3vec4 a); +c3vec4 c3vec4_normalize(c3vec4 a); // it is up to caller to avoid divide-by-zero +c3vec4 c3vec4_homogenize(c3vec4 a); // it is up to caller to avoid divide-by-zero +c3vec4 c3vec4_apply(c3vec4 a, V_FCT_PTR fct); +c3vec4 c3vec4_minus(const c3vec4 a); +int c3vec4_equal(const c3vec4 a, const c3vec4 b); +c3vec4 c3vec4_min(const c3vec4 a, const c3vec4 b); +c3vec4 c3vec4_max(const c3vec4 a, const c3vec4 b); +c3vec4 c3vec4_prod(const c3vec4 a, const c3vec4 b); + +/* + * c3mat3 related + */ + +c3mat3 c3mat3_identity(); +c3mat3 c3mat3_vec3(const c3vec3 v0, const c3vec3 v1, const c3vec3 v2); +c3mat3p c3mat3_add(const c3mat3p a, const c3mat3p m); +c3mat3p c3mat3_sub(const c3mat3p a, const c3mat3p m); +c3mat3p c3mat3_mulf(const c3mat3p a, c3f d); +c3mat3p c3mat3_divf(const c3mat3p a, c3f d); + +c3mat3 c3mat3_transpose(const c3mat3p a); +c3mat3 c3mat3_inverse(const c3mat3p m); // Gauss-Jordan elimination with partial pivoting +c3mat3p c3mat3_apply(c3mat3p a, V_FCT_PTR fct); +c3mat3 c3mat3_minus(const c3mat3p a); + +c3mat3 c3mat3_mul(const c3mat3p a, const c3mat3p b); +int c3mat3_equal(const c3mat3p a, const c3mat3p b); + +c3vec2 c3mat3_mulv2(const c3mat3p a, const c3vec2 v); +c3vec3 c3mat3_mulv3(const c3mat3p a, const c3vec3 v); +c3vec2 c3vec2_mulm3(const c3vec2 v, const c3mat3p a); +c3vec3 c3vec3_mulm3(const c3vec3 v, const c3mat3p a); + +c3mat3 identity2D(); +c3mat3 translation2D(const c3vec2 v); +c3mat3 rotation2D(const c3vec2 Center, c3f angleDeg); +c3mat3 scaling2D(const c3vec2 scaleVector); + +/* + * c3mat4 related + */ + +c3mat4 c3mat4_identity(); +c3mat4 c3mat4_vec4(const c3vec4 v0, const c3vec4 v1, const c3vec4 v2, const c3vec4 v3); +c3mat4 c3mat4f( + c3f a00, c3f a01, c3f a02, c3f a03, + c3f a10, c3f a11, c3f a12, c3f a13, + c3f a20, c3f a21, c3f a22, c3f a23, + c3f a30, c3f a31, c3f a32, c3f a33 ); + +c3mat4 c3mat4_minus(const c3mat4p a); +c3mat4p c3mat4p_add(c3mat4p a, const c3mat4p m); +c3mat4 c3mat4_add(const c3mat4p a, const c3mat4p b); +c3mat4p c3mat4p_sub(c3mat4p a, const c3mat4p m); +c3mat4 c3mat4_sub(const c3mat4p a, const c3mat4p b); +c3mat4p c3mat4p_mulf(c3mat4p a, c3f d); +c3mat4 c3mat4_mulf(const c3mat4p a, c3f d); +c3mat4 c3mat4_mul(const c3mat4p a, const c3mat4p b); +c3mat4p c3mat4p_divf(c3mat4p a, c3f d); +c3mat4 c3mat4_divf(const c3mat4p a, c3f d); + +c3mat4 c3mat4_transpose(const c3mat4p a); +c3mat4 c3mat4_inverse(const c3mat4p m); // Gauss-Jordan elimination with partial pivoting +c3mat4p c3mat4p_apply(c3mat4p a, V_FCT_PTR fct); +c3mat4p c3mat4p_swap_rows(c3mat4p a, int i, int j); +c3mat4p c3mat4p_swap_cols(c3mat4p a, int i, int j); +int c3mat4_equal(const c3mat4p a, const c3mat4p b); + +c3vec4 c3vec4_mulm4(const c3vec4 v, const c3mat4p a); +c3vec4 c3mat4_mulv4(const c3mat4p a, const c3vec4 v); +c3vec3 c3mat4_mulv3(const c3mat4p a, const c3vec3 v); + +c3mat4 identity3D(); +c3mat4 translation3D(const c3vec3 v); +c3mat4 rotation3D(const c3vec3 Axis, c3f angleDeg); +c3mat4 rotation3Drad(const c3vec3 Axis, c3f angleRad); +c3mat4 scaling3D(const c3vec3 scaleVector); +c3mat4 perspective3D(c3f d); + +#endif /* __C3ALGEBRA_H___ */ diff --git a/examples/board_reprap/src/c3/c3arcball.c b/examples/board_reprap/src/c3/c3arcball.c new file mode 100644 index 0000000..ff412d8 --- /dev/null +++ b/examples/board_reprap/src/c3/c3arcball.c @@ -0,0 +1,254 @@ +/* + c3arcball.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + +#include +#include "c3/c3arcball.h" + + +/**************************************** c3arcball_init_mat4() ****/ +/* Takes as argument a c3mat4 to use instead of the internal rot */ + +void +c3arcball_init_mat4( + c3arcballp a, + c3mat4p mtx ) +{ + c3arcball_init(a); + a->rot_ptr = mtx; +} + + +/**************************************** c3arcball_init_center() ****/ +/* A constructor that accepts the screen center and arcball radius*/ + +void +c3arcball_init_center( + c3arcballp a, + const c3vec2 center, + c3f radius ) +{ + c3arcball_init(a); + c3arcball_set_params(a, center, radius); +} + + +/************************************** c3arcball_set_params() ****/ + +void +c3arcball_set_params( + c3arcballp a, + const c3vec2 center, + c3f radius) +{ + a->center = center; + a->radius = radius; +} + +/*************************************** c3arcball_init() **********/ + +void +c3arcball_init( + c3arcballp a ) +{ + a->center = c3vec2f( 0.0, 0.0 ); + a->radius = 1.0; + a->q_now = c3quat_identity(); + a->rot_ptr = &a->rot; + a->rot = identity3D(); + a->q_increment = c3quat_identity(); + a->rot_increment = identity3D(); + a->is_mouse_down = false; + a->is_spinning = false; + a->damp_factor = 0.0; + a->zero_increment = true; +} + +/*********************************** c3arcball_mouse_to_sphere() ****/ + +c3vec3 +c3arcball_mouse_to_sphere( + c3arcballp a, + const c3vec2 p) +{ + c3f mag; + c3vec2 v2 = c3vec2_divf(c3vec2_sub(p, a->center), a->radius); + c3vec3 v3 = c3vec3f( v2.n[0], v2.n[1], 0.0 ); + + mag = c3vec2_dot(v2, v2); + + if ( mag > 1.0 ) + v3 = c3vec3_normalize(v3); + else + v3.n[VZ] = (c3f) sqrt( 1.0 - mag ); + + /* Now we add constraints - X takes precedence over Y */ + if ( a->constraint_x ) { + v3 = c3arcball_constrain_vector( v3, c3vec3f( 1.0, 0.0, 0.0 )); + } else if ( a->constraint_y ) { + v3 = c3arcball_constrain_vector( v3, c3vec3f( 0.0, 1.0, 0.0 )); + } + + return v3; +} + + +/************************************ c3arcball_constrain_vector() ****/ + +c3vec3 +c3arcball_constrain_vector( + const c3vec3 vector, + const c3vec3 axis) +{ +// return (vector - (vector * axis) * axis).normalize(); + return vector; +} + +/************************************ c3arcball_mouse_down() **********/ + +void +c3arcball_mouse_down( + c3arcballp a, + int x, + int y) +{ + a->down_pt = c3vec2f( (c3f)x, (c3f) y ); + a->is_mouse_down = true; + + a->q_increment = c3quat_identity(); + a->rot_increment = identity3D(); + a->zero_increment = true; +} + + +/************************************ c3arcball_mouse_up() **********/ + +void +c3arcball_mouse_up( + c3arcballp a) +{ + a->q_now = c3quat_mul(a->q_drag, a->q_now); + a->is_mouse_down = false; +} + + +/********************************** c3arcball_mouse_motion() **********/ + +void +c3arcball_mouse_motion( + c3arcballp a, + int x, + int y, + int shift, + int ctrl, + int alt) +{ + /* Set the X constraint if CONTROL key is pressed, Y if ALT key */ + c3arcball_set_constraints(a, ctrl != 0, alt != 0 ); + + c3vec2 new_pt = c3vec2f( (c3f)x, (c3f) y ); + c3vec3 v0 = c3arcball_mouse_to_sphere(a, a->down_pt ); + c3vec3 v1 = c3arcball_mouse_to_sphere(a, new_pt ); + + c3vec3 cross = c3vec3_cross(v0, v1); + + a->q_drag = c3quat_vec3(cross, c3vec3_dot(v0, v1)); + + // *rot_ptr = (q_drag * q_now).to_mat4(); + c3mat4 temp = c3quat_to_mat4(a->q_drag); + *a->rot_ptr = c3mat4_mul(a->rot_ptr, &temp); + + a->down_pt = new_pt; + + /* We keep a copy of the current incremental rotation (= q_drag) */ + a->q_increment = a->q_drag; + a->rot_increment = c3quat_to_mat4(a->q_increment); + + c3arcball_set_constraints(a, false, false); + + if (a->q_increment.s < .999999) { + a->is_spinning = true; + a->zero_increment = false; + } else { + a->is_spinning = false; + a->zero_increment = true; + } +} + + +/********************************** c3arcball_mouse_motion() **********/ +#if 0 +void +c3arcball_mouse_motion( + c3arcballp a, + int x, + int y) +{ + mouse_motion(x, y, 0, 0, 0); +} +#endif + +/***************************** c3arcball_set_constraints() **********/ + +void +c3arcball_set_constraints( + c3arcballp a, + bool _constraint_x, + bool _constraint_y) +{ + a->constraint_x = _constraint_x; + a->constraint_y = _constraint_y; +} + +/***************************** c3arcball_idle() *********************/ + +void +c3arcball_idle( + c3arcballp a) +{ + if (a->is_mouse_down) { + a->is_spinning = false; + a->zero_increment = true; + } + + if (a->damp_factor < 1.0f) + c3quat_scale_angle(&a->q_increment, 1.0f - a->damp_factor); + + a->rot_increment = c3quat_to_mat4(a->q_increment); + + if (a->q_increment.s >= .999999f) { + a->is_spinning = false; + a->zero_increment = true; + } +} + + +/************************ c3arcball_set_damping() *********************/ + +void +c3arcball_set_damping( + c3arcballp a, + c3f d) +{ + a->damp_factor = d; +} + + + diff --git a/examples/board_reprap/src/c3/c3arcball.h b/examples/board_reprap/src/c3/c3arcball.h new file mode 100644 index 0000000..2adba8b --- /dev/null +++ b/examples/board_reprap/src/c3/c3arcball.h @@ -0,0 +1,125 @@ +/* + c3arcball.h + + Copyright 2008-2012 Michel Pollet + Copyright (c) 1998 Paul Rademacher + Feb 1998, Paul Rademacher (rademach@cs.unc.edu) + Oct 2003, Nigel Stewart - GLUI Code Cleaning + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + +/* + Arcball, as described by Ken + Shoemake in Graphics Gems IV. + This class takes as input mouse events (mouse down, mouse drag, + mouse up), and creates the appropriate quaternions and 4x4 matrices + to represent the rotation given by the mouse. + + This class is used as follows: + - initialize [either in the constructor or with set_params()], the + center position (x,y) of the arcball on the screen, and the radius + - on mouse down, call mouse_down(x,y) with the mouse position + - as the mouse is dragged, repeatedly call mouse_motion() with the + current x and y positions. One can optionally pass in the current + state of the SHIFT, ALT, and CONTROL keys (passing zero if keys + are not pressed, non-zero otherwise), which constrains + the rotation to certain axes (X for CONTROL, Y for ALT). + - when the mouse button is released, call mouse_up() + + Axis constraints can also be explicitly set with the + set_constraints() function. + + The current rotation is stored in the 4x4 float matrix 'rot'. + It is also stored in the quaternion 'q_now'. + */ + +#ifndef __C3ARCBALL_H___ +#define __C3ARCBALL_H___ + +#include "c3/c3quaternion.h" + +typedef struct c3arcball { + int is_mouse_down : 1, /* true for down, false for up */ + is_spinning : 1, + constraint_x : 1, + constraint_y : 1, + zero_increment : 1; + c3quat q_now, q_down, q_drag, q_increment; + c3vec2 down_pt; + c3mat4 rot, rot_increment; + c3mat4 *rot_ptr; + + c3vec2 center; + c3f radius, damp_factor; +} c3arcball, *c3arcballp; + +void +c3arcball_init( + c3arcballp a ); +void +c3arcball_init_mat4( + c3arcballp a, + c3mat4p mtx ); +void +c3arcball_init_center( + c3arcballp a, + const c3vec2 center, + c3f radius ); +void +c3arcball_set_params( + c3arcballp a, + const c3vec2 center, + c3f radius); +c3vec3 +c3arcball_mouse_to_sphere( + c3arcballp a, + const c3vec2 p); +c3vec3 +c3arcball_constrain_vector( + const c3vec3 vector, + const c3vec3 axis); +void +c3arcball_mouse_down( + c3arcballp a, + int x, + int y); +void +c3arcball_mouse_up( + c3arcballp a); + +void +c3arcball_mouse_motion( + c3arcballp a, + int x, + int y, + int shift, + int ctrl, + int alt); +void +c3arcball_set_constraints( + c3arcballp a, + bool _constraint_x, + bool _constraint_y); +void +c3arcball_idle( + c3arcballp a); +void +c3arcball_set_damping( + c3arcballp a, + c3f d); + +#endif /* __C3ARCBALL_H___ */ diff --git a/examples/board_reprap/src/c3/c3cairo.c b/examples/board_reprap/src/c3/c3cairo.c new file mode 100644 index 0000000..6472b62 --- /dev/null +++ b/examples/board_reprap/src/c3/c3cairo.c @@ -0,0 +1,41 @@ +/* + c3cairo.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#include "c3/c3cairo.h" + +c3cairo_p +c3cairo_new( + c3object_p parent) +{ + c3cairo_p res = malloc(sizeof(*res)); + memset(res, 0, sizeof(*res)); + return c3cairo_init(res, parent); +} + +c3cairo_p +c3cairo_init( + c3cairo_p o, + c3object_p parent) +{ + c3object_init(&o->object, parent); + return o; +} diff --git a/examples/board_reprap/src/c3/c3cairo.h b/examples/board_reprap/src/c3/c3cairo.h new file mode 100644 index 0000000..cecae0e --- /dev/null +++ b/examples/board_reprap/src/c3/c3cairo.h @@ -0,0 +1,41 @@ +/* + c3cairo.h + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3CAIRO_H___ +#define __C3CAIRO_H___ + +#include "c3/c3object.h" + +typedef struct c3cairo_t { + c3object_t object; +} c3cairo_t, *c3cairo_p; + +c3cairo_p +c3cairo_new( + c3object_p parent); + +c3cairo_p +c3cairo_init( + c3cairo_p o, + c3object_p parent); + +#endif /* __C3CAIRO_H___ */ diff --git a/examples/board_reprap/src/c3/c3camera.c b/examples/board_reprap/src/c3/c3camera.c new file mode 100644 index 0000000..009672b --- /dev/null +++ b/examples/board_reprap/src/c3/c3camera.c @@ -0,0 +1,403 @@ +/* + c3camera.c + + Copyright 2008-2012 Michel Pollet + Copyright (c) 1998 Paul Rademacher + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#include "c3/c3camera.h" + + +void +c3cam_set_distance( + c3camp c, + const c3f new_distance) +{ + if ( new_distance <= 0.0 ) /* Distance has to be positive */ + return; + + /* We find the current forward vector */ +// forward = lookat - eye; + c->forward = c3vec3_normalize(c3vec3_sub(c->lookat, c->eye)); + + /* Set distance */ + c->distance = new_distance; + + /* Find new eye point */ + c->eye = c3vec3_sub(c->lookat, c3vec3_mulf(c->forward, c->distance)); + c3cam_update(c); +} + +void +c3cam_set_upv( + c3camp c, + const c3vec3 new_up) +{ + c->up = new_up; + c3cam_update(c); +} + +void +c3cam_set_upf( + c3camp c, + const c3f x, + const c3f y, + const c3f z) +{ + c3cam_set_upv(c, c3vec3f(x,y,z)); +} + +void +c3cam_set_eyev( + c3camp c, + const c3vec3 new_eye) +{ + c->eye = new_eye; + c3cam_update(c); +} + +void +c3cam_set_eyef( + c3camp c, + const c3f x, + const c3f y, + const c3f z) +{ + c3cam_set_eyev(c, c3vec3f(x,y,z)); +} + +void +c3cam_set_lookatv( + c3camp c, + const c3vec3 new_lookat) +{ + c->lookat = new_lookat; + c3cam_update(c); +} + +void +c3cam_set_lookatf( + c3camp c, + const c3f x, + const c3f y, + const c3f z) +{ + c3cam_set_lookatv(c, c3vec3f(x,y,z)); +} + + +void +c3cam_roll( + c3camp c, + const c3f angle) +{ + c3mat4 rot = rotation3D(c->forward, angle ); + c->up = c3mat4_mulv3(&rot, c->up); + c3cam_update(c); +} + +void +c3cam_eye_yaw( + c3camp c, + const c3f angle) +{ + c3vec3 eye_pt = c3vec3_sub(c->eye, c->lookat); /* eye w/lookat at center */ + c3mat4 rot = rotation3D( c->up, angle ); + + eye_pt = c3mat4_mulv3(&rot, eye_pt); + c->eye = c3vec3_add(c->lookat, eye_pt); + + c3cam_update(c); +} + +void +c3cam_eye_yaw_abs( + c3camp c, + const c3f angle, + const c3vec3 axis) +{ + c3vec3 eye_pt = c3vec3_sub(c->eye, c->lookat); /* eye w/lookat at center */ + c3mat4 rot = rotation3D( axis, angle ); + + eye_pt = c3mat4_mulv3(&rot, eye_pt); + c->eye = c3vec3_add(c->lookat, eye_pt); + + c->up = c3mat4_mulv3(&rot, c->up); + + c3cam_update(c); +} + + +void +c3cam_eye_pitch( + c3camp c, + const c3f angle) +{ + c3vec3 eye_pt = c3vec3_sub(c->eye, c->lookat); /* eye w/lookat at center */ + c3mat4 rot = rotation3D( c->side, angle ); + + eye_pt = c3mat4_mulv3(&rot, eye_pt); + c->eye = c3vec3_add(c->lookat, eye_pt); + + c->up = c3mat4_mulv3(&rot, c->up); + + c3cam_update(c); +} + +void +c3cam_lookat_yaw( + c3camp c, + const c3f angle) +{ + c3vec3 lookat_pt = c3vec3_sub(c->lookat, c->eye); /* lookat w/eye at center */ + c3mat4 rot = rotation3D( c->up, -angle ); + + lookat_pt = c3mat4_mulv3(&rot, lookat_pt); + c->lookat = c3vec3_add(c->eye, lookat_pt); + + c3cam_update(c); +} + +void +c3cam_lookat_pitch( + c3camp c, + const c3f angle) +{ + c3vec3 lookat_pt = c3vec3_sub(c->lookat, c->eye); /* lookat w/eye at center */ + c3mat4 rot = rotation3D( c->side, -angle ); + + lookat_pt = c3mat4_mulv3(&rot, lookat_pt); + c->lookat = c3vec3_add(c->eye, lookat_pt); + + c->up = c3mat4_mulv3(&rot, c->up); + + c3cam_update(c); +} + +void +c3cam_reset_up_axis( + c3camp c, + const int axis_num) +{ + c3vec3 eye_pt = c3vec3_sub(c->lookat, c->eye); /* eye w/lookat at center */ + c3f eye_distance = c3vec3_length(eye_pt); + c->eye.n[axis_num] = c->lookat.n[axis_num]; + /* Bring eye to same level as lookat */ + + c3vec3 vector = c3vec3_sub(c->eye, c->lookat); + vector = c3vec3_normalize(vector); + vector = c3vec3_mulf(vector, eye_distance); + + c->eye = c3vec3_add(c->lookat, vector); + c->up = c3vec3f( 0.0, 0.0, 0.0 ); + c->up.n[axis_num] = 1.0; + + c3cam_update(c); +} + +void +c3cam_reset_up( + c3camp c) +{ + c3cam_reset_up_axis(c, VY ); /* Resets to the Y axis */ +} + +void +c3cam_movef( + c3camp c, + const c3f side_move, + const c3f up_move, + const c3f forw_move) +{ + c->eye = c3vec3_add(c->eye, c3vec3_mulf(c->forward, forw_move)); + c->eye = c3vec3_add(c->eye, c3vec3_mulf(c->side, side_move)); + c->eye = c3vec3_add(c->eye, c3vec3_mulf(c->up, up_move)); + c->lookat = c3vec3_add(c->lookat, c3vec3_mulf(c->forward, forw_move)); + c->lookat = c3vec3_add(c->lookat, c3vec3_mulf(c->side, side_move)); + c->lookat = c3vec3_add(c->lookat, c3vec3_mulf(c->up, up_move)); + c3cam_update(c); +} + +void +c3cam_movev( + c3camp c, + const c3vec3 v) /* A vector version of the above command */ +{ + c3cam_movef(c, v.n[VX], v.n[VY], v.n[VZ] ); +} + +void +c3cam_move_by_eye( + c3camp c, + const c3vec3 new_eye) +{ + c3vec3 diff = c3vec3_sub(new_eye, c->eye); + + c->lookat = c3vec3_add(c->lookat, diff); + c->eye = c3vec3_add(c->eye, diff); + + c3cam_update(c); +} + +void +c3cam_move_by_lookat( + c3camp c, + const c3vec3 new_lookat) +{ + c3vec3 diff = c3vec3_sub(new_lookat, c->lookat); + + c->lookat = c3vec3_add(c->lookat, diff); + c->eye = c3vec3_add(c->eye, diff); + + c3cam_update(c); +} + +void +c3cam_move_abs( + c3camp c, + const c3vec3 v) +{ + c->lookat = c3vec3_add(c->lookat, v); + c->eye = c3vec3_add(c->eye, v); + + c3cam_update(c); +} + +void +c3cam_rot_about_eye( + c3camp c, + const c3mat4p rot) +{ + c3vec3 view = c3vec3_sub(c->lookat, c->eye); + + view = c3mat4_mulv3(rot, view); + c->up = c3mat4_mulv3(rot, c->up); + + c->lookat = c3vec3_add(c->eye, view); + + c3cam_update(c); +} + +void +c3cam_rot_about_lookat( + c3camp c, + const c3mat4p rot) +{ + // NOT QUITE RIGHT YET + + c3vec3 view = c3vec3_sub(c->eye, c->lookat); + + view = c3mat4_mulv3(rot, view); + c->up = c3mat4_mulv3(rot, c->up); + + c->eye = c3vec3_add(c->lookat, view); + + c3cam_update(c); +} + +void +c3cam_update_matrix( + c3camp c) +{ + c3cam_update(c); + + c->mtx = c3mat4_vec4( + c3vec4f(c->side.n[VX], c->up.n[VX], c->forward.n[VX], 0.0), + c3vec4f(c->side.n[VY], c->up.n[VY], c->forward.n[VY], 0.0), + c3vec4f(c->side.n[VZ], c->up.n[VZ], c->forward.n[VZ], 0.0), + c3vec4f(0.0, 0.0, 0.0, 1.0)); +} +#if 0 +void +c3cam_load_to_openGL(c3camp c) +{ + c3mat4 m; + + make_mtx(); + + glMatrixMode( GL_MODELVIEW ); + glLoadIdentity(); + glMultMatrixf( (c3f*) &mtx[0][0]); + glTranslatef( -eye[VX], -eye[VY], -eye[VZ] ); +} + +void +c3cam_load_to_openGL_noident(c3camp c) +{ + c3mat4 m; + + make_mtx(); + + glMatrixMode( GL_MODELVIEW ); + glMultMatrixf( (c3f*) &mtx[0][0]); + glTranslatef( -eye[VX], -eye[VY], -eye[VZ] ); +} +#endif + +void +c3cam_reset( + c3camp c) +{ + c->up = c3vec3f( 0.0, 1.0, 0.0 ); + c->eye = c3vec3f(0.0, 0.0, 10.0); + c->lookat = c3vec3f(0.0,0.0,0.0); + + c->mtx = identity3D(); + + c3cam_update(c); +} + +c3cam +c3cam_new() +{ + c3cam c; + c3cam_reset(&c); + return c; +} + +void +c3cam_update( + c3camp c) +{ + /* get proper side and forward vectors, and distance */ + c->forward = c3vec3_minus(c3vec3_sub(c->lookat, c->eye)); + c->distance = c3vec3_length(c->forward); + c->forward = c3vec3_divf(c->forward, c->distance); + + c->side = c3vec3_cross(c->up, c->forward); + c->up = c3vec3_cross(c->forward, c->side); + + c->forward = c3vec3_normalize(c->forward); + c->up = c3vec3_normalize(c->up); + c->side = c3vec3_normalize(c->side); +} + +# if 0 +void +c3cam_dump(c3camp c, FILE *output) const +{ + fprintf( output, "Viewmodel: \n" ); + eye.print( output, " eye" ); + lookat.print( output, " lookat" ); + up.print( output, " up" ); + side.print( output, " side" ); + forward.print(output, " forward"); + mtx.print( output, " mtx" ); +} +#endif diff --git a/examples/board_reprap/src/c3/c3camera.h b/examples/board_reprap/src/c3/c3camera.h new file mode 100644 index 0000000..cc27c14 --- /dev/null +++ b/examples/board_reprap/src/c3/c3camera.h @@ -0,0 +1,236 @@ +/* + c3camera.h + + Copyright 2008-2012 Michel Pollet + Copyright (c) 1998 Paul Rademacher + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + +#ifndef __C3VIEW_H___ +#define __C3VIEW_H___ + +#include "c3/c3algebra.h" + +typedef struct c3cam { + c3vec3 eye, lookat; + c3vec3 up, side, forward; + c3mat4 mtx; + c3f distance; +} c3cam, *c3camp; + +/******************************* set_distance() ***********/ +/* This readjusts the distance from the eye to the lookat */ +/* (changing the eye point in the process) */ +/* The lookat point is unaffected */ +void +c3cam_set_distance( + c3camp c, + const c3f new_distance); + +/******************************* set_up() ***************/ +void +c3cam_set_upv( + c3camp c, + const c3vec3 new_up); +void +c3cam_set_upf( + c3camp c, + const c3f x, + const c3f y, + const c3f z); + +/******************************* set_eye() ***************/ +void +c3cam_set_eyev( + c3camp c, + const c3vec3 new_eye); +void +c3cam_set_eyef( + c3camp c, + const c3f x, + const c3f y, + const c3f z); + +/******************************* set_lookat() ***************/ +void +c3cam_set_lookatv( + c3camp c, + const c3vec3 new_lookat); +void +c3cam_set_lookatf( + c3camp c, + const c3f x, + const c3f y, + const c3f z); + +/******************************* roll() *****************/ +/* Rotates about the forward vector */ +/* eye and lookat remain unchanged */ +void +c3cam_roll( + c3camp c, + const c3f angle); + +/******************************* eye_yaw() *********************/ +/* Rotates the eye about the lookat point, using the up vector */ +/* Lookat is unaffected */ +void +c3cam_eye_yaw( + c3camp c, + const c3f angle); + +/******************************* eye_yaw_abs() ******************/ +/* Rotates the eye about the lookat point, with a specific axis */ +/* Lookat is unaffected */ +void +c3cam_eye_yaw_abs( + c3camp c, + const c3f angle, + const c3vec3 axis); + +/******************************* eye_pitch() ************/ +/* Rotates the eye about the side vector */ +/* Lookat is unaffected */ +void +c3cam_eye_pitch( + c3camp c, + const c3f angle); + +/******************************* lookat_yaw()************/ +/* This assumes the up vector is correct. */ +/* Rotates the lookat about the side vector */ +/* Eye point is unaffected */ +void +c3cam_lookat_yaw( + c3camp c, + const c3f angle); + +/******************************* lookat_pitch() *********/ +/* Rotates the lookat point about the side vector */ +/* This assumes the side vector is correct. */ +/* Eye point is unaffected */ +void +c3cam_lookat_pitch( + c3camp c, + const c3f angle); + +/******************************* reset_up() ******************/ +/* Resets the up vector to a specified axis (0=X, 1=Y, 2=Z) */ +/* Also sets the eye point level with the lookat point, */ +/* along the specified axis */ +void +c3cam_reset_up_axis( + c3camp c, + const int axis_num); +void +c3cam_reset_up( + c3camp c); + +/******************************* move() ********************/ +/* Moves a specified distance in the forward, side, and up */ +/* directions. This function does NOT move by world */ +/* coordinates. To move by world coords, use the move_abs */ +/* function. */ +void +c3cam_movef( + c3camp c, + const c3f side_move, + const c3f up_move, + const c3f forw_move); +void +c3cam_movev( + c3camp c, + const c3vec3 v); /* A vector version of the above command */ + +/******************************* move_by_eye() ***********/ +/* Sets the eye point, AND moves the lookat point by the */ +/* same amount as the eye is moved. */ +void +c3cam_move_by_eye( + c3camp c, + const c3vec3 new_eye); + +/******************************* move_by_lookat() *********/ +/* Sets the lookat point, AND moves the eye point by the */ +/* same amount as the lookat is moved. */ +void +c3cam_move_by_lookat( + c3camp c, + const c3vec3 new_lookat); + +/******************************* move_abs() *****************/ +/* Move the eye and lookat in world coordinates */ +void +c3cam_move_abs( + c3camp c, + const c3vec3 v); + +/****************************** rot_about_eye() ************/ +/* Rotates the lookat point about the eye, based on a 4x4 */ +/* (pure) rotation matrix */ +void +c3cam_rot_about_eye( + c3camp c, + const c3mat4p rot); + +/****************************** rot_about_lookat() ************/ +/* Rotates the lookat point about the lookat, based on a 4x4 */ +/* (pure) rotation matrix */ +void +c3cam_rot_about_lookat( + c3camp c, + const c3mat4p rot); + +/******************************* make_mtx() *************/ +/* Constructs a 4x4 matrix - used by load_to_openGL() */ +void +c3cam_update_matrix( + c3camp c); + +/******************************* load_to_openGL() ********/ +/* Sets the OpenGL modelview matrix based on the current */ +/* camera coordinates */ +//void c3cam_load_to_openGL(); + +/******************************* load_to_openGL_noident() ******/ +/* Multiplies the current camera matrix by the existing openGL */ +/* modelview matrix. This is same as above function, but */ +/* does not set the OpenGL matrix to identity first */ +//void c3cam_load_to_openGL_noident(); + +/******************************* reset() ****************/ +/* Resets the parameters of this class */ +void +c3cam_reset( + c3camp c); + +/******************************* ViewModel() ************/ +/* Constructor */ +c3cam c3cam_new(); + +/******************************* update() ****************/ +/* updates the view params. Call this after making */ +/* direct changes to the vectors or points of this class */ +void c3cam_update( + c3camp c); + +/******************************* dump() *******************/ +/* Prints the contents of this class to a file, typically */ +/* stdin or stderr */ +//void c3cam_dump(FILE *output); + +#endif /* __C3VIEW_H___ */ diff --git a/examples/board_reprap/src/c3/c3geometry.c b/examples/board_reprap/src/c3/c3geometry.c new file mode 100644 index 0000000..c81df63 --- /dev/null +++ b/examples/board_reprap/src/c3/c3geometry.c @@ -0,0 +1,61 @@ +/* + c3geometry.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#include "c3/c3object.h" + + +c3geometry_p +c3geometry_new( + int type, + c3object_p o /* = NULL */) +{ + c3geometry_p res = malloc(sizeof(c3geometry_t)); + memset(res, 0, sizeof(*res)); + res->type = type; + res->dirty = 1; + c3object_add_geometry(o, res); + return res; +} + +void +c3geometry_dispose( + c3geometry_p g) +{ + /* + * If we're still attached to an object, detach + */ + if (g->object) { + for (int oi = 0; oi < g->object->geometry.count; oi++) + if (g->object->geometry.e[oi] == g) { + c3geometry_array_delete(&g->object->geometry, oi, 1); + c3object_set_dirty(g->object, true); + break; + } + g->object = NULL; + } + str_free(g->name); + c3vertex_array_free(&g->vertice); + c3vertex_array_free(&g->projected); + c3tex_array_free(&g->textures); + c3colorf_array_free(&g->colorf); + free(g); +} diff --git a/examples/board_reprap/src/c3/c3geometry.h b/examples/board_reprap/src/c3/c3geometry.h new file mode 100644 index 0000000..e3a6d6e --- /dev/null +++ b/examples/board_reprap/src/c3/c3geometry.h @@ -0,0 +1,72 @@ +/* + c3geometry.h + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3GEOMETRY_H___ +#define __C3GEOMETRY_H___ + +#include "c3/c3algebra.h" +#include "c_utils.h" + +typedef c3vec3 c3vertex; +typedef c3vec4 c3colorf; +typedef c3vec2 c3tex; + +struct c3object_t; + +DECLARE_C_ARRAY(c3vertex, c3vertex_array, 16); +DECLARE_C_ARRAY(c3tex, c3tex_array, 16); +DECLARE_C_ARRAY(c3colorf, c3colorf_array, 16); + +typedef struct c3material_t { + c3colorf color; +} c3material_t; + +typedef struct c3geometry_t { + int type; // GL_LINES etc + int dirty : 1; + str_p name; + c3material_t mat; + struct c3object_t * object; + c3vertex_array_t vertice; + c3tex_array_t textures; + c3colorf_array_t colorf; + + // projected version of the vertice + c3vertex_array_t projected; +} c3geometry_t, *c3geometry_p; + +DECLARE_C_ARRAY(c3geometry_p, c3geometry_array, 4); + +c3geometry_p +c3geometry_new( + int type, + struct c3object_t * o /* = NULL */); +void +c3geometry_dispose( + c3geometry_p g); + +IMPLEMENT_C_ARRAY(c3geometry_array); +IMPLEMENT_C_ARRAY(c3vertex_array); +IMPLEMENT_C_ARRAY(c3tex_array); +IMPLEMENT_C_ARRAY(c3colorf_array); + +#endif /* __C3GEOMETRY_H___ */ diff --git a/examples/board_reprap/src/c3/c3object.c b/examples/board_reprap/src/c3/c3object.c new file mode 100644 index 0000000..0b1d31c --- /dev/null +++ b/examples/board_reprap/src/c3/c3object.c @@ -0,0 +1,241 @@ +/* + c3object.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#include "c3/c3object.h" + +void +_c3object_clear( + c3object_driver_p d) +{ + c3object_p o = d->object; + for (int oi = 0; oi < o->transform.count; oi++) { + o->transform.e[oi]->object = NULL; + c3transform_dispose(o->transform.e[oi]); + } + for (int oi = 0; oi < o->geometry.count; oi++) { + o->geometry.e[oi]->object = NULL; // don't try to detach + c3geometry_dispose(o->geometry.e[oi]); + } + for (int oi = 0; oi < o->objects.count; oi++) { + o->objects.e[oi]->parent = NULL; // don't try to detach + c3object_dispose(o->objects.e[oi]); + } + c3object_array_free(&o->objects); + c3geometry_array_free(&o->geometry); + c3transform_array_free(&o->transform); +} + +void +_c3object_dispose( + c3object_driver_p d) +{ + c3object_p o = d->object; + if (o->parent) { + for (int oi = 0; oi < o->parent->objects.count; oi++) + if (o->parent->objects.e[oi] == o) { + c3object_array_delete(&o->parent->objects, oi, 1); + c3object_set_dirty(o->parent, true); + break; + } + o->parent = NULL; + } + //C3O_DRIVER_INHERITED(dispose, d); + /* + * free the driver chain + */ + c3object_driver_p driver = o->driver; + while (driver) { + c3object_driver_p n = driver->next; + free(n); + driver = n; + } + free(o); +} + +void +_c3object_get_geometry( + c3object_driver_p d, + c3geometry_array_p out) +{ + c3object_p o = d->object; + for (int oi = 0; oi < o->geometry.count; oi++) + c3geometry_array_add(out, o->geometry.e[oi]); + for (int oi = 0; oi < o->objects.count; oi++) + c3object_get_geometry(o->objects.e[oi], out); +} + +void +_c3object_project( + c3object_driver_p d, + c3mat4p m) +{ + c3object_p o = d->object; + if (!o->dirty) + return; + + c3mat4 identity = identity3D(); + c3mat4 p = *m; + for (int pi = 0; pi < o->transform.count; pi++) + p = c3mat4_mul(&p, &o->transform.e[pi]->matrix); + bool is_identity = c3mat4_equal(m, &identity); + + for (int gi = 0; gi < o->geometry.count; gi++) { + c3geometry_p g = o->geometry.e[gi]; + c3vertex_array_clear(&g->projected); + if (1) { + c3vertex_array_realloc(&g->projected, g->vertice.count); + g->projected.count = g->vertice.count; + for (int vi = 0; vi < g->vertice.count; vi++) + g->projected.e[vi] = c3mat4_mulv3(&p, g->vertice.e[vi]); + } + } + for (int oi = 0; oi < o->objects.count; oi++) + c3object_project(o->objects.e[oi], &p); + o->dirty = false; +} + +const c3object_driver_t c3object_base_driver = { + .clear = _c3object_clear, + .dispose = _c3object_dispose, + .get_geometry = _c3object_get_geometry, + .project = _c3object_project, +}; + +c3object_p +c3object_init( + c3object_p o /* = NULL */, + c3object_p parent) +{ + memset(o, 0, sizeof(*o)); + o->parent = parent; + o->driver = malloc(sizeof(c3object_driver_t)); + *o->driver = c3object_base_driver; + o->driver->object = o; + if (parent) + c3object_array_add(&parent->objects, o); + return o; +} + +c3object_p +c3object_new( + c3object_p o /* = NULL */) +{ + c3object_p res = malloc(sizeof(*o)); + return c3object_init(res, o); +} + +void +c3object_clear( + c3object_p o) +{ + C3O_DRIVER(o, clear); +} + +void +c3object_dispose( + c3object_p o) +{ + c3object_clear(o); + C3O_DRIVER(o, dispose); +} + +void +c3object_set_dirty( + c3object_p o, + bool dirty) +{ + if (dirty) { + while (o) { + o->dirty = true; + o = o->parent; + } + } else { + for (int oi = 0; oi < o->objects.count; oi++) + if (o->objects.e[oi]->dirty) + c3object_set_dirty(o->objects.e[oi], false); + o->dirty = false; + } +} + +void +c3object_add_object( + c3object_p o, + c3object_p sub) +{ + if (sub->parent == o) + return; + if (sub->parent) { + for (int oi = 0; oi < sub->parent->objects.count; oi++) { + if (sub->parent->objects.e[oi] == sub) { + c3object_array_delete(&sub->parent->objects, oi, 1); + c3object_set_dirty(sub->parent, true); + break; + } + } + sub->parent = NULL; + } + sub->parent = o; + if (o) { + c3object_array_add(&o->objects, sub); + c3object_set_dirty(o, true); + } +} + +void +c3object_add_geometry( + c3object_p o, + c3geometry_p g) +{ + if (g->object == o) + return; + if (g->object) { + for (int oi = 0; oi < g->object->geometry.count; oi++) { + if (g->object->geometry.e[oi] == g) { + c3geometry_array_delete(&g->object->geometry, oi, 1); + c3object_set_dirty(g->object, true); + break; + } + } + g->object = NULL; + } + g->object = o; + if (o) { + c3geometry_array_add(&o->geometry, g); + c3object_set_dirty(o, true); + } +} + +void +c3object_get_geometry( + c3object_p o, + c3geometry_array_p array ) +{ + C3O_DRIVER(o, get_geometry, array); +} + +void +c3object_project( + c3object_p o, + const c3mat4p m) +{ + C3O_DRIVER(o, project, m); +} diff --git a/examples/board_reprap/src/c3/c3object.h b/examples/board_reprap/src/c3/c3object.h new file mode 100644 index 0000000..d221393 --- /dev/null +++ b/examples/board_reprap/src/c3/c3object.h @@ -0,0 +1,85 @@ +/* + c3object.h + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3OBJECT_H___ +#define __C3OBJECT_H___ + +#include +#include "c3/c3transform.h" +#include "c3/c3geometry.h" +#include "c3/c3object_driver.h" + +struct c3object_t; + +DECLARE_C_ARRAY(struct c3object_t*, c3object_array, 4); + +typedef struct c3object_t { + str_p name; + int dirty : 1; + struct c3object_t * parent; + c3object_driver_p driver; + c3transform_array_t transform; + c3object_array_t objects; + c3geometry_array_t geometry; +} c3object_t, *c3object_p; + +c3object_p +c3object_new( + c3object_p o /* = NULL */); +void +c3object_dispose( + c3object_p o); +void +c3object_clear( + c3object_p o); + +c3object_p +c3object_init( + c3object_p o /* = NULL */, + c3object_p parent); +void +c3object_set_dirty( + c3object_p o, + bool dirty); +void +c3object_add_geometry( + c3object_p o, + c3geometry_p g); +void +c3object_add_object( + c3object_p o, + c3object_p sub); +c3transform_p +c3object_add_transform( + c3object_p o ); +void +c3object_get_geometry( + c3object_p o, + c3geometry_array_p array ); +void +c3object_project( + c3object_p o, + const c3mat4p m); + +IMPLEMENT_C_ARRAY(c3object_array); + +#endif /* __C3OBJECT_H___ */ diff --git a/examples/board_reprap/src/c3/c3object_driver.h b/examples/board_reprap/src/c3/c3object_driver.h new file mode 100644 index 0000000..056ea22 --- /dev/null +++ b/examples/board_reprap/src/c3/c3object_driver.h @@ -0,0 +1,73 @@ +/* + c3object_driver.h + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3OBJECT_DRIVER_H___ +#define __C3OBJECT_DRIVER_H___ + +#include "c3/c3geometry.h" +struct c3object_t; +struct c3geometry_array_t; +union c3mat4; + +typedef struct c3object_driver_t { + struct c3object_driver_t * next; + struct c3object_t * object; + /* + * Delete any object related to this object, geometry etc + * The object will still exist, just empty + */ + void (*clear)(struct c3object_driver_t * d); + /* + * Dispose of the remaining memory for an object, detaches it + * and frees remaining traces of it + */ + void (*dispose)(struct c3object_driver_t * d); + /* + * Adds sub objects geometry and self geometry to array 'out' + */ + void (*get_geometry)(struct c3object_driver_t * d, + struct c3geometry_array_t * out); + /* + * Reproject geometry along matrix 'mat', applies our own + * transform and call down the chain for sub-objects + */ + void (*project)(struct c3object_driver_t * d, + union c3mat4 * mat); +} c3object_driver_t, *c3object_driver_p; + + +#define C3O_DRIVER_CALL(__callback, __driver, __args...) { \ + typeof(__driver) __d = __driver;\ + while (__d) {\ + if (__d->__callback) {\ + __d->__callback(__d, ##__args);\ + break;\ + }\ + __d = __d->next;\ + }\ + } +#define C3O_DRIVER(__o, __callback, __args...) \ + C3O_DRIVER_CALL(__callback, __o->driver, ##__args) +#define C3O_DRIVER_INHERITED(__callback, __driver, __args...) \ + C3O_DRIVER_CALL(__callback, __driver->next, ##__args) + +#endif /* __C3OBJECT_DRIVER_H___ */ diff --git a/examples/board_reprap/src/c3/c3quaternion.c b/examples/board_reprap/src/c3/c3quaternion.c new file mode 100644 index 0000000..a0816bf --- /dev/null +++ b/examples/board_reprap/src/c3/c3quaternion.c @@ -0,0 +1,249 @@ +/* + c3quaternion.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + +#include +#include "c3/c3quaternion.h" + +#ifndef DEG2RAD +#define DEG2RAD(x) ((x)/180.0*M_PI) +#define RAD2DEG(x) ((x)/M_PI*180.0) +#endif +#ifndef FUDGE +#define FUDGE .00001 +#endif + +c3quat +c3quat_new() +{ + return c3quat_identity(); +} + +/************************************************* c3quat_identity() *****/ +/* Returns quaternion identity element */ + +c3quat +c3quat_identity() +{ + return c3quat_vec3( c3vec3f( 0.0, 0.0, 0.0 ), 1.0 ); +} + +c3quat +c3quatf( + const c3f x, + const c3f y, + const c3f z, + const c3f w) +{ + c3quat q = { .v = c3vec3f(x,y,z), .s = w }; + return q; +} + +c3quat +c3quat_vec3( + const c3vec3 v, + const c3f s) +{ + c3quat q = { .v = v, .s = s }; + return q; +} + +c3quat +c3quat_vec4( + const c3vec4 v) +{ + c3quat q = { .v = c3vec3f(v.n[0], v.n[1], v.n[2]), .s = v.n[3] }; + return q; +} + +c3quat +c3quat_double( + const double *d) +{ + c3quat q; + q.v.n[0] = (c3f) d[0]; + q.v.n[1] = (c3f) d[1]; + q.v.n[2] = (c3f) d[2]; + q.s = (c3f) d[3]; + return q; +} + + +c3quat +c3quat_add( + const c3quat a, + const c3quat b) +{ + return c3quat_vec3(c3vec3_add(a.v, b.v), a.s + b.s ); +} + +c3quat +c3quat_sub( + const c3quat a, + const c3quat b) +{ + return c3quat_vec3(c3vec3_sub(a.v, b.v), a.s - b.s ); +} + +c3quat +c3quat_minus( + const c3quat a ) +{ + return c3quat_vec3(c3vec3_minus(a.v), -a.s); +} + +c3quat +c3quat_mul( + const c3quat a, + const c3quat b) +{ +// return c3quat( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v ); + return c3quat_vec3( + c3vec3_add(c3vec3_mulf(b.v, a.s), c3vec3_add(c3vec3_mulf(a.v, b.s), c3vec3_cross(a.v, b.v))), + (a.s * b.s) - c3vec3_dot(a.v, b.v)); +} + +c3quat +c3quat_mulf( const c3quat a, const c3f t) +{ + return c3quat_vec3(c3vec3_mulf(a.v, t), a.s * t ); +} + +c3mat4 +c3quat_to_mat4( + const c3quat a ) +{ + c3f xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; + + c3f t = 2.0f / (c3vec3_dot(a.v, a.v) + (a.s * a.s)); + + xs = a.v.n[VX]*t; ys = a.v.n[VY]*t; zs = a.v.n[VZ]*t; + wx = a.s*xs; wy = a.s*ys; wz = a.s*zs; + xx = a.v.n[VX]*xs; xy = a.v.n[VX]*ys; xz = a.v.n[VX]*zs; + yy = a.v.n[VY]*ys; yz = a.v.n[VY]*zs; zz = a.v.n[VZ]*zs; + + c3mat4 m = c3mat4_vec4( + c3vec4f(1.0f-(yy+zz), xy+wz, xz-wy, 0.0f), + c3vec4f(xy-wz, 1.0f-(xx+zz), yz+wx, 0.0f), + c3vec4f(xz+wy, yz-wx, 1.0f-(xx+yy), 0.0f), + c3vec4f(0.0f, 0.0f, 0.0f, 1.0f )); + + return m; +} + + +/************************************************ quat_slerp() ********/ +/* Quaternion spherical interpolation */ + +c3quat +quat_slerp( + const c3quat from, + const c3quat to, + c3f t) +{ + c3quat to1; + c3f omega, cosom, sinom, scale0, scale1; + + /* calculate cosine */ + cosom = c3vec3_dot(from.v, to.v) + from.s + to.s; + + /* Adjust signs (if necessary) */ + if (cosom < 0.0) { + cosom = -cosom; + to1 = c3quat_minus(to); + } else { + to1 = to; + } + + /* Calculate coefficients */ + if ((1.0 - cosom) > FUDGE ) { + /* standard case (slerp) */ + omega = (c3f) acos( cosom ); + sinom = (c3f) sin( omega ); + scale0 = (c3f) sin((1.0 - t) * omega) / sinom; + scale1 = (c3f) sin(t * omega) / sinom; + } else { + /* 'from' and 'to' are very close - just do linear interpolation */ + scale0 = 1.0f - t; + scale1 = t; + } + + return c3quat_add(c3quat_mulf(from, scale0), c3quat_mulf(to1, scale1)); +} + +/********************************************** set_angle() ************/ +/* set rot angle (degrees) */ + +c3quatp +c3quat_set_angle( + c3quatp a, + c3f f) +{ + c3vec3 axis = c3quat_get_axis(a); + + a->s = (c3f) cos( DEG2RAD( f ) / 2.0 ); + + a->v = c3vec3_mulf(axis, (c3f) sin(DEG2RAD(f) / 2.0)); + return a; +} + +/********************************************** scale_angle() ************/ +/* scale rot angle (degrees) */ + +c3quatp +c3quat_scale_angle( + c3quatp a, + c3f f) +{ + return c3quat_set_angle(a, f * c3quat_get_angle(a) ); +} + +/********************************************** get_angle() ************/ +/* get rot angle (degrees). Assumes s is between -1 and 1 */ + +c3f +c3quat_get_angle( + const c3quatp a) +{ + return (c3f) RAD2DEG( 2.0 * acos( a->s ) ); +} + +/********************************************* get_axis() **************/ + +c3vec3 +c3quat_get_axis( + c3quatp a) +{ + c3f scale = (c3f) sin( acos( a->s ) ); + + if ( scale < FUDGE && scale > -FUDGE ) + return c3vec3f( 0.0, 0.0, 0.0 ); + else + return c3vec3_divf(a->v, scale); +} + +/******************************************* c3quat_print() ************/ +#if 0 +void c3quat_print(FILE *dest, const char *name) const +{ + fprintf( dest, "%s: v:<%3.2f %3.2f %3.2f> s:%3.2f\n", + name, v[0], v[1], v[2], s ); +} +#endif diff --git a/examples/board_reprap/src/c3/c3quaternion.h b/examples/board_reprap/src/c3/c3quaternion.h new file mode 100644 index 0000000..6e3f042 --- /dev/null +++ b/examples/board_reprap/src/c3/c3quaternion.h @@ -0,0 +1,93 @@ +/* + c3quaternion.h + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3QUATERNION_H___ +#define __C3QUATERNION_H___ + +#include +#include "c3/c3algebra.h" + +typedef struct c3quat { + c3vec3 v; /* vector component */ + c3f s; /* scalar component */ +} c3quat, *c3quatp; + +c3quat +c3quat_new(); +c3quat +c3quat_identity(); + +c3quat +c3quatf( + const c3f x, + const c3f y, + const c3f z, + const c3f w); +c3quat +c3quat_vec3( + const c3vec3 v, + const c3f s); +c3quat +c3quat_vec4( + const c3vec4 v); + +c3quat +c3quat_double( + const double *d); + +c3quat +c3quat_add( + const c3quat a, + const c3quat b); +c3quat +c3quat_sub( + const c3quat a, + const c3quat b); +c3quat +c3quat_minus( + const c3quat a ); + +c3quat +c3quat_mul( + const c3quat a, + const c3quat b); + +c3mat4 +c3quat_to_mat4( + const c3quat a ); + +c3quatp +c3quat_set_angle( + c3quatp a, + c3f f); +c3quatp +c3quat_scale_angle( + c3quatp a, + c3f f); +c3f +c3quat_get_angle( + const c3quatp a); +c3vec3 +c3quat_get_axis( + c3quatp a); + +#endif /* __C3QUATERNION_H___ */ diff --git a/examples/board_reprap/src/c3/c3transform.c b/examples/board_reprap/src/c3/c3transform.c new file mode 100644 index 0000000..4c68b6b --- /dev/null +++ b/examples/board_reprap/src/c3/c3transform.c @@ -0,0 +1,63 @@ +/* + c3transform.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#include "c3/c3object.h" + +c3transform_p +c3transform_new( + c3object_p o ) +{ + c3transform_p res = malloc(sizeof(*res)); + res->matrix = identity3D(); + res->object = o; + res->name = NULL; + c3transform_array_add(&o->transform, res); + return res; +} + +void +c3transform_dispose( + c3transform_p t ) +{ + if (t->object) { + for (int oi = 0; oi < t->object->transform.count; oi++) + if (t->object->transform.e[oi] == t) { + c3transform_array_delete(&t->object->transform, oi, 1); + c3object_set_dirty(t->object, true); + break; + } + t->object = NULL; + } + str_free(t->name); + free(t); +} + +void +c3transform_set( + c3transform_p t, + const c3mat4p m ) +{ + if (c3mat4_equal(m, &t->matrix)) + return; + t->matrix = *m; + c3object_set_dirty(t->object, true); +} diff --git a/examples/board_reprap/src/c3/c3transform.h b/examples/board_reprap/src/c3/c3transform.h new file mode 100644 index 0000000..9a50e3b --- /dev/null +++ b/examples/board_reprap/src/c3/c3transform.h @@ -0,0 +1,49 @@ +/* + c3transform.h + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#ifndef __C3TRANSFORM_H___ +#define __C3TRANSFORM_H___ + +#include "c3/c3algebra.h" +#include "c_utils.h" + +typedef struct c3transform_t { + str_p name; + struct c3object_t * object; + c3mat4 matrix; +} c3transform_t, *c3transform_p; + +c3transform_p +c3transform_new( + struct c3object_t * o ); +void +c3transform_set( + c3transform_p t, + c3mat4p m ); +void +c3transform_dispose( + c3transform_p t ); + +DECLARE_C_ARRAY(c3transform_p, c3transform_array, 4); +IMPLEMENT_C_ARRAY(c3transform_array); + +#endif /* __C3TRANSFORM_H___ */ diff --git a/examples/board_reprap/src/c3/c_array.h b/examples/board_reprap/src/c3/c_array.h new file mode 100644 index 0000000..8c08efa --- /dev/null +++ b/examples/board_reprap/src/c3/c_array.h @@ -0,0 +1,134 @@ +/* + c_array.h + + Copyright 2012 Michel Pollet + + This file is part of gcodepp. + + gcodepp is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + gcodepp is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with gcodepp. If not, see . + */ + + +#ifndef __C_ARRAY_H___ +#define __C_ARRAY_H___ + +#include +#include +#include + +#ifndef C_ARRAY_INLINE +#define C_ARRAY_INLINE inline +#endif +#ifndef C_ARRAY_SIZE_TYPE +#define C_ARRAY_SIZE_TYPE uint32_t +#endif + +#define DECLARE_C_ARRAY(__type, __name, __page) \ +enum { __name##_page_size = __page }; \ +typedef __type __name##_element_t; \ +typedef C_ARRAY_SIZE_TYPE __name##_count_t; \ +typedef struct __name##_t {\ + volatile __name##_count_t count;\ + volatile __name##_count_t size;\ + __name##_element_t * e;\ +} __name##_t, *__name##_p; + +#define C_ARRAY_NULL { 0, 0, NULL } + +#define IMPLEMENT_C_ARRAY(__name) \ +static const __name##_t __name##_zero = C_ARRAY_NULL; \ +static C_ARRAY_INLINE \ + void __name##_free(\ + __name##_p a) \ +{\ + if (!a) return;\ + if (a->e) free(a->e);\ + *a = __name##_zero;\ +}\ +static C_ARRAY_INLINE \ + void __name##_clear(\ + __name##_p a) \ +{\ + if (!a) return;\ + a->count = 0;\ +}\ +static C_ARRAY_INLINE \ + void __name##_realloc(\ + __name##_p a, __name##_count_t size) \ +{\ + if (!a || a->size == size) return; \ + a->e = realloc(a->e, size * sizeof(__name##_element_t));\ + a->size = size; \ +}\ +static C_ARRAY_INLINE \ + void __name##_trim(\ + __name##_p a) \ +{\ + if (!a) return;\ + __name##_count_t n = a->count + __name##_page_size;\ + n -= (n % __name##_page_size);\ + if (n != a->size)\ + __name##_realloc(a, n);\ +}\ +static C_ARRAY_INLINE \ + __name##_element_t * __name##_get_ptr(\ + __name##_p a, __name##_count_t index) \ +{\ + if (!a) return NULL;\ + if (index > a->count) index = a->count;\ + return index < a->count ? a->e + index : NULL;\ +}\ +static C_ARRAY_INLINE \ + __name##_count_t __name##_add(\ + __name##_p a, __name##_element_t e) \ +{\ + if (!a) return 0;\ + if (a->count + 1 >= a->size)\ + __name##_realloc(a, a->size + __name##_page_size);\ + a->e[a->count++] = e;\ + return a->count;\ +}\ +static C_ARRAY_INLINE \ + __name##_count_t __name##_insert(\ + __name##_p a, __name##_count_t index, \ + __name##_element_t * e, __name##_count_t count) \ +{\ + if (!a) return 0;\ + if (index > a->count) index = a->count;\ + if (a->count + count >= a->size) \ + __name##_realloc(a, (((a->count + count) / __name##_page_size)+1) * __name##_page_size);\ + if (index < a->count)\ + memmove(&a->e[index + count], &a->e[index], \ + (a->count - index + count) * sizeof(__name##_element_t));\ + memmove(&a->e[index], e, count * sizeof(__name##_element_t));\ + a->count += count;\ + return a->count;\ +}\ +static C_ARRAY_INLINE \ + __name##_count_t __name##_delete(\ + __name##_p a, __name##_count_t index, __name##_count_t count) \ +{\ + if (!a) return 0;\ + if (index > a->count) index = a->count;\ + if (index + count > a->count) \ + count = a->count - index;\ + if (count && a->count - index) { \ + memmove(&a->e[index], &a->e[index + count], \ + (a->count - index - count) * sizeof(__name##_element_t));\ + }\ + a->count -= count;\ + return a->count;\ +} + +#endif /* __C_ARRAY_H___ */ diff --git a/examples/board_reprap/src/c3/c_utils.c b/examples/board_reprap/src/c3/c_utils.c new file mode 100644 index 0000000..e3bb673 --- /dev/null +++ b/examples/board_reprap/src/c3/c_utils.c @@ -0,0 +1,67 @@ +/* + c_utils.c + + Copyright 2008-2012 Michel Pollet + + This file is part of simavr. + + simavr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + simavr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with simavr. If not, see . + */ + + +#include "c_utils.h" + +void +str_hash_init(str_hash_p h) +{ + memset(h, 0, sizeof(*h)); +} + +void +str_hash_add( + str_hash_p h, + str_p k, + void * v) +{ + uint16_t hv = str_hash(k); + hashval_array_p bin = &h->bin[hv & (STR_HASH_SIZE-1)]; + int inserti = bin->count; + + for (int i = 0; i < bin->count; i++) + if (bin->e[i].key->hash >= hv) { + inserti = i; + break; + } + str_hashval_t n = { .key = str_dup(k), .val = v }; + hashval_array_insert(bin, inserti, &n, 1); + return; +} + +void * +str_hash_lookup( + str_hash_p h, + str_p k ) +{ + uint16_t hv = str_hash(k); + hashval_array_p bin = &h->bin[hv & (STR_HASH_SIZE-1)]; + + for (int i = 0; i < bin->count; i++) { + uint16_t h = bin->e[i].key->hash; + if (h == hv && !str_cmp(k, bin->e[i].key)) + return bin->e[i].val; + else if (h > hv) + break; + } + return NULL; +} diff --git a/examples/board_reprap/src/c3/c_utils.h b/examples/board_reprap/src/c3/c_utils.h new file mode 100644 index 0000000..e6bad57 --- /dev/null +++ b/examples/board_reprap/src/c3/c_utils.h @@ -0,0 +1,166 @@ +/* + c_utils.h + + Copyright 2008-11 Michel Pollet + + This program cross examines a root filesystem, loads all the elf + files it can find, see what other library they load and then + find the orphans. In then remove the orphans as "user" for it's + dependencies and continues removing until everything has at least + one user, OR is a program itself (ie, not a shared library) + + cross_linker is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + cross_linker is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with cross_linker. If not, see . + */ + +#ifndef __C_UTILS_H__ +#define __C_UTILS_H__ + +#include "c_array.h" + +/******************************************************************** + * CRC16 + ********************************************************************/ + +static uint8_t _crc16_lh[16] = { 0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, + 0x70, 0x81, 0x91, 0xA1, 0xB1, 0xC1, 0xD1, 0xE1, 0xF1 }; +static uint8_t _crc16_ll[16] = { 0x00, 0x21, 0x42, 0x63, 0x84, 0xA5, 0xC6, + 0xE7, 0x08, 0x29, 0x4A, 0x6B, 0x8C, 0xAD, 0xCE, 0xEF }; + +static uint16_t crc16_u4(uint16_t crc, uint8_t val) +{ + uint8_t h = crc >> 8, l = crc & 0xff; + uint8_t t = (h >> 4) ^ val; + + // Shift the CRC Register left 4 bits + h = (h << 4) | (l >> 4); + l = l << 4; + // Do the table lookups and XOR the result into the CRC Tables + h = h ^ _crc16_lh[t]; + l = l ^ _crc16_ll[t]; + return (h << 8) | l; +} + +static uint16_t crc16_update(uint16_t crc, uint8_t val) +{ + crc = crc16_u4(crc, val >> 4); // High nibble first + crc = crc16_u4(crc, val & 0x0F); // Low nibble + return crc; +} + +static uint16_t crc16_string(char * str) +{ + uint16_t crc = 0xffff; + while (*str) + crc = crc16_update(crc, *str++); + return crc; +} + +/******************************************************************** + * Hashed strings + ********************************************************************/ + +#include +typedef struct str_t { + uint32_t hash : 16, rom : 1, len : 15; + char str[0]; +} str_t, *str_p; + +static inline str_p str_new_i(const char *s, void * (*_alloc)(size_t)) +{ + int l = s ? strlen(s) : 0; + str_p r = (str_p)_alloc(sizeof(*r) + l + 1); + r->hash = 0; r->len = l; + if (s) + strcpy(r->str, s); + return r; +} +static inline void str_free(str_p s) +{ + if (s && !s->rom) + free(s); +} +static inline str_p str_new(const char *s) +{ + return str_new_i(s, malloc); +} +static inline str_p str_anew(const char *s) +{ + str_p r = str_new_i(s, alloca); + r->rom = 1; + return r; +} +static inline str_p str_dup(const str_p s) +{ + size_t l = sizeof(*s) + s->len + 1; + str_p r = (str_p)malloc(l); + memcpy(r, s, l); + return r; +} +static inline str_p str_adup(const str_p s) +{ + size_t l = sizeof(*s) + s->len + 1; + str_p r = (str_p)alloca(l); + memcpy(r, s, l); + r->rom = 1; + return r; +} +static inline uint16_t str_hash(str_p s) +{ + if (!s->hash) s->hash = crc16_string(s->str); + return s->hash; +} +static inline int str_cmp(str_p s1, str_p s2) +{ + if (s1 == s2) return 1; + if (s1->len != s2->len) return 1; + str_hash(s1); + str_hash(s2); + return s1->hash == s2->hash ? strcmp(s1->str, s2->str) : 1; +} + +/******************************************************************** + * Hash table of strings. Key/value pair + ********************************************************************/ + +typedef struct str_hashval_t { + str_p key; + void * val; +} str_hashval_t; + +DECLARE_C_ARRAY(str_hashval_t, hashval_array, 16); +IMPLEMENT_C_ARRAY(hashval_array); + +#ifndef STR_HASH_SIZE +#define STR_HASH_SIZE 512 // use 9 bits of the 16 of the CRC +#endif +/* uses bins to store the strings as per their hash values */ +typedef struct str_hash_t { + hashval_array_t bin[STR_HASH_SIZE]; +} str_hash_t, *str_hash_p; + +void +str_hash_init( + str_hash_p h); +void +str_hash_add( + str_hash_p h, + str_p k, + void * v); + +void * +str_hash_lookup( + str_hash_p h, + str_p k ); + +#endif /* __C_UTILS_H__ */ -- 2.39.5