code: 9ferno

ref: 44ce0097b612a1fefd754065bdf8d9d2e5ef60c8
dir: /appl/wm/collide.b/

View raw version
#
#	initially generated by c2l
#

implement Collide;

include "draw.m";
	draw: Draw;
	Display, Image: import draw;

Collide: module
{
	init: fn(nil: ref Draw->Context, argl: list of string);
};

include "sys.m";
	sys: Sys;
include "tk.m";
	tk: Tk;
	Toplevel: import tk;
include "tkclient.m";
	tkclient: Tkclient;
include "math.m";
	maths: Math;
include "rand.m";
	rand: Rand;
include "daytime.m";
	daytime: Daytime;
include "bufio.m";
include "arg.m";
	arg: Arg;
include "math/polyhedra.m";
	polyhedra: Polyhedra;

init(ctxt: ref Draw->Context, argl: list of string)
{
	sys = load Sys Sys->PATH;
	draw = load Draw Draw->PATH;
	tk = load Tk Tk->PATH;
	tkclient = load Tkclient Tkclient->PATH;
	maths = load Math Math->PATH;
	rand = load Rand Rand->PATH;
	arg = load Arg Arg->PATH;
	daytime = load Daytime Daytime->PATH;
	main(ctxt, argl);
}

π: con Math->Pi;
∞: con real (1<<30);
ε: con 0.001;
√2: con 1.4142135623730950488016887242096980785696718753769486732;

M1: con 1.0;
M2: con 1.0;
E: con 1.0;	# coefficient of restitution/elasticity

COLLIDE, REFLECT: con 1<<iota;

MAXX, MAXY: con 512;

RDisp: ref Draw->Image;
black, white, red: ref Draw->Image;
display: ref Draw->Display;
toplev: ref Toplevel;

Vector: adt{
	x: real;
	y: real;
	z: real;
};

Line: adt{
	a: Vector;
	d: Vector;		# normalized
};

Plane: adt{
	id: int;
	n: Vector;		# normalized
	d: real;
	min: Vector;
	max: Vector;
	far: Vector;
	v: array of Vector;
};

Object: adt{
	id: int;
	poly: ref Polyhedra->Polyhedron;	# if any
	c: ref Draw->Image;		#  colour 
	cb: ref Draw->Image;	#  border colour
	l: ref Line;				#  initial point and direction 
	p: Vector;				#  current position
	rp: Vector;			# position after reflection
	cp: Vector;			# any collision point
	rt: real;				# time to reflection
	ct: real;				# time to collision
	plane: ref Plane;		# reflecting off
	pmask: int;			# plane mask
	obj: cyclic ref Object;	# colliding with
	v: real;				#  speed
	ω: real;				# speed of rotation
	roll: real;				# roll
	pitch: real;			# pitch
	yaw: real;				# yaw
	todo: int;				# work to do
};

planes: list of ref Plane;

V0: con Vector(real 0, real 0, real 0);
VZ: con Vector(0.0, 0.0, 1.0);

far: Vector;

DOCIRCLE: con 1;
POLY, FILLPOLY, CIRCLE, FILLCIRCLE, ELLIPSE, FILLELLIPSE: con iota;

#
#  final object is centred on (0, 0, -objd)
#  viewer is at origin looking along (0 0 -1)
#  
maxx, maxy: int;

SCRW: con 320;	#  screen width
SCRH: con 240;		# screen height

frac := 0.5;	# % of screen for cube
front := 0.5;	# % of cube in front of screen
hpar := 0.0;	# horizontal parallax
fov := -1.0;	# field of view : 0 for parallel projection, -1 for unspecified
objd := 500.0;	# eye to middle of cube
cubd := 100.0;	# half side of cube
icubd: real;	# half side of inner cube
icubd2: real;	# square of above
eyed := 32.0;	# half eye to eye
trkd := 5.0;	# side/diameter of object
trkd2: real;	# square of above
rpy := 0;
roll := 0.0;		# z
pitch := 0.0;	# y
yaw := 0.0;	# x

scrd, objD, scrD: real;	# screen distance
left := 0;		# left or right eye
sx, sy, sz: real;	#  screen scale factors 
sf: real;		#  perspective scale factor 
fbpar: real;	#  -1 for front of cube, 1 for back
vf := 1.0;		# current velocity factor

cmin, cmax: Vector;	# cube extents

#  special transformation matrix without roll, pitch, yaw 
#  this is needed so that spheres can be drawn as circles 
mod0 := array[4] of array of real;

stereo := 0;	# stereopsis

SPHERE, ELLIPSOID, CUBE, POLYHEDRON: con iota;
surr := CUBE;	# surround

poly := 0;		# show polyhedra
flat: int;		# objects in one plane
projx: int;		# strange projection

# ellipse parameters
ef: Vector = (1.0, 0.8, 1.0);
e2: Vector;

# objects
nobjs: int;
objs: array of ref Object;
me: ref Object;

# circle drawing
NC: con 72;
cost, sint: array of real;

# polyhedra
polys: ref Polyhedra->Polyhedron;
npolys: int;
polyh: ref Polyhedra->Polyhedron;

rgba(r: int, g: int, b: int, α: int): ref Image
{
	c := draw->setalpha((r<<24)|(g<<16)|(b<<8), α);
	return display.newimage(((0, 0), (1, 1)), display.image.chans, 1, c);
}

random(a: int, b: int): int
{
	return a+rand->rand(b-a+1);
}

urand(): real
{
	M: con 1000;
	return real random(0, M)/real M;
}

randomr(a: real, b: real): real
{
	return a+urand()*(b-a);
}

randomc(): ref Image
{
	r, g, b: int;

	do{
		r = random(0, 255);
		g = random(0, 255);
		b = random(0, 255);
	}while(r+g+b < 384);
	return rgba(r, g, b, 255);
}

randomv(a: real, b: real): Vector
{
	x := randomr(a, b);
	y := randomr(a, b);
	if(flat)
		return (x, y, (a+b)/2.0);
	return (x, y, randomr(a, b));
}

randomd(): Vector
{
	M: con 1000.0;
	v := randomv(-M, M);
	while(vlen(v) == 0.0)
		v = randomv(-M, M);
	return vnorm(v);
}

randomp(min: real, max: real): Vector
{
	case(surr){
		SPHERE =>
			return vmul(randomd(), max*maths->sqrt(urand()));
		ELLIPSOID =>
			return vmul(randomd(), max*vmin(ef)*maths->sqrt(urand()));
		CUBE =>
			return randomv(min, max);
		* =>
			v := randomv(min, max);
			while(outside3(v, cmin, cmax))
				v = randomv(min, max);
			return v;
	}
}

det(a: real, b: real, c: real, d: real): real
{
	return a*d-b*c;
}

simeq(a: real, b: real, c: real, d: real, e: real, f: real): (real, real)
{
	de := det(a, b, c, d);
	return (det(e, b, f, d)/de, det(a, e, c, f)/de);
}

cksimeq(a: real, b: real, c: real, d: real, e: real, f: real): (int, real, real)
{
	ade := de := det(a, b, c, d);
	if(ade < 0.0)
		ade = -ade;
	if(ade < ε)
		return (0, 0.0, 0.0);
	return (1, det(e, b, f, d)/de, det(a, e, c, f)/de);
}

ostring(o: ref Object): string
{
	return lstring(o.l) + "+" + vstring(o.p) + "+" + string o.v;
}

