diff options
| -rw-r--r-- | dat.h | 42 | ||||
| -rw-r--r-- | fns.h | 46 | ||||
| -rw-r--r-- | main.c | 382 | ||||
| -rw-r--r-- | matrix.c | 82 | ||||
| -rw-r--r-- | mdl/rocket.obj | 143 | ||||
| -rw-r--r-- | mkfile | 19 | ||||
| -rw-r--r-- | obj.c | 65 | ||||
| -rw-r--r-- | qb.c | 96 | ||||
| -rw-r--r-- | qball.c | 108 | ||||
| -rw-r--r-- | quat.c | 93 | ||||
| -rw-r--r-- | readme.md | 3 | ||||
| -rw-r--r-- | triangle.c | 58 | ||||
| -rw-r--r-- | util.c | 29 | ||||
| -rw-r--r-- | vector.c | 60 | ||||
| -rw-r--r-- | vector3.c | 65 |
15 files changed, 1291 insertions, 0 deletions
@@ -0,0 +1,42 @@ +#define DEG 0.01745329251994330 + +enum { + STACK = 8192, + SEC = 1000, + FPS = 60, + FOV = 90 +}; + +typedef struct Vector Vector; +typedef struct Vector3 Vector3; +typedef double Matrix[4][4]; +typedef struct Quaternion Quaternion; +typedef struct Triangle Triangle; +typedef struct Triangle3 Triangle3; +typedef struct Mesh Mesh; + +struct Vector { + double x, y; +}; + +struct Vector3 { + double x, y, z; +}; + +struct Quaternion { + double r, i, j, k; +}; + +struct Triangle { + Point p0, p1, p2; +}; + +struct Triangle3 { + Vector3 p0, p1, p2; + Image *tx; +}; + +struct Mesh { + Triangle3 *tris; + int ntri; +}; @@ -0,0 +1,46 @@ +int objread(char *, Triangle3 **); +Triangle Trian(int, int, int, int, int, int); +Triangle Trianpt(Point, Point, Point); +Point centroid(Triangle); +void triangle(Image *, Triangle, int, Image *, Point); +void filltriangle(Image *, Triangle, Image *, Point); +Triangle rotatriangle(Triangle, double, Point); +Vector Vec(double, double); +Vector addvec(Vector, Vector); +Vector subvec(Vector, Vector); +Vector mulvec(Vector, double); +double dotvec(Vector, Vector); +Vector normvec(Vector); +Vector3 Vec3(double, double, double); +Vector3 Vecquat(Quaternion); +Vector3 addvec3(Vector3, Vector3); +Vector3 subvec3(Vector3, Vector3); +Vector3 mulvec3(Vector3, double); +double dotvec3(Vector3, Vector3); +Vector3 crossvec(Vector3, Vector3); +Vector3 normvec3(Vector3); +void addm(Matrix, Matrix); +void subm(Matrix, Matrix); +void mulm(Matrix, Matrix); +void transm(Matrix); +double detm(Matrix); +Vector3 mulvecm(Vector3, Matrix); +Quaternion Quat(double, double, double, double); +Quaternion Quatvec(double, Vector3); +Quaternion addq(Quaternion, Quaternion); +Quaternion subq(Quaternion, Quaternion); +Quaternion mulq(Quaternion, Quaternion); +Quaternion smulq(Quaternion, double); +Quaternion sdivq(Quaternion, double); +double dotq(Quaternion, Quaternion); +Quaternion invq(Quaternion); +double qlen(Quaternion); +Quaternion normq(Quaternion); +Vector3 qrotate(Vector3, Vector3, double); +void qball(Rectangle, Point, Quaternion*, Quaternion*); +void qb(Rectangle, Point, Point, Quaternion*, Quaternion*); +double round(double); +Point rotatept(Point, double, Point); +double hypot3(double, double, double); +void *emalloc(ulong); +void *erealloc(void *, ulong); @@ -0,0 +1,382 @@ +#include <u.h> +#include <libc.h> +#include <thread.h> +#include <draw.h> +#include <mouse.h> +#include <keyboard.h> +#include "dat.h" +#include "fns.h" + +Mousectl *mctl; +Keyboardctl *kctl; +Mouse om; +Point orig; +Vector basis; +double Znear, Zfar, Zoff, a; +Matrix proj; +Vector3 light; +double θ, ω; +double t0, Δt; +int dowireframe; +RWLock worldlock; +Quaternion orient; + +Mesh model; +/*Triangle3 cube[] = { + 0,0,0, 0,1,0, 1,1,0, + 0,0,0, 1,1,0, 1,0,0, + + 1,0,0, 1,1,0, 1,1,1, + 1,0,0, 1,1,1, 1,0,1, + + 1,0,1, 1,1,1, 0,1,1, + 1,0,1, 0,1,1, 0,0,1, + + 0,0,1, 0,1,1, 0,1,0, + 0,0,1, 0,1,0, 0,0,0, + + 0,1,0, 0,1,1, 1,1,1, + 0,1,0, 1,1,1, 1,1,0, + + 1,0,1, 0,0,1, 0,0,0, + 1,0,1, 0,0,0, 1,0,0, +};*/ + +void * +emalloc(ulong n) +{ + void *p; + + p = malloc(n); + if(p == nil) + sysfatal("malloc: %r"); + memset(p, 0, n); + setmalloctag(p, getcallerpc(&n)); + return p; +} + +void * +erealloc(void *ptr, ulong n) +{ + void *p; + + p = realloc(ptr, n); + if(p == nil) + sysfatal("realloc: %r"); + setrealloctag(p, getcallerpc(&ptr)); + return p; +} + +Image * +eallocimage(Display *d, Rectangle r, ulong chan, int repl, ulong col) +{ + Image *i; + + i = allocimage(d, r, chan, repl, col); + if(i == nil) + sysfatal("allocimage: %r"); + return i; +} + +#pragma varargck type "V" Vector3 +int +Vfmt(Fmt *f) +{ + Vector3 v; + + v = va_arg(f->args, Vector3); + return fmtprint(f, "(%g %g %g)", v.x, v.y, v.z); +} + +#pragma varargck type "H" Quaternion +int +Hfmt(Fmt *f) +{ + Quaternion q; + + q = va_arg(f->args, Quaternion); + return fmtprint(f, "(%g %g %g %g)", q.r, q.i, q.j, q.k); +} + +#pragma varargck type "T" Triangle3 +int +Tfmt(Fmt *f) +{ + Triangle3 t; + + t = va_arg(f->args, Triangle3); + return fmtprint(f, "%V%V%V", t.p0, t.p1, t.p2); +} + +Point +vectopt(Vector v) +{ + return Pt(v.x, v.y); +} + +Point +toscreen(Vector p) +{ + return addpt(orig, Pt(p.x*basis.x, p.y*basis.y)); +} + +int +depthcmp(void *a, void *b) +{ + Triangle3 *ta, *tb; + double za, zb; + + ta = (Triangle3 *)a; + tb = (Triangle3 *)b; + za = (ta->p0.z + ta->p1.z + ta->p2.z)/3; + zb = (tb->p0.z + tb->p1.z + tb->p2.z)/3; + return za > zb ? -1 : 1; +} + +void +drawinfo(void) +{ + char buf[128]; + + snprint(buf, sizeof buf, "%H", orient); + string(screen, addpt(screen->r.min, Pt(30,30)), display->white, ZP, font, buf); +} + +void +redraw(void) +{ + Triangle3 trans, *vistris; + Vector3 n; +// Quaternion Xaxis, Yaxis, Zaxis; +// Quaternion q; + Matrix T = { + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, Zoff, + 0, 0, 0, 1, + }, S = { + Dx(screen->r)/2, 0, 0, 0, + 0, Dy(screen->r)/2, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1, + }; + u32int c; + int nvistri, i; + + vistris = nil; + nvistri = 0; +// Xaxis = Quat(cos(θ/4), sin(θ/4), 0, 0); +// Yaxis = Quat(cos(θ/6), 0, sin(θ/6), 0); +// Zaxis = Quat(cos(θ/2), 0, 0, sin(θ/2)); +// q = mulq(Zaxis, mulq(Yaxis, Xaxis)); + lockdisplay(display); + draw(screen, screen->r, display->black, nil, ZP); + for(i = 0; i < model.ntri; i++){ +// trans.p0 = Vecquat(mulq(mulq(q, Quatvec(0, model.tris[i].p0)), invq(q))); +// trans.p1 = Vecquat(mulq(mulq(q, Quatvec(0, model.tris[i].p1)), invq(q))); +// trans.p2 = Vecquat(mulq(mulq(q, Quatvec(0, model.tris[i].p2)), invq(q))); + +// trans.p0 = Vecquat(mulq(mulq(invq(orient), Quatvec(0, model.tris[i].p0)), orient)); +// trans.p1 = Vecquat(mulq(mulq(invq(orient), Quatvec(0, model.tris[i].p1)), orient)); +// trans.p2 = Vecquat(mulq(mulq(invq(orient), Quatvec(0, model.tris[i].p2)), orient)); + + trans.p0 = Vecquat(mulq(mulq(orient, Quatvec(0, model.tris[i].p0)), invq(orient))); + trans.p1 = Vecquat(mulq(mulq(orient, Quatvec(0, model.tris[i].p1)), invq(orient))); + trans.p2 = Vecquat(mulq(mulq(orient, Quatvec(0, model.tris[i].p2)), invq(orient))); + + trans.p0 = mulvecm(trans.p0, T); + trans.p1 = mulvecm(trans.p1, T); + trans.p2 = mulvecm(trans.p2, T); + + n = normvec3(crossvec( + subvec3(trans.p1, trans.p0), + subvec3(trans.p2, trans.p0))); + if(dotvec3(n, subvec3(trans.p0, Vec3(0, 0, 0))) < 0){ + trans.p0 = mulvecm(trans.p0, proj); + trans.p1 = mulvecm(trans.p1, proj); + trans.p2 = mulvecm(trans.p2, proj); + trans.p0 = addvec3(trans.p0, Vec3(1, 1, 0)); + trans.p1 = addvec3(trans.p1, Vec3(1, 1, 0)); + trans.p2 = addvec3(trans.p2, Vec3(1, 1, 0)); + trans.p0 = mulvecm(trans.p0, S); + trans.p1 = mulvecm(trans.p1, S); + trans.p2 = mulvecm(trans.p2, S); + c = 0xff*fabs(dotvec3(n, light)); + trans.tx = eallocimage(display, Rect(0, 0, 1, 1), screen->chan, 1, c<<24|c<<16|c<<8|0xff); + vistris = erealloc(vistris, ++nvistri*sizeof(Triangle3)); + vistris[nvistri-1] = trans; + } + } + qsort(vistris, nvistri, sizeof(Triangle3), depthcmp); + for(i = 0; i < nvistri; i++){ + filltriangle(screen, Trianpt( + toscreen(Vec(vistris[i].p0.x, vistris[i].p0.y)), + toscreen(Vec(vistris[i].p1.x, vistris[i].p1.y)), + toscreen(Vec(vistris[i].p2.x, vistris[i].p2.y)) + ), vistris[i].tx, ZP); + if(dowireframe) + triangle(screen, Trianpt( + toscreen(Vec(vistris[i].p0.x, vistris[i].p0.y)), + toscreen(Vec(vistris[i].p1.x, vistris[i].p1.y)), + toscreen(Vec(vistris[i].p2.x, vistris[i].p2.y)) + ), 0, display->black, ZP); + freeimage(vistris[i].tx); + } + free(vistris); + drawinfo(); + flushimage(display, 1); + unlockdisplay(display); +} + +void +lmb(void) +{ +// double tmp; + +// qball(screen->r, mctl->xy, &orient, nil); + if(om.buttons & 1) + qb(screen->r, om.xy, mctl->xy, &orient, nil); +// fprint(2, "orient %H\n", orient); +} + +void +rmb(void) +{ + enum { + DOWIREFRM, + }; + static char *items[] = { + [DOWIREFRM] "toggle wireframe", + nil, + }; + static Menu menu = { .item = items }; + + switch(menuhit(3, mctl, &menu, nil)){ + case DOWIREFRM: + dowireframe ^= 1; + break; + } + t0 = nsec(); +} + +void +mouse(void) +{ + if(mctl->buttons & 1) + lmb(); + if(mctl->buttons & 4) + rmb(); + if(mctl->buttons & 8) + Zoff -= 0.2; + if(mctl->buttons & 16) + Zoff += 0.2; + om = mctl->Mouse; +} + +void +key(Rune r) +{ + switch(r){ + case Kdel: + case 'q': + threadexitsall(nil); + case Kpgup: + Zoff += 0.2; + break; + case Kpgdown: + Zoff -= 0.2; + break; + } +} + +void +resized(void) +{ + lockdisplay(display); + if(getwindow(display, Refnone) < 0) + fprint(2, "can't reattach to window\n"); + unlockdisplay(display); + orig = Pt(screen->r.min.x, screen->r.max.y); + a = (double)Dy(screen->r)/Dx(screen->r); + proj[0][0] = a*(1/tan(FOV/2*DEG)); + redraw(); +} + +void +scrsynproc(void *) +{ + threadsetname("scrsynproc"); + for(;;){ + rlock(&worldlock); + redraw(); + runlock(&worldlock); + sleep(SEC/FPS); + } +} + +void +usage(void) +{ + fprint(2, "usage: %s\n", argv0); + threadexitsall("usage"); +} + +void +threadmain(int argc, char *argv[]) +{ + Rune r; + + fmtinstall('V', Vfmt); + fmtinstall('H', Hfmt); + fmtinstall('T', Tfmt); + ARGBEGIN{ + default: usage(); + }ARGEND; + + if(initdraw(nil, nil, "threedee") < 0) + sysfatal("initdraw: %r"); + if((mctl = initmouse(nil, screen)) == nil) + sysfatal("initmouse: %r"); + if((kctl = initkeyboard(nil)) == nil) + sysfatal("initkeyboard: %r"); + orig = Pt(screen->r.min.x, screen->r.max.y); + basis = Vec(1, -1); + orient = Quat(1, 1, 0, 0); + Znear = 0.1; + Zfar = 1000; + Zoff = 2; + a = (double)Dy(screen->r)/Dx(screen->r); + proj[0][0] = a*(1/tan(FOV/2*DEG)); + proj[1][1] = 1/tan(FOV/2*DEG); + proj[2][2] = Zfar / (Zfar + Znear); + proj[2][3] = -Zfar * Znear / (Zfar + Znear); + proj[3][2] = 1; + light = Vec3(0, 0, -1); + if((model.ntri = objread("mdl/rocket.obj", &model.tris)) < 0) + sysfatal("objread: %r"); + display->locking = 1; + unlockdisplay(display); + proccreate(scrsynproc, nil, STACK); + ω = 1; + t0 = nsec(); + for(;;){ + enum {MOUSE, RESIZE, KBD}; + Alt a[] = { + {mctl->c, &mctl->Mouse, CHANRCV}, + {mctl->resizec, nil, CHANRCV}, + {kctl->c, &r, CHANRCV}, + {nil, nil, CHANNOBLK} + }; + wlock(&worldlock); + switch(alt(a)){ + case MOUSE: mouse(); break; + case RESIZE: resized(); break; + case KBD: key(r); break; + } + Δt = (nsec()-t0)/1e9; + θ += ω*Δt; + t0 += Δt*1e9; + wunlock(&worldlock); + sleep(SEC/FPS); + } +} diff --git a/matrix.c b/matrix.c new file mode 100644 index 0000000..e38722c --- /dev/null +++ b/matrix.c @@ -0,0 +1,82 @@ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" + +void +addm(Matrix a, Matrix b) +{ + int i, j; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++) + a[i][j] += b[i][j]; +} + +void +subm(Matrix a, Matrix b) +{ + int i, j; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++) + a[i][j] -= b[i][j]; +} + +void +mulm(Matrix a, Matrix b) +{ + int i, j, k; + Matrix r; + + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++){ + r[i][j] = 0; + for(k = 0; k < 4; k++) + r[i][j] += a[i][k]*b[k][j]; + } + for(i = 0; i < 4; i++) + for(j = 0; j < 4; j++) + a[i][j] = r[i][j]; +} + +void +transm(Matrix m) +{ + int i, j; + double tmp; + + for(i = 0; i < 4; i++) + for(j = i; j < 4; j++){ + tmp = m[i][j]; + m[i][j] = m[j][i]; + m[j][i] = tmp; + } +} + +double +detm(Matrix m) +{ + return m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1]) + + m[0][1]*(m[1][2]*m[2][0] - m[1][0]*m[2][2]) + + m[0][2]*(m[1][0]*m[2][1] - m[1][1]*m[2][0]); +} + +Vector3 +mulvecm(Vector3 v, Matrix m) +{ + Vector3 r; + double w; + + r.x = v.x * m[0][0] + v.y * m[0][1] + v.z * m[0][2] + m[0][3]; + r.y = v.x * m[1][0] + v.y * m[1][1] + v.z * m[1][2] + m[1][3]; + r.z = v.x * m[2][0] + v.y * m[2][1] + v.z * m[2][2] + m[2][3]; + w = v.x * m[3][0] + v.y * m[3][1] + v.z * m[3][2] + m[3][3]; + + if(w == 0) + return r; + r.x /= w; + r.y /= w; + r.z /= w; + return r; +} diff --git a/mdl/rocket.obj b/mdl/rocket.obj new file mode 100644 index 0000000..834fdbc --- /dev/null +++ b/mdl/rocket.obj @@ -0,0 +1,143 @@ +# Blender v2.79 (sub 0) OBJ File: '' +# www.blender.org +v -0.334186 0.372621 -0.333268 +v -0.334186 0.370788 0.335102 +v 0.334186 0.370788 0.335102 +v 0.334186 0.372621 -0.333268 +v -0.334186 -0.295749 -0.335102 +v -0.334186 -0.297582 0.333269 +v 0.334186 -0.297582 0.333268 +v 0.334186 -0.295749 -0.335102 +v -0.334186 -0.695815 -0.336199 +v -0.334186 -0.697648 0.332171 +v 0.334186 -0.695815 -0.336199 +v 0.334186 -0.697648 0.332171 +v -0.120197 -0.754088 -0.122369 +v -0.120197 -0.754748 0.118024 +v 0.120197 -0.754088 -0.122369 +v 0.120197 -0.754748 0.118024 +v -0.306281 -1.117213 -0.309450 +v -0.306281 -1.118893 0.303111 +v 0.306281 -1.117213 -0.309450 +v 0.306281 -1.118893 0.303110 +v -0.289248 0.712766 -0.287397 +v -0.289248 0.711179 0.291097 +v 0.289248 0.712766 -0.287397 +v 0.289248 0.711179 0.291097 +v -0.233218 0.885662 -0.230892 +v -0.233218 0.884382 0.235542 +v 0.233218 0.885662 -0.230893 +v 0.233218 0.884382 0.235542 +v -0.165619 1.041305 -0.162866 +v -0.165619 1.040397 0.168371 +v 0.165619 1.041305 -0.162866 +v 0.165619 1.040397 0.168370 +v -0.109259 1.126811 -0.106272 +v -0.109259 1.126212 0.112246 +v 0.109259 1.126811 -0.106272 +v 0.109259 1.126212 0.112246 +v -0.052090 1.177090 -0.048964 +v -0.052090 1.176804 0.055215 +v 0.052090 1.177090 -0.048964 +v 0.052090 1.176804 0.055215 +v -0.026259 1.194980 -0.023084 +v -0.026259 1.194836 0.029434 +v 0.026259 1.194980 -0.023084 +v 0.026259 1.194836 0.029434 +v -0.010547 1.200304 -0.007358 +v -0.010547 1.200246 0.013737 +v 0.010547 1.200304 -0.007358 +v 0.010547 1.200246 0.013737 +s off +f 1 22 21 +f 7 10 12 +f 1 6 2 +f 2 7 3 +f 3 8 4 +f 5 4 8 +f 11 16 15 +f 6 9 10 +f 8 12 11 +f 5 11 9 +f 13 19 17 +f 9 15 13 +f 12 14 16 +f 10 13 14 +f 17 20 18 +f 16 18 20 +f 14 17 18 +f 15 20 19 +f 24 27 28 +f 3 23 24 +f 2 24 22 +f 4 21 23 +f 26 32 30 +f 23 25 27 +f 22 28 26 +f 21 26 25 +f 30 36 34 +f 25 30 29 +f 28 31 32 +f 27 29 31 +f 36 39 40 +f 29 34 33 +f 32 35 36 +f 31 33 35 +f 38 44 42 +f 35 37 39 +f 34 40 38 +f 33 38 37 +f 41 46 45 +f 37 42 41 +f 40 43 44 +f 39 41 43 +f 45 48 47 +f 44 47 48 +f 43 45 47 +f 42 48 46 +f 1 2 22 +f 7 6 10 +f 1 5 6 +f 2 6 7 +f 3 7 8 +f 5 1 4 +f 11 12 16 +f 6 5 9 +f 8 7 12 +f 5 8 11 +f 13 15 19 +f 9 11 15 +f 12 10 14 +f 10 9 13 +f 17 19 20 +f 16 14 18 +f 14 13 17 +f 15 16 20 +f 24 23 27 +f 3 4 23 +f 2 3 24 +f 4 1 21 +f 26 28 32 +f 23 21 25 +f 22 24 28 +f 21 22 26 +f 30 32 36 +f 25 26 30 +f 28 27 31 +f 27 25 29 +f 36 35 39 +f 29 30 34 +f 32 31 35 +f 31 29 33 +f 38 40 44 +f 35 33 37 +f 34 36 40 +f 33 34 38 +f 41 42 46 +f 37 38 42 +f 40 39 43 +f 39 37 41 +f 45 46 48 +f 44 43 47 +f 43 41 45 +f 42 44 48 @@ -0,0 +1,19 @@ +</$objtype/mkfile + +BIN=/$objtype/bin/ +TARG=threedee +OFILES=\ + main.$O\ + util.$O\ + matrix.$O\ + vector3.$O\ + vector.$O\ + triangle.$O\ + obj.$O\ + quat.$O\ + qball.$O\ + qb.$O + +HFILES=dat.h fns.h + +</sys/src/cmd/mkone @@ -0,0 +1,65 @@ +#include <u.h> +#include <libc.h> +#include <ctype.h> +#include <bio.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +Vector3 *verts; +Triangle3 *tris; +int nvert, ntri; +int lineno; + +int +objread(char *f, Triangle3 **mesh) +{ + Biobuf *bin; + char *s; + int idx; + double *coord; + Vector3 *vert; + + bin = Bopen(f, OREAD); + if(bin == nil) + sysfatal("Bopen: %r"); + while((s = Brdline(bin, '\n')) != nil){ + lineno++; + if(*s == 'v'){ + verts = erealloc(verts, ++nvert*sizeof(Vector3)); + coord = (double *)(verts+nvert-1); + while(*s != '\n' && (Vector3 *)coord-verts < nvert){ + while(isspace(*++s)) + ; + *coord++ = strtod(s, &s); + } + if(coord-(double *)(verts+nvert-1) < 3){ + werrstr("%s:%d insufficient coordinates", f, lineno); + goto error; + } + } + if(*s == 'f'){ + tris = erealloc(tris, ++ntri*sizeof(Triangle3)); + vert = (Vector3 *)(tris+ntri-1); + while(*s != '\n' && (Triangle3 *)vert-tris < ntri){ + while(isspace(*++s)) + ; + idx = strtol(s, &s, 0); + if(idx > nvert){ + werrstr("%s:%d insufficient vertices", f, lineno); + goto error; + } + *vert++ = verts[idx-1]; + } + } + } + *mesh = tris; + free(verts); + Bterm(bin); + return ntri; +error: + free(verts); + free(tris); + Bterm(bin); + return -1; +} @@ -0,0 +1,96 @@ +/* + * Ken Shoemake's Quaternion rotation controller + */ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +typedef struct Point2 Point2; +struct Point2 { + double x, y; +}; + +static Point2 +Pt2(double x, double y) +{ + return (Point2){x, y}; +} + +static Point2 +addpt2(Point2 a, Point2 b) +{ + return (Point2){a.x+b.x, a.y+b.y}; +} + +static Point2 +subpt2(Point2 a, Point2 b) +{ + return (Point2){a.x-b.x, a.y-b.y}; +} + +static Point2 +divpt2(Point2 p, double s) +{ + return (Point2){p.x/s, p.y/s}; +} + +/* + * Convert a mouse point into a unit quaternion, flattening if + * constrained to a particular plane. + */ +static Quaternion +mouseq(Point2 p, Quaternion *axis){ + double l; + Quaternion q; + double rsq = p.x*p.x + p.y*p.y; + + if(rsq > 1){ + rsq = sqrt(rsq); + q.k = 0; + q.i = p.x/rsq; + q.j = p.y/rsq; + q.r = 0; + } + else{ + q.r = 0; + q.i = p.x; + q.j = p.y; + q.k = sqrt(1 - rsq); + } + if(axis != nil){ + l = q.i*axis->i + q.j*axis->j + q.k*axis->k; + q.i -= l*axis->i; + q.j -= l*axis->j; + q.k -= l*axis->k; + l = sqrt(q.i*q.i + q.j*q.j + q.k*q.k); + if(l != 0.){ + q.i /= l; + q.j /= l; + q.k /= l; + } + } + return q; +} + +void +qb(Rectangle r, Point p1, Point p2, Quaternion *orient, Quaternion *axis){ + Quaternion q, down; + Point2 rmin, rmax; + Point2 ctlcen, ctlrad; + double qx, qy; + + rmin = Pt2(r.min.x, r.min.y); + rmax = Pt2(r.max.x, r.max.y); + ctlcen = divpt2(addpt2(rmin, rmax), 2); + ctlrad = divpt2(subpt2(rmax, rmin), 2); + qx = (p1.x-ctlcen.x)/ctlrad.x; + qy = (ctlcen.y-p1.y)/ctlrad.y; + down = invq(mouseq(Pt2(qx, qy), axis)); + + q = *orient; + qx = (p2.x-ctlcen.x)/ctlrad.x; + qy = (ctlcen.y-p2.y)/ctlrad.y; + *orient = mulq(q, mulq(down, mouseq(Pt2(qx, qy), axis))); +} @@ -0,0 +1,108 @@ +/* + * Ken Shoemake's Quaternion rotation controller + */ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +typedef struct Point2 Point2; +struct Point2 { + double x, y; +}; + +static Point2 +Pt2(double x, double y) +{ + return (Point2){x, y}; +} + +static Point2 +addpt2(Point2 a, Point2 b) +{ + return (Point2){a.x+b.x, a.y+b.y}; +} + +static Point2 +subpt2(Point2 a, Point2 b) +{ + return (Point2){a.x-b.x, a.y-b.y}; +} + +static Point2 +divpt2(Point2 p, double s) +{ + return (Point2){p.x/s, p.y/s}; +} + +static Point2 ctlcen; /* center of qball */ +static double ctlrad; /* radius of qball */ +static Quaternion *axis; /* constraint plane orientation, 0 if none */ + +static double +fmin(double a, double b) +{ + return a < b? a: b; +} + +/* + * Convert a mouse point into a unit quaternion, flattening if + * constrained to a particular plane. + */ +static Quaternion +mouseq(Point2 p) +{ + double qx = (p.x-ctlcen.x)/ctlrad; + double qy = (p.y-ctlcen.y)/ctlrad; + double rsq = qx*qx + qy*qy; + double l; + Quaternion q; + + if(rsq > 1){ + rsq = sqrt(rsq); + q.r = 0; + q.i = qx/rsq; + q.j = qy/rsq; + q.k = 0; + }else{ + q.r = 0; + q.i = qx; + q.j = qy; + q.k = sqrt(1-rsq); + } + + if(axis != nil){ + l = q.i*axis->i + q.j*axis->j + q.k*axis->k; + q.i -= l*axis->i; + q.j -= l*axis->j; + q.k -= l*axis->k; + l = sqrt(q.i*q.i + q.j*q.j + q.k*q.k); + if(l != 0){ + q.i /= l; + q.j /= l; + q.k /= l; + } + } + return q; +} + +void +qball(Rectangle r, Point mxy, Quaternion *orient, Quaternion *ap) +{ + Quaternion down; + Point2 rmin, rmax; + Point2 rad; + + if(orient == nil) + return; + + axis = ap; + rmin = Pt2(r.min.x, r.min.y); + rmax = Pt2(r.max.x, r.max.y); + ctlcen = divpt2(addpt2(rmin, rmax), 2); + rad = divpt2(subpt2(rmax, rmin), 2); + ctlrad = fmin(rad.x, rad.y); + down = invq(mouseq(Pt2(mxy.x, mxy.y))); + *orient = mulq(*orient, mulq(down, mouseq(Pt2(mxy.x, mxy.y)))); +} @@ -0,0 +1,93 @@ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +Quaternion +Quat(double r, double i, double j, double k) +{ + return (Quaternion){r, i, j, k}; +} + +Quaternion +Quatvec(double s, Vector3 v) +{ + return (Quaternion){s, v.x, v.y, v.z}; +} + +Quaternion +addq(Quaternion a, Quaternion b) +{ + return Quat(a.r+b.r, a.i+b.i, a.j+b.j, a.k+b.k); +} + +Quaternion +subq(Quaternion a, Quaternion b) +{ + return Quat(a.r-b.r, a.i-b.i, a.j-b.j, a.k-b.k); +} + +Quaternion +mulq(Quaternion q, Quaternion r) +{ + Vector3 qv, rv, tmp; + + qv = Vec3(q.i, q.j, q.k); + rv = Vec3(r.i, r.j, r.k); + tmp = addvec3(addvec3(mulvec3(rv, q.r), mulvec3(qv, r.r)), crossvec(qv, rv)); + return Quatvec(q.r*r.r - dotvec3(qv, rv), tmp); +} + +Quaternion +smulq(Quaternion q, double s) +{ + return Quat(q.r*s, q.i*s, q.j*s, q.k*s); +} + +Quaternion +sdivq(Quaternion q, double s) +{ + return Quat(q.r/s, q.i/s, q.j/s, q.k/s); +} + +double +dotq(Quaternion q, Quaternion r) +{ + return q.r*r.r + q.i*r.i + q.j*r.j + q.k*r.k; +} + +Quaternion +invq(Quaternion q) +{ + double len²; + + len² = dotq(q, q); + if(len² == 0) + return Quat(0,0,0,0); + return Quat(q.r/len², -q.i/len², -q.j/len², -q.k/len²); +} + +double +qlen(Quaternion q) +{ + return sqrt(dotq(q, q)); +} + +Quaternion +normq(Quaternion q) +{ + return sdivq(q, qlen(q)); +} + +Vector3 +qrotate(Vector3 p, Vector3 axis, double θ) +{ + Quaternion qaxis, qr; + + θ /= 2; + qaxis = Quatvec(cos(θ), mulvec3(axis, sin(θ))); + qr = mulq(mulq(qaxis, Quatvec(0, p)), invq(qaxis)); /* qpq⁻¹ */ + return Vec3(qr.i, qr.j, qr.k); +} + diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..061369e --- /dev/null +++ b/readme.md @@ -0,0 +1,3 @@ +# Qball + +Fork of [threedee](http://git.antares-labs.eu/threedee) that serves as a testbed for the development of an Arcball rotation controller. diff --git a/triangle.c b/triangle.c new file mode 100644 index 0000000..5301956 --- /dev/null +++ b/triangle.c @@ -0,0 +1,58 @@ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +Triangle +Trian(int x0, int y0, int x1, int y1, int x2, int y2) +{ + return (Triangle){Pt(x0, y0), Pt(x1, y1), Pt(x2, y2)}; +} + +Triangle +Trianpt(Point p0, Point p1, Point p2) +{ + return (Triangle){p0, p1, p2}; +}; + +Point +centroid(Triangle t) +{ + return divpt(addpt(t.p0, addpt(t.p1, t.p2)), 3); +} + +void +triangle(Image *dst, Triangle t, int thick, Image *src, Point sp) +{ + Point pl[4]; + + pl[0] = t.p0; + pl[1] = t.p1; + pl[2] = t.p2; + pl[3] = pl[0]; + + poly(dst, pl, nelem(pl), 0, 0, thick, src, sp); +} + +void +filltriangle(Image *dst, Triangle t, Image *src, Point sp) +{ + Point pl[4]; + + pl[0] = t.p0; + pl[1] = t.p1; + pl[2] = t.p2; + pl[3] = pl[0]; + + fillpoly(dst, pl, nelem(pl), 0, src, sp); +} + +Triangle +rotatriangle(Triangle t, double θ, Point c) +{ + t.p0 = rotatept(t.p0, θ, c); + t.p1 = rotatept(t.p1, θ, c); + t.p2 = rotatept(t.p2, θ, c); + return t; +} @@ -0,0 +1,29 @@ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +double +round(double n) +{ + return floor(n + 0.5); +} + +Point +rotatept(Point p, double θ, Point c) +{ + Point r; + + p = subpt(p, c); + r.x = round(p.x*cos(θ) - p.y*sin(θ)); + r.y = round(p.x*sin(θ) + p.y*cos(θ)); + r = addpt(r, c); + return r; +} + +double +hypot3(double x, double y, double z) +{ + return hypot(x, hypot(y, z)); +} diff --git a/vector.c b/vector.c new file mode 100644 index 0000000..3595c1f --- /dev/null +++ b/vector.c @@ -0,0 +1,60 @@ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +Vector +Vec(double x, double y) +{ + return (Vector){x, y}; +} + +Vector +addvec(Vector v, Vector u) +{ + return (Vector){v.x+u.x, v.y+u.y}; +} + +Vector +subvec(Vector v, Vector u) +{ + return (Vector){v.x-u.x, v.y-u.y}; +} + +Vector +mulvec(Vector v, double s) +{ + return (Vector){v.x*s, v.y*s}; +} + +double +dotvec(Vector v, Vector u) +{ + return v.x*u.x + v.y*u.y; +} + +Vector +normvec(Vector v) +{ + double len; + + len = hypot(v.x, v.y); + if(len == 0) + return (Vector){0, 0}; + v.x /= len; + v.y /= len; + return v; +} + +Vector +rotatevec(Vector p, double θ, Vector c) +{ + Vector r; + + p = subvec(p, c); + r.x = p.x*cos(θ) - p.y*sin(θ); + r.y = p.x*sin(θ) + p.y*cos(θ); + r = addvec(r, c); + return r; +} diff --git a/vector3.c b/vector3.c new file mode 100644 index 0000000..61dc819 --- /dev/null +++ b/vector3.c @@ -0,0 +1,65 @@ +#include <u.h> +#include <libc.h> +#include <draw.h> +#include "dat.h" +#include "fns.h" + +Vector3 +Vec3(double x, double y, double z) +{ + return (Vector3){x, y, z}; +} + +Vector3 +Vecquat(Quaternion q) +{ + return (Vector3){q.i, q.j, q.k}; +} + +Vector3 +addvec3(Vector3 v, Vector3 u) +{ + return (Vector3){v.x+u.x, v.y+u.y, v.z+u.z}; +} + +Vector3 +subvec3(Vector3 v, Vector3 u) +{ + return (Vector3){v.x-u.x, v.y-u.y, v.z-u.z}; +} + +Vector3 +mulvec3(Vector3 v, double s) +{ + return (Vector3){v.x*s, v.y*s, v.z*s}; +} + +double +dotvec3(Vector3 v, Vector3 u) +{ + return v.x*u.x + v.y*u.y + v.z*u.z; +} + +Vector3 +crossvec(Vector3 v, Vector3 u) +{ + return (Vector3){ + v.y*u.z - v.z*u.y, + v.z*u.x - v.x*u.z, + v.x*u.y - v.y*u.x + }; +} + +Vector3 +normvec3(Vector3 v) +{ + double len; + + len = hypot3(v.x, v.y, v.z); + if(len == 0) + return (Vector3){0, 0, 0}; + v.x /= len; + v.y /= len; + v.z /= len; + return v; +} |