pstring(p: ref Plane): string
{
	return vstring(p.n) + "=" + string p.d;
}

lstring(l: ref Line): string
{
	return vstring(l.a) + "->" + vstring(l.d);
}

vstring(v: Vector): string
{
	return "(" + string v.x + " " + string v.y + " " + string v.z + ")";
}

vpt(x: real, y: real, z: real): Vector
{
	p: Vector;

	p.x = x;
	p.y = y;
	p.z = z;
	return p;
}

vdot(v1: Vector, v2: Vector): real
{
	return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z;
}

vcross(v1: Vector, v2: Vector): Vector
{
	v: Vector;

	v.x = v1.y*v2.z-v1.z*v2.y;
	v.y = v1.z*v2.x-v1.x*v2.z;
	v.z = v1.x*v2.y-v1.y*v2.x;
	return v;
}

vadd(v1: Vector, v2: Vector): Vector
{
	v: Vector;

	v.x = v1.x+v2.x;
	v.y = v1.y+v2.y;
	v.z = v1.z+v2.z;
	return v;
}

vsub(v1: Vector, v2: Vector): Vector
{
	v: Vector;

	v.x = v1.x-v2.x;
	v.y = v1.y-v2.y;
	v.z = v1.z-v2.z;
	return v;
}

vmul(v1: Vector, s: real): Vector
{
	v: Vector;

	v.x = s*v1.x;
	v.y = s*v1.y;
	v.z = s*v1.z;
	return v;
}

vdiv(v1: Vector, s: real): Vector
{
	v: Vector;

	v.x = v1.x/s;
	v.y = v1.y/s;
	v.z = v1.z/s;
	return v;
}

vlen(v: Vector): real
{
	return maths->sqrt(vdot(v, v));
}

vlen2(v: Vector): (real, real)
{
	d2 := vdot(v, v);
	d := maths->sqrt(d2);
	return (d, d2);
}

vnorm(v: Vector): Vector
{
	d := maths->sqrt(vdot(v, v));
	if(d == 0.0)
		return v;
	return vmul(v, real 1/d);
}

vnorm2(v: Vector): (real, Vector)
{
	d := maths->sqrt(vdot(v, v));
	if(d == 0.0)
		return (0.0, VZ);
	return (d, vmul(v, real 1/d));
}

clip(x: real, d: real): real
{
	if(x < -d)
		x = -d;
	if(x > d)
		x = d;
	return x;
}

vclip(v: Vector, d: real): Vector
{
	c: Vector;

	c.x = clip(v.x, d);
	c.y = clip(v.y, d);
	c.z = clip(v.z, d);
	return c;
}

vout(v1: Vector, v2: Vector): int
{
	v := vsub(v2, v1);
	return v.x < 0.0 || v.y < 0.0 || v.z < 0.0;
}

vmin(v: Vector): real
{
	m := v.x;
	if(v.y < m)
		m = v.y;
	if(v.z < m)
		m = v.z;
	return m;
}

vvmul(v1: Vector, v2: Vector): Vector
{
	v: Vector;

	v.x = v1.x*v2.x;
	v.y = v1.y*v2.y;
	v.z = v1.z*v2.z;
	return v;
}

vvdiv(v1: Vector, v2: Vector): Vector
{
	v: Vector;

	v.x = v1.x/v2.x;
	v.y = v1.y/v2.y;
	v.z = v1.z/v2.z;
	return v;
}

vmuldiv(v1: Vector, v2: Vector, v3: Vector): real
{
	return vdot(vvdiv(v1, v3), v2);
}

newp(id: int, a: real, b: real, c: real, d: real, m: real, v: array of Vector)
{
	p := ref Plane;
	p.id = id;
	p.n = (a, b, c);
	p.d = d;
	m += ε;
	p.min = (-m, -m, -m);
	p.max = (+m, +m, +m);
	p.v = v;
	if(v != nil){
		p.min = (∞, ∞, ∞);
		p.max = (-∞, -∞, -∞);
		for(i := 0; i < len v; i++){
			vtx := v[i];
			if(vtx.x < p.min.x)
				p.min.x = vtx.x;
			if(vtx.y < p.min.y)
				p.min.y = vtx.y;
			if(vtx.z < p.min.z)
				p.min.z = vtx.z;
			if(vtx.x > p.max.x)
				p.max.x = vtx.x;
			if(vtx.y > p.max.y)
				p.max.y = vtx.y;
			if(vtx.z > p.max.z)
				p.max.z = vtx.z;
		}
		(x, y, z) := p.far = vmul(p.max, 2.0);
		if(a != 0.0)
			p.far.x = (d-b*y-c*z)/a;
		else if(b != 0.0)
			p.far.y = (d-c*z-a*x)/b;
		else if(c != 0.0)
			p.far.z = (d-a*x-b*y)/c;
		else
			fatal("null plane");
	}
	planes = p :: planes;
}

pinit()
{
	case(surr){
		SPHERE or
		ELLIPSOID =>
			newp(0, 0.0, 0.0, 1.0, ∞, ∞, nil);
		CUBE =>
			newp(0, 1.0, 0.0, 0.0, -icubd, icubd, nil);
			newp(1, 1.0, 0.0, 0.0, +icubd, icubd, nil);
			newp(2, 0.0, 1.0, 0.0, -icubd, icubd, nil);
			newp(3, 0.0, 1.0, 0.0, +icubd, icubd, nil);
			newp(4, 0.0, 0.0, 1.0, -icubd, icubd, nil);
			newp(5, 0.0, 0.0, 1.0, +icubd, icubd, nil);
		* =>
			p := polyh;
			F := p.F;
			v := p.v;
			f := p.f;
			fv := p.fv;
			d := 0.0;
			for(i := 0; i < F; i++){
				n := vnorm(f[i]);
				dn := vmul(n, cubd-icubd);
				fvi := fv[i];
				m := fvi[0];
				av := array[m] of Vector;
				for(j := 0; j < m; j++){
					av[j] = vtx := vsub(vmul(v[fvi[j+1]], 2.0*cubd), dn);
					d += vdot(n, vtx);
				}
				d /= real m;
				newp(i, n.x, n.y, n.z, d, 0.0, av);
			}
	}
}

inside(v: Vector, vmin: Vector, vmax: Vector): int
{
	return !vout(vmin, v) && !vout(v, vmax);
}

inside2(v: Vector, p: ref Plane): int
{
	h := 0;
	pt := p.far;
	vs := p.v;
	n := len p.v;
	j := n-1;
	for(i := 0; i < n; i++){
		(ok, λ, μ) := cksimeq(vs[j].x-vs[i].x, v.x-pt.x, vs[j].y-vs[i].y, v.y-pt.y, v.x-vs[i].x, v.y-vs[i].y);
		if(!ok)
		(ok, λ, μ) = cksimeq(vs[j].y-vs[i].y, v.y-pt.y, vs[j].z-vs[i].z, v.z-pt.z, v.y-vs[i].y, v.z-vs[i].z);
		if(!ok)
		(ok, λ, μ) = cksimeq(vs[j].z-vs[i].z, v.z-pt.z, vs[j].x-vs[i].x, v.x-pt.x, v.z-vs[i].z, v.x-vs[i].x);
		if(ok && μ >= 0.0 && λ >= 0.0 && λ < 1.0)
			h++;
		j = i;
	}
	return h&1;
}

inside3(v: Vector, lp: list of ref Plane): int
{
	h := 0;
	l := ref Line;
	l.a = v;
	l.d = vnorm(vsub(far, v));
	for( ; lp != nil; lp = tl lp){
		(ok, nil, nil) := intersect(l, hd lp);
		if(ok)
			h++;
	}
	return h&1;
}

# outside of a face
outside2(v: Vector, p: ref Plane): int
{
	if(surr == CUBE)
		return vout(p.min, v) || vout(v, p.max);
	else
		return !inside2(v, p);
}

# outside of a polyhedron
outside3(v: Vector, vmin: Vector, vmax: Vector): int
{
	case(surr){
		SPHERE =>
			return vout(vmin, v) || vout(v, vmax) || vdot(v, v) > icubd2 ;
		ELLIPSOID =>
			return vout(vmin, v) || vout(v, vmax) || vmuldiv(v, v, e2) > 1.0;
		CUBE =>
			return vout(vmin, v) || vout(v, vmax);
		* =>
			return !inside3(v, planes);
	}
}

intersect(l: ref Line, p: ref Plane): (int, real, Vector)
{
	m := vdot(p.n, l.d);
	if(m == real 0)
		return (0, real 0, V0);
	c := vdot(p.n, l.a);
	λ := (p.d-c)/m;
	if(λ < real 0)
		return (0, λ, V0);
	pt := vadd(l.a, vmul(l.d, λ));
	if(outside2(pt, p))
		return (0, λ, pt);
	return (1, λ, pt);
}

reflection(tr: ref Object, lp: list of ref Plane)
{
	ok: int;
	λ: real;

	l := tr.l;
	if(surr == SPHERE){
		(ok, λ) = quadratic(1.0, 2.0*vdot(l.a, l.d), vdot(l.a, l.a)-icubd2);
		if(!ok || λ < 0.0)
			fatal("no sphere intersections");
		tr.rp = vadd(l.a, vmul(l.d, λ));
		tr.plane = hd lp;	# anything
	}
	else if(surr == ELLIPSOID){
		(ok, λ) = quadratic(vmuldiv(l.d, l.d, e2), 2.0*vmuldiv(l.a, l.d, e2), vmuldiv(l.a, l.a, e2)-1.0);
		if(!ok || λ < 0.0)
			fatal("no ellipsoid intersections");
		tr.rp = vadd(l.a, vmul(l.d, λ));
		tr.plane = hd lp;	# anything
	}
	else{
		p: ref Plane;
		pt := V0;
		λ = ∞;
		for( ; lp != nil; lp = tl lp){
			p0 := hd lp;
			if((1<<p0.id)&tr.pmask)
				continue;
			(ok0, λ0, pt0) := intersect(l, p0);
			if(ok0 && λ0 < λ){
				λ = λ0;
				p = p0;
				pt = pt0;
			}
		}
		if(λ == ∞)
			fatal("no intersections");
		tr.rp = pt;
		tr.plane = p;
	}
	if(tr.v == 0.0)
		tr.rt = ∞;
	else
		tr.rt = λ/tr.v;
}

reflect(tr: ref Object)
{
	l := tr.l;
	if(surr == SPHERE)
		n := vdiv(tr.rp, -icubd);
	else if(surr == ELLIPSOID)
		n = vnorm(vdiv(vvdiv(tr.rp, e2), -1.0));
	else
		n = tr.plane.n;
	tr.l.a = tr.rp;
	tr.l.d = vnorm(vsub(l.d, vmul(n, 2.0*vdot(n, l.d))));
}

impact(u2: real): (real, real)
{
	# u1 == 0
	return simeq(M1, M2, -1.0, 1.0, M2*u2, -E*u2);
}

collision(t1: ref Object, t2: ref Object): (int, real, Vector, Vector)
{
	# stop t2
	(v3, f) := vnorm2(vsub(vmul(t1.l.d, t1.v), vmul(t2.l.d, t2.v)));
	if(v3 == 0.0)
		return (0, 0.0, V0, V0);
	ab := vsub(t2.p, t1.p);
	(d, d2) := vlen2(ab);
	cos := vdot(f, ab)/d;
	cos2 := cos*cos;
	if(cos < 0.0 || (disc := trkd2 - d2*(1.0-cos2)) < 0.0)
		return (0, 0.0, V0, V0);
	s := d*cos - maths->sqrt(disc);
	t := s/v3;
	s1 := t1.v*t;
	s2 := t2.v*t;
	cp1 := vadd(t1.p, vmul(t1.l.d, s1));
	if(outside3(cp1, cmin, cmax))
		return (0, 0.0, V0, V0);
	cp2 := vadd(t2.p, vmul(t2.l.d, s2));
	if(outside3(cp2, cmin, cmax))
		return (0, 0.0, V0, V0);
	return (1, t, cp1, cp2);
}

collisions(tr: ref Object)
{
	mincp1, mincp2: Vector;

	n := nobjs;
	t := objs;
	tr0 := tr.obj;
	mint := tr.ct;
	tr1: ref Object;
	for(i := 0; i < n; i++){
		if((tr3 := t[i]) == tr || tr3 == tr0)
			continue;
		(c, tm, cp1, cp2) := collision(tr, tr3);
		if(c && tm < mint && tm < tr3.ct){
			mint = tm;
			tr1 = tr3;
			mincp1 = cp1;
			mincp2 = cp2;
		}
	}
	if(tr1 != nil){
		tr.ct = mint;
		tr1.ct = mint;
		tr.obj = tr1;
		tr2 := tr1.obj;
		tr1.obj  = tr;
		tr.cp = mincp1;
		tr1.cp = mincp2;
		zerot(tr0, COLLIDE, 0);
		zerot(tr2, COLLIDE, 0);
		if(tr0 != nil && tr0 != tr2)
			collisions(tr0);
		if(tr2 != nil)
			collisions(tr2);
	}
}

collide(t1: ref Object, t2: ref Object)
{
	# stop t2
	ov := vmul(t2.l.d, t2.v);
	(v3, f) := vnorm2(vsub(vmul(t1.l.d, t1.v), ov));
	ab := vsub(t2.cp, t1.cp);
	α := vdot(f, ab)/vdot(ab, ab);
	abr := vsub(f, vmul(ab, α));
	(v2, v1) := impact(α*v3);
	t1.l.a = t1.cp;
	t2.l.a = t2.cp;
	dir1 := vadd(vmul(ab, v1), vmul(abr, v3));
	dir2 := vmul(ab, v2);
	# start t2
	(t1.v, t1.l.d) = vnorm2(vadd(dir1, ov));
	(t2.v, t2.l.d) = vnorm2(vadd(dir2, ov));
}

deg2rad(d: real): real
{
	return π*d/180.0;
}

rad2deg(r: real): real
{
	return 180.0*r/π;
}

rp2d(r: real, p: real): Vector
{
	r = deg2rad(r);
	cr := maths->cos(r);
	sr := maths->sin(r);
	p = deg2rad(p);
	cp := maths->cos(p);
	sp := maths->sin(p);
	return (cr*cp, sr*cp, sp);
}

d2rp(v: Vector): (real, real)
{
	r := maths->atan2(v.y, v.x);
	p := maths->asin(v.z);
	return (rad2deg(r), rad2deg(p));
}

collideω(t1: ref Object, t2: ref Object)
{
	d1 := rp2d(t1.roll, t1.pitch);
	d2 := rp2d(t2.roll, t2.pitch);
	oω := vmul(d2, t2.ω);
	(ω3, f) := vnorm2(vsub(vmul(d1, t1.ω), oω));
	ab := vsub(t2.cp, t1.cp);
	α := vdot(f, ab)/vdot(ab, ab);
	abr := vsub(f, vmul(ab, α));
	(ω2, ω1) := impact(α*ω3);
	dir1 := vadd(vmul(ab, ω1), vmul(abr, ω3));
	dir2 := vmul(ab, ω2);
	(t1.ω, d1) = vnorm2(vadd(dir1, oω));
	(t2.ω, d2) = vnorm2(vadd(dir2, oω));
	(t1.roll, t1.pitch) = d2rp(d1);
	(t2.roll, t2.pitch) = d2rp(d2);
}

plane(p1: Vector, p2: Vector, p3: Vector): (Vector, real)
{
	n := vnorm(vcross(vsub(p2, p1), vsub(p3, p1)));
	d := vdot(n, p1);
	return (n, d);
}

#  angle subtended by the eyes at p in minutes 
angle(p: Vector): real
{
	l, r: Vector;

	#  left  eye at (-eyed, 0, 0)
	#  right eye at (+eyed, 0, 0)
	#      
	l = p;
	l.x += eyed;
	r = p;
	r.x -= eyed;
	return real 60*(real 180*maths->acos(vdot(l, r)/(maths->sqrt(vdot(l, l))*maths->sqrt(vdot(r, r))))/π);
}

#  given coordinates relative to centre of cube 
disparity(p: Vector, b: Vector): real
{
	p.z -= objd;
	b.z -= objd;
	return angle(p)-angle(b);
}

#  rotation about any of the axes 
#  rotate(theta, &x, &y, &z) for x-axis
#    rotate(theta, &y, &z, &x) for y-axis
#    rotate(theta, &z, &x, &y) for z-axis
#  
rotate(theta: int, x: real, y: real, z: real): (real, real, real)
{
	a := π*real theta/real 180;
	c := maths->cos(a);
	s := maths->sin(a);
	oy := y;
	oz := z;
	y = c*oy-s*oz;
	z = c*oz+s*oy;
	return (x, y, z);
}

#  solve the quadratic ax^2 + bx + c = 0, returning the larger root
#  * (a > 0)
#  
quadratic(a: real, b: real, c: real): (int, real)
{
	d := b*b-real 4*a*c;
	if(d < real 0)
		return (0, 0.0);	#  no real roots 
	x := (maths->sqrt(d)-b)/(real 2*a);
	return (1, x);
}

#  calculate the values of objD, scrD given the required parallax 
dopar()
{
	a := real 1;
	b, c: real;
	f := real 2*front-real 1;
	x: real;
	s: int;
	w := sx*real SCRW;
	ok: int;

	if(hpar == 0.0){	#  natural parallax 
		objD = objd;
		scrD = scrd;
		return;
	}
	if(fbpar < f)
		s = -1;
	else
		s = 1;
	if(fbpar == f)
		fatal("parallax value is zero at screen distance");
	b = (fbpar+f)*cubd-(fbpar-f)*w*eyed*real s*frac/hpar;
	c = fbpar*f*cubd*cubd;
	(ok, x) = quadratic(a, b, c);
	if(ok){
		objD = x;
		scrD = x+f*cubd;
		if(objD > real 0 && scrD > real 0)
			return;
	}
	fatal("unachievable parallax value");
}

#  update graphics parameters 
update(init: int)
{
	if(fov != real 0){
		if(objd == real 0)
			fov = 180.0;
		else
			fov = real 2*(real 180*maths->atan(cubd/(frac*objd))/π);
	}
	scrd = objd+(real 2*front-real 1)*cubd;
	if(init){
		if(objd < ε)
			objd = ε;
		if(fov != real 0)
			sf = real (1<<2)*cubd/(objd*frac);
		else
			sf = cubd/frac;
	}
	# dopar();
	domats();
}

fovtodist()
{
	if(fov != real 0)
		objd = cubd/(frac*maths->tan(π*(fov/real 2)/real 180));
}

getpolys()
{
	(n, p, b) := polyhedra->scanpolyhedra("/lib/polyhedra");
	polyhedra->getpolyhedra(p, b);
	polys = p;
	npolys = n;
	do{
		for(i := 0; i < p.V; i++)
			p.v[i] = vmul(p.v[i], 0.5);
		for(i = 0; i < p.F; i++)
			p.f[i] = vmul(p.f[i], 0.5);
		p = p.nxt;
	}while(p != polys);
}

randpoly(p: ref Polyhedra->Polyhedron, n: int): ref Polyhedra->Polyhedron
{
	r := random(0, n-1);
	for( ; --r >= 0; p = p.nxt)
		;
	return p;
}

drawpoly(p: ref Polyhedra->Polyhedron, typex: int)
{
	# V := p.V;
	F := p.F;
	v := p.v;
	# f := p.f;
	fv := p.fv;
	for(i := 0; i < F; i++){
		fvi := fv[i];
		n := fvi[0];
		m_begin(typex, n);
		for(j := 0; j < n; j++){
			vtx := v[fvi[j+1]];
			m_vertex(vtx.x, vtx.y, vtx.z);
		}
		m_end();
	}
}

#  objects with unit sides/diameter 
H: con 0.5;

square(typex: int)
{
	m_begin(typex, 4);
	m_vertex(-H, -H, 0.0);
	m_vertex(-H, +H, 0.0);
	m_vertex(+H, +H, 0.0);
	m_vertex(+H, -H, 0.0);
	m_end();
}

diamond(typex: int)
{
	m_pushmatrix();
	m_rotatez(45.0);
	square(typex);
	m_popmatrix();
}

circleinit()
{
	i: int;
	a := 0.0;
	cost = array[NC] of real;
	sint = array[NC] of real;
	for(i = 0; i < NC; i++){
		cost[i] = H*maths->cos(a);
		sint[i] = H*maths->sin(a);
		a += (2.0*π)/real NC;
	}
}

circle(typex: int)
{
	i: int;

	if(DOCIRCLE){
		m_begin(typex, 2);
		m_circle(0.0, 0.0, 0.0, 0.5);
		m_end();
		return;
	}
	else{
		m_begin(typex, NC);
		for(i = 0; i < NC; i++)
			m_vertex(cost[i], sint[i], 0.0);
		m_end();
	}
}

ellipse(typex: int)
{
	m_begin(typex, 4);
	m_ellipse(0.0, 0.0, 0.0, vmul(ef, 0.5));
	m_end();
}

hexahedron(typex: int)
{
	i, j, k: int;
	V := array[8] of {
		array[3] of {
			-H, -H, -H,
		},
		array[3] of {
			-H, -H, +H,
		},
		array[3] of {
			-H, +H, -H,
		},
		array[3] of {
			-H, +H, +H,
		},
		array[3] of {
			+H, -H, -H,
		},
		array[3] of {
			+H, -H, +H,
		},
		array[3] of {
			+H, +H, -H,
		},
		array[3] of {
			+H, +H, +H,
		},
	};
	F := array[6] of {
		array[4] of {
			0, 4, 6, 2,
		},
		array[4] of {
			0, 4, 5, 1,
		},
		array[4] of {
			0, 1, 3, 2,
		},
		array[4] of {
			1, 5, 7, 3,
		},
		array[4] of {
			2, 6, 7, 3,
		},
		array[4] of {
			4, 5, 7, 6,
		},
	};

	for(i = 0; i < 6; i++){
		m_begin(typex, 4);
		for(j = 0; j < 4; j++){
			k = F[i][j];
			m_vertex(V[k][0], V[k][1], V[k][2]);
		}
		m_end();
	}
}

zerot(tr: ref Object, zero: int, note: int)
{
	if(tr != nil){
		if(zero&REFLECT){
			tr.rt = ∞;
			tr.plane = nil;
		}
		if(zero&COLLIDE){
			tr.ct = ∞;
			tr.obj  = nil;
		}
		if(note)
			tr.todo = zero;
	}
}

newobj(t: array of ref Object, n: int, vel: int, velf: real): ref Object
{
	tr: ref Object;
	p1: Vector;
	again: int;

	d := icubd-1.0;
	cnt := 1024;
	do{
		p1 = randomp(-d, d);
		again = 0;
		for(i := 0; i < n; i++){
			(nil, d2) := vlen2(vsub(t[i].p, p1));
			if(d2 <= trkd2){
				again = 1;
				break;
			}
		}
		cnt--;
	}while(again && cnt > 0);
	if(again)
		return nil;
	# p2 := randomp(-d, d);
	p21 := randomd();
	tr = ref Object;
	tr.id = n;
	tr.poly = nil;
	if(poly){
		if(n == 0)
			tr.poly = randpoly(polys, npolys);
		else
			tr.poly = t[0].poly;
	}
	tr.c = randomc();
	tr.cb = tr.c;	# randomc();
	if(vel)
		tr.v = vf*velf*randomr(0.5, 2.0);
	else
		tr.v = 0.0;
	tr.ω = vf*randomr(1.0, 10.0);
	tr.roll = randomr(0.0, 360.0);
	tr.pitch = randomr(0.0, 360.0);
	tr.yaw = randomr(0.0, 360.0);
	tr.l = ref Line(p1, vnorm(p21));
	tr.p = p1;
	tr.todo = 0;
	zerot(tr, REFLECT|COLLIDE, 0);
	tr.pmask = 0;
	reflection(tr, planes);
	return tr;
}

objinit(m: int, v: int)
{
	velf := real m/real v;
	p := nobjs;
	n := p+m;
	v += p;
	t := array[n] of ref Object;
	for(i := 0; i < p; i++)
		t[i] = objs[i];
	for(i = p; i < n; i++){
		t[i] = newobj(t, i, i < v, velf);
		if(t[i] == nil)
			return;
	}
	sort(t, n);
	nobjs = n;
	objs = t;
	for(i = p; i < n; i++)
		collisions(t[i]);
}

zobj: Object;

newo(n: int, p: Vector, c: ref Draw->Image): ref Object
{
	o := ref Object;
	*o = zobj;
	o.id = n;
	o.c = o.cb = c;
	o.l = ref Line(p, VZ);
	o.p = p;
	zerot(o, REFLECT|COLLIDE, 0);
	reflection(o, planes);
	return o;
}

objinits(nil: int, nil: int)
{
	n := 16;
	t := array[n] of ref Object;
	r := trkd/2.0;
	i := 0;
	yc := 0.0;
	for(y := 0; y < 5; y++){
		xc := -real y*r;
		for(x := 0; x <= y; x++){
			t[i] = newo(i, (xc, yc, 0.0), red);
			xc += trkd;
			i++;
		}
		yc += trkd;
	}
	t[i] = newo(i, (0.0, -50.0, 0.0), white);
	t[i].l.d = (0.0, 1.0, 0.0);
	t[i].v = 1.0;
	sort(t, n);
	nobjs = n;
	objs = t;
	for(i = 0; i < n; i++)
		collisions(t[i]);
}

initme(): ref Object
{
	t := newobj(nil, 0, 1, 1.0);
	t.roll = t.pitch = t.yaw = 0.0;
	t.v = t.ω = 0.0;
	t.l.a = (0.0, 0.0, objd);	# origin when translated
	t.l.d = (0.0, 0.0, -1.0);
	t.p = t.l.a;
	zerot(t, REFLECT|COLLIDE, 0);
	return t;
}

retime(s: real)
{
	r := 1.0/s;
	n := nobjs;
	t := objs;
	for(i := 0; i < n; i++){
		tr := t[i];
		tr.v *= s;
		tr.ω *= s;
		tr.rt *= r;
		tr.ct *= r;
	}
	me.v *= s;
	me.ω *= s;
	me.rt *= r;
	me.ct *= r;
}

drawobjs()
{
	tr: ref Object;
	p: Vector;

	n := nobjs;
	t := objs;

	for(i := 0; i < n; i++){
		tr = t[i];
		tr.pmask = 0;
		p = tr.p;
		m_pushmatrix();
		if(rpy && tr.poly ==  nil){
			m_loadmatrix(mod0);
			(p.x, p.y, p.z) = rotate(int yaw, p.x, p.y, p.z);
			(p.y, p.z, p.x) = rotate(int pitch, p.y, p.z, p.x);
			(p.z, p.x, p.y) = rotate(int roll, p.z, p.x, p.y);
		}
		m_translate(p.x, p.y, p.z);
		m_scale(trkd, trkd, trkd);
		if(tr.poly != nil){
			m_rotatez(tr.roll);
			m_rotatey(tr.pitch);
			m_rotatex(tr.yaw);
			tr.yaw += tr.ω;
		}
		m_matmul();
		if(tr.cb != tr.c){
			m_colour(tr.cb);
			if(tr.poly != nil)
				drawpoly(tr.poly, POLY);
			else if(DOCIRCLE)
				circle(CIRCLE);
			else
				circle(POLY);
		}
		m_colour(tr.c);
		if(tr.poly != nil)
			drawpoly(tr.poly, FILLPOLY);
		else if(DOCIRCLE)
			circle(FILLCIRCLE);
		else
			circle(FILLPOLY);
		m_popmatrix();
	}

	tm := 1.0;
	do{
		δt := ∞;

		for(i = 0; i < n; i++){
			tr = t[i];
			if(tr.rt < δt)
				δt = tr.rt;
			if(tr.ct < δt)
				δt = tr.ct;
		}

		if(δt > tm)
			δt = tm;

		for(i = 0; i < n; i++){
			tr = t[i];
			if(tr.rt == δt){
				tr1 := tr.obj;
				reflect(tr);
				tr.p = tr.rp;
				if(δt > 0.0)
					tr.pmask = (1<<tr.plane.id);
				else
					tr.pmask |= (1<<tr.plane.id);
				zerot(tr, REFLECT|COLLIDE, 1);
				zerot(tr1, COLLIDE, 1);
			}
			else if(tr.ct == δt){
				tr1 := tr.obj ;
				collide(tr, tr1);
				if(0 && poly)
					collideω(tr, tr1);
				tr.p = tr.cp;
				tr1.p = tr1.cp;
				tr.pmask = tr1.pmask = 0;
				zerot(tr, REFLECT|COLLIDE, 1);
				zerot(tr1, REFLECT|COLLIDE, 1);
			}
			else if(tr.todo != (REFLECT|COLLIDE)){
				tr.p = vclip(vadd(tr.p, vmul(tr.l.d, tr.v*δt)), icubd);
				tr.rt -= δt;
				tr.ct -= δt;
			}
		}

		for(i = 0; i < n; i++){
			tr = t[i];
			if(tr.todo){
				if(tr.todo&REFLECT)
					reflection(tr, planes);
				if(tr.todo&COLLIDE)
					collisions(tr);
				tr.todo = 0;
			}
		}

		tm -= δt;

	}while(tm > 0.0);

	sort(t, n);
}

drawscene()
{
	m_pushmatrix();
	m_scale(real 2*cubd, real 2*cubd, real 2*cubd);
	m_colour(white);
	m_matmul();
	case(surr){
		SPHERE =>
			if(DOCIRCLE)
				circle(CIRCLE);
			else
				circle(POLY);
		ELLIPSOID =>
			ellipse(ELLIPSE);
		CUBE =>
			if(flat)
				square(POLY);
			else
				hexahedron(POLY);
		* =>
			drawpoly(polyh, POLY);
	}
	m_popmatrix();
	drawobjs();
}

#  ensure parallax doesn't alter between images 
adjpar(x: array of real, y: array of real, z: array of real)
{
	zed, incr: real;

	y = nil;
	if(z[0] < real 0)
		zed = -z[0];
	else
		zed = z[0];
	incr = eyed*zed*(real 1-scrD/(zed+objD-objd))/scrd;
	if(!stereo || fov == real 0)
		return;
	if(left)
		x[0] -= incr;
	else
		x[0] += incr;
}

view()
{
	m_mode(PROJ);
	m_loadidentity();
	m_scale(sx, sy, sz);
	if(fov != real 0)
		m_frustum(sf, real (1<<2), real (1<<20));
	else
		m_ortho(sf, real (1<<2), real (1<<20));
	# m_print();
	m_mode(MODEL);
}

model(rot: int)
{
	m_loadidentity();
	m_translate(0.0, 0.0, -objd);
	if(rpy && rot){
		m_rotatez(roll);
		m_rotatey(pitch);
		m_rotatex(yaw);
	}
}

#  store projection and modelview matrices 
domats()
{
	model(0);
	m_storematrix(mod0);
	model(1);
	view();
}

scale()
{
	if(maxx > maxy)
		sx = real maxy/real maxx;
	else
		sx = 1.0;
	if(maxy > maxx)
		sy = real maxx/real maxy;
	else
		sy = 1.0;
	sz = 1.0;
}

rescale(w: int, h: int)
{
	maxx = w;
	maxy = h;
	scale();
	m_viewport(0, 0, maxx, maxy);
}

grinit()
{
	for(i := 0; i < 4; i++)
		mod0[i] = array[4] of real;
	far = (2.0*cubd, 2.0*cubd, 2.0*cubd);
	icubd = cubd-trkd/2.0;
	icubd2 = icubd*icubd;
	trkd2 = trkd*trkd;
	cmin = vpt(-icubd, -icubd, -icubd);
	cmax = vpt(+icubd, +icubd, +icubd);
	maxx = MAXX;
	maxy = MAXY;
	e2 = vmul(vvmul(ef, ef), icubd2);

	m_init();
	pinit();
	circleinit();

	m_viewport(0, 0, maxx, maxy);

	scale();
	if(fov > real 0)
		fovtodist();
	update(1);
}

newimage(win: ref Toplevel, init: int)
{
	maxx = int cmd(win, ".p cget -actwidth");
	maxy = int cmd(win, ".p cget -actheight");
	RDisp = display.newimage(((0, 0), (maxx, maxy)), win.image.chans, 0, Draw->Black);
	tk->putimage(win, ".p", RDisp, nil);
	RDisp.draw(RDisp.r, black, nil, (0, 0));
	reveal();
	rescale(maxx, maxy);
	update(init);
}

reveal()
{
	cmd(toplev, ".p dirty; update");
}

usage()
{
	sys->fprint(sys->fildes(2), "usage: collide [-cse] [-f] [-op] [-b num] [-v num]\n");
	exit;
}

main(ctxt: ref Draw->Context, args: list of string)
{
	rand->init(daytime->now());
	daytime = nil;

	b := v := random(16, 32);

	arg->init(args);
	while((o := arg->opt()) != 0){
		case(o){
			* =>
				usage();
			's' =>
				surr = SPHERE;
			'e' =>
				surr = ELLIPSOID;
			'c' =>
				surr = CUBE;
			'o' =>
				fov = 0.0;
			'p' =>
				fov = -1.0;
			'f' =>
				flat = 1;
			'b' =>
				b = v = int arg->arg();
			'v' =>
				v = int arg->arg();
		}
	}
	if(arg->argv() != nil)
		usage();

	if(b <= 0)
		b = 1;
	if(b > 100)
		b = 100;
	if(v <= 0)
		v = 1;
	if(v > b)
		v = b;

	if(poly || surr == POLYHEDRON){
		polyhedra = load Polyhedra Polyhedra->PATH;
		getpolys();
	}
	if(surr == POLYHEDRON)
		polyh = randpoly(polys, npolys);

	grinit();
	
	tkclient->init();
	(win, wmch) := tkclient->toplevel(ctxt, "", "Collide", Tkclient->Resize | Tkclient->Hide);
	toplev = win;
	sys->pctl(Sys->NEWPGRP, nil);
	cmdch := chan of string;
	tk->namechan(win, cmdch, "cmd");
	for(i := 0; i < len winconfig; i++)
		cmd(win, winconfig[i]);
	cmd(win, "update");

	tkclient->onscreen(win, nil);
	tkclient->startinput(win, "kbd"::"ptr"::nil);

	display = win.image.display;
	newimage(win, 1);

	black = display.color(Draw->Black);
	white = display.color(Draw->White);
	red = display.color(Draw->Red);

	objinit(b, v);
	me = initme();

	pid := -1;
	sync := chan of int;
	cmdc := chan of int;
	spawn animate(sync, cmdc);
	pid = <- sync;

	for(;;){
		alt{
			c := <-win.ctxt.kbd =>
				tk->keyboard(win, c);
			c := <-win.ctxt.ptr =>
				tk->pointer(win, *c);
			c := <-win.ctxt.ctl or
			c = <-win.wreq =>
				tkclient->wmctl(win, c);
			c := <- wmch =>
				case c{
					"exit" =>
						if(pid != -1)
							kill(pid);
						exit;
					* =>
						sync <-= 0;
						tkclient->wmctl(win, c);
						if(c[0] == '!')
							newimage(win, 0);
						sync <-= 1;
				}
			c := <- cmdch =>
				case c{
					"stop" =>
						cmdc <-= 's';
					"zoomin" =>
						cmdc <-= 'z';
					"zoomout" =>
						cmdc <-= 'o';
					"slow" =>
						cmdc <-= '-';
					"fast" =>
						cmdc <-= '+';
					"objs" =>
						sync <-= 0;
						b >>= 1;
						if(b == 0)
							b = 1;
						objinit(b, b);
						sync <-= 1;
				}
		}
	}
}

sign(r: real): real
{
	if(r < 0.0)
		return -1.0;
	return 1.0;
}

abs(r: real): real
{
	if(r < 0.0)
		return -r;
	return r;
}

animate(sync: chan of int, cmd: chan of int)
{
	zd := objd/250.0;
	δ := θ := 0.0;
	f := 8;

	sync <- = sys->pctl(0, nil);
	for(;;){
		σ := 1.0;
		alt{
			<- sync =>
				<- sync;
			c := <- cmd =>
				case(c){
					's' =>
						δ = θ = 0.0;
					'z' =>
						δ = zd;
					'o' =>
						δ = -zd;
					'r' =>
						θ = 1.0;
					'+' =>
						σ = 1.25;
						f++;
						if(f > 16){
							f--;
							σ = 1.0;
						}
						else
							vf *= σ;
					'-' =>
						σ = 0.8;
						f--;
						if(f < 0){
							f++;
							σ = 1.0;
						}
						else
							vf *= σ;	
				}
			* =>
				sys->sleep(0);
		}

		RDisp.draw(RDisp.r, black, nil, (0, 0));
		drawscene();
		reveal();

		if(δ != 0.0 || θ != 0.0){
			objd -= δ;
			me.l.a.z -= δ;
			if(θ != 0.0){
				roll += θ;
				pitch += θ;
				yaw += θ;
				rpy = 1;
			}
			update(projx);
		}
		if(σ != 1.0)
			retime(σ);
	}
}

# usually almost sorted
sort(ts: array of ref Object, n: int)
{
	done: int;
	t: ref Object;
	q, p: int;

	q = n;
	do{
		done = 1;
		q--;
		for(p = 0; p < q; p++){
			if(ts[p].p.z > ts[p+1].p.z){
				t = ts[p];
				ts[p] = ts[p+1];
				ts[p+1] = t;
				done = 0;
			}
		}
	}while(!done);
}

kill(pid: int): int
{
	fd := sys->open("#p/"+string pid+"/ctl", Sys->OWRITE);
	if(fd == nil)
		return -1;
	if(sys->write(fd, array of byte "kill", 4) != 4)
		return -1;
	return 0;
}

fatal(e: string)
{
	sys->fprint(sys->fildes(2), "%s\n", e);
	raise "fatal";
}

cmd(top: ref Toplevel, s: string): string
{
	e := tk->cmd(top, s);
	if (e != nil && e[0] == '!')
		fatal(sys->sprint("tk error on '%s': %s", s, e));
	return e;
}

winconfig := array[] of {
	"frame .f",
	"button .f.zoomin -text {zoomin} -command {send cmd zoomin}",
	"button .f.zoomout -text {zoomout} -command {send cmd zoomout}",
	"button .f.stop -text {stop} -command {send cmd stop}",
	"pack .f.zoomin -side left",
	"pack .f.zoomout -side right",
	"pack .f.stop -side top",

	"frame .f2",
	"button .f2.slow -text {slow} -command {send cmd slow}",
	"button .f2.fast -text {fast} -command {send cmd fast}",
	"button .f2.objs -text {objects} -command {send cmd objs}",
	"pack .f2.slow -side left",
	"pack .f2.fast -side right",
	"pack .f2.objs -side top",

	"panel .p -width " + string MAXX + " -height " + string MAXY,

	"pack .f -side top -fill x",
	"pack .f2 -side top -fill x",
	"pack .p -side bottom -fill both -expand 1",

	"pack propagate . 0",
	"update"
};

############################################################

# gl.b

#
#	initially generated by c2l
#

MODEL, PROJ: con iota;

Matrix: type array of array of real;

Mstate: adt{
	matl: list of Matrix;
	modl: list of Matrix;
	prjl: list of Matrix;
	mull: Matrix;
	freel: list of Matrix;
	vk: int;
	vr: int;
	vrr: int;
	vc: ref Draw->Image;
	ap: array of Draw->Point;
	apn: int;
	mx, cx, my, cy: real;
	ignore: int;
};

ms: Mstate;

m_new(): Matrix
{
	if(ms.freel != nil){
		m := hd ms.freel;
		ms.freel = tl ms.freel;
		return m;
	}
	m := array[4] of array of real;
	for(i := 0; i < 4; i++)
		m[i] = array[4] of real;
	return m;
}

m_free(m: Matrix)
{
	ms.freel = m :: ms.freel;
}

m_init()
{
	ms.modl = m_new() :: nil;
	ms.prjl = m_new() :: nil;
	ms.matl = ms.modl;
	ms.mull = m_new();
	ms.vk = 0;
	ms.apn = 0;
	ms.mx = ms.cx = ms.my = ms.cy = 0.0;
	ms.ignore = 0;
}

m_print()
{
	m := hd ms.matl;

	for(i := 0; i < 4; i++){
		for(j := 0; j < 4; j++)
			sys->print("%f	", m[i][j]);
		sys->print("\n");
	}
	sys->print("\n");
}

m_mode(m: int)
{
	if(m == PROJ)
		ms.matl = ms.prjl;
	else
		ms.matl = ms.modl;
}

m_pushmatrix()
{
	if(ms.matl == ms.modl){
		ms.modl = m_new() :: ms.modl;
		ms.matl = ms.modl;
	}
	else{
		ms.prjl = m_new() :: ms.prjl;
		ms.matl = ms.prjl;
	}
	s := hd tl ms.matl;
	d := hd ms.matl;
	for(i := 0; i < 4; i++)
		for(j := 0; j < 4; j++)
			d[i][j] = s[i][j];
}

m_popmatrix()
{
	if(ms.matl == ms.modl){
		m_free(hd ms.modl);
		ms.modl = tl ms.modl;
		ms.matl = ms.modl;
	}
	else{
		m_free(hd ms.prjl);
		ms.prjl = tl ms.prjl;
		ms.matl = ms.prjl;
	}
}

m_loadidentity()
{
	i, j: int;
	m := hd ms.matl;

	for(i = 0; i < 4; i++){
		for(j = 0; j < 4; j++)
			m[i][j] = real 0;
		m[i][i] = real 1;
	}	
}

m_translate(x: real, y: real, z: real)
{
	i: int;
	m := hd ms.matl;

	for(i = 0; i < 4; i++)
		m[i][3] = x*m[i][0]+y*m[i][1]+z*m[i][2]+m[i][3];
}

m_scale(x: real, y: real, z: real)
{
	i: int;
	m := hd ms.matl;

	for(i = 0; i < 4; i++){
		m[i][0] *= x;
		m[i][1] *= y;
		m[i][2] *= z;
	}
}

#  rotate about x, y or z axes 
rot(deg: real, j: int, k: int)
{
	i: int;
	m := hd ms.matl;
	rad := Math->Pi*deg/real 180;
	s := maths->sin(rad);
	c := maths->cos(rad);
	a, b: real;

	for(i = 0; i < 4; i++){
		a = m[i][j];
		b = m[i][k];
		m[i][j] = c*a+s*b;
		m[i][k] = c*b-s*a;
	}
}

m_rotatex(a: real)
{
	rot(a, 1, 2);
}

m_rotatey(a: real)
{
	rot(a, 2, 0);
}

m_rotatez(a: real)
{
	rot(a, 0, 1);
}

# (l m n) normalized
m_rotate(deg: real, l: real, m: real, n:real)
{
	i: int;
	mx := hd ms.matl;
	rad := Math->Pi*deg/real 180;
	s := maths->sin(rad);
	c := maths->cos(rad);
	f := 1.0-c;
	m0, m1, m2: real;

	for(i = 0; i < 4; i++){
		m0 = mx[i][0];
		m1 = mx[i][1];
		m2 = mx[i][2];
		mx[i][0] = m0*(l*l*f+c)+m1*(l*m*f+n*s)+m2*(l*n*f-m*s);
		mx[i][1] = m0*(l*m*f-n*s)+m1*(m*m*f+c)+m2*(m*n*f+l*s);
		mx[i][2] = m0*(l*n*f+m*s)+m1*(m*n*f-l*s)+m2*(n*n*f+c);
	}
}

#  Frustum(-l, l, -l, l, n, f) 
m_frustum(l: real, n: real, f: real)
{
	i: int;
	m := hd ms.matl;
	r := n/l;
	a, b: real;

	f = ∞;
	for(i = 0; i < 4; i++){
		a = m[i][2];
		b = m[i][3];
		m[i][0] *= r;
		m[i][1] *= r;
		m[i][2] = a+b;
		m[i][3] = 0.0;
		# m[i][2] = -(a*(f+n)/(f-n)+b);
		# m[i][3] = real -2*f*n*a/(f-n);
	}
}

#  Ortho(-l, l, -l, l, n, f) 
m_ortho(l: real, n: real, f: real)
{
	i: int;
	m := hd ms.matl;
	r := real 1/l;
	# a: real;

	n = 0.0;
	f = ∞;
	for(i = 0; i < 4; i++){
		# a = m[i][2];
		m[i][0] *= r;
		m[i][1] *= r;
		# m[i][2] *= real -2/(f-n);
		# m[i][3] -= a*(f+n)/(f-n);
	}
}

m_loadmatrix(u: array of array of real)
{
	m := hd ms.matl;

	for(i := 0; i < 4; i++)
		for(j := 0; j < 4; j++)
			m[i][j] = u[i][j];
}

m_storematrix(u: array of array of real)
{
	m := hd ms.matl;

	for(i := 0; i < 4; i++)
		for(j := 0; j < 4; j++)
			u[i][j] = m[i][j];
}

m_matmul()
{
	m, p, r: Matrix;

	m = hd ms.modl;
	p = hd ms.prjl;
	r = ms.mull;
	for(i := 0; i < 4; i++){
		pr := p[i];
		rr := r[i];
		for(j := 0; j < 4; j++)
			rr[j] = pr[0]*m[0][j]+pr[1]*m[1][j]+pr[2]*m[2][j]+pr[3]*m[3][j];
	}
}

m_vertexo(x: real, y: real, z: real)
{
	m: Matrix;
	mr: array of real;
	w, x1, y1, z1: real;

	m = hd ms.modl;
	mr = m[0]; x1 = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	mr = m[1]; y1 = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	mr = m[2]; z1 = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	mr = m[3]; w = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	if(w != real 1){
		x1 /= w;
		y1 /= w;
		z1 /= w;
	}
	if(z1 >= 0.0){
		ms.ignore = 1;
		return;
	}
	m = hd ms.prjl;
	mr = m[0]; x = x1*mr[0]+y1*mr[1]+z1*mr[2]+mr[3];
	mr = m[1]; y = x1*mr[0]+y1*mr[1]+z1*mr[2]+mr[3];
	mr = m[2]; z = x1*mr[0]+y1*mr[1]+z1*mr[2]+mr[3];
	mr = m[3]; w = x1*mr[0]+y1*mr[1]+z1*mr[2]+mr[3];
	if(w == real 0){
		ms.ignore = 1;
		return;
	}
	if(w != real 1){
		x /= w;
		y /= w;
		# z /= w;
	}
	ms.ap[ms.apn++] = (int (ms.mx*x+ms.cx), int (ms.my*y+ms.cy));
}

m_vertex(x: real, y: real, z: real): (real, real)
{
	m: Matrix;
	mr: array of real;
	w, x1, y1, z1: real;

	m = ms.mull;
	mr = m[0]; x1 = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	mr = m[1]; y1 = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	mr = m[2]; z1 = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	mr = m[3]; w = x*mr[0]+y*mr[1]+z*mr[2]+mr[3];
	if(w == real 0){
		ms.ignore = 1;
		return (x1, y1);
	}
	if(w != real 1){
		x1 /= w;
		y1 /= w;
		# z1 /= w;
	}
	if(z1 >= 0.0){
		ms.ignore = 1;
		return (x1, y1);
	}
	ms.ap[ms.apn++] = (int (ms.mx*x1+ms.cx), int (ms.my*y1+ms.cy));
	return (x1, y1);
}

m_circle(x: real, y: real, z: real, r: real)
{
	(d, nil) := m_vertex(x, y, z);
	(e, nil) := m_vertex(x+r, y, z);
	d -= e;
	if(d < 0.0)
		d = -d;
	ms.vr = int (ms.mx*d);
}

m_ellipse(x: real, y: real, z: real, v: Vector)
{
	m_circle(x, y, z, v.x);
	(nil, d) := m_vertex(x, y, z);
	(nil, e) := m_vertex(x, y+v.y, z);
	d -= e;
	if(d < 0.0)
		d = -d;
	ms.vrr = int (ms.my*d);
}

m_begin(k: int, n: int)
{
	ms.ignore = 0;
	ms.vk = k;
	ms.ap = array[n+1] of Draw->Point;
	ms.apn = 0;
}

m_end()
{
	if(ms.ignore)
		return;
	case(ms.vk){
		CIRCLE =>
			RDisp.ellipse(ms.ap[0], ms.vr, ms.vr, 0, ms.vc, (0, 0));
		FILLCIRCLE =>
			RDisp.fillellipse(ms.ap[0], ms.vr, ms.vr, ms.vc, (0, 0));
		ELLIPSE =>
			RDisp.ellipse(ms.ap[0], ms.vr, ms.vrr, 0, ms.vc, (0, 0));
		FILLELLIPSE =>
			RDisp.fillellipse(ms.ap[0], ms.vr, ms.vrr, ms.vc, (0, 0));
		POLY =>
			ms.ap[len ms.ap-1] = ms.ap[0];
			RDisp.poly(ms.ap, Draw->Endsquare, Draw->Endsquare, 0, ms.vc, (0, 0));
		FILLPOLY =>
			ms.ap[len ms.ap-1] = ms.ap[0];
			RDisp.fillpoly(ms.ap, ~0, ms.vc, (0, 0));
	}
}

m_colour(i: ref Draw->Image)
{
	ms.vc = i;
}

m_viewport(x1: int, y1: int, x2: int, y2: int)
{
	ms.mx = real (x2-x1)/2.0;
	ms.cx = real (x2+x1)/2.0;
	ms.my = real (y2-y1)/2.0;
	ms.cy = real (y2+y1)/2.0;
}

# sys->print("%d %f (%f %f %f) %s\n", ok, λ, 1.0, 2.0*vdot(l.a, l.d), vdot(l.a, l.a)-icubd2, lstring(l));

# sys->print("%d %f (%f %f %f) %s\n", ok, λ, vmuldiv(l.d, l.d, e2), 2.0*vmuldiv(l.a, l.d, e2), vmuldiv(l.a, l.a, e2)-1.0, lstring(l));

# 			for(lp = lp0 ; lp != nil; lp = tl lp){
# 				p := hd lp;
# 				(ok, λ, pt) := intersect(l, p);
# 				sys->print("%d	%x	%d	%f	%s	%s	%s\n", p.id, tr.pmask, ok, λ, vstring(pt), pstring(p), lstring(l));
# 			}