1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/prototype/Makefile	Thu Jun 28 06:05:50 2012 +0300
     1.3 @@ -0,0 +1,30 @@
     1.4 +csrc = $(wildcard src/*.c) $(wildcard vmath/*.c)
     1.5 +ccsrc = $(wildcard src/*.cc) $(wildcard vmath/*.cc)
     1.6 +obj = $(csrc:.c=.o) $(ccsrc:.cc=.o)
     1.7 +dep = $(obj:.o=.d)
     1.8 +bin = proto
     1.9 +
    1.10 +CFLAGS = -pedantic -Wall -g -Ivmath
    1.11 +CXXFLAGS = $(CFLAGS)
    1.12 +LDFLAGS = $(libgl) -lm
    1.13 +
    1.14 +ifeq ($(shell uname -s), Darwin)
    1.15 +	libgl = -framework OpenGL -framework GLUT -lglew
    1.16 +else
    1.17 +	libgl = -lGL -lGLU -lglut -lGLEW
    1.18 +endif
    1.19 +
    1.20 +$(bin): $(obj)
    1.21 +	$(CXX) -o $@ $(obj) $(LDFLAGS)
    1.22 +
    1.23 +-include $(dep)
    1.24 +
    1.25 +%.d: %.c
    1.26 +	@$(CPP) $(CFLAGS) $< -MM -MT $(@:.d=.o) >$@
    1.27 +
    1.28 +%.d: %.cc
    1.29 +	@$(CPP) $(CXXFLAGS) $< -MM -MT $(@:.d=.o) >$@
    1.30 +
    1.31 +.PHONY: clean
    1.32 +clean:
    1.33 +	rm -f $(obj) $(bin) $(dep)

     2.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     2.2 +++ b/prototype/src/camera.cc	Thu Jun 28 06:05:50 2012 +0300
     2.3 @@ -0,0 +1,177 @@
     2.4 +#include "opengl.h"
     2.5 +#include "camera.h"
     2.6 +
     2.7 +Camera::Camera()
     2.8 +{
     2.9 +	inval_cache();
    2.10 +}
    2.11 +
    2.12 +Camera::~Camera()
    2.13 +{
    2.14 +}
    2.15 +
    2.16 +void Camera::calc_inv_matrix(Matrix4x4 *mat) const
    2.17 +{
    2.18 +	*mat = matrix().inverse();
    2.19 +}
    2.20 +
    2.21 +void Camera::set_glmat(const Matrix4x4 &mat) const
    2.22 +{
    2.23 +#ifdef SINGLE_PRECISION_MATH
    2.24 +	if(glLoadTransposeMatrixfARB) {
    2.25 +		glLoadTransposeMatrixfARB((float*)&mat);
    2.26 +	} else {
    2.27 +		Matrix4x4 tmat = mat.transposed();
    2.28 +		glLoadMatrixf((float*)&tmat);
    2.29 +	}
    2.30 +#else
    2.31 +	if(glLoadTransposeMatrixdARB) {
    2.32 +		glLoadTransposeMatrixdARB((double*)&mat);
    2.33 +	} else {
    2.34 +		Matrix4x4 tmat = mat.transposed();
    2.35 +		glLoadMatrixd((double*)&tmat);
    2.36 +	}
    2.37 +#endif
    2.38 +}
    2.39 +
    2.40 +const Matrix4x4 &Camera::matrix() const
    2.41 +{
    2.42 +	if(!mcache.valid) {
    2.43 +		calc_matrix(&mcache.mat);
    2.44 +		mcache.valid = true;
    2.45 +	}
    2.46 +	return mcache.mat;
    2.47 +}
    2.48 +
    2.49 +const Matrix4x4 &Camera::inv_matrix() const
    2.50 +{
    2.51 +	if(!mcache_inv.valid) {
    2.52 +		calc_inv_matrix(&mcache_inv.mat);
    2.53 +		mcache_inv.valid = true;
    2.54 +	}
    2.55 +	return mcache_inv.mat;
    2.56 +}
    2.57 +
    2.58 +void Camera::use() const
    2.59 +{
    2.60 +	set_glmat(matrix());
    2.61 +}
    2.62 +
    2.63 +void Camera::use_inverse() const
    2.64 +{
    2.65 +	set_glmat(inv_matrix());
    2.66 +}
    2.67 +
    2.68 +void Camera::input_move(float x, float y, float z)
    2.69 +{
    2.70 +}
    2.71 +
    2.72 +void Camera::input_rotate(float x, float y, float z)
    2.73 +{
    2.74 +}
    2.75 +
    2.76 +void Camera::input_zoom(float factor)
    2.77 +{
    2.78 +}
    2.79 +
    2.80 +
    2.81 +// ---- orbit camera ----
    2.82 +
    2.83 +OrbitCamera::OrbitCamera()
    2.84 +{
    2.85 +	theta = 0.0;
    2.86 +	phi = 0.0;
    2.87 +	rad = 10.0;
    2.88 +}
    2.89 +
    2.90 +OrbitCamera::~OrbitCamera()
    2.91 +{
    2.92 +}
    2.93 +
    2.94 +void OrbitCamera::calc_matrix(Matrix4x4 *mat) const
    2.95 +{
    2.96 +	mat->reset_identity();
    2.97 +	mat->translate(Vector3(0, 0, -rad));
    2.98 +	mat->rotate(Vector3(phi, 0, 0));
    2.99 +	mat->rotate(Vector3(0, theta, 0));
   2.100 +}
   2.101 +
   2.102 +void OrbitCamera::calc_inv_matrix(Matrix4x4 *mat) const
   2.103 +{
   2.104 +	mat->reset_identity();
   2.105 +	mat->rotate(Vector3(0, theta, 0));
   2.106 +	mat->rotate(Vector3(phi, 0, 0));
   2.107 +	mat->translate(Vector3(0, 0, -rad));
   2.108 +}
   2.109 +
   2.110 +void OrbitCamera::input_rotate(float x, float y, float z)
   2.111 +{
   2.112 +	theta += x;
   2.113 +	phi += y;
   2.114 +
   2.115 +	if(phi < -M_PI / 2.0)
   2.116 +		phi = -M_PI / 2.0;
   2.117 +	if(phi > M_PI / 2.0)
   2.118 +		phi = M_PI / 2.0;
   2.119 +
   2.120 +	inval_cache();
   2.121 +}
   2.122 +
   2.123 +void OrbitCamera::input_zoom(float factor)
   2.124 +{
   2.125 +	rad += factor;
   2.126 +	if(rad < 0.0)
   2.127 +		rad = 0.0;
   2.128 +
   2.129 +	inval_cache();
   2.130 +}
   2.131 +
   2.132 +
   2.133 +FlyCamera::FlyCamera()
   2.134 +{
   2.135 +	pos.z = 10.0f;
   2.136 +}
   2.137 +
   2.138 +void FlyCamera::calc_matrix(Matrix4x4 *mat) const
   2.139 +{
   2.140 +	Matrix3x3 rmat = rot.get_rotation_matrix().transposed();
   2.141 +	Matrix4x4 tmat;
   2.142 +	tmat.set_translation(pos);
   2.143 +	*mat = tmat * Matrix4x4(rmat);
   2.144 +}
   2.145 +
   2.146 +/*void FlyCamera::calc_inv_matrix(Matrix4x4 *mat) const
   2.147 +{
   2.148 +}*/
   2.149 +
   2.150 +const Vector3 &FlyCamera::get_position() const
   2.151 +{
   2.152 +	return pos;
   2.153 +}
   2.154 +
   2.155 +const Quaternion &FlyCamera::get_rotation() const
   2.156 +{
   2.157 +	return rot;
   2.158 +}
   2.159 +
   2.160 +void FlyCamera::input_move(float x, float y, float z)
   2.161 +{
   2.162 +	static const Vector3 vfwd(0, 0, 1), vright(1, 0, 0);
   2.163 +
   2.164 +	Vector3 k = vfwd.transformed(rot);
   2.165 +	Vector3	i = vright.transformed(rot);
   2.166 +	Vector3 j = cross_product(k, i);
   2.167 +
   2.168 +	pos += i * x + j * y + k * z;
   2.169 +	inval_cache();
   2.170 +}
   2.171 +
   2.172 +void FlyCamera::input_rotate(float x, float y, float z)
   2.173 +{
   2.174 +	Vector3 axis(x, y, z);
   2.175 +	float axis_len = axis.length();
   2.176 +	rot.rotate(axis / axis_len, axis_len);
   2.177 +	rot.normalize();
   2.178 +
   2.179 +	inval_cache();
   2.180 +}

     3.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     3.2 +++ b/prototype/src/camera.h	Thu Jun 28 06:05:50 2012 +0300
     3.3 @@ -0,0 +1,69 @@
     3.4 +#ifndef CAMERA_H_
     3.5 +#define CAMERA_H_
     3.6 +
     3.7 +#include "vmath/vmath.h"
     3.8 +
     3.9 +class Camera {
    3.10 +protected:
    3.11 +	mutable struct {
    3.12 +		bool valid;
    3.13 +		Matrix4x4 mat;
    3.14 +	} mcache, mcache_inv;
    3.15 +
    3.16 +	virtual void calc_matrix(Matrix4x4 *mat) const = 0;
    3.17 +	virtual void calc_inv_matrix(Matrix4x4 *mat) const;
    3.18 +
    3.19 +	void inval_cache() { mcache.valid = mcache_inv.valid = false; }
    3.20 +	void set_glmat(const Matrix4x4 &m) const;
    3.21 +
    3.22 +public:
    3.23 +	Camera();
    3.24 +	virtual ~Camera();
    3.25 +
    3.26 +	const Matrix4x4 &matrix() const;
    3.27 +	const Matrix4x4 &inv_matrix() const;
    3.28 +
    3.29 +	void use() const;
    3.30 +	void use_inverse() const;
    3.31 +
    3.32 +	// these do nothing, override to provide input handling
    3.33 +	virtual void input_move(float x, float y, float z);
    3.34 +	virtual void input_rotate(float x, float y, float z);
    3.35 +	virtual void input_zoom(float factor);
    3.36 +};
    3.37 +
    3.38 +class OrbitCamera : public Camera {
    3.39 +private:
    3.40 +	float theta, phi, rad;
    3.41 +
    3.42 +	void calc_matrix(Matrix4x4 *mat) const;
    3.43 +	void calc_inv_matrix(Matrix4x4 *mat) const;
    3.44 +
    3.45 +public:
    3.46 +	OrbitCamera();
    3.47 +	virtual ~OrbitCamera();
    3.48 +
    3.49 +	void input_rotate(float x, float y, float z);
    3.50 +	void input_zoom(float factor);
    3.51 +};
    3.52 +
    3.53 +class FlyCamera : public Camera {
    3.54 +private:
    3.55 +	Vector3 pos;
    3.56 +	Quaternion rot;
    3.57 +
    3.58 +	void calc_matrix(Matrix4x4 *mat) const;
    3.59 +	//void calc_inv_matrix(Matrix4x4 *mat) const;
    3.60 +
    3.61 +public:
    3.62 +	FlyCamera();
    3.63 +
    3.64 +	const Vector3 &get_position() const;
    3.65 +	const Quaternion &get_rotation() const;
    3.66 +
    3.67 +	void input_move(float x, float y, float z);
    3.68 +	void input_rotate(float x, float y, float z);
    3.69 +};
    3.70 +
    3.71 +
    3.72 +#endif	// CAMERA_H_

     4.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     4.2 +++ b/prototype/src/level.cc	Thu Jun 28 06:05:50 2012 +0300
     4.3 @@ -0,0 +1,114 @@
     4.4 +#include <stdio.h>
     4.5 +#include <string.h>
     4.6 +#include "opengl.h"
     4.7 +#include "level.h"
     4.8 +#include "tile.h"
     4.9 +
    4.10 +Level::Level()
    4.11 +{
    4.12 +	cell_size = 1.0;
    4.13 +
    4.14 +	cells = 0;
    4.15 +	xsz = ysz = 0;
    4.16 +}
    4.17 +
    4.18 +Level::~Level()
    4.19 +{
    4.20 +	delete [] cells;
    4.21 +}
    4.22 +
    4.23 +bool Level::load(const char *fname)
    4.24 +{
    4.25 +	xsz = ysz = 64;
    4.26 +
    4.27 +	cells = new GridCell*[xsz * ysz];
    4.28 +	memset(cells, 0, xsz * ysz * sizeof *cells);
    4.29 +
    4.30 +	GridCell *g = new GridCell;
    4.31 +	g->add_tile(new Tile);
    4.32 +	cells[ysz / 2 * xsz + xsz / 2] = g;
    4.33 +
    4.34 +	return true;
    4.35 +}
    4.36 +
    4.37 +bool Level::save(const char *fname) const
    4.38 +{
    4.39 +	return false;
    4.40 +}
    4.41 +
    4.42 +const GridCell *Level::get_cell(int x, int y) const
    4.43 +{
    4.44 +	if(x < 0 || x >= xsz || y < 0 || y >= ysz) {
    4.45 +		return 0;
    4.46 +	}
    4.47 +	return cells[y * xsz + x];
    4.48 +}
    4.49 +
    4.50 +Vector3 Level::get_cell_pos(int x, int y) const
    4.51 +{
    4.52 +	float posx = (x - xsz / 2) * cell_size;
    4.53 +	float posy = (y - ysz / 2) * cell_size;
    4.54 +	return Vector3(posx, 0, posy);
    4.55 +}
    4.56 +
    4.57 +void Level::draw() const
    4.58 +{
    4.59 +	glMatrixMode(GL_MODELVIEW);
    4.60 +
    4.61 +	draw_grid();
    4.62 +
    4.63 +	for(int i=0; i<ysz; i++) {
    4.64 +		for(int j=0; j<xsz; j++) {
    4.65 +			const GridCell *cell = get_cell(j, i);
    4.66 +			if(cell) {
    4.67 +				Vector3 pos = get_cell_pos(j, i);
    4.68 +				glPushMatrix();
    4.69 +				glTranslatef(pos.x, pos.y, pos.z);
    4.70 +				glScalef(cell_size, cell_size, cell_size);
    4.71 +				cell->draw();
    4.72 +				glPopMatrix();
    4.73 +			}
    4.74 +		}
    4.75 +	}
    4.76 +}
    4.77 +
    4.78 +void Level::draw_grid() const
    4.79 +{
    4.80 +	float xlen = xsz * cell_size;
    4.81 +	float ylen = ysz * cell_size;
    4.82 +
    4.83 +	glPushAttrib(GL_ENABLE_BIT);
    4.84 +	glDisable(GL_LIGHTING);
    4.85 +
    4.86 +	glBegin(GL_LINES);
    4.87 +	glColor3f(0.4, 0.4, 0.4);
    4.88 +
    4.89 +	float y = -ylen / 2.0 - cell_size / 2.0;
    4.90 +	for(int i=0; i<ysz; i++) {
    4.91 +		glVertex3f(-xlen / 2.0, 0, y);
    4.92 +		glVertex3f(xlen / 2.0, 0, y);
    4.93 +		y += cell_size;
    4.94 +	}
    4.95 +
    4.96 +	float x = -xlen / 2.0 - cell_size / 2.0;
    4.97 +	for(int i=0; i<xsz; i++) {
    4.98 +		glVertex3f(x, 0, -ylen / 2.0);
    4.99 +		glVertex3f(x, 0, ylen / 2.0);
   4.100 +		x += cell_size;
   4.101 +	}
   4.102 +	glEnd();
   4.103 +
   4.104 +	glPopAttrib();
   4.105 +}
   4.106 +
   4.107 +void GridCell::add_tile(const Tile *tile)
   4.108 +{
   4.109 +	tiles.push_back(tile);
   4.110 +}
   4.111 +
   4.112 +void GridCell::draw() const
   4.113 +{
   4.114 +	for(size_t i=0; i<tiles.size(); i++) {
   4.115 +		tiles[i]->draw();
   4.116 +	}
   4.117 +}

     5.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     5.2 +++ b/prototype/src/level.h	Thu Jun 28 06:05:50 2012 +0300
     5.3 @@ -0,0 +1,44 @@
     5.4 +#ifndef LEVEL_H_
     5.5 +#define LEVEL_H_
     5.6 +
     5.7 +#include <vector>
     5.8 +#include "vmath/vmath.h"
     5.9 +
    5.10 +class GridCell;
    5.11 +class Tile;
    5.12 +
    5.13 +class Level {
    5.14 +private:
    5.15 +	// cells are stored as a linear array of pointers to GridCells
    5.16 +	// null pointers mean unpopulated cells.
    5.17 +	GridCell **cells;
    5.18 +	int xsz, ysz;
    5.19 +	float cell_size;
    5.20 +
    5.21 +	void draw_grid() const;
    5.22 +
    5.23 +public:
    5.24 +	Level();
    5.25 +	~Level();
    5.26 +
    5.27 +	bool load(const char *fname);
    5.28 +	bool save(const char *fname) const;
    5.29 +
    5.30 +	const GridCell *get_cell(int x, int y) const;
    5.31 +	Vector3 get_cell_pos(int x, int y) const;
    5.32 +
    5.33 +	void draw() const;
    5.34 +};
    5.35 +
    5.36 +class GridCell {
    5.37 +private:
    5.38 +	// each grid-cell might contain multiple tiles.
    5.39 +	std::vector<const Tile*> tiles;
    5.40 +
    5.41 +public:
    5.42 +	void add_tile(const Tile *tile);
    5.43 +
    5.44 +	void draw() const;
    5.45 +};
    5.46 +
    5.47 +#endif	// LEVEL_H_

     6.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     6.2 +++ b/prototype/src/main.cc	Thu Jun 28 06:05:50 2012 +0300
     6.3 @@ -0,0 +1,105 @@
     6.4 +#include <stdio.h>
     6.5 +#include <stdlib.h>
     6.6 +#include <assert.h>
     6.7 +#include "opengl.h"
     6.8 +#include "level.h"
     6.9 +#include "camera.h"
    6.10 +
    6.11 +void disp();
    6.12 +void reshape(int x, int y);
    6.13 +void keyb(unsigned char key, int x, int y);
    6.14 +void mouse(int bn, int state, int x, int y);
    6.15 +void motion(int x, int y);
    6.16 +
    6.17 +static Level *level;
    6.18 +static OrbitCamera cam;
    6.19 +
    6.20 +int main(int argc, char **argv)
    6.21 +{
    6.22 +	glutInit(&argc, argv);
    6.23 +	glutInitWindowSize(800, 600);
    6.24 +	glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE | GLUT_MULTISAMPLE);
    6.25 +	glutCreateWindow("prototype");
    6.26 +
    6.27 +	glutDisplayFunc(disp);
    6.28 +	glutReshapeFunc(reshape);
    6.29 +	glutKeyboardFunc(keyb);
    6.30 +	glutMouseFunc(mouse);
    6.31 +	glutMotionFunc(motion);
    6.32 +
    6.33 +	glewInit();
    6.34 +
    6.35 +	glEnable(GL_LIGHTING);
    6.36 +	glEnable(GL_LIGHT0);
    6.37 +	float ldir[] = {-1, 1, 2, 0};
    6.38 +	glLightfv(GL_LIGHT0, GL_POSITION, ldir);
    6.39 +	glEnable(GL_NORMALIZE);
    6.40 +
    6.41 +	glEnable(GL_DEPTH_TEST);
    6.42 +	glEnable(GL_CULL_FACE);
    6.43 +	glEnable(GL_MULTISAMPLE);
    6.44 +
    6.45 +	level = new Level;
    6.46 +	if(!level->load("foobar")) {
    6.47 +		return 1;
    6.48 +	}
    6.49 +
    6.50 +	glutMainLoop();
    6.51 +}
    6.52 +
    6.53 +void disp()
    6.54 +{
    6.55 +	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    6.56 +
    6.57 +	glMatrixMode(GL_MODELVIEW);
    6.58 +	glLoadIdentity();
    6.59 +	cam.use();
    6.60 +
    6.61 +	level->draw();
    6.62 +
    6.63 +	glutSwapBuffers();
    6.64 +	assert(glGetError() == GL_NO_ERROR);
    6.65 +}
    6.66 +
    6.67 +void reshape(int x, int y)
    6.68 +{
    6.69 +	glViewport(0, 0, x, y);
    6.70 +	glMatrixMode(GL_PROJECTION);
    6.71 +	glLoadIdentity();
    6.72 +	gluPerspective(45.0, (float)x / (float)y, 1.0, 1000.0);
    6.73 +}
    6.74 +
    6.75 +void keyb(unsigned char key, int x, int y)
    6.76 +{
    6.77 +	switch(key) {
    6.78 +	case 27:
    6.79 +		exit(0);
    6.80 +	}
    6.81 +}
    6.82 +
    6.83 +static int prev_x, prev_y;
    6.84 +static bool bnstate[32];
    6.85 +
    6.86 +void mouse(int bn, int state, int x, int y)
    6.87 +{
    6.88 +	prev_x = x;
    6.89 +	prev_y = y;
    6.90 +	bnstate[bn - GLUT_LEFT_BUTTON] = state == GLUT_DOWN;
    6.91 +}
    6.92 +
    6.93 +void motion(int x, int y)
    6.94 +{
    6.95 +	int dx = x - prev_x;
    6.96 +	int dy = y - prev_y;
    6.97 +	prev_x = x;
    6.98 +	prev_y = y;
    6.99 +
   6.100 +	if(bnstate[0]) {
   6.101 +		cam.input_rotate(dx * 0.01, dy * 0.01, 0);
   6.102 +		glutPostRedisplay();
   6.103 +	}
   6.104 +	if(bnstate[2]) {
   6.105 +		cam.input_zoom(dy * 0.1);
   6.106 +		glutPostRedisplay();
   6.107 +	}
   6.108 +}

     7.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     7.2 +++ b/prototype/src/opengl.h	Thu Jun 28 06:05:50 2012 +0300
     7.3 @@ -0,0 +1,12 @@
     7.4 +#ifndef OPENGL_H_
     7.5 +#define OPENGL_H_
     7.6 +
     7.7 +#include <GL/glew.h>
     7.8 +
     7.9 +#ifndef __APPLE__
    7.10 +#include <GL/glut.h>
    7.11 +#else
    7.12 +#include <GLUT/glut.h>
    7.13 +#endif
    7.14 +
    7.15 +#endif	/* OPENGL_H_ */

     8.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     8.2 +++ b/prototype/src/tile.cc	Thu Jun 28 06:05:50 2012 +0300
     8.3 @@ -0,0 +1,23 @@
     8.4 +#include <stdio.h>
     8.5 +#include "opengl.h"
     8.6 +#include "tile.h"
     8.7 +
     8.8 +bool Tile::load(const char *fname)
     8.9 +{
    8.10 +	return true;
    8.11 +}
    8.12 +
    8.13 +void Tile::draw() const
    8.14 +{
    8.15 +	float color[] = {1, 0, 0, 1};
    8.16 +	float white[] = {1, 1, 1, 1};
    8.17 +
    8.18 +	glPushAttrib(GL_LIGHTING_BIT);
    8.19 +
    8.20 +	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
    8.21 +	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, white);
    8.22 +	glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 60.0);
    8.23 +	glutSolidSphere(0.5, 16, 8);
    8.24 +
    8.25 +	glPopAttrib();
    8.26 +}

     9.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     9.2 +++ b/prototype/src/tile.h	Thu Jun 28 06:05:50 2012 +0300
     9.3 @@ -0,0 +1,11 @@
     9.4 +#ifndef TILE_H_
     9.5 +#define TILE_H_
     9.6 +
     9.7 +class Tile {
     9.8 +public:
     9.9 +	bool load(const char *fname);
    9.10 +
    9.11 +	void draw() const;
    9.12 +};
    9.13 +
    9.14 +#endif	// TILE_H_

    10.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    10.2 +++ b/prototype/vmath/basis.cc	Thu Jun 28 06:05:50 2012 +0300
    10.3 @@ -0,0 +1,63 @@
    10.4 +#include "basis.h"
    10.5 +#include "vmath.h"
    10.6 +
    10.7 +Basis::Basis()
    10.8 +{
    10.9 +	i = Vector3(1, 0, 0);
   10.10 +	j = Vector3(0, 1, 0);
   10.11 +	k = Vector3(0, 0, 1);
   10.12 +}
   10.13 +
   10.14 +Basis::Basis(const Vector3 &i, const Vector3 &j, const Vector3 &k)
   10.15 +{
   10.16 +	this->i = i;
   10.17 +	this->j = j;
   10.18 +	this->k = k;
   10.19 +}
   10.20 +
   10.21 +Basis::Basis(const Vector3 &dir, bool left_handed)
   10.22 +{
   10.23 +	k = dir;
   10.24 +	j = Vector3(0, 1, 0);
   10.25 +	i = cross_product(j, k);
   10.26 +	j = cross_product(k, i);
   10.27 +}
   10.28 +
   10.29 +/** Rotate with euler angles */
   10.30 +void Basis::rotate(scalar_t x, scalar_t y, scalar_t z) {
   10.31 +	Matrix4x4 RotMat;
   10.32 +	RotMat.set_rotation(Vector3(x, y, z));
   10.33 +	i.transform(RotMat);
   10.34 +	j.transform(RotMat);
   10.35 +	k.transform(RotMat);
   10.36 +}
   10.37 +
   10.38 +/** Rotate by axis-angle specification */
   10.39 +void Basis::rotate(const Vector3 &axis, scalar_t angle) {
   10.40 +	Quaternion q;
   10.41 +	q.set_rotation(axis, angle);
   10.42 +	i.transform(q);
   10.43 +	j.transform(q);
   10.44 +	k.transform(q);
   10.45 +}
   10.46 +
   10.47 +/** Rotate with a 4x4 matrix */
   10.48 +void Basis::rotate(const Matrix4x4 &mat) {
   10.49 +	i.transform(mat);
   10.50 +	j.transform(mat);
   10.51 +	k.transform(mat);
   10.52 +}
   10.53 +
   10.54 +/** Rotate with a quaternion */
   10.55 +void Basis::rotate(const Quaternion &quat) {
   10.56 +	i.transform(quat);
   10.57 +	j.transform(quat);
   10.58 +	k.transform(quat);
   10.59 +}
   10.60 +
   10.61 +/** Extract a rotation matrix from the orthogonal basis */
   10.62 +Matrix3x3 Basis::create_rotation_matrix() const {
   10.63 +	return Matrix3x3(	i.x, j.x, k.x,
   10.64 +						i.y, j.y, k.y,
   10.65 +						i.z, j.z, k.z);
   10.66 +}

    11.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    11.2 +++ b/prototype/vmath/basis.h	Thu Jun 28 06:05:50 2012 +0300
    11.3 @@ -0,0 +1,57 @@
    11.4 +/*
    11.5 +libvmath - a vector math library
    11.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    11.7 +
    11.8 +This program is free software: you can redistribute it and/or modify
    11.9 +it under the terms of the GNU Lesser General Public License as published
   11.10 +by the Free Software Foundation, either version 3 of the License, or
   11.11 +(at your option) any later version.
   11.12 +
   11.13 +This program is distributed in the hope that it will be useful,
   11.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   11.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   11.16 +GNU Lesser General Public License for more details.
   11.17 +
   11.18 +You should have received a copy of the GNU Lesser General Public License
   11.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   11.20 +*/
   11.21 +
   11.22 +#ifndef VMATH_BASIS_H_
   11.23 +#define VMATH_BASIS_H_
   11.24 +
   11.25 +#include "vector.h"
   11.26 +#include "matrix.h"
   11.27 +
   11.28 +enum {
   11.29 +	LEFT_HANDED,
   11.30 +	RIGHT_HANDED
   11.31 +};
   11.32 +
   11.33 +#ifdef __cplusplus
   11.34 +extern "C" {
   11.35 +#endif	/* __cplusplus */
   11.36 +
   11.37 +void basis_matrix(mat4_t res, vec3_t i, vec3_t j, vec3_t k);
   11.38 +void basis_matrix_dir(mat4_t res, vec3_t dir);
   11.39 +
   11.40 +#ifdef __cplusplus
   11.41 +}	/* extern "C" */
   11.42 +
   11.43 +class Basis {
   11.44 +public:
   11.45 +	Vector3 i, j, k;
   11.46 +
   11.47 +	Basis();
   11.48 +	Basis(const Vector3 &i, const Vector3 &j, const Vector3 &k);
   11.49 +	Basis(const Vector3 &dir, bool left_handed = true);
   11.50 +
   11.51 +	void rotate(scalar_t x, scalar_t y, scalar_t z);
   11.52 +	void rotate(const Vector3 &axis, scalar_t angle);
   11.53 +	void rotate(const Matrix4x4 &mat);
   11.54 +	void rotate(const Quaternion &quat);
   11.55 +
   11.56 +	Matrix3x3 create_rotation_matrix() const;
   11.57 +};
   11.58 +#endif	/* __cplusplus */
   11.59 +
   11.60 +#endif	/* VMATH_BASIS_H_ */

    12.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    12.2 +++ b/prototype/vmath/basis_c.c	Thu Jun 28 06:05:50 2012 +0300
    12.3 @@ -0,0 +1,37 @@
    12.4 +/*
    12.5 +libvmath - a vector math library
    12.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    12.7 +
    12.8 +This program is free software: you can redistribute it and/or modify
    12.9 +it under the terms of the GNU Lesser General Public License as published
   12.10 +by the Free Software Foundation, either version 3 of the License, or
   12.11 +(at your option) any later version.
   12.12 +
   12.13 +This program is distributed in the hope that it will be useful,
   12.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   12.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   12.16 +GNU Lesser General Public License for more details.
   12.17 +
   12.18 +You should have received a copy of the GNU Lesser General Public License
   12.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   12.20 +*/
   12.21 +
   12.22 +#include "basis.h"
   12.23 +#include "matrix.h"
   12.24 +
   12.25 +void basis_matrix(mat4_t res, vec3_t i, vec3_t j, vec3_t k)
   12.26 +{
   12.27 +	m4_identity(res);
   12.28 +	m4_set_column(res, v4_cons(i.x, i.y, i.z, 1.0), 0);
   12.29 +	m4_set_column(res, v4_cons(j.x, j.y, j.z, 1.0), 1);
   12.30 +	m4_set_column(res, v4_cons(k.x, k.y, k.z, 1.0), 2);
   12.31 +}
   12.32 +
   12.33 +void basis_matrix_dir(mat4_t res, vec3_t dir)
   12.34 +{
   12.35 +	vec3_t k = v3_normalize(dir);
   12.36 +	vec3_t j = {0, 1, 0};
   12.37 +	vec3_t i = v3_cross(j, k);
   12.38 +	j = v3_cross(k, i);
   12.39 +	basis_matrix(res, i, j, k);
   12.40 +}

    13.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    13.2 +++ b/prototype/vmath/geom.c	Thu Jun 28 06:05:50 2012 +0300
    13.3 @@ -0,0 +1,150 @@
    13.4 +/*
    13.5 +libvmath - a vector math library
    13.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    13.7 +
    13.8 +This program is free software: you can redistribute it and/or modify
    13.9 +it under the terms of the GNU Lesser General Public License as published
   13.10 +by the Free Software Foundation, either version 3 of the License, or
   13.11 +(at your option) any later version.
   13.12 +
   13.13 +This program is distributed in the hope that it will be useful,
   13.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   13.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   13.16 +GNU Lesser General Public License for more details.
   13.17 +
   13.18 +You should have received a copy of the GNU Lesser General Public License
   13.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   13.20 +*/
   13.21 +
   13.22 +
   13.23 +#include <math.h>
   13.24 +#include "geom.h"
   13.25 +#include "vector.h"
   13.26 +
   13.27 +plane_t plane_cons(scalar_t nx, scalar_t ny, scalar_t nz, scalar_t d)
   13.28 +{
   13.29 +	plane_t p;
   13.30 +	p.norm.x = nx;
   13.31 +	p.norm.y = ny;
   13.32 +	p.norm.z = nz;
   13.33 +	p.d = d;
   13.34 +	return p;
   13.35 +}
   13.36 +
   13.37 +plane_t plane_poly(vec3_t v0, vec3_t v1, vec3_t v2)
   13.38 +{
   13.39 +	vec3_t a, b, norm;
   13.40 +
   13.41 +	a = v3_sub(v1, v0);
   13.42 +	b = v3_sub(v2, v0);
   13.43 +	norm = v3_cross(a, b);
   13.44 +	norm = v3_normalize(norm);
   13.45 +
   13.46 +	return plane_ptnorm(v0, norm);
   13.47 +}
   13.48 +
   13.49 +plane_t plane_ptnorm(vec3_t pt, vec3_t normal)
   13.50 +{
   13.51 +	plane_t plane;
   13.52 +
   13.53 +	plane.norm = normal;
   13.54 +	plane.d = v3_dot(pt, normal);
   13.55 +
   13.56 +	return plane;
   13.57 +}
   13.58 +
   13.59 +plane_t plane_invert(plane_t p)
   13.60 +{
   13.61 +	p.norm = v3_neg(p.norm);
   13.62 +	p.d = -p.d;
   13.63 +	return p;
   13.64 +}
   13.65 +
   13.66 +scalar_t plane_signed_dist(plane_t plane, vec3_t pt)
   13.67 +{
   13.68 +	vec3_t pp = plane_point(plane);
   13.69 +	vec3_t pptopt = v3_sub(pt, pp);
   13.70 +	return v3_dot(pptopt, plane.norm);
   13.71 +}
   13.72 +
   13.73 +scalar_t plane_dist(plane_t plane, vec3_t pt)
   13.74 +{
   13.75 +	return fabs(plane_signed_dist(plane, pt));
   13.76 +}
   13.77 +
   13.78 +vec3_t plane_point(plane_t plane)
   13.79 +{
   13.80 +	return v3_scale(plane.norm, plane.d);
   13.81 +}
   13.82 +
   13.83 +int plane_ray_intersect(ray_t ray, plane_t plane, scalar_t *pos)
   13.84 +{
   13.85 +	vec3_t pt, orig_to_pt;
   13.86 +	scalar_t ndotdir;
   13.87 +
   13.88 +	pt = plane_point(plane);
   13.89 +	ndotdir = v3_dot(plane.norm, ray.dir);
   13.90 +
   13.91 +	if(fabs(ndotdir) < 1e-7) {
   13.92 +		return 0;
   13.93 +	}
   13.94 +
   13.95 +	if(pos) {
   13.96 +		orig_to_pt = v3_sub(pt, ray.origin);
   13.97 +		*pos = v3_dot(plane.norm, orig_to_pt) / ndotdir;
   13.98 +	}
   13.99 +	return 1;
  13.100 +}
  13.101 +
  13.102 +sphere_t sphere_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t rad)
  13.103 +{
  13.104 +	sphere_t sph;
  13.105 +	sph.pos.x = x;
  13.106 +	sph.pos.y = y;
  13.107 +	sph.pos.z = z;
  13.108 +	sph.rad = rad;
  13.109 +	return sph;
  13.110 +}
  13.111 +
  13.112 +int sphere_ray_intersect(ray_t ray, sphere_t sph, scalar_t *pos)
  13.113 +{
  13.114 +	scalar_t a, b, c, d, sqrt_d, t1, t2, t;
  13.115 +
  13.116 +	a = v3_dot(ray.dir, ray.dir);
  13.117 +	b = 2.0 * ray.dir.x * (ray.origin.x - sph.pos.x) +
  13.118 +		2.0 * ray.dir.y * (ray.origin.y - sph.pos.y) +
  13.119 +		2.0 * ray.dir.z * (ray.origin.z - sph.pos.z);
  13.120 +	c = v3_dot(sph.pos, sph.pos) + v3_dot(ray.origin, ray.origin) +
  13.121 +		2.0 * v3_dot(v3_neg(sph.pos), ray.origin) - sph.rad * sph.rad;
  13.122 +
  13.123 +	d = b * b - 4.0 * a * c;
  13.124 +	if(d < 0.0) {
  13.125 +		return 0;
  13.126 +	}
  13.127 +
  13.128 +	sqrt_d = sqrt(d);
  13.129 +	t1 = (-b + sqrt_d) / (2.0 * a);
  13.130 +	t2 = (-b - sqrt_d) / (2.0 * a);
  13.131 +
  13.132 +	if(t1 < 1e-7 || t1 > 1.0) {
  13.133 +		t1 = t2;
  13.134 +	}
  13.135 +	if(t2 < 1e-7 || t2 > 1.0) {
  13.136 +		t2 = t1;
  13.137 +	}
  13.138 +	t = t1 < t2 ? t1 : t2;
  13.139 +
  13.140 +	if(t < 1e-7 || t > 1.0) {
  13.141 +		return 0;
  13.142 +	}
  13.143 +
  13.144 +	if(pos) {
  13.145 +		*pos = t;
  13.146 +	}
  13.147 +	return 1;
  13.148 +}
  13.149 +
  13.150 +int sphere_sphere_intersect(sphere_t sph1, sphere_t sph2, scalar_t *pos, scalar_t *rad)
  13.151 +{
  13.152 +	return -1;
  13.153 +}

    14.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    14.2 +++ b/prototype/vmath/geom.h	Thu Jun 28 06:05:50 2012 +0300
    14.3 @@ -0,0 +1,72 @@
    14.4 +/*
    14.5 +libvmath - a vector math library
    14.6 +Copyright (C) 2004-2012 John Tsiombikas <nuclear@member.fsf.org>
    14.7 +
    14.8 +This program is free software: you can redistribute it and/or modify
    14.9 +it under the terms of the GNU Lesser General Public License as published
   14.10 +by the Free Software Foundation, either version 3 of the License, or
   14.11 +(at your option) any later version.
   14.12 +
   14.13 +This program is distributed in the hope that it will be useful,
   14.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   14.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   14.16 +GNU Lesser General Public License for more details.
   14.17 +
   14.18 +You should have received a copy of the GNU Lesser General Public License
   14.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   14.20 +*/
   14.21 +#ifndef LIBVMATH_GEOM_H_
   14.22 +#define LIBVMATH_GEOM_H_
   14.23 +
   14.24 +#include "vector.h"
   14.25 +#include "ray.h"
   14.26 +
   14.27 +typedef struct {
   14.28 +	vec3_t norm;
   14.29 +	scalar_t d;
   14.30 +} plane_t;
   14.31 +
   14.32 +typedef struct {
   14.33 +	vec3_t pos;
   14.34 +	scalar_t rad;
   14.35 +} sphere_t;
   14.36 +
   14.37 +typedef struct {
   14.38 +	vec3_t min, max;
   14.39 +} aabox_t;
   14.40 +
   14.41 +#ifdef __cplusplus
   14.42 +extern "C" {
   14.43 +#endif
   14.44 +
   14.45 +/* planes are good... you need planes, yes you do */
   14.46 +plane_t plane_cons(scalar_t nx, scalar_t ny, scalar_t nz, scalar_t d);
   14.47 +plane_t plane_poly(vec3_t v0, vec3_t v1, vec3_t v2);
   14.48 +plane_t plane_ptnorm(vec3_t pt, vec3_t normal);
   14.49 +
   14.50 +plane_t plane_invert(plane_t p);
   14.51 +
   14.52 +scalar_t plane_signed_dist(plane_t plane, vec3_t pt);
   14.53 +scalar_t plane_dist(plane_t plane, vec3_t pt);
   14.54 +vec3_t plane_point(plane_t plane);
   14.55 +
   14.56 +int plane_ray_intersect(ray_t ray, plane_t plane, scalar_t *pos);
   14.57 +
   14.58 +/* spheres always come in handy */
   14.59 +sphere_t sphere_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t rad);
   14.60 +
   14.61 +int sphere_ray_intersect(ray_t ray, sphere_t sph, scalar_t *pos);
   14.62 +int sphere_sphere_intersect(sphere_t sph1, sphere_t sph2, scalar_t *pos, scalar_t *rad);
   14.63 +
   14.64 +#ifdef __cplusplus
   14.65 +}
   14.66 +
   14.67 +/* TODO
   14.68 +class Plane : public plane_t {
   14.69 +public:
   14.70 +};
   14.71 +*/
   14.72 +
   14.73 +#endif
   14.74 +
   14.75 +#endif	/* LIBVMATH_GEOM_H_ */

    15.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    15.2 +++ b/prototype/vmath/matrix.cc	Thu Jun 28 06:05:50 2012 +0300
    15.3 @@ -0,0 +1,743 @@
    15.4 +#include <cstdio>
    15.5 +#include <cmath>
    15.6 +#include "matrix.h"
    15.7 +#include "vector.h"
    15.8 +#include "quat.h"
    15.9 +
   15.10 +using namespace std;
   15.11 +
   15.12 +// ----------- Matrix3x3 --------------
   15.13 +
   15.14 +Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
   15.15 +
   15.16 +Matrix3x3::Matrix3x3()
   15.17 +{
   15.18 +	*this = identity;
   15.19 +}
   15.20 +
   15.21 +Matrix3x3::Matrix3x3(	scalar_t m11, scalar_t m12, scalar_t m13,
   15.22 +						scalar_t m21, scalar_t m22, scalar_t m23,
   15.23 +						scalar_t m31, scalar_t m32, scalar_t m33)
   15.24 +{
   15.25 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
   15.26 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
   15.27 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
   15.28 +}
   15.29 +
   15.30 +Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)
   15.31 +{
   15.32 +	set_row_vector(ivec, 0);
   15.33 +	set_row_vector(jvec, 1);
   15.34 +	set_row_vector(kvec, 2);
   15.35 +}
   15.36 +
   15.37 +Matrix3x3::Matrix3x3(const mat3_t cmat)
   15.38 +{
   15.39 +	memcpy(m, cmat, sizeof(mat3_t));
   15.40 +}
   15.41 +
   15.42 +Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)
   15.43 +{
   15.44 +	for(int i=0; i<3; i++) {
   15.45 +		for(int j=0; j<3; j++) {
   15.46 +			m[i][j] = mat4x4[i][j];
   15.47 +		}
   15.48 +	}
   15.49 +}
   15.50 +
   15.51 +Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)
   15.52 +{
   15.53 +	Matrix3x3 res;
   15.54 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   15.55 +	scalar_t *dest = res.m[0];
   15.56 +
   15.57 +	for(int i=0; i<9; i++) {
   15.58 +		*dest++ = *op1++ + *op2++;
   15.59 +	}
   15.60 +	return res;
   15.61 +}
   15.62 +
   15.63 +Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)
   15.64 +{
   15.65 +	Matrix3x3 res;
   15.66 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   15.67 +	scalar_t *dest = res.m[0];
   15.68 +
   15.69 +	for(int i=0; i<9; i++) {
   15.70 +		*dest++ = *op1++ - *op2++;
   15.71 +	}
   15.72 +	return res;
   15.73 +}
   15.74 +
   15.75 +Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)
   15.76 +{
   15.77 +	Matrix3x3 res;
   15.78 +	for(int i=0; i<3; i++) {
   15.79 +		for(int j=0; j<3; j++) {
   15.80 +			res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
   15.81 +		}
   15.82 +	}
   15.83 +	return res;
   15.84 +}
   15.85 +
   15.86 +void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)
   15.87 +{
   15.88 +	scalar_t *op1 = m1.m[0];
   15.89 +	const scalar_t *op2 = m2.m[0];
   15.90 +
   15.91 +	for(int i=0; i<9; i++) {
   15.92 +		*op1++ += *op2++;
   15.93 +	}
   15.94 +}
   15.95 +
   15.96 +void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)
   15.97 +{
   15.98 +	scalar_t *op1 = m1.m[0];
   15.99 +	const scalar_t *op2 = m2.m[0];
  15.100 +
  15.101 +	for(int i=0; i<9; i++) {
  15.102 +		*op1++ -= *op2++;
  15.103 +	}
  15.104 +}
  15.105 +
  15.106 +void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)
  15.107 +{
  15.108 +	Matrix3x3 res;
  15.109 +	for(int i=0; i<3; i++) {
  15.110 +		for(int j=0; j<3; j++) {
  15.111 +			res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
  15.112 +		}
  15.113 +	}
  15.114 +	memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
  15.115 +}
  15.116 +
  15.117 +Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)
  15.118 +{
  15.119 +	Matrix3x3 res;
  15.120 +	const scalar_t *mptr = mat.m[0];
  15.121 +	scalar_t *dptr = res.m[0];
  15.122 +
  15.123 +	for(int i=0; i<9; i++) {
  15.124 +		*dptr++ = *mptr++ * scalar;
  15.125 +	}
  15.126 +	return res;
  15.127 +}
  15.128 +
  15.129 +Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)
  15.130 +{
  15.131 +	Matrix3x3 res;
  15.132 +	const scalar_t *mptr = mat.m[0];
  15.133 +	scalar_t *dptr = res.m[0];
  15.134 +
  15.135 +	for(int i=0; i<9; i++) {
  15.136 +		*dptr++ = *mptr++ * scalar;
  15.137 +	}
  15.138 +	return res;
  15.139 +}
  15.140 +
  15.141 +void operator *=(Matrix3x3 &mat, scalar_t scalar)
  15.142 +{
  15.143 +	scalar_t *mptr = mat.m[0];
  15.144 +
  15.145 +	for(int i=0; i<9; i++) {
  15.146 +		*mptr++ *= scalar;
  15.147 +	}
  15.148 +}
  15.149 +
  15.150 +void Matrix3x3::translate(const Vector2 &trans)
  15.151 +{
  15.152 +	Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
  15.153 +	*this *= tmat;
  15.154 +}
  15.155 +
  15.156 +void Matrix3x3::set_translation(const Vector2 &trans)
  15.157 +{
  15.158 +	*this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
  15.159 +}
  15.160 +
  15.161 +void Matrix3x3::rotate(scalar_t angle)
  15.162 +{
  15.163 +	scalar_t cos_a = cos(angle);
  15.164 +	scalar_t sin_a = sin(angle);
  15.165 +	Matrix3x3 rmat(	cos_a,	-sin_a,		0,
  15.166 +					sin_a,	cos_a,		0,
  15.167 +					0,		0,			1);
  15.168 +	*this *= rmat;
  15.169 +}
  15.170 +
  15.171 +void Matrix3x3::set_rotation(scalar_t angle)
  15.172 +{
  15.173 +	scalar_t cos_a = cos(angle);
  15.174 +	scalar_t sin_a = sin(angle);
  15.175 +	*this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
  15.176 +}
  15.177 +
  15.178 +void Matrix3x3::rotate(const Vector3 &euler_angles)
  15.179 +{
  15.180 +	Matrix3x3 xrot, yrot, zrot;
  15.181 +
  15.182 +	xrot = Matrix3x3(	1,			0,					0,
  15.183 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
  15.184 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
  15.185 +
  15.186 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
  15.187 +								0,				1,				0,
  15.188 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
  15.189 +
  15.190 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
  15.191 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
  15.192 +								0,						0,				1);
  15.193 +
  15.194 +	*this *= xrot * yrot * zrot;
  15.195 +}
  15.196 +
  15.197 +void Matrix3x3::set_rotation(const Vector3 &euler_angles)
  15.198 +{
  15.199 +	Matrix3x3 xrot, yrot, zrot;
  15.200 +
  15.201 +	xrot = Matrix3x3(	1,			0,					0,
  15.202 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
  15.203 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
  15.204 +
  15.205 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
  15.206 +								0,				1,				0,
  15.207 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
  15.208 +
  15.209 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
  15.210 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
  15.211 +								0,						0,				1);
  15.212 +
  15.213 +	*this = xrot * yrot * zrot;
  15.214 +}
  15.215 +
  15.216 +void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)
  15.217 +{
  15.218 +	scalar_t sina = (scalar_t)sin(angle);
  15.219 +	scalar_t cosa = (scalar_t)cos(angle);
  15.220 +	scalar_t invcosa = 1-cosa;
  15.221 +	scalar_t nxsq = axis.x * axis.x;
  15.222 +	scalar_t nysq = axis.y * axis.y;
  15.223 +	scalar_t nzsq = axis.z * axis.z;
  15.224 +
  15.225 +	Matrix3x3 xform;
  15.226 +	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
  15.227 +	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
  15.228 +	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
  15.229 +
  15.230 +	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
  15.231 +	xform.m[1][1] = nysq + (1-nysq) * cosa;
  15.232 +	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
  15.233 +
  15.234 +	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
  15.235 +	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
  15.236 +	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
  15.237 +
  15.238 +	*this *= xform;
  15.239 +}
  15.240 +
  15.241 +void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)
  15.242 +{
  15.243 +	scalar_t sina = (scalar_t)sin(angle);
  15.244 +	scalar_t cosa = (scalar_t)cos(angle);
  15.245 +	scalar_t invcosa = 1-cosa;
  15.246 +	scalar_t nxsq = axis.x * axis.x;
  15.247 +	scalar_t nysq = axis.y * axis.y;
  15.248 +	scalar_t nzsq = axis.z * axis.z;
  15.249 +
  15.250 +	reset_identity();
  15.251 +	m[0][0] = nxsq + (1-nxsq) * cosa;
  15.252 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
  15.253 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
  15.254 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
  15.255 +	m[1][1] = nysq + (1-nysq) * cosa;
  15.256 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
  15.257 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
  15.258 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
  15.259 +	m[2][2] = nzsq + (1-nzsq) * cosa;
  15.260 +}
  15.261 +
  15.262 +void Matrix3x3::scale(const Vector3 &scale_vec)
  15.263 +{
  15.264 +	Matrix3x3 smat(	scale_vec.x, 0, 0,
  15.265 +					0, scale_vec.y, 0,
  15.266 +					0, 0, scale_vec.z);
  15.267 +	*this *= smat;
  15.268 +}
  15.269 +
  15.270 +void Matrix3x3::set_scaling(const Vector3 &scale_vec)
  15.271 +{
  15.272 +	*this = Matrix3x3(	scale_vec.x, 0, 0,
  15.273 +						0, scale_vec.y, 0,
  15.274 +						0, 0, scale_vec.z);
  15.275 +}
  15.276 +
  15.277 +void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
  15.278 +{
  15.279 +	m[0][col_index] = vec.x;
  15.280 +	m[1][col_index] = vec.y;
  15.281 +	m[2][col_index] = vec.z;
  15.282 +}
  15.283 +
  15.284 +void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
  15.285 +{
  15.286 +	m[row_index][0] = vec.x;
  15.287 +	m[row_index][1] = vec.y;
  15.288 +	m[row_index][2] = vec.z;
  15.289 +}
  15.290 +
  15.291 +Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
  15.292 +{
  15.293 +	return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
  15.294 +}
  15.295 +
  15.296 +Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
  15.297 +{
  15.298 +	return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
  15.299 +}
  15.300 +
  15.301 +void Matrix3x3::transpose()
  15.302 +{
  15.303 +	Matrix3x3 tmp = *this;
  15.304 +	for(int i=0; i<3; i++) {
  15.305 +		for(int j=0; j<3; j++) {
  15.306 +			m[i][j] = tmp[j][i];
  15.307 +		}
  15.308 +	}
  15.309 +}
  15.310 +
  15.311 +Matrix3x3 Matrix3x3::transposed() const
  15.312 +{
  15.313 +	Matrix3x3 res;
  15.314 +	for(int i=0; i<3; i++) {
  15.315 +		for(int j=0; j<3; j++) {
  15.316 +			res[i][j] = m[j][i];
  15.317 +		}
  15.318 +	}
  15.319 +	return res;
  15.320 +}
  15.321 +
  15.322 +scalar_t Matrix3x3::determinant() const
  15.323 +{
  15.324 +	return	m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
  15.325 +			m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
  15.326 +			m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
  15.327 +}
  15.328 +
  15.329 +Matrix3x3 Matrix3x3::inverse() const
  15.330 +{
  15.331 +	// TODO: implement 3x3 inverse
  15.332 +	return *this;
  15.333 +}
  15.334 +
  15.335 +ostream &operator <<(ostream &out, const Matrix3x3 &mat)
  15.336 +{
  15.337 +	for(int i=0; i<3; i++) {
  15.338 +		char str[100];
  15.339 +		sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
  15.340 +		out << str;
  15.341 +	}
  15.342 +	return out;
  15.343 +}
  15.344 +
  15.345 +
  15.346 +
  15.347 +/* ----------------- Matrix4x4 implementation --------------- */
  15.348 +
  15.349 +Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
  15.350 +
  15.351 +Matrix4x4::Matrix4x4()
  15.352 +{
  15.353 +	*this = identity;
  15.354 +}
  15.355 +
  15.356 +Matrix4x4::Matrix4x4(	scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
  15.357 +						scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
  15.358 +						scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
  15.359 +						scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
  15.360 +{
  15.361 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
  15.362 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
  15.363 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
  15.364 +	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
  15.365 +}
  15.366 +
  15.367 +Matrix4x4::Matrix4x4(const mat4_t cmat)
  15.368 +{
  15.369 +	memcpy(m, cmat, sizeof(mat4_t));
  15.370 +}
  15.371 +
  15.372 +Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
  15.373 +{
  15.374 +	reset_identity();
  15.375 +	for(int i=0; i<3; i++) {
  15.376 +		for(int j=0; j<3; j++) {
  15.377 +			m[i][j] = mat3x3[i][j];
  15.378 +		}
  15.379 +	}
  15.380 +}
  15.381 +
  15.382 +Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
  15.383 +{
  15.384 +	Matrix4x4 res;
  15.385 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
  15.386 +	scalar_t *dest = res.m[0];
  15.387 +
  15.388 +	for(int i=0; i<16; i++) {
  15.389 +		*dest++ = *op1++ + *op2++;
  15.390 +	}
  15.391 +	return res;
  15.392 +}
  15.393 +
  15.394 +Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
  15.395 +{
  15.396 +	Matrix4x4 res;
  15.397 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
  15.398 +	scalar_t *dest = res.m[0];
  15.399 +
  15.400 +	for(int i=0; i<16; i++) {
  15.401 +		*dest++ = *op1++ - *op2++;
  15.402 +	}
  15.403 +	return res;
  15.404 +}
  15.405 +
  15.406 +/*
  15.407 +Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
  15.408 +	Matrix4x4 res;
  15.409 +
  15.410 +	for(int i=0; i<4; i++) {
  15.411 +		for(int j=0; j<4; j++) {
  15.412 +			res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
  15.413 +		}
  15.414 +	}
  15.415 +
  15.416 +	return res;
  15.417 +}
  15.418 +*/
  15.419 +
  15.420 +void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
  15.421 +{
  15.422 +	scalar_t *op1 = m1.m[0];
  15.423 +	const scalar_t *op2 = m2.m[0];
  15.424 +
  15.425 +	for(int i=0; i<16; i++) {
  15.426 +		*op1++ += *op2++;
  15.427 +	}
  15.428 +}
  15.429 +
  15.430 +void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
  15.431 +{
  15.432 +	scalar_t *op1 = m1.m[0];
  15.433 +	const scalar_t *op2 = m2.m[0];
  15.434 +
  15.435 +	for(int i=0; i<16; i++) {
  15.436 +		*op1++ -= *op2++;
  15.437 +	}
  15.438 +}
  15.439 +
  15.440 +void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2)
  15.441 +{
  15.442 +	Matrix4x4 res;
  15.443 +	for(int i=0; i<4; i++) {
  15.444 +		for(int j=0; j<4; j++) {
  15.445 +			res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
  15.446 +		}
  15.447 +	}
  15.448 +	memcpy(m1.m, res.m, 16 * sizeof(scalar_t));
  15.449 +}
  15.450 +
  15.451 +Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
  15.452 +{
  15.453 +	Matrix4x4 res;
  15.454 +	const scalar_t *mptr = mat.m[0];
  15.455 +	scalar_t *dptr = res.m[0];
  15.456 +
  15.457 +	for(int i=0; i<16; i++) {
  15.458 +		*dptr++ = *mptr++ * scalar;
  15.459 +	}
  15.460 +	return res;
  15.461 +}
  15.462 +
  15.463 +Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
  15.464 +{
  15.465 +	Matrix4x4 res;
  15.466 +	const scalar_t *mptr = mat.m[0];
  15.467 +	scalar_t *dptr = res.m[0];
  15.468 +
  15.469 +	for(int i=0; i<16; i++) {
  15.470 +		*dptr++ = *mptr++ * scalar;
  15.471 +	}
  15.472 +	return res;
  15.473 +}
  15.474 +
  15.475 +void operator *=(Matrix4x4 &mat, scalar_t scalar)
  15.476 +{
  15.477 +	scalar_t *mptr = mat.m[0];
  15.478 +
  15.479 +	for(int i=0; i<16; i++) {
  15.480 +		*mptr++ *= scalar;
  15.481 +	}
  15.482 +}
  15.483 +
  15.484 +void Matrix4x4::translate(const Vector3 &trans)
  15.485 +{
  15.486 +	Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
  15.487 +	*this *= tmat;
  15.488 +}
  15.489 +
  15.490 +void Matrix4x4::set_translation(const Vector3 &trans)
  15.491 +{
  15.492 +	*this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
  15.493 +}
  15.494 +
  15.495 +void Matrix4x4::rotate(const Vector3 &euler_angles)
  15.496 +{
  15.497 +	Matrix3x3 xrot, yrot, zrot;
  15.498 +
  15.499 +	xrot = Matrix3x3(	1,			0,					0,
  15.500 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
  15.501 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
  15.502 +
  15.503 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
  15.504 +								0,				1,				0,
  15.505 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
  15.506 +
  15.507 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
  15.508 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
  15.509 +								0,						0,				1);
  15.510 +
  15.511 +	*this *= Matrix4x4(xrot * yrot * zrot);
  15.512 +}
  15.513 +
  15.514 +void Matrix4x4::set_rotation(const Vector3 &euler_angles)
  15.515 +{
  15.516 +	Matrix3x3 xrot, yrot, zrot;
  15.517 +
  15.518 +	xrot = Matrix3x3(	1,			0,					0,
  15.519 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
  15.520 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
  15.521 +
  15.522 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
  15.523 +								0,				1,				0,
  15.524 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
  15.525 +
  15.526 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
  15.527 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
  15.528 +								0,						0,				1);
  15.529 +
  15.530 +	*this = Matrix4x4(xrot * yrot * zrot);
  15.531 +}
  15.532 +
  15.533 +void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
  15.534 +{
  15.535 +	scalar_t sina = (scalar_t)sin(angle);
  15.536 +	scalar_t cosa = (scalar_t)cos(angle);
  15.537 +	scalar_t invcosa = 1-cosa;
  15.538 +	scalar_t nxsq = axis.x * axis.x;
  15.539 +	scalar_t nysq = axis.y * axis.y;
  15.540 +	scalar_t nzsq = axis.z * axis.z;
  15.541 +
  15.542 +	Matrix3x3 xform;
  15.543 +	xform[0][0] = nxsq + (1-nxsq) * cosa;
  15.544 +	xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
  15.545 +	xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
  15.546 +	xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
  15.547 +	xform[1][1] = nysq + (1-nysq) * cosa;
  15.548 +	xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
  15.549 +	xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
  15.550 +	xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
  15.551 +	xform[2][2] = nzsq + (1-nzsq) * cosa;
  15.552 +
  15.553 +	*this *= Matrix4x4(xform);
  15.554 +}
  15.555 +
  15.556 +void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
  15.557 +{
  15.558 +	scalar_t sina = (scalar_t)sin(angle);
  15.559 +	scalar_t cosa = (scalar_t)cos(angle);
  15.560 +	scalar_t invcosa = 1-cosa;
  15.561 +	scalar_t nxsq = axis.x * axis.x;
  15.562 +	scalar_t nysq = axis.y * axis.y;
  15.563 +	scalar_t nzsq = axis.z * axis.z;
  15.564 +
  15.565 +	reset_identity();
  15.566 +	m[0][0] = nxsq + (1-nxsq) * cosa;
  15.567 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
  15.568 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
  15.569 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
  15.570 +	m[1][1] = nysq + (1-nysq) * cosa;
  15.571 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
  15.572 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
  15.573 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
  15.574 +	m[2][2] = nzsq + (1-nzsq) * cosa;
  15.575 +}
  15.576 +
  15.577 +void Matrix4x4::scale(const Vector4 &scale_vec)
  15.578 +{
  15.579 +	Matrix4x4 smat(	scale_vec.x, 0, 0, 0,
  15.580 +					0, scale_vec.y, 0, 0,
  15.581 +					0, 0, scale_vec.z, 0,
  15.582 +					0, 0, 0, scale_vec.w);
  15.583 +	*this *= smat;
  15.584 +}
  15.585 +
  15.586 +void Matrix4x4::set_scaling(const Vector4 &scale_vec)
  15.587 +{
  15.588 +	*this = Matrix4x4(	scale_vec.x, 0, 0, 0,
  15.589 +						0, scale_vec.y, 0, 0,
  15.590 +						0, 0, scale_vec.z, 0,
  15.591 +						0, 0, 0, scale_vec.w);
  15.592 +}
  15.593 +
  15.594 +void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
  15.595 +{
  15.596 +	m[0][col_index] = vec.x;
  15.597 +	m[1][col_index] = vec.y;
  15.598 +	m[2][col_index] = vec.z;
  15.599 +	m[3][col_index] = vec.w;
  15.600 +}
  15.601 +
  15.602 +void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
  15.603 +{
  15.604 +	m[row_index][0] = vec.x;
  15.605 +	m[row_index][1] = vec.y;
  15.606 +	m[row_index][2] = vec.z;
  15.607 +	m[row_index][3] = vec.w;
  15.608 +}
  15.609 +
  15.610 +Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
  15.611 +{
  15.612 +	return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
  15.613 +}
  15.614 +
  15.615 +Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
  15.616 +{
  15.617 +	return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
  15.618 +}
  15.619 +
  15.620 +void Matrix4x4::transpose()
  15.621 +{
  15.622 +	Matrix4x4 tmp = *this;
  15.623 +	for(int i=0; i<4; i++) {
  15.624 +		for(int j=0; j<4; j++) {
  15.625 +			m[i][j] = tmp[j][i];
  15.626 +		}
  15.627 +	}
  15.628 +}
  15.629 +
  15.630 +Matrix4x4 Matrix4x4::transposed() const
  15.631 +{
  15.632 +	Matrix4x4 res;
  15.633 +	for(int i=0; i<4; i++) {
  15.634 +		for(int j=0; j<4; j++) {
  15.635 +			res[i][j] = m[j][i];
  15.636 +		}
  15.637 +	}
  15.638 +	return res;
  15.639 +}
  15.640 +
  15.641 +scalar_t Matrix4x4::determinant() const
  15.642 +{
  15.643 +	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  15.644 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  15.645 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  15.646 +
  15.647 +	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  15.648 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  15.649 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  15.650 +
  15.651 +	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  15.652 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  15.653 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  15.654 +
  15.655 +	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  15.656 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  15.657 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  15.658 +
  15.659 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
  15.660 +}
  15.661 +
  15.662 +
  15.663 +Matrix4x4 Matrix4x4::adjoint() const
  15.664 +{
  15.665 +	Matrix4x4 coef;
  15.666 +
  15.667 +	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  15.668 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  15.669 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  15.670 +	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  15.671 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  15.672 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  15.673 +	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  15.674 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  15.675 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  15.676 +	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  15.677 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  15.678 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  15.679 +
  15.680 +	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  15.681 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  15.682 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  15.683 +	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  15.684 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  15.685 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  15.686 +	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  15.687 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  15.688 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  15.689 +	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  15.690 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  15.691 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  15.692 +
  15.693 +	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
  15.694 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
  15.695 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
  15.696 +	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
  15.697 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
  15.698 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
  15.699 +	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
  15.700 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
  15.701 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
  15.702 +	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
  15.703 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
  15.704 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
  15.705 +
  15.706 +	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
  15.707 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
  15.708 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
  15.709 +	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
  15.710 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
  15.711 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
  15.712 +	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
  15.713 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
  15.714 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
  15.715 +	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
  15.716 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
  15.717 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
  15.718 +
  15.719 +	coef.transpose();
  15.720 +
  15.721 +	for(int i=0; i<4; i++) {
  15.722 +		for(int j=0; j<4; j++) {
  15.723 +			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
  15.724 +			if(i%2) coef.m[i][j] = -coef.m[i][j];
  15.725 +		}
  15.726 +	}
  15.727 +
  15.728 +	return coef;
  15.729 +}
  15.730 +
  15.731 +Matrix4x4 Matrix4x4::inverse() const
  15.732 +{
  15.733 +	Matrix4x4 adj = adjoint();
  15.734 +
  15.735 +	return adj * (1.0f / determinant());
  15.736 +}
  15.737 +
  15.738 +ostream &operator <<(ostream &out, const Matrix4x4 &mat)
  15.739 +{
  15.740 +	for(int i=0; i<4; i++) {
  15.741 +		char str[100];
  15.742 +		sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]);
  15.743 +		out << str;
  15.744 +	}
  15.745 +	return out;
  15.746 +}

    16.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    16.2 +++ b/prototype/vmath/matrix.h	Thu Jun 28 06:05:50 2012 +0300
    16.3 @@ -0,0 +1,206 @@
    16.4 +/*
    16.5 +libvmath - a vector math library
    16.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    16.7 +
    16.8 +This program is free software: you can redistribute it and/or modify
    16.9 +it under the terms of the GNU Lesser General Public License as published
   16.10 +by the Free Software Foundation, either version 3 of the License, or
   16.11 +(at your option) any later version.
   16.12 +
   16.13 +This program is distributed in the hope that it will be useful,
   16.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   16.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   16.16 +GNU Lesser General Public License for more details.
   16.17 +
   16.18 +You should have received a copy of the GNU Lesser General Public License
   16.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   16.20 +*/
   16.21 +
   16.22 +#ifndef VMATH_MATRIX_H_
   16.23 +#define VMATH_MATRIX_H_
   16.24 +
   16.25 +#include <stdio.h>
   16.26 +#include "vmath_types.h"
   16.27 +
   16.28 +#ifdef __cplusplus
   16.29 +extern "C" {
   16.30 +#endif	/* __cplusplus */
   16.31 +
   16.32 +/* C matrix 3x3 functions */
   16.33 +static inline void m3_identity(mat3_t m);
   16.34 +static inline void m3_cons(mat3_t m,
   16.35 +		scalar_t m11, scalar_t m12, scalar_t m13,
   16.36 +		scalar_t m21, scalar_t m22, scalar_t m23,
   16.37 +		scalar_t m31, scalar_t m32, scalar_t m33);
   16.38 +static inline void m3_copy(mat3_t dest, mat3_t src);
   16.39 +void m3_to_m4(mat4_t dest, mat3_t src);
   16.40 +
   16.41 +void m3_print(FILE *fp, mat3_t m);
   16.42 +
   16.43 +/* C matrix 4x4 functions */
   16.44 +static inline void m4_identity(mat4_t m);
   16.45 +static inline void m4_cons(mat4_t m,
   16.46 +		scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
   16.47 +		scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
   16.48 +		scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
   16.49 +		scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44);
   16.50 +static inline void m4_copy(mat4_t dest, mat4_t src);
   16.51 +void m4_to_m3(mat3_t dest, mat4_t src);
   16.52 +
   16.53 +static inline void m4_mult(mat4_t res, mat4_t m1, mat4_t m2);
   16.54 +
   16.55 +void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z);
   16.56 +void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z);
   16.57 +void m4_rotate_x(mat4_t m, scalar_t angle);
   16.58 +void m4_rotate_y(mat4_t m, scalar_t angle);
   16.59 +void m4_rotate_z(mat4_t m, scalar_t angle);
   16.60 +void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z);
   16.61 +void m4_rotate_quat(mat4_t m, quat_t q);
   16.62 +void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z);
   16.63 +static inline void m4_set_column(mat4_t m, vec4_t v, int idx);
   16.64 +static inline void m4_set_row(mat4_t m, vec4_t v, int idx);
   16.65 +
   16.66 +void m4_transpose(mat4_t res, mat4_t m);
   16.67 +scalar_t m4_determinant(mat4_t m);
   16.68 +void m4_adjoint(mat4_t res, mat4_t m);
   16.69 +void m4_inverse(mat4_t res, mat4_t m);
   16.70 +
   16.71 +void m4_print(FILE *fp, mat4_t m);
   16.72 +
   16.73 +#ifdef __cplusplus
   16.74 +}
   16.75 +
   16.76 +/* when included from C++ source files, also define the matrix classes */
   16.77 +#include <iostream>
   16.78 +
   16.79 +/** 3x3 matrix */
   16.80 +class Matrix3x3 {
   16.81 +private:
   16.82 +	scalar_t m[3][3];
   16.83 +
   16.84 +public:
   16.85 +
   16.86 +	static Matrix3x3 identity;
   16.87 +
   16.88 +	Matrix3x3();
   16.89 +	Matrix3x3(	scalar_t m11, scalar_t m12, scalar_t m13,
   16.90 +				scalar_t m21, scalar_t m22, scalar_t m23,
   16.91 +				scalar_t m31, scalar_t m32, scalar_t m33);
   16.92 +	Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec);
   16.93 +	Matrix3x3(const mat3_t cmat);
   16.94 +
   16.95 +	Matrix3x3(const Matrix4x4 &mat4x4);
   16.96 +
   16.97 +	/* binary operations matrix (op) matrix */
   16.98 +	friend Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2);
   16.99 +	friend Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2);
  16.100 +	friend Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2);
  16.101 +
  16.102 +	friend void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2);
  16.103 +	friend void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2);
  16.104 +	friend void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2);
  16.105 +
  16.106 +	/* binary operations matrix (op) scalar and scalar (op) matrix */
  16.107 +	friend Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar);
  16.108 +	friend Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat);
  16.109 +
  16.110 +	friend void operator *=(Matrix3x3 &mat, scalar_t scalar);
  16.111 +
  16.112 +	inline scalar_t *operator [](int index);
  16.113 +	inline const scalar_t *operator [](int index) const;
  16.114 +
  16.115 +	inline void reset_identity();
  16.116 +
  16.117 +	void translate(const Vector2 &trans);
  16.118 +	void set_translation(const Vector2 &trans);
  16.119 +
  16.120 +	void rotate(scalar_t angle);						/* 2d rotation */
  16.121 +	void rotate(const Vector3 &euler_angles);			/* 3d rotation with euler angles */
  16.122 +	void rotate(const Vector3 &axis, scalar_t angle);	/* 3d axis/angle rotation */
  16.123 +	void set_rotation(scalar_t angle);
  16.124 +	void set_rotation(const Vector3 &euler_angles);
  16.125 +	void set_rotation(const Vector3 &axis, scalar_t angle);
  16.126 +
  16.127 +	void scale(const Vector3 &scale_vec);
  16.128 +	void set_scaling(const Vector3 &scale_vec);
  16.129 +
  16.130 +	void set_column_vector(const Vector3 &vec, unsigned int col_index);
  16.131 +	void set_row_vector(const Vector3 &vec, unsigned int row_index);
  16.132 +	Vector3 get_column_vector(unsigned int col_index) const;
  16.133 +	Vector3 get_row_vector(unsigned int row_index) const;
  16.134 +
  16.135 +	void transpose();
  16.136 +	Matrix3x3 transposed() const;
  16.137 +	scalar_t determinant() const;
  16.138 +	Matrix3x3 inverse() const;
  16.139 +
  16.140 +	friend std::ostream &operator <<(std::ostream &out, const Matrix3x3 &mat);
  16.141 +};
  16.142 +
  16.143 +/** 4x4 matrix */
  16.144 +class Matrix4x4 {
  16.145 +private:
  16.146 +	scalar_t m[4][4];
  16.147 +
  16.148 +public:
  16.149 +
  16.150 +	static Matrix4x4 identity;
  16.151 +
  16.152 +	Matrix4x4();
  16.153 +	Matrix4x4(	scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
  16.154 +				scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
  16.155 +				scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
  16.156 +				scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44);
  16.157 +	Matrix4x4(const mat4_t cmat);
  16.158 +
  16.159 +	Matrix4x4(const Matrix3x3 &mat3x3);
  16.160 +
  16.161 +	/* binary operations matrix (op) matrix */
  16.162 +	friend Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2);
  16.163 +	friend Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2);
  16.164 +	friend Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2);
  16.165 +
  16.166 +	friend void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2);
  16.167 +	friend void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2);
  16.168 +	friend inline void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2);
  16.169 +
  16.170 +	/* binary operations matrix (op) scalar and scalar (op) matrix */
  16.171 +	friend Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar);
  16.172 +	friend Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat);
  16.173 +
  16.174 +	friend void operator *=(Matrix4x4 &mat, scalar_t scalar);
  16.175 +
  16.176 +	inline scalar_t *operator [](int index);
  16.177 +	inline const scalar_t *operator [](int index) const;
  16.178 +
  16.179 +	inline void reset_identity();
  16.180 +
  16.181 +	void translate(const Vector3 &trans);
  16.182 +	void set_translation(const Vector3 &trans);
  16.183 +
  16.184 +	void rotate(const Vector3 &euler_angles);			/* 3d rotation with euler angles */
  16.185 +	void rotate(const Vector3 &axis, scalar_t angle);	/* 3d axis/angle rotation */
  16.186 +	void set_rotation(const Vector3 &euler_angles);
  16.187 +	void set_rotation(const Vector3 &axis, scalar_t angle);
  16.188 +
  16.189 +	void scale(const Vector4 &scale_vec);
  16.190 +	void set_scaling(const Vector4 &scale_vec);
  16.191 +
  16.192 +	void set_column_vector(const Vector4 &vec, unsigned int col_index);
  16.193 +	void set_row_vector(const Vector4 &vec, unsigned int row_index);
  16.194 +	Vector4 get_column_vector(unsigned int col_index) const;
  16.195 +	Vector4 get_row_vector(unsigned int row_index) const;
  16.196 +
  16.197 +	void transpose();
  16.198 +	Matrix4x4 transposed() const;
  16.199 +	scalar_t determinant() const;
  16.200 +	Matrix4x4 adjoint() const;
  16.201 +	Matrix4x4 inverse() const;
  16.202 +
  16.203 +	friend std::ostream &operator <<(std::ostream &out, const Matrix4x4 &mat);
  16.204 +};
  16.205 +#endif	/* __cplusplus */
  16.206 +
  16.207 +#include "matrix.inl"
  16.208 +
  16.209 +#endif	/* VMATH_MATRIX_H_ */

    17.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    17.2 +++ b/prototype/vmath/matrix.inl	Thu Jun 28 06:05:50 2012 +0300
    17.3 @@ -0,0 +1,165 @@
    17.4 +/*
    17.5 +libvmath - a vector math library
    17.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    17.7 +
    17.8 +This program is free software: you can redistribute it and/or modify
    17.9 +it under the terms of the GNU Lesser General Public License as published
   17.10 +by the Free Software Foundation, either version 3 of the License, or
   17.11 +(at your option) any later version.
   17.12 +
   17.13 +This program is distributed in the hope that it will be useful,
   17.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   17.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   17.16 +GNU Lesser General Public License for more details.
   17.17 +
   17.18 +You should have received a copy of the GNU Lesser General Public License
   17.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   17.20 +*/
   17.21 +
   17.22 +#include <string.h>
   17.23 +
   17.24 +#ifdef __cplusplus
   17.25 +extern "C" {
   17.26 +#endif	/* __cplusplus */
   17.27 +
   17.28 +/* C matrix 3x3 functions */
   17.29 +static inline void m3_identity(mat3_t m)
   17.30 +{
   17.31 +	static const mat3_t id = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
   17.32 +	memcpy(m, id, sizeof id);
   17.33 +}
   17.34 +
   17.35 +static inline void m3_cons(mat3_t m,
   17.36 +		scalar_t m11, scalar_t m12, scalar_t m13,
   17.37 +		scalar_t m21, scalar_t m22, scalar_t m23,
   17.38 +		scalar_t m31, scalar_t m32, scalar_t m33)
   17.39 +{
   17.40 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
   17.41 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
   17.42 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
   17.43 +}
   17.44 +
   17.45 +static inline void m3_copy(mat3_t dest, mat3_t src)
   17.46 +{
   17.47 +	memcpy(dest, src, sizeof(mat3_t));
   17.48 +}
   17.49 +
   17.50 +
   17.51 +/* C matrix 4x4 functions */
   17.52 +static inline void m4_identity(mat4_t m)
   17.53 +{
   17.54 +	static const mat4_t id = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
   17.55 +	memcpy(m, id, sizeof id);
   17.56 +}
   17.57 +
   17.58 +static inline void m4_cons(mat4_t m,
   17.59 +		scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
   17.60 +		scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
   17.61 +		scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
   17.62 +		scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
   17.63 +{
   17.64 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
   17.65 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
   17.66 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
   17.67 +	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
   17.68 +}
   17.69 +
   17.70 +static inline void m4_copy(mat4_t dest, mat4_t src)
   17.71 +{
   17.72 +	memcpy(dest, src, sizeof(mat4_t));
   17.73 +}
   17.74 +
   17.75 +static inline void m4_mult(mat4_t res, mat4_t m1, mat4_t m2)
   17.76 +{
   17.77 +	res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0] + m1[0][3] * m2[3][0];
   17.78 +	res[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1] + m1[0][3] * m2[3][1];
   17.79 +	res[0][2] = m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2] + m1[0][3] * m2[3][2];
   17.80 +	res[0][3] = m1[0][0] * m2[0][3] + m1[0][1] * m2[1][3] + m1[0][2] * m2[2][3] + m1[0][3] * m2[3][3];
   17.81 +
   17.82 +	res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0] + m1[1][3] * m2[3][0];
   17.83 +	res[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1] + m1[1][3] * m2[3][1];
   17.84 +	res[1][2] = m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2] + m1[1][3] * m2[3][2];
   17.85 +	res[1][3] = m1[1][0] * m2[0][3] + m1[1][1] * m2[1][3] + m1[1][2] * m2[2][3] + m1[1][3] * m2[3][3];
   17.86 +
   17.87 +	res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0] + m1[2][3] * m2[3][0];
   17.88 +	res[2][1] = m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1] + m1[2][3] * m2[3][1];
   17.89 +	res[2][2] = m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2] + m1[2][3] * m2[3][2];
   17.90 +	res[2][3] = m1[2][0] * m2[0][3] + m1[2][1] * m2[1][3] + m1[2][2] * m2[2][3] + m1[2][3] * m2[3][3];
   17.91 +
   17.92 +	res[3][0] = m1[3][0] * m2[0][0] + m1[3][1] * m2[1][0] + m1[3][2] * m2[2][0] + m1[3][3] * m2[3][0];
   17.93 +	res[3][1] = m1[3][0] * m2[0][1] + m1[3][1] * m2[1][1] + m1[3][2] * m2[2][1] + m1[3][3] * m2[3][1];
   17.94 +	res[3][2] = m1[3][0] * m2[0][2] + m1[3][1] * m2[1][2] + m1[3][2] * m2[2][2] + m1[3][3] * m2[3][2];
   17.95 +	res[3][3] = m1[3][0] * m2[0][3] + m1[3][1] * m2[1][3] + m1[3][2] * m2[2][3] + m1[3][3] * m2[3][3];
   17.96 +}
   17.97 +
   17.98 +static inline void m4_set_column(mat4_t m, vec4_t v, int idx)
   17.99 +{
  17.100 +	m[0][idx] = v.x;
  17.101 +	m[1][idx] = v.y;
  17.102 +	m[2][idx] = v.z;
  17.103 +	m[3][idx] = v.w;
  17.104 +}
  17.105 +
  17.106 +static inline void m4_set_row(mat4_t m, vec4_t v, int idx)
  17.107 +{
  17.108 +	m[idx][0] = v.x;
  17.109 +	m[idx][1] = v.y;
  17.110 +	m[idx][2] = v.z;
  17.111 +	m[idx][3] = v.w;
  17.112 +}
  17.113 +
  17.114 +#ifdef __cplusplus
  17.115 +}	/* extern "C" */
  17.116 +
  17.117 +
  17.118 +/* unrolled to hell and inline */
  17.119 +inline Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
  17.120 +	Matrix4x4 res;
  17.121 +
  17.122 +	res.m[0][0] = m1.m[0][0] * m2.m[0][0] + m1.m[0][1] * m2.m[1][0] + m1.m[0][2] * m2.m[2][0] + m1.m[0][3] * m2.m[3][0];
  17.123 +	res.m[0][1] = m1.m[0][0] * m2.m[0][1] + m1.m[0][1] * m2.m[1][1] + m1.m[0][2] * m2.m[2][1] + m1.m[0][3] * m2.m[3][1];
  17.124 +	res.m[0][2] = m1.m[0][0] * m2.m[0][2] + m1.m[0][1] * m2.m[1][2] + m1.m[0][2] * m2.m[2][2] + m1.m[0][3] * m2.m[3][2];
  17.125 +	res.m[0][3] = m1.m[0][0] * m2.m[0][3] + m1.m[0][1] * m2.m[1][3] + m1.m[0][2] * m2.m[2][3] + m1.m[0][3] * m2.m[3][3];
  17.126 +
  17.127 +	res.m[1][0] = m1.m[1][0] * m2.m[0][0] + m1.m[1][1] * m2.m[1][0] + m1.m[1][2] * m2.m[2][0] + m1.m[1][3] * m2.m[3][0];
  17.128 +	res.m[1][1] = m1.m[1][0] * m2.m[0][1] + m1.m[1][1] * m2.m[1][1] + m1.m[1][2] * m2.m[2][1] + m1.m[1][3] * m2.m[3][1];
  17.129 +	res.m[1][2] = m1.m[1][0] * m2.m[0][2] + m1.m[1][1] * m2.m[1][2] + m1.m[1][2] * m2.m[2][2] + m1.m[1][3] * m2.m[3][2];
  17.130 +	res.m[1][3] = m1.m[1][0] * m2.m[0][3] + m1.m[1][1] * m2.m[1][3] + m1.m[1][2] * m2.m[2][3] + m1.m[1][3] * m2.m[3][3];
  17.131 +
  17.132 +	res.m[2][0] = m1.m[2][0] * m2.m[0][0] + m1.m[2][1] * m2.m[1][0] + m1.m[2][2] * m2.m[2][0] + m1.m[2][3] * m2.m[3][0];
  17.133 +	res.m[2][1] = m1.m[2][0] * m2.m[0][1] + m1.m[2][1] * m2.m[1][1] + m1.m[2][2] * m2.m[2][1] + m1.m[2][3] * m2.m[3][1];
  17.134 +	res.m[2][2] = m1.m[2][0] * m2.m[0][2] + m1.m[2][1] * m2.m[1][2] + m1.m[2][2] * m2.m[2][2] + m1.m[2][3] * m2.m[3][2];
  17.135 +	res.m[2][3] = m1.m[2][0] * m2.m[0][3] + m1.m[2][1] * m2.m[1][3] + m1.m[2][2] * m2.m[2][3] + m1.m[2][3] * m2.m[3][3];
  17.136 +
  17.137 +	res.m[3][0] = m1.m[3][0] * m2.m[0][0] + m1.m[3][1] * m2.m[1][0] + m1.m[3][2] * m2.m[2][0] + m1.m[3][3] * m2.m[3][0];
  17.138 +	res.m[3][1] = m1.m[3][0] * m2.m[0][1] + m1.m[3][1] * m2.m[1][1] + m1.m[3][2] * m2.m[2][1] + m1.m[3][3] * m2.m[3][1];
  17.139 +	res.m[3][2] = m1.m[3][0] * m2.m[0][2] + m1.m[3][1] * m2.m[1][2] + m1.m[3][2] * m2.m[2][2] + m1.m[3][3] * m2.m[3][2];
  17.140 +	res.m[3][3] = m1.m[3][0] * m2.m[0][3] + m1.m[3][1] * m2.m[1][3] + m1.m[3][2] * m2.m[2][3] + m1.m[3][3] * m2.m[3][3];
  17.141 +
  17.142 +	return res;
  17.143 +}
  17.144 +
  17.145 +inline scalar_t *Matrix3x3::operator [](int index) {
  17.146 +	return m[index];
  17.147 +}
  17.148 +
  17.149 +inline const scalar_t *Matrix3x3::operator [](int index) const {
  17.150 +	return m[index];
  17.151 +}
  17.152 +
  17.153 +inline void Matrix3x3::reset_identity() {
  17.154 +	memcpy(this->m, identity.m, 9 * sizeof(scalar_t));
  17.155 +}
  17.156 +
  17.157 +inline scalar_t *Matrix4x4::operator [](int index) {
  17.158 +	return m[index];
  17.159 +}
  17.160 +
  17.161 +inline const scalar_t *Matrix4x4::operator [](int index) const {
  17.162 +	return m[index];
  17.163 +}
  17.164 +
  17.165 +inline void Matrix4x4::reset_identity() {
  17.166 +	memcpy(this->m, identity.m, 16 * sizeof(scalar_t));
  17.167 +}
  17.168 +#endif	/* __cplusplus */

    18.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    18.2 +++ b/prototype/vmath/matrix_c.c	Thu Jun 28 06:05:50 2012 +0300
    18.3 @@ -0,0 +1,265 @@
    18.4 +/*
    18.5 +libvmath - a vector math library
    18.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    18.7 +
    18.8 +This program is free software: you can redistribute it and/or modify
    18.9 +it under the terms of the GNU Lesser General Public License as published
   18.10 +by the Free Software Foundation, either version 3 of the License, or
   18.11 +(at your option) any later version.
   18.12 +
   18.13 +This program is distributed in the hope that it will be useful,
   18.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   18.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   18.16 +GNU Lesser General Public License for more details.
   18.17 +
   18.18 +You should have received a copy of the GNU Lesser General Public License
   18.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   18.20 +*/
   18.21 +
   18.22 +
   18.23 +#include <stdio.h>
   18.24 +#include "matrix.h"
   18.25 +#include "vector.h"
   18.26 +#include "quat.h"
   18.27 +
   18.28 +void m3_to_m4(mat4_t dest, mat3_t src)
   18.29 +{
   18.30 +	int i, j;
   18.31 +
   18.32 +	memset(dest, 0, sizeof(mat4_t));
   18.33 +	for(i=0; i<3; i++) {
   18.34 +		for(j=0; j<3; j++) {
   18.35 +			dest[i][j] = src[i][j];
   18.36 +		}
   18.37 +	}
   18.38 +	dest[3][3] = 1.0;
   18.39 +}
   18.40 +
   18.41 +void m3_print(FILE *fp, mat3_t m)
   18.42 +{
   18.43 +	int i;
   18.44 +	for(i=0; i<3; i++) {
   18.45 +		fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]);
   18.46 +	}
   18.47 +}
   18.48 +
   18.49 +/* C matrix 4x4 functions */
   18.50 +void m4_to_m3(mat3_t dest, mat4_t src)
   18.51 +{
   18.52 +	int i, j;
   18.53 +	for(i=0; i<3; i++) {
   18.54 +		for(j=0; j<3; j++) {
   18.55 +			dest[i][j] = src[i][j];
   18.56 +		}
   18.57 +	}
   18.58 +}
   18.59 +
   18.60 +void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
   18.61 +{
   18.62 +	mat4_t tm;
   18.63 +	m4_identity(tm);
   18.64 +	tm[0][3] = x;
   18.65 +	tm[1][3] = y;
   18.66 +	tm[2][3] = z;
   18.67 +	m4_mult(m, m, tm);
   18.68 +}
   18.69 +
   18.70 +void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
   18.71 +{
   18.72 +	m4_rotate_x(m, x);
   18.73 +	m4_rotate_y(m, y);
   18.74 +	m4_rotate_z(m, z);
   18.75 +}
   18.76 +
   18.77 +void m4_rotate_x(mat4_t m, scalar_t angle)
   18.78 +{
   18.79 +	mat4_t rm;
   18.80 +	m4_identity(rm);
   18.81 +	rm[1][1] = cos(angle); rm[1][2] = -sin(angle);
   18.82 +	rm[2][1] = sin(angle); rm[2][2] = cos(angle);
   18.83 +	m4_mult(m, m, rm);
   18.84 +}
   18.85 +
   18.86 +void m4_rotate_y(mat4_t m, scalar_t angle)
   18.87 +{
   18.88 +	mat4_t rm;
   18.89 +	m4_identity(rm);
   18.90 +	rm[0][0] = cos(angle); rm[0][2] = sin(angle);
   18.91 +	rm[2][0] = -sin(angle); rm[2][2] = cos(angle);
   18.92 +	m4_mult(m, m, rm);
   18.93 +}
   18.94 +
   18.95 +void m4_rotate_z(mat4_t m, scalar_t angle)
   18.96 +{
   18.97 +	mat4_t rm;
   18.98 +	m4_identity(rm);
   18.99 +	rm[0][0] = cos(angle); rm[0][1] = -sin(angle);
  18.100 +	rm[1][0] = sin(angle); rm[1][1] = cos(angle);
  18.101 +	m4_mult(m, m, rm);
  18.102 +}
  18.103 +
  18.104 +void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
  18.105 +{
  18.106 +	mat4_t xform;
  18.107 +	scalar_t sina = sin(angle);
  18.108 +	scalar_t cosa = cos(angle);
  18.109 +	scalar_t one_minus_cosa = 1.0 - cosa;
  18.110 +	scalar_t nxsq = x * x;
  18.111 +	scalar_t nysq = y * y;
  18.112 +	scalar_t nzsq = z * z;
  18.113 +
  18.114 +	m4_identity(xform);
  18.115 +	xform[0][0] = nxsq + (1.0 - nxsq) * cosa;
  18.116 +	xform[0][1] = x * y * one_minus_cosa - z * sina;
  18.117 +	xform[0][2] = x * z * one_minus_cosa + y * sina;
  18.118 +	xform[1][0] = x * y * one_minus_cosa + z * sina;
  18.119 +	xform[1][1] = nysq + (1.0 - nysq) * cosa;
  18.120 +	xform[1][2] = y * z * one_minus_cosa - x * sina;
  18.121 +	xform[2][0] = x * z * one_minus_cosa - y * sina;
  18.122 +	xform[2][1] = y * z * one_minus_cosa + x * sina;
  18.123 +	xform[2][2] = nzsq + (1.0 - nzsq) * cosa;
  18.124 +
  18.125 +	m4_mult(m, m, xform);
  18.126 +}
  18.127 +
  18.128 +void m4_rotate_quat(mat4_t m, quat_t q)
  18.129 +{
  18.130 +	mat4_t rm;
  18.131 +	quat_to_mat4(rm, q);
  18.132 +	m4_mult(m, m, rm);
  18.133 +}
  18.134 +
  18.135 +void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
  18.136 +{
  18.137 +	mat4_t sm;
  18.138 +	m4_identity(sm);
  18.139 +	sm[0][0] = x;
  18.140 +	sm[1][1] = y;
  18.141 +	sm[2][2] = z;
  18.142 +	m4_mult(m, m, sm);
  18.143 +}
  18.144 +
  18.145 +void m4_transpose(mat4_t res, mat4_t m)
  18.146 +{
  18.147 +	int i, j;
  18.148 +	mat4_t tmp;
  18.149 +	m4_copy(tmp, m);
  18.150 +
  18.151 +	for(i=0; i<4; i++) {
  18.152 +		for(j=0; j<4; j++) {
  18.153 +			res[i][j] = tmp[j][i];
  18.154 +		}
  18.155 +	}
  18.156 +}
  18.157 +
  18.158 +scalar_t m4_determinant(mat4_t m)
  18.159 +{
  18.160 +	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  18.161 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  18.162 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  18.163 +
  18.164 +	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  18.165 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  18.166 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  18.167 +
  18.168 +	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  18.169 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  18.170 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  18.171 +
  18.172 +	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  18.173 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  18.174 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  18.175 +
  18.176 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
  18.177 +}
  18.178 +
  18.179 +void m4_adjoint(mat4_t res, mat4_t m)
  18.180 +{
  18.181 +	int i, j;
  18.182 +	mat4_t coef;
  18.183 +
  18.184 +	coef[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  18.185 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  18.186 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  18.187 +	coef[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  18.188 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  18.189 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  18.190 +	coef[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  18.191 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  18.192 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  18.193 +	coef[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  18.194 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  18.195 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  18.196 +
  18.197 +	coef[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  18.198 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  18.199 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  18.200 +	coef[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  18.201 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  18.202 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  18.203 +	coef[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  18.204 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  18.205 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  18.206 +	coef[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  18.207 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  18.208 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  18.209 +
  18.210 +	coef[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
  18.211 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
  18.212 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
  18.213 +	coef[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
  18.214 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
  18.215 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
  18.216 +	coef[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
  18.217 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
  18.218 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
  18.219 +	coef[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
  18.220 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
  18.221 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
  18.222 +
  18.223 +	coef[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
  18.224 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
  18.225 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
  18.226 +	coef[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
  18.227 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
  18.228 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
  18.229 +	coef[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
  18.230 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
  18.231 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
  18.232 +	coef[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
  18.233 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
  18.234 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
  18.235 +
  18.236 +	m4_transpose(res, coef);
  18.237 +
  18.238 +	for(i=0; i<4; i++) {
  18.239 +		for(j=0; j<4; j++) {
  18.240 +			res[i][j] = j % 2 ? -res[i][j] : res[i][j];
  18.241 +			if(i % 2) res[i][j] = -res[i][j];
  18.242 +		}
  18.243 +	}
  18.244 +}
  18.245 +
  18.246 +void m4_inverse(mat4_t res, mat4_t m)
  18.247 +{
  18.248 +	int i, j;
  18.249 +	mat4_t adj;
  18.250 +	scalar_t det;
  18.251 +
  18.252 +	m4_adjoint(adj, m);
  18.253 +	det = m4_determinant(m);
  18.254 +	
  18.255 +	for(i=0; i<4; i++) {
  18.256 +		for(j=0; j<4; j++) {
  18.257 +			res[i][j] = adj[i][j] / det;
  18.258 +		}
  18.259 +	}
  18.260 +}
  18.261 +
  18.262 +void m4_print(FILE *fp, mat4_t m)
  18.263 +{
  18.264 +	int i;
  18.265 +	for(i=0; i<4; i++) {
  18.266 +		fprintf(fp, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2], (float)m[i][3]);
  18.267 +	}
  18.268 +}

    19.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    19.2 +++ b/prototype/vmath/quat.cc	Thu Jun 28 06:05:50 2012 +0300
    19.3 @@ -0,0 +1,159 @@
    19.4 +#include "quat.h"
    19.5 +
    19.6 +Quaternion::Quaternion() {
    19.7 +	s = 1.0;
    19.8 +	v.x = v.y = v.z = 0.0;
    19.9 +}
   19.10 +
   19.11 +Quaternion::Quaternion(scalar_t s, const Vector3 &v) {
   19.12 +	this->s = s;
   19.13 +	this->v = v;
   19.14 +}
   19.15 +
   19.16 +Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z) {
   19.17 +	v.x = x;
   19.18 +	v.y = y;
   19.19 +	v.z = z;
   19.20 +	this->s = s;
   19.21 +}
   19.22 +
   19.23 +Quaternion::Quaternion(const Vector3 &axis, scalar_t angle) {
   19.24 +	set_rotation(axis, angle);
   19.25 +}
   19.26 +
   19.27 +Quaternion::Quaternion(const quat_t &quat)
   19.28 +{
   19.29 +	v.x = quat.x;
   19.30 +	v.y = quat.y;
   19.31 +	v.z = quat.z;
   19.32 +	s = quat.w;
   19.33 +}
   19.34 +
   19.35 +Quaternion Quaternion::operator +(const Quaternion &quat) const {
   19.36 +	return Quaternion(s + quat.s, v + quat.v);
   19.37 +}
   19.38 +
   19.39 +Quaternion Quaternion::operator -(const Quaternion &quat) const {
   19.40 +	return Quaternion(s - quat.s, v - quat.v);
   19.41 +}
   19.42 +
   19.43 +Quaternion Quaternion::operator -() const {
   19.44 +	return Quaternion(-s, -v);
   19.45 +}
   19.46 +
   19.47 +/** Quaternion Multiplication:
   19.48 + * Q1*Q2 = [s1*s2 - v1.v2,  s1*v2 + s2*v1 + v1(x)v2]
   19.49 + */
   19.50 +Quaternion Quaternion::operator *(const Quaternion &quat) const {
   19.51 +	Quaternion newq;
   19.52 +	newq.s = s * quat.s - dot_product(v, quat.v);
   19.53 +	newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
   19.54 +	return newq;
   19.55 +}
   19.56 +
   19.57 +void Quaternion::operator +=(const Quaternion &quat) {
   19.58 +	*this = Quaternion(s + quat.s, v + quat.v);
   19.59 +}
   19.60 +
   19.61 +void Quaternion::operator -=(const Quaternion &quat) {
   19.62 +	*this = Quaternion(s - quat.s, v - quat.v);
   19.63 +}
   19.64 +
   19.65 +void Quaternion::operator *=(const Quaternion &quat) {
   19.66 +	*this = *this * quat;
   19.67 +}
   19.68 +
   19.69 +void Quaternion::reset_identity() {
   19.70 +	s = 1.0;
   19.71 +	v.x = v.y = v.z = 0.0;
   19.72 +}
   19.73 +
   19.74 +Quaternion Quaternion::conjugate() const {
   19.75 +	return Quaternion(s, -v);
   19.76 +}
   19.77 +
   19.78 +scalar_t Quaternion::length() const {
   19.79 +	return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   19.80 +}
   19.81 +
   19.82 +/** Q * ~Q = ||Q||^2 */
   19.83 +scalar_t Quaternion::length_sq() const {
   19.84 +	return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
   19.85 +}
   19.86 +
   19.87 +void Quaternion::normalize() {
   19.88 +	scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   19.89 +	v.x /= len;
   19.90 +	v.y /= len;
   19.91 +	v.z /= len;
   19.92 +	s /= len;
   19.93 +}
   19.94 +
   19.95 +Quaternion Quaternion::normalized() const {
   19.96 +	Quaternion nq = *this;
   19.97 +	scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   19.98 +	nq.v.x /= len;
   19.99 +	nq.v.y /= len;
  19.100 +	nq.v.z /= len;
  19.101 +	nq.s /= len;
  19.102 +	return nq;
  19.103 +}
  19.104 +
  19.105 +/** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
  19.106 +Quaternion Quaternion::inverse() const {
  19.107 +	Quaternion inv = conjugate();
  19.108 +	scalar_t lensq = length_sq();
  19.109 +	inv.v /= lensq;
  19.110 +	inv.s /= lensq;
  19.111 +
  19.112 +	return inv;
  19.113 +}
  19.114 +
  19.115 +
  19.116 +void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle) {
  19.117 +	scalar_t half_angle = angle / 2.0;
  19.118 +	s = cos(half_angle);
  19.119 +	v = axis * sin(half_angle);
  19.120 +}
  19.121 +
  19.122 +void Quaternion::rotate(const Vector3 &axis, scalar_t angle) {
  19.123 +	Quaternion q;
  19.124 +	scalar_t half_angle = angle / 2.0;
  19.125 +	q.s = cos(half_angle);
  19.126 +	q.v = axis * sin(half_angle);
  19.127 +
  19.128 +	*this *= q;
  19.129 +}
  19.130 +
  19.131 +void Quaternion::rotate(const Quaternion &q) {
  19.132 +	*this = q * *this * q.conjugate();
  19.133 +}
  19.134 +
  19.135 +Matrix3x3 Quaternion::get_rotation_matrix() const {
  19.136 +	return Matrix3x3(	1.0 - 2.0 * v.y*v.y - 2.0 * v.z*v.z,	2.0 * v.x * v.y + 2.0 * s * v.z,		2.0 * v.z * v.x - 2.0 * s * v.y,
  19.137 +						2.0 * v.x * v.y - 2.0 * s * v.z,		1.0 - 2.0 * v.x*v.x - 2.0 * v.z*v.z,	2.0 * v.y * v.z + 2.0 * s * v.x,
  19.138 +						2.0 * v.z * v.x + 2.0 * s * v.y,		2.0 * v.y * v.z - 2.0 * s * v.x,		1.0 - 2.0 * v.x*v.x - 2.0 * v.y*v.y);
  19.139 +}
  19.140 +
  19.141 +
  19.142 +/** Spherical linear interpolation (slerp) */
  19.143 +Quaternion slerp(const Quaternion &q1, const Quaternion &q2, scalar_t t) {
  19.144 +	scalar_t angle = acos(q1.s * q2.s + q1.v.x * q2.v.x + q1.v.y * q2.v.y + q1.v.z * q2.v.z);
  19.145 +	scalar_t a = sin((1.0f - t) * angle);
  19.146 +	scalar_t b = sin(t * angle);
  19.147 +	scalar_t c = sin(angle);
  19.148 +
  19.149 +	scalar_t x = (q1.v.x * a + q2.v.x * b) / c;
  19.150 +	scalar_t y = (q1.v.y * a + q2.v.y * b) / c;
  19.151 +	scalar_t z = (q1.v.z * a + q2.v.z * b) / c;
  19.152 +	scalar_t s = (q1.s * a + q2.s * b) / c;
  19.153 +
  19.154 +	return Quaternion(s, Vector3(x, y, z)).normalized();
  19.155 +}
  19.156 +	
  19.157 +
  19.158 +
  19.159 +std::ostream &operator <<(std::ostream &out, const Quaternion &q) {
  19.160 +	out << "(" << q.s << ", " << q.v << ")";
  19.161 +	return out;
  19.162 +}

    20.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    20.2 +++ b/prototype/vmath/quat.h	Thu Jun 28 06:05:50 2012 +0300
    20.3 @@ -0,0 +1,119 @@
    20.4 +/*
    20.5 +libvmath - a vector math library
    20.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    20.7 +
    20.8 +This program is free software: you can redistribute it and/or modify
    20.9 +it under the terms of the GNU Lesser General Public License as published
   20.10 +by the Free Software Foundation, either version 3 of the License, or
   20.11 +(at your option) any later version.
   20.12 +
   20.13 +This program is distributed in the hope that it will be useful,
   20.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   20.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   20.16 +GNU Lesser General Public License for more details.
   20.17 +
   20.18 +You should have received a copy of the GNU Lesser General Public License
   20.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   20.20 +*/
   20.21 +
   20.22 +#ifndef VMATH_QUATERNION_H_
   20.23 +#define VMATH_QUATERNION_H_
   20.24 +
   20.25 +#include <stdio.h>
   20.26 +#include "vmath_types.h"
   20.27 +#include "vector.h"
   20.28 +
   20.29 +#ifdef __cplusplus
   20.30 +extern "C" {
   20.31 +#endif	/* __cplusplus */
   20.32 +
   20.33 +#define quat_cons(s, x, y, z)	v4_cons(x, y, z, s)
   20.34 +#define quat_vec(q)				v3_cons((q).x, (q).y, (q).z)
   20.35 +#define quat_s(q)				((q).w)
   20.36 +#define quat_identity()			quat_cons(1.0, 0.0, 0.0, 0.0)
   20.37 +void quat_print(FILE *fp, quat_t q);
   20.38 +
   20.39 +#define quat_add		v4_add
   20.40 +#define quat_sub		v4_sub
   20.41 +#define quat_neg		v4_neg
   20.42 +
   20.43 +static inline quat_t quat_mul(quat_t q1, quat_t q2);
   20.44 +
   20.45 +static inline quat_t quat_conjugate(quat_t q);
   20.46 +
   20.47 +#define quat_length		v4_length
   20.48 +#define quat_length_sq	v4_length_sq
   20.49 +
   20.50 +#define quat_normalize	v4_normalize
   20.51 +static inline quat_t quat_inverse(quat_t q);
   20.52 +
   20.53 +quat_t quat_rotate(quat_t q, scalar_t angle, scalar_t x, scalar_t y, scalar_t z);
   20.54 +quat_t quat_rotate_quat(quat_t q, quat_t rotq);
   20.55 +
   20.56 +static inline void quat_to_mat3(mat3_t res, quat_t q);
   20.57 +static inline void quat_to_mat4(mat4_t res, quat_t q);
   20.58 +
   20.59 +#define quat_lerp quat_slerp
   20.60 +quat_t quat_slerp(quat_t q1, quat_t q2, scalar_t t);
   20.61 +
   20.62 +
   20.63 +#ifdef __cplusplus
   20.64 +}	/* extern "C" */
   20.65 +
   20.66 +#include <iostream>
   20.67 +
   20.68 +/* Quaternion */
   20.69 +class Quaternion {
   20.70 +public:
   20.71 +	scalar_t s;
   20.72 +	Vector3 v;
   20.73 +
   20.74 +	Quaternion();
   20.75 +	Quaternion(scalar_t s, const Vector3 &v);
   20.76 +	Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z);
   20.77 +	Quaternion(const Vector3 &axis, scalar_t angle);
   20.78 +	Quaternion(const quat_t &quat);
   20.79 +
   20.80 +	Quaternion operator +(const Quaternion &quat) const;
   20.81 +	Quaternion operator -(const Quaternion &quat) const;
   20.82 +	Quaternion operator -() const;
   20.83 +	Quaternion operator *(const Quaternion &quat) const;
   20.84 +
   20.85 +	void operator +=(const Quaternion &quat);
   20.86 +	void operator -=(const Quaternion &quat);
   20.87 +	void operator *=(const Quaternion &quat);
   20.88 +
   20.89 +	void reset_identity();
   20.90 +
   20.91 +	Quaternion conjugate() const;
   20.92 +
   20.93 +	scalar_t length() const;
   20.94 +	scalar_t length_sq() const;
   20.95 +
   20.96 +	void normalize();
   20.97 +	Quaternion normalized() const;
   20.98 +
   20.99 +	Quaternion inverse() const;
  20.100 +
  20.101 +	void set_rotation(const Vector3 &axis, scalar_t angle);
  20.102 +	void rotate(const Vector3 &axis, scalar_t angle);
  20.103 +	/* note: this is a totally different operation from the above
  20.104 +	 * this treats the quaternion as signifying direction and rotates
  20.105 +	 * it by a rotation quaternion by rot * q * rot'
  20.106 +	 */
  20.107 +	void rotate(const Quaternion &q);
  20.108 +
  20.109 +	Matrix3x3 get_rotation_matrix() const;
  20.110 +
  20.111 +	friend Quaternion slerp(const Quaternion &q1, const Quaternion &q2, scalar_t t);
  20.112 +
  20.113 +	friend std::ostream &operator <<(std::ostream &out, const Quaternion &q);
  20.114 +};
  20.115 +
  20.116 +Quaternion slerp(const Quaternion &q1, const Quaternion &q2, scalar_t t);
  20.117 +inline Quaternion lerp(const Quaternion &q1, const Quaternion &q2, scalar_t t);
  20.118 +#endif	/* __cplusplus */
  20.119 +
  20.120 +#include "quat.inl"
  20.121 +
  20.122 +#endif	/* VMATH_QUATERNION_H_ */

    21.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    21.2 +++ b/prototype/vmath/quat.inl	Thu Jun 28 06:05:50 2012 +0300
    21.3 @@ -0,0 +1,81 @@
    21.4 +/*
    21.5 +libvmath - a vector math library
    21.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    21.7 +
    21.8 +This program is free software: you can redistribute it and/or modify
    21.9 +it under the terms of the GNU Lesser General Public License as published
   21.10 +by the Free Software Foundation, either version 3 of the License, or
   21.11 +(at your option) any later version.
   21.12 +
   21.13 +This program is distributed in the hope that it will be useful,
   21.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   21.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   21.16 +GNU Lesser General Public License for more details.
   21.17 +
   21.18 +You should have received a copy of the GNU Lesser General Public License
   21.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   21.20 +*/
   21.21 +
   21.22 +#include "vector.h"
   21.23 +#include "matrix.h"
   21.24 +
   21.25 +#ifdef __cplusplus
   21.26 +extern "C" {
   21.27 +#endif	/* __cplusplus */
   21.28 +
   21.29 +static inline quat_t quat_mul(quat_t q1, quat_t q2)
   21.30 +{
   21.31 +	quat_t res;
   21.32 +	vec3_t v1 = quat_vec(q1);
   21.33 +	vec3_t v2 = quat_vec(q2);
   21.34 +
   21.35 +	res.w = q1.w * q2.w - v3_dot(v1, v2);
   21.36 +	/* resvec = v2 * q1 + v1 * q2 + cross(v1, v2) */
   21.37 +	res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);
   21.38 +	res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);
   21.39 +	res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);
   21.40 +	return res;
   21.41 +}
   21.42 +
   21.43 +static inline quat_t quat_conjugate(quat_t q)
   21.44 +{
   21.45 +	q.x = -q.x;
   21.46 +	q.y = -q.y;
   21.47 +	q.z = -q.z;
   21.48 +	return q;
   21.49 +}
   21.50 +
   21.51 +static inline quat_t quat_inverse(quat_t q)
   21.52 +{
   21.53 +	scalar_t lensq = quat_length_sq(q);
   21.54 +	q = quat_conjugate(q);
   21.55 +	q.x /= lensq;
   21.56 +	q.y /= lensq;
   21.57 +	q.z /= lensq;
   21.58 +	q.w /= lensq;
   21.59 +	return q;
   21.60 +}
   21.61 +
   21.62 +static inline void quat_to_mat3(mat3_t res, quat_t q)
   21.63 +{
   21.64 +	m3_cons(res, 1.0 - 2.0 * q.y*q.y - 2.0 * q.z*q.z, 2.0 * q.x * q.y + 2.0 * q.w * q.z,   2.0 * q.z * q.x - 2.0 * q.w * q.y,
   21.65 +				 2.0 * q.x * q.y - 2.0 * q.w * q.z,   1.0 - 2.0 * q.x*q.x - 2.0 * q.z*q.z, 2.0 * q.y * q.z + 2.0 * q.w * q.x,
   21.66 +				 2.0 * q.z * q.x + 2.0 * q.w * q.y,   2.0 * q.y * q.z - 2.0 * q.w * q.x,   1.0 - 2.0 * q.x*q.x - 2.0 * q.y*q.y);
   21.67 +}
   21.68 +
   21.69 +static inline void quat_to_mat4(mat4_t res, quat_t q)
   21.70 +{
   21.71 +	m4_cons(res, 1.0 - 2.0 * q.y*q.y - 2.0 * q.z*q.z, 2.0 * q.x * q.y + 2.0 * q.w * q.z,   2.0 * q.z * q.x - 2.0 * q.w * q.y,   0,
   21.72 +				 2.0 * q.x * q.y - 2.0 * q.w * q.z,   1.0 - 2.0 * q.x*q.x - 2.0 * q.z*q.z, 2.0 * q.y * q.z + 2.0 * q.w * q.x,   0,
   21.73 +				 2.0 * q.z * q.x + 2.0 * q.w * q.y,   2.0 * q.y * q.z - 2.0 * q.w * q.x,   1.0 - 2.0 * q.x*q.x - 2.0 * q.y*q.y, 0,
   21.74 +				 0, 0, 0, 1);
   21.75 +}
   21.76 +
   21.77 +#ifdef __cplusplus
   21.78 +}	/* extern "C" */
   21.79 +
   21.80 +inline Quaternion lerp(const Quaternion &a, const Quaternion &b, scalar_t t)
   21.81 +{
   21.82 +	return slerp(a, b, t);
   21.83 +}
   21.84 +#endif	/* __cplusplus */

    22.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    22.2 +++ b/prototype/vmath/quat_c.c	Thu Jun 28 06:05:50 2012 +0300
    22.3 @@ -0,0 +1,61 @@
    22.4 +/*
    22.5 +libvmath - a vector math library
    22.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    22.7 +
    22.8 +This program is free software: you can redistribute it and/or modify
    22.9 +it under the terms of the GNU Lesser General Public License as published
   22.10 +by the Free Software Foundation, either version 3 of the License, or
   22.11 +(at your option) any later version.
   22.12 +
   22.13 +This program is distributed in the hope that it will be useful,
   22.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   22.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   22.16 +GNU Lesser General Public License for more details.
   22.17 +
   22.18 +You should have received a copy of the GNU Lesser General Public License
   22.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   22.20 +*/
   22.21 +
   22.22 +
   22.23 +#include <stdio.h>
   22.24 +#include <math.h>
   22.25 +#include "quat.h"
   22.26 +
   22.27 +void quat_print(FILE *fp, quat_t q)
   22.28 +{
   22.29 +	fprintf(fp, "([ %.4f %.4f %.4f ] %.4f)", q.x, q.y, q.z, q.w);
   22.30 +}
   22.31 +
   22.32 +quat_t quat_rotate(quat_t q, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
   22.33 +{
   22.34 +	quat_t rq;
   22.35 +	scalar_t half_angle = angle * 0.5;
   22.36 +	scalar_t sin_half = sin(half_angle);
   22.37 +
   22.38 +	rq.w = cos(half_angle);
   22.39 +	rq.x = x * sin_half;
   22.40 +	rq.y = y * sin_half;
   22.41 +	rq.z = z * sin_half;
   22.42 +
   22.43 +	return quat_mul(q, rq);
   22.44 +}
   22.45 +
   22.46 +quat_t quat_rotate_quat(quat_t q, quat_t rotq)
   22.47 +{
   22.48 +	return quat_mul(quat_mul(rotq, q), quat_conjugate(rotq));
   22.49 +}
   22.50 +
   22.51 +quat_t quat_slerp(quat_t q1, quat_t q2, scalar_t t)
   22.52 +{
   22.53 +	quat_t res;
   22.54 +	scalar_t angle = acos(q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z);
   22.55 +	scalar_t a = sin((1.0f - t) * angle);
   22.56 +	scalar_t b = sin(t * angle);
   22.57 +	scalar_t c = sin(angle);
   22.58 +
   22.59 +	res.x = (q1.x * a + q2.x * b) / c;
   22.60 +	res.y = (q1.y * a + q2.y * b) / c;
   22.61 +	res.z = (q1.z * a + q2.z * b) / c;
   22.62 +	res.w = (q1.w * a + q2.w * b) / c;
   22.63 +	return quat_normalize(res);
   22.64 +}

    23.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    23.2 +++ b/prototype/vmath/ray.cc	Thu Jun 28 06:05:50 2012 +0300
    23.3 @@ -0,0 +1,66 @@
    23.4 +#include "ray.h"
    23.5 +#include "vector.h"
    23.6 +
    23.7 +scalar_t Ray::env_ior = 1.0;
    23.8 +
    23.9 +Ray::Ray() {
   23.10 +	ior = env_ior;
   23.11 +	energy = 1.0;
   23.12 +	time = 0;
   23.13 +	iter = 0;
   23.14 +}
   23.15 +
   23.16 +Ray::Ray(const Vector3 &origin, const Vector3 &dir) {
   23.17 +	this->origin = origin;
   23.18 +	this->dir = dir;
   23.19 +	ior = env_ior;
   23.20 +	energy = 1.0;
   23.21 +	time = 0;
   23.22 +	iter = 0;
   23.23 +}
   23.24 +
   23.25 +void Ray::transform(const Matrix4x4 &xform) {
   23.26 +	Matrix4x4 upper;
   23.27 +	Vector3 dir;
   23.28 +
   23.29 +	upper = xform;
   23.30 +	upper[0][3] = upper[1][3] = upper[2][3] = upper[3][0] = upper[3][1] = upper[3][2] = 0.0;
   23.31 +	upper[3][3] = 1.0;
   23.32 +
   23.33 +	dir = (this->dir - origin).transformed(upper);
   23.34 +	origin.transform(xform);
   23.35 +	this->dir = dir + origin;
   23.36 +}
   23.37 +
   23.38 +Ray Ray::transformed(const Matrix4x4 &xform) const {
   23.39 +	Ray foo = *this;
   23.40 +	foo.transform(xform);
   23.41 +	return foo;
   23.42 +}
   23.43 +
   23.44 +void Ray::enter(scalar_t new_ior) {
   23.45 +	ior = new_ior;
   23.46 +	ior_stack.push(ior);
   23.47 +}
   23.48 +
   23.49 +void Ray::leave() {
   23.50 +	if(ior_stack.empty()) {
   23.51 +		//std::cerr << "empty ior stack?\n";
   23.52 +		return;
   23.53 +	}
   23.54 +	ior_stack.pop();
   23.55 +	ior = ior_stack.empty() ? env_ior : ior_stack.top();
   23.56 +}
   23.57 +
   23.58 +scalar_t Ray::calc_ior(bool entering, scalar_t mat_ior) const
   23.59 +{
   23.60 +	scalar_t from_ior = this->ior;
   23.61 +
   23.62 +	if(entering) {
   23.63 +		return from_ior / mat_ior;
   23.64 +	}
   23.65 +
   23.66 +	Ray tmp = *this;
   23.67 +	tmp.leave();
   23.68 +	return from_ior / tmp.ior;
   23.69 +}

    24.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    24.2 +++ b/prototype/vmath/ray.h	Thu Jun 28 06:05:50 2012 +0300
    24.3 @@ -0,0 +1,73 @@
    24.4 +/*
    24.5 +libvmath - a vector math library
    24.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    24.7 +
    24.8 +This program is free software: you can redistribute it and/or modify
    24.9 +it under the terms of the GNU Lesser General Public License as published
   24.10 +by the Free Software Foundation, either version 3 of the License, or
   24.11 +(at your option) any later version.
   24.12 +
   24.13 +This program is distributed in the hope that it will be useful,
   24.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   24.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   24.16 +GNU Lesser General Public License for more details.
   24.17 +
   24.18 +You should have received a copy of the GNU Lesser General Public License
   24.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   24.20 +*/
   24.21 +
   24.22 +#ifndef VMATH_RAY_H_
   24.23 +#define VMATH_RAY_H_
   24.24 +
   24.25 +#include "matrix.h"
   24.26 +#include "vector.h"
   24.27 +
   24.28 +typedef struct {
   24.29 +	vec3_t origin, dir;
   24.30 +} ray_t;
   24.31 +
   24.32 +#ifdef __cplusplus
   24.33 +extern "C" {
   24.34 +#endif	/* __cplusplus */
   24.35 +
   24.36 +static inline ray_t ray_cons(vec3_t origin, vec3_t dir);
   24.37 +ray_t ray_transform(ray_t r, mat4_t m);
   24.38 +
   24.39 +#ifdef __cplusplus
   24.40 +}	/* __cplusplus */
   24.41 +
   24.42 +#include <stack>
   24.43 +
   24.44 +class Ray {
   24.45 +private:
   24.46 +	std::stack<scalar_t> ior_stack;
   24.47 +
   24.48 +public:
   24.49 +	/* enviornmental index of refraction, normally 1.0 */
   24.50 +	static scalar_t env_ior;
   24.51 +	
   24.52 +	Vector3 origin, dir;
   24.53 +	scalar_t energy;
   24.54 +	int iter;
   24.55 +	scalar_t ior;
   24.56 +	long time;
   24.57 +
   24.58 +	Ray();
   24.59 +	Ray(const Vector3 &origin, const Vector3 &dir);
   24.60 +
   24.61 +	void transform(const Matrix4x4 &xform);
   24.62 +	Ray transformed(const Matrix4x4 &xform) const;
   24.63 +
   24.64 +	void enter(scalar_t new_ior);
   24.65 +	void leave();
   24.66 +
   24.67 +	scalar_t calc_ior(bool entering, scalar_t mat_ior = 1.0) const;
   24.68 +};
   24.69 +
   24.70 +inline Ray reflect_ray(const Ray &inray, const Vector3 &norm);
   24.71 +inline Ray refract_ray(const Ray &inray, const Vector3 &norm, scalar_t ior, bool entering, scalar_t ray_mag = -1.0);
   24.72 +#endif	/* __cplusplus */
   24.73 +
   24.74 +#include "ray.inl"
   24.75 +
   24.76 +#endif	/* VMATH_RAY_H_ */

    25.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    25.2 +++ b/prototype/vmath/ray.inl	Thu Jun 28 06:05:50 2012 +0300
    25.3 @@ -0,0 +1,70 @@
    25.4 +/*
    25.5 +libvmath - a vector math library
    25.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    25.7 +
    25.8 +This program is free software: you can redistribute it and/or modify
    25.9 +it under the terms of the GNU Lesser General Public License as published
   25.10 +by the Free Software Foundation, either version 3 of the License, or
   25.11 +(at your option) any later version.
   25.12 +
   25.13 +This program is distributed in the hope that it will be useful,
   25.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   25.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   25.16 +GNU Lesser General Public License for more details.
   25.17 +
   25.18 +You should have received a copy of the GNU Lesser General Public License
   25.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   25.20 +*/
   25.21 +
   25.22 +#ifdef __cplusplus
   25.23 +extern "C" {
   25.24 +#endif	/* __cplusplus */
   25.25 +
   25.26 +static inline ray_t ray_cons(vec3_t origin, vec3_t dir)
   25.27 +{
   25.28 +	ray_t r;
   25.29 +	r.origin = origin;
   25.30 +	r.dir = dir;
   25.31 +	return r;
   25.32 +}
   25.33 +
   25.34 +#ifdef __cplusplus
   25.35 +}
   25.36 +
   25.37 +inline Ray reflect_ray(const Ray &inray, const Vector3 &norm)
   25.38 +{
   25.39 +	Ray ray = inray;
   25.40 +	ray.iter--;
   25.41 +	ray.dir = ray.dir.reflection(norm);
   25.42 +	return ray;
   25.43 +}
   25.44 +
   25.45 +inline Ray refract_ray(const Ray &inray, const Vector3 &norm, scalar_t mat_ior, bool entering, scalar_t ray_mag)
   25.46 +{
   25.47 +	Ray ray = inray;
   25.48 +	ray.iter--;
   25.49 +
   25.50 +	scalar_t ior = ray.calc_ior(entering, mat_ior);
   25.51 +	
   25.52 +	if(entering) {
   25.53 +		ray.enter(mat_ior);
   25.54 +	} else {
   25.55 +		ray.leave();
   25.56 +	}
   25.57 +
   25.58 +	if(ray_mag < 0.0) {
   25.59 +		ray_mag = ray.dir.length();
   25.60 +	}
   25.61 +	ray.dir = (ray.dir / ray_mag).refraction(norm, ior) * ray_mag;
   25.62 +
   25.63 +	/* check TIR */
   25.64 +	if(dot_product(ray.dir, norm) > 0.0) {
   25.65 +		if(entering) {
   25.66 +			ray.leave();
   25.67 +		} else {
   25.68 +			ray.enter(mat_ior);
   25.69 +		}
   25.70 +	}
   25.71 +	return ray;
   25.72 +}
   25.73 +#endif	/* __cplusplus */

    26.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    26.2 +++ b/prototype/vmath/ray_c.c	Thu Jun 28 06:05:50 2012 +0300
    26.3 @@ -0,0 +1,36 @@
    26.4 +/*
    26.5 +libvmath - a vector math library
    26.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    26.7 +
    26.8 +This program is free software: you can redistribute it and/or modify
    26.9 +it under the terms of the GNU Lesser General Public License as published
   26.10 +by the Free Software Foundation, either version 3 of the License, or
   26.11 +(at your option) any later version.
   26.12 +
   26.13 +This program is distributed in the hope that it will be useful,
   26.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   26.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   26.16 +GNU Lesser General Public License for more details.
   26.17 +
   26.18 +You should have received a copy of the GNU Lesser General Public License
   26.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   26.20 +*/
   26.21 +
   26.22 +#include "ray.h"
   26.23 +#include "vector.h"
   26.24 +
   26.25 +ray_t ray_transform(ray_t r, mat4_t xform)
   26.26 +{
   26.27 +	mat4_t upper;
   26.28 +	vec3_t dir;
   26.29 +
   26.30 +	m4_copy(upper, xform);
   26.31 +	upper[0][3] = upper[1][3] = upper[2][3] = upper[3][0] = upper[3][1] = upper[3][2] = 0.0;
   26.32 +	upper[3][3] = 1.0;
   26.33 +
   26.34 +	dir = v3_sub(r.dir, r.origin);
   26.35 +	dir = v3_transform(dir, upper);
   26.36 +	r.origin = v3_transform(r.origin, xform);
   26.37 +	r.dir = v3_add(dir, r.origin);
   26.38 +	return r;
   26.39 +}

    27.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    27.2 +++ b/prototype/vmath/sphvec.cc	Thu Jun 28 06:05:50 2012 +0300
    27.3 @@ -0,0 +1,27 @@
    27.4 +#include "sphvec.h"
    27.5 +#include "vector.h"
    27.6 +
    27.7 +/* theta: 0 <= theta <= 2pi, the angle around Y axis.
    27.8 + * phi: 0 <= phi <= pi, the angle from Y axis.
    27.9 + * r: radius.
   27.10 + */
   27.11 +SphVector::SphVector(scalar_t theta, scalar_t phi, scalar_t r) {
   27.12 +	this->theta = theta;
   27.13 +	this->phi = phi;
   27.14 +	this->r = r;
   27.15 +}
   27.16 +
   27.17 +/* Constructs a spherical coordinate vector from a cartesian vector */
   27.18 +SphVector::SphVector(const Vector3 &cvec) {
   27.19 +	*this = cvec;
   27.20 +}
   27.21 +
   27.22 +/* Assignment operator that converts cartesian to spherical coords */
   27.23 +SphVector &SphVector::operator =(const Vector3 &cvec) {
   27.24 +	r = cvec.length();
   27.25 +	//theta = atan2(cvec.y, cvec.x);
   27.26 +	theta = atan2(cvec.z, cvec.x);
   27.27 +	//phi = acos(cvec.z / r);
   27.28 +	phi = acos(cvec.y / r);
   27.29 +	return *this;
   27.30 +}

    28.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    28.2 +++ b/prototype/vmath/sphvec.h	Thu Jun 28 06:05:50 2012 +0300
    28.3 @@ -0,0 +1,36 @@
    28.4 +/*
    28.5 +libvmath - a vector math library
    28.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    28.7 +
    28.8 +This program is free software: you can redistribute it and/or modify
    28.9 +it under the terms of the GNU Lesser General Public License as published
   28.10 +by the Free Software Foundation, either version 3 of the License, or
   28.11 +(at your option) any later version.
   28.12 +
   28.13 +This program is distributed in the hope that it will be useful,
   28.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   28.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   28.16 +GNU Lesser General Public License for more details.
   28.17 +
   28.18 +You should have received a copy of the GNU Lesser General Public License
   28.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   28.20 +*/
   28.21 +
   28.22 +#ifndef VMATH_SPHVEC_H_
   28.23 +#define VMATH_SPHVEC_H_
   28.24 +
   28.25 +#include "vmath_types.h"
   28.26 +
   28.27 +#ifdef __cplusplus
   28.28 +/* Vector in spherical coordinates */
   28.29 +class SphVector {
   28.30 +public:
   28.31 +	scalar_t theta, phi, r;
   28.32 +
   28.33 +	SphVector(scalar_t theta = 0.0, scalar_t phi = 0.0, scalar_t r = 1.0);
   28.34 +	SphVector(const Vector3 &cvec);
   28.35 +	SphVector &operator =(const Vector3 &cvec);
   28.36 +};
   28.37 +#endif	/* __cplusplus */
   28.38 +
   28.39 +#endif	/* VMATH_SPHVEC_H_ */

    29.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    29.2 +++ b/prototype/vmath/vector.cc	Thu Jun 28 06:05:50 2012 +0300
    29.3 @@ -0,0 +1,326 @@
    29.4 +#include "vector.h"
    29.5 +#include "vmath.h"
    29.6 +
    29.7 +// ---------- Vector2 -----------
    29.8 +
    29.9 +Vector2::Vector2(scalar_t x, scalar_t y)
   29.10 +{
   29.11 +	this->x = x;
   29.12 +	this->y = y;
   29.13 +}
   29.14 +
   29.15 +Vector2::Vector2(const vec2_t &vec)
   29.16 +{
   29.17 +	x = vec.x;
   29.18 +	y = vec.y;
   29.19 +}
   29.20 +
   29.21 +Vector2::Vector2(const Vector3 &vec)
   29.22 +{
   29.23 +	x = vec.x;
   29.24 +	y = vec.y;
   29.25 +}
   29.26 +
   29.27 +Vector2::Vector2(const Vector4 &vec)
   29.28 +{
   29.29 +	x = vec.x;
   29.30 +	y = vec.y;
   29.31 +}
   29.32 +
   29.33 +void Vector2::normalize()
   29.34 +{
   29.35 +	scalar_t len = length();
   29.36 +	x /= len;
   29.37 +	y /= len;
   29.38 +}
   29.39 +
   29.40 +Vector2 Vector2::normalized() const
   29.41 +{
   29.42 +	scalar_t len = length();
   29.43 +	return Vector2(x / len, y / len);
   29.44 +}
   29.45 +
   29.46 +void Vector2::transform(const Matrix3x3 &mat)
   29.47 +{
   29.48 +	scalar_t nx = mat[0][0] * x + mat[0][1] * y + mat[0][2];
   29.49 +	y = mat[1][0] * x + mat[1][1] * y + mat[1][2];
   29.50 +	x = nx;
   29.51 +}
   29.52 +
   29.53 +Vector2 Vector2::transformed(const Matrix3x3 &mat) const
   29.54 +{
   29.55 +	Vector2 vec;
   29.56 +	vec.x = mat[0][0] * x + mat[0][1] * y + mat[0][2];
   29.57 +	vec.y = mat[1][0] * x + mat[1][1] * y + mat[1][2];
   29.58 +	return vec;
   29.59 +}
   29.60 +
   29.61 +void Vector2::rotate(scalar_t angle)
   29.62 +{
   29.63 +	*this = Vector2(cos(angle) * x - sin(angle) * y, sin(angle) * x + cos(angle) * y);
   29.64 +}
   29.65 +
   29.66 +Vector2 Vector2::rotated(scalar_t angle) const
   29.67 +{
   29.68 +	return Vector2(cos(angle) * x - sin(angle) * y, sin(angle) * x + cos(angle) * y);
   29.69 +}
   29.70 +
   29.71 +Vector2 Vector2::reflection(const Vector2 &normal) const
   29.72 +{
   29.73 +	return 2.0 * dot_product(*this, normal) * normal - *this;
   29.74 +}
   29.75 +
   29.76 +Vector2 Vector2::refraction(const Vector2 &normal, scalar_t src_ior, scalar_t dst_ior) const
   29.77 +{
   29.78 +	// quick and dirty implementation :)
   29.79 +	Vector3 v3refr = Vector3(this->x, this->y, 1.0).refraction(Vector3(this->x, this->y, 1), src_ior, dst_ior);
   29.80 +	return Vector2(v3refr.x, v3refr.y);
   29.81 +}
   29.82 +
   29.83 +std::ostream &operator <<(std::ostream &out, const Vector2 &vec)
   29.84 +{
   29.85 +	out << "[" << vec.x << " " << vec.y << "]";
   29.86 +	return out;
   29.87 +}
   29.88 +
   29.89 +
   29.90 +
   29.91 +// --------- Vector3 ----------
   29.92 +
   29.93 +Vector3::Vector3(scalar_t x, scalar_t y, scalar_t z)
   29.94 +{
   29.95 +	this->x = x;
   29.96 +	this->y = y;
   29.97 +	this->z = z;
   29.98 +}
   29.99 +
  29.100 +Vector3::Vector3(const vec3_t &vec)
  29.101 +{
  29.102 +	x = vec.x;
  29.103 +	y = vec.y;
  29.104 +	z = vec.z;
  29.105 +}
  29.106 +
  29.107 +Vector3::Vector3(const Vector2 &vec)
  29.108 +{
  29.109 +	x = vec.x;
  29.110 +	y = vec.y;
  29.111 +	z = 1;
  29.112 +}
  29.113 +
  29.114 +Vector3::Vector3(const Vector4 &vec)
  29.115 +{
  29.116 +	x = vec.x;
  29.117 +	y = vec.y;
  29.118 +	z = vec.z;
  29.119 +}
  29.120 +
  29.121 +Vector3::Vector3(const SphVector &sph)
  29.122 +{
  29.123 +	*this = sph;
  29.124 +}
  29.125 +
  29.126 +Vector3 &Vector3::operator =(const SphVector &sph)
  29.127 +{
  29.128 +	x = sph.r * cos(sph.theta) * sin(sph.phi);
  29.129 +	z = sph.r * sin(sph.theta) * sin(sph.phi);
  29.130 +	y = sph.r * cos(sph.phi);
  29.131 +	return *this;
  29.132 +}
  29.133 +
  29.134 +void Vector3::normalize()
  29.135 +{
  29.136 +	scalar_t len = length();
  29.137 +	x /= len;
  29.138 +	y /= len;
  29.139 +	z /= len;
  29.140 +}
  29.141 +
  29.142 +Vector3 Vector3::normalized() const
  29.143 +{
  29.144 +	scalar_t len = length();
  29.145 +	return Vector3(x / len, y / len, z / len);
  29.146 +}
  29.147 +
  29.148 +Vector3 Vector3::reflection(const Vector3 &normal) const
  29.149 +{
  29.150 +	return 2.0 * dot_product(*this, normal) * normal - *this;
  29.151 +}
  29.152 +
  29.153 +Vector3 Vector3::refraction(const Vector3 &normal, scalar_t src_ior, scalar_t dst_ior) const
  29.154 +{
  29.155 +	return refraction(normal, src_ior / dst_ior);
  29.156 +}
  29.157 +
  29.158 +Vector3 Vector3::refraction(const Vector3 &normal, scalar_t ior) const
  29.159 +{
  29.160 +	scalar_t cos_inc = dot_product(*this, -normal);
  29.161 +
  29.162 +	scalar_t radical = 1.0 + SQ(ior) * (SQ(cos_inc) - 1.0);
  29.163 +
  29.164 +	if(radical < 0.0) {		// total internal reflection
  29.165 +		return -reflection(normal);
  29.166 +	}
  29.167 +
  29.168 +	scalar_t beta = ior * cos_inc - sqrt(radical);
  29.169 +
  29.170 +	return *this * ior + normal * beta;
  29.171 +}
  29.172 +
  29.173 +void Vector3::transform(const Matrix3x3 &mat)
  29.174 +{
  29.175 +	scalar_t nx = mat[0][0] * x + mat[0][1] * y + mat[0][2] * z;
  29.176 +	scalar_t ny = mat[1][0] * x + mat[1][1] * y + mat[1][2] * z;
  29.177 +	z = mat[2][0] * x + mat[2][1] * y + mat[2][2] * z;
  29.178 +	x = nx;
  29.179 +	y = ny;
  29.180 +}
  29.181 +
  29.182 +Vector3 Vector3::transformed(const Matrix3x3 &mat) const
  29.183 +{
  29.184 +	Vector3 vec;
  29.185 +	vec.x = mat[0][0] * x + mat[0][1] * y + mat[0][2] * z;
  29.186 +	vec.y = mat[1][0] * x + mat[1][1] * y + mat[1][2] * z;
  29.187 +	vec.z = mat[2][0] * x + mat[2][1] * y + mat[2][2] * z;
  29.188 +	return vec;
  29.189 +}
  29.190 +
  29.191 +void Vector3::transform(const Matrix4x4 &mat)
  29.192 +{
  29.193 +	scalar_t nx = mat[0][0] * x + mat[0][1] * y + mat[0][2] * z + mat[0][3];
  29.194 +	scalar_t ny = mat[1][0] * x + mat[1][1] * y + mat[1][2] * z + mat[1][3];
  29.195 +	z = mat[2][0] * x + mat[2][1] * y + mat[2][2] * z + mat[2][3];
  29.196 +	x = nx;
  29.197 +	y = ny;
  29.198 +}
  29.199 +
  29.200 +Vector3 Vector3::transformed(const Matrix4x4 &mat) const
  29.201 +{
  29.202 +	Vector3 vec;
  29.203 +	vec.x = mat[0][0] * x + mat[0][1] * y + mat[0][2] * z + mat[0][3];
  29.204 +	vec.y = mat[1][0] * x + mat[1][1] * y + mat[1][2] * z + mat[1][3];
  29.205 +	vec.z = mat[2][0] * x + mat[2][1] * y + mat[2][2] * z + mat[2][3];
  29.206 +	return vec;
  29.207 +}
  29.208 +
  29.209 +void Vector3::transform(const Quaternion &quat)
  29.210 +{
  29.211 +	Quaternion vq(0.0f, *this);
  29.212 +	vq = quat * vq * quat.inverse();
  29.213 +	*this = vq.v;
  29.214 +}
  29.215 +
  29.216 +Vector3 Vector3::transformed(const Quaternion &quat) const
  29.217 +{
  29.218 +	Quaternion vq(0.0f, *this);
  29.219 +	vq = quat * vq * quat.inverse();
  29.220 +	return vq.v;
  29.221 +}
  29.222 +
  29.223 +void Vector3::rotate(const Vector3 &euler)
  29.224 +{
  29.225 +	Matrix4x4 rot;
  29.226 +	rot.set_rotation(euler);
  29.227 +	transform(rot);
  29.228 +}
  29.229 +
  29.230 +Vector3 Vector3::rotated(const Vector3 &euler) const
  29.231 +{
  29.232 +	Matrix4x4 rot;
  29.233 +	rot.set_rotation(euler);
  29.234 +	return transformed(rot);
  29.235 +}
  29.236 +
  29.237 +std::ostream &operator <<(std::ostream &out, const Vector3 &vec)
  29.238 +{
  29.239 +	out << "[" << vec.x << " " << vec.y << " " << vec.z << "]";
  29.240 +	return out;
  29.241 +}
  29.242 +
  29.243 +
  29.244 +// -------------- Vector4 --------------
  29.245 +Vector4::Vector4(scalar_t x, scalar_t y, scalar_t z, scalar_t w)
  29.246 +{
  29.247 +	this->x = x;
  29.248 +	this->y = y;
  29.249 +	this->z = z;
  29.250 +	this->w = w;
  29.251 +}
  29.252 +
  29.253 +Vector4::Vector4(const vec4_t &vec)
  29.254 +{
  29.255 +	x = vec.x;
  29.256 +	y = vec.y;
  29.257 +	z = vec.z;
  29.258 +	w = vec.w;
  29.259 +}
  29.260 +
  29.261 +Vector4::Vector4(const Vector2 &vec)
  29.262 +{
  29.263 +	x = vec.x;
  29.264 +	y = vec.y;
  29.265 +	z = 1;
  29.266 +	w = 1;
  29.267 +}
  29.268 +
  29.269 +Vector4::Vector4(const Vector3 &vec)
  29.270 +{
  29.271 +	x = vec.x;
  29.272 +	y = vec.y;
  29.273 +	z = vec.z;
  29.274 +	w = 1;
  29.275 +}
  29.276 +
  29.277 +void Vector4::normalize()
  29.278 +{
  29.279 +	scalar_t len = (scalar_t)sqrt(x*x + y*y + z*z + w*w);
  29.280 +	x /= len;
  29.281 +	y /= len;
  29.282 +	z /= len;
  29.283 +	w /= len;
  29.284 +}
  29.285 +
  29.286 +Vector4 Vector4::normalized() const
  29.287 +{
  29.288 +	scalar_t len = (scalar_t)sqrt(x*x + y*y + z*z + w*w);
  29.289 +	return Vector4(x / len, y / len, z / len, w / len);
  29.290 +}
  29.291 +
  29.292 +void Vector4::transform(const Matrix4x4 &mat)
  29.293 +{
  29.294 +	scalar_t nx = mat[0][0] * x + mat[0][1] * y + mat[0][2] * z + mat[0][3] * w;
  29.295 +	scalar_t ny = mat[1][0] * x + mat[1][1] * y + mat[1][2] * z + mat[1][3] * w;
  29.296 +	scalar_t nz = mat[2][0] * x + mat[2][1] * y + mat[2][2] * z + mat[2][3] * w;
  29.297 +	w = mat[3][0] * x + mat[3][1] * y + mat[3][2] * z + mat[3][3] * w;
  29.298 +	x = nx;
  29.299 +	y = ny;
  29.300 +	z = nz;
  29.301 +}
  29.302 +
  29.303 +Vector4 Vector4::transformed(const Matrix4x4 &mat) const
  29.304 +{
  29.305 +	Vector4 vec;
  29.306 +	vec.x = mat[0][0] * x + mat[0][1] * y + mat[0][2] * z + mat[0][3] * w;
  29.307 +	vec.y = mat[1][0] * x + mat[1][1] * y + mat[1][2] * z + mat[1][3] * w;
  29.308 +	vec.z = mat[2][0] * x + mat[2][1] * y + mat[2][2] * z + mat[2][3] * w;
  29.309 +	vec.w = mat[3][0] * x + mat[3][1] * y + mat[3][2] * z + mat[3][3] * w;
  29.310 +	return vec;
  29.311 +}
  29.312 +
  29.313 +// TODO: implement 4D vector reflection
  29.314 +Vector4 Vector4::reflection(const Vector4 &normal) const
  29.315 +{
  29.316 +	return *this;
  29.317 +}
  29.318 +
  29.319 +// TODO: implement 4D vector refraction
  29.320 +Vector4 Vector4::refraction(const Vector4 &normal, scalar_t src_ior, scalar_t dst_ior) const
  29.321 +{
  29.322 +	return *this;
  29.323 +}
  29.324 +
  29.325 +std::ostream &operator <<(std::ostream &out, const Vector4 &vec)
  29.326 +{
  29.327 +	out << "[" << vec.x << " " << vec.y << " " << vec.z << " " << vec.w << "]";
  29.328 +	return out;
  29.329 +}

    30.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    30.2 +++ b/prototype/vmath/vector.h	Thu Jun 28 06:05:50 2012 +0300
    30.3 @@ -0,0 +1,292 @@
    30.4 +/*
    30.5 +libvmath - a vector math library
    30.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    30.7 +
    30.8 +This program is free software: you can redistribute it and/or modify
    30.9 +it under the terms of the GNU Lesser General Public License as published
   30.10 +by the Free Software Foundation, either version 3 of the License, or
   30.11 +(at your option) any later version.
   30.12 +
   30.13 +This program is distributed in the hope that it will be useful,
   30.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   30.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   30.16 +GNU Lesser General Public License for more details.
   30.17 +
   30.18 +You should have received a copy of the GNU Lesser General Public License
   30.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   30.20 +*/
   30.21 +
   30.22 +#ifndef VMATH_VECTOR_H_
   30.23 +#define VMATH_VECTOR_H_
   30.24 +
   30.25 +#include <stdio.h>
   30.26 +#include "vmath_types.h"
   30.27 +
   30.28 +#ifdef __cplusplus
   30.29 +extern "C" {
   30.30 +#endif	/* __cplusplus */
   30.31 +
   30.32 +/* C 2D vector functions */
   30.33 +static inline vec2_t v2_cons(scalar_t x, scalar_t y);
   30.34 +static inline void v2_print(FILE *fp, vec2_t v);
   30.35 +
   30.36 +static inline vec2_t v2_add(vec2_t v1, vec2_t v2);
   30.37 +static inline vec2_t v2_sub(vec2_t v1, vec2_t v2);
   30.38 +static inline vec2_t v2_scale(vec2_t v, scalar_t s);
   30.39 +static inline scalar_t v2_dot(vec2_t v1, vec2_t v2);
   30.40 +static inline scalar_t v2_length(vec2_t v);
   30.41 +static inline scalar_t v2_length_sq(vec2_t v);
   30.42 +static inline vec2_t v2_normalize(vec2_t v);
   30.43 +
   30.44 +static inline vec2_t v2_lerp(vec2_t v1, vec2_t v2, scalar_t t);
   30.45 +
   30.46 +/* C 3D vector functions */
   30.47 +static inline vec3_t v3_cons(scalar_t x, scalar_t y, scalar_t z);
   30.48 +static inline void v3_print(FILE *fp, vec3_t v);
   30.49 +
   30.50 +static inline vec3_t v3_add(vec3_t v1, vec3_t v2);
   30.51 +static inline vec3_t v3_sub(vec3_t v1, vec3_t v2);
   30.52 +static inline vec3_t v3_neg(vec3_t v);
   30.53 +static inline vec3_t v3_mul(vec3_t v1, vec3_t v2);
   30.54 +static inline vec3_t v3_scale(vec3_t v1, scalar_t s);
   30.55 +static inline scalar_t v3_dot(vec3_t v1, vec3_t v2);
   30.56 +static inline vec3_t v3_cross(vec3_t v1, vec3_t v2);
   30.57 +static inline scalar_t v3_length(vec3_t v);
   30.58 +static inline scalar_t v3_length_sq(vec3_t v);
   30.59 +static inline vec3_t v3_normalize(vec3_t v);
   30.60 +static inline vec3_t v3_transform(vec3_t v, mat4_t m);
   30.61 +
   30.62 +static inline vec3_t v3_rotate(vec3_t v, scalar_t x, scalar_t y, scalar_t z);
   30.63 +static inline vec3_t v3_rotate_axis(vec3_t v, scalar_t angle, scalar_t x, scalar_t y, scalar_t z);
   30.64 +static inline vec3_t v3_rotate_quat(vec3_t v, quat_t q);
   30.65 +
   30.66 +static inline vec3_t v3_reflect(vec3_t v, vec3_t n);
   30.67 +
   30.68 +static inline vec3_t v3_lerp(vec3_t v1, vec3_t v2, scalar_t t);
   30.69 +
   30.70 +/* C 4D vector functions */
   30.71 +static inline vec4_t v4_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t w);
   30.72 +static inline void v4_print(FILE *fp, vec4_t v);
   30.73 +
   30.74 +static inline vec4_t v4_add(vec4_t v1, vec4_t v2);
   30.75 +static inline vec4_t v4_sub(vec4_t v1, vec4_t v2);
   30.76 +static inline vec4_t v4_neg(vec4_t v);
   30.77 +static inline vec4_t v4_mul(vec4_t v1, vec4_t v2);
   30.78 +static inline vec4_t v4_scale(vec4_t v, scalar_t s);
   30.79 +static inline scalar_t v4_dot(vec4_t v1, vec4_t v2);
   30.80 +static inline scalar_t v4_length(vec4_t v);
   30.81 +static inline scalar_t v4_length_sq(vec4_t v);
   30.82 +static inline vec4_t v4_normalize(vec4_t v);
   30.83 +static inline vec4_t v4_transform(vec4_t v, mat4_t m);
   30.84 +
   30.85 +#ifdef __cplusplus
   30.86 +}	/* extern "C" */
   30.87 +
   30.88 +/* when included from C++ source files, also define the vector classes */
   30.89 +#include <iostream>
   30.90 +
   30.91 +/** 2D Vector */
   30.92 +class Vector2 {
   30.93 +public:
   30.94 +	scalar_t x, y;
   30.95 +
   30.96 +	Vector2(scalar_t x = 0.0, scalar_t y = 0.0);
   30.97 +	Vector2(const vec2_t &vec);
   30.98 +	Vector2(const Vector3 &vec);
   30.99 +	Vector2(const Vector4 &vec);
  30.100 +
  30.101 +	inline scalar_t &operator [](int elem);
  30.102 +	inline const scalar_t &operator [](int elem) const;
  30.103 +
  30.104 +	inline scalar_t length() const;
  30.105 +	inline scalar_t length_sq() const;
  30.106 +	void normalize();
  30.107 +	Vector2 normalized() const;
  30.108 +
  30.109 +	void transform(const Matrix3x3 &mat);
  30.110 +	Vector2 transformed(const Matrix3x3 &mat) const;
  30.111 +
  30.112 +	void rotate(scalar_t angle);
  30.113 +	Vector2 rotated(scalar_t angle) const;
  30.114 +
  30.115 +	Vector2 reflection(const Vector2 &normal) const;
  30.116 +	Vector2 refraction(const Vector2 &normal, scalar_t src_ior, scalar_t dst_ior) const;
  30.117 +};
  30.118 +
  30.119 +/* unary operations */
  30.120 +inline Vector2 operator -(const Vector2 &vec);
  30.121 +
  30.122 +/* binary vector (op) vector operations */
  30.123 +inline scalar_t dot_product(const Vector2 &v1, const Vector2 &v2);
  30.124 +
  30.125 +inline Vector2 operator +(const Vector2 &v1, const Vector2 &v2);
  30.126 +inline Vector2 operator -(const Vector2 &v1, const Vector2 &v2);
  30.127 +inline Vector2 operator *(const Vector2 &v1, const Vector2 &v2);
  30.128 +inline Vector2 operator /(const Vector2 &v1, const Vector2 &v2);
  30.129 +inline bool operator ==(const Vector2 &v1, const Vector2 &v2);
  30.130 +
  30.131 +inline void operator +=(Vector2 &v1, const Vector2 &v2);
  30.132 +inline void operator -=(Vector2 &v1, const Vector2 &v2);
  30.133 +inline void operator *=(Vector2 &v1, const Vector2 &v2);
  30.134 +inline void operator /=(Vector2 &v1, const Vector2 &v2);
  30.135 +
  30.136 +/* binary vector (op) scalar and scalar (op) vector operations */
  30.137 +inline Vector2 operator +(const Vector2 &vec, scalar_t scalar);
  30.138 +inline Vector2 operator +(scalar_t scalar, const Vector2 &vec);
  30.139 +inline Vector2 operator -(const Vector2 &vec, scalar_t scalar);
  30.140 +inline Vector2 operator *(const Vector2 &vec, scalar_t scalar);
  30.141 +inline Vector2 operator *(scalar_t scalar, const Vector2 &vec);
  30.142 +inline Vector2 operator /(const Vector2 &vec, scalar_t scalar);
  30.143 +
  30.144 +inline void operator +=(Vector2 &vec, scalar_t scalar);
  30.145 +inline void operator -=(Vector2 &vec, scalar_t scalar);
  30.146 +inline void operator *=(Vector2 &vec, scalar_t scalar);
  30.147 +inline void operator /=(Vector2 &vec, scalar_t scalar);
  30.148 +
  30.149 +std::ostream &operator <<(std::ostream &out, const Vector2 &vec);
  30.150 +
  30.151 +inline Vector2 lerp(const Vector2 &a, const Vector2 &b, scalar_t t);
  30.152 +inline Vector2 catmull_rom_spline(const Vector2 &v0, const Vector2 &v1,
  30.153 +		const Vector2 &v2, const Vector2 &v3, scalar_t t);
  30.154 +
  30.155 +/* 3D Vector */
  30.156 +class Vector3 {
  30.157 +public:
  30.158 +	scalar_t x, y, z;
  30.159 +
  30.160 +	Vector3(scalar_t x = 0.0, scalar_t y = 0.0, scalar_t z = 0.0);
  30.161 +	Vector3(const vec3_t &vec);
  30.162 +	Vector3(const Vector2 &vec);
  30.163 +	Vector3(const Vector4 &vec);
  30.164 +	Vector3(const SphVector &sph);
  30.165 +
  30.166 +	Vector3 &operator =(const SphVector &sph);
  30.167 +
  30.168 +	inline scalar_t &operator [](int elem);
  30.169 +	inline const scalar_t &operator [](int elem) const;
  30.170 +
  30.171 +	inline scalar_t length() const;
  30.172 +	inline scalar_t length_sq() const;
  30.173 +	void normalize();
  30.174 +	Vector3 normalized() const;
  30.175 +
  30.176 +	void transform(const Matrix3x3 &mat);
  30.177 +	Vector3 transformed(const Matrix3x3 &mat) const;
  30.178 +	void transform(const Matrix4x4 &mat);
  30.179 +	Vector3 transformed(const Matrix4x4 &mat) const;
  30.180 +	void transform(const Quaternion &quat);
  30.181 +	Vector3 transformed(const Quaternion &quat) const;
  30.182 +
  30.183 +	void rotate(const Vector3 &euler);
  30.184 +	Vector3 rotated(const Vector3 &euler) const;
  30.185 +
  30.186 +	Vector3 reflection(const Vector3 &normal) const;
  30.187 +	Vector3 refraction(const Vector3 &normal, scalar_t src_ior, scalar_t dst_ior) const;
  30.188 +	Vector3 refraction(const Vector3 &normal, scalar_t ior) const;
  30.189 +};
  30.190 +
  30.191 +/* unary operations */
  30.192 +inline Vector3 operator -(const Vector3 &vec);
  30.193 +
  30.194 +/* binary vector (op) vector operations */
  30.195 +inline scalar_t dot_product(const Vector3 &v1, const Vector3 &v2);
  30.196 +inline Vector3 cross_product(const Vector3 &v1, const Vector3 &v2);
  30.197 +
  30.198 +inline Vector3 operator +(const Vector3 &v1, const Vector3 &v2);
  30.199 +inline Vector3 operator -(const Vector3 &v1, const Vector3 &v2);
  30.200 +inline Vector3 operator *(const Vector3 &v1, const Vector3 &v2);
  30.201 +inline Vector3 operator /(const Vector3 &v1, const Vector3 &v2);
  30.202 +inline bool operator ==(const Vector3 &v1, const Vector3 &v2);
  30.203 +
  30.204 +inline void operator +=(Vector3 &v1, const Vector3 &v2);
  30.205 +inline void operator -=(Vector3 &v1, const Vector3 &v2);
  30.206 +inline void operator *=(Vector3 &v1, const Vector3 &v2);
  30.207 +inline void operator /=(Vector3 &v1, const Vector3 &v2);
  30.208 +
  30.209 +/* binary vector (op) scalar and scalar (op) vector operations */
  30.210 +inline Vector3 operator +(const Vector3 &vec, scalar_t scalar);
  30.211 +inline Vector3 operator +(scalar_t scalar, const Vector3 &vec);
  30.212 +inline Vector3 operator -(const Vector3 &vec, scalar_t scalar);
  30.213 +inline Vector3 operator *(const Vector3 &vec, scalar_t scalar);
  30.214 +inline Vector3 operator *(scalar_t scalar, const Vector3 &vec);
  30.215 +inline Vector3 operator /(const Vector3 &vec, scalar_t scalar);
  30.216 +
  30.217 +inline void operator +=(Vector3 &vec, scalar_t scalar);
  30.218 +inline void operator -=(Vector3 &vec, scalar_t scalar);
  30.219 +inline void operator *=(Vector3 &vec, scalar_t scalar);
  30.220 +inline void operator /=(Vector3 &vec, scalar_t scalar);
  30.221 +
  30.222 +std::ostream &operator <<(std::ostream &out, const Vector3 &vec);
  30.223 +
  30.224 +inline Vector3 lerp(const Vector3 &a, const Vector3 &b, scalar_t t);
  30.225 +inline Vector3 catmull_rom_spline(const Vector3 &v0, const Vector3 &v1,
  30.226 +		const Vector3 &v2, const Vector3 &v3, scalar_t t);
  30.227 +
  30.228 +/* 4D Vector */
  30.229 +class Vector4 {
  30.230 +public:
  30.231 +	scalar_t x, y, z, w;
  30.232 +
  30.233 +	Vector4(scalar_t x = 0.0, scalar_t y = 0.0, scalar_t z = 0.0, scalar_t w = 0.0);
  30.234 +	Vector4(const vec4_t &vec);
  30.235 +	Vector4(const Vector2 &vec);
  30.236 +	Vector4(const Vector3 &vec);
  30.237 +
  30.238 +	inline scalar_t &operator [](int elem);
  30.239 +	inline const scalar_t &operator [](int elem) const;
  30.240 +
  30.241 +	inline scalar_t length() const;
  30.242 +	inline scalar_t length_sq() const;
  30.243 +	void normalize();
  30.244 +	Vector4 normalized() const;
  30.245 +
  30.246 +	void transform(const Matrix4x4 &mat);
  30.247 +	Vector4 transformed(const Matrix4x4 &mat) const;
  30.248 +
  30.249 +	Vector4 reflection(const Vector4 &normal) const;
  30.250 +	Vector4 refraction(const Vector4 &normal, scalar_t src_ior, scalar_t dst_ior) const;
  30.251 +};
  30.252 +
  30.253 +
  30.254 +/* unary operations */
  30.255 +inline Vector4 operator -(const Vector4 &vec);
  30.256 +
  30.257 +/* binary vector (op) vector operations */
  30.258 +inline scalar_t dot_product(const Vector4 &v1, const Vector4 &v2);
  30.259 +inline Vector4 cross_product(const Vector4 &v1, const Vector4 &v2, const Vector4 &v3);
  30.260 +
  30.261 +inline Vector4 operator +(const Vector4 &v1, const Vector4 &v2);
  30.262 +inline Vector4 operator -(const Vector4 &v1, const Vector4 &v2);
  30.263 +inline Vector4 operator *(const Vector4 &v1, const Vector4 &v2);
  30.264 +inline Vector4 operator /(const Vector4 &v1, const Vector4 &v2);
  30.265 +inline bool operator ==(const Vector4 &v1, const Vector4 &v2);
  30.266 +
  30.267 +inline void operator +=(Vector4 &v1, const Vector4 &v2);
  30.268 +inline void operator -=(Vector4 &v1, const Vector4 &v2);
  30.269 +inline void operator *=(Vector4 &v1, const Vector4 &v2);
  30.270 +inline void operator /=(Vector4 &v1, const Vector4 &v2);
  30.271 +
  30.272 +/* binary vector (op) scalar and scalar (op) vector operations */
  30.273 +inline Vector4 operator +(const Vector4 &vec, scalar_t scalar);
  30.274 +inline Vector4 operator +(scalar_t scalar, const Vector4 &vec);
  30.275 +inline Vector4 operator -(const Vector4 &vec, scalar_t scalar);
  30.276 +inline Vector4 operator *(const Vector4 &vec, scalar_t scalar);
  30.277 +inline Vector4 operator *(scalar_t scalar, const Vector4 &vec);
  30.278 +inline Vector4 operator /(const Vector4 &vec, scalar_t scalar);
  30.279 +
  30.280 +inline void operator +=(Vector4 &vec, scalar_t scalar);
  30.281 +inline void operator -=(Vector4 &vec, scalar_t scalar);
  30.282 +inline void operator *=(Vector4 &vec, scalar_t scalar);
  30.283 +inline void operator /=(Vector4 &vec, scalar_t scalar);
  30.284 +
  30.285 +std::ostream &operator <<(std::ostream &out, const Vector4 &vec);
  30.286 +
  30.287 +inline Vector4 lerp(const Vector4 &v0, const Vector4 &v1, scalar_t t);
  30.288 +inline Vector4 catmull_rom_spline(const Vector4 &v0, const Vector4 &v1,
  30.289 +		const Vector4 &v2, const Vector4 &v3, scalar_t t);
  30.290 +
  30.291 +#endif	/* __cplusplus */
  30.292 +
  30.293 +#include "vector.inl"
  30.294 +
  30.295 +#endif	/* VMATH_VECTOR_H_ */

    31.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
    31.2 +++ b/prototype/vmath/vector.inl	Thu Jun 28 06:05:50 2012 +0300
    31.3 @@ -0,0 +1,761 @@
    31.4 +/*
    31.5 +libvmath - a vector math library
    31.6 +Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
    31.7 +
    31.8 +This program is free software: you can redistribute it and/or modify
    31.9 +it under the terms of the GNU Lesser General Public License as published
   31.10 +by the Free Software Foundation, either version 3 of the License, or
   31.11 +(at your option) any later version.
   31.12 +
   31.13 +This program is distributed in the hope that it will be useful,
   31.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
   31.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   31.16 +GNU Lesser General Public License for more details.
   31.17 +
   31.18 +You should have received a copy of the GNU Lesser General Public License
   31.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
   31.20 +*/
   31.21 +
   31.22 +#include <math.h>
   31.23 +
   31.24 +#ifdef __cplusplus
   31.25 +extern "C" {
   31.26 +#endif	/* __cplusplus */
   31.27 +
   31.28 +/* C 2D vector functions */
   31.29 +static inline vec2_t v2_cons(scalar_t x, scalar_t y)
   31.30 +{
   31.31 +	vec2_t v;
   31.32 +	v.x = x;
   31.33 +	v.y = y;
   31.34 +	return v;
   31.35 +}
   31.36 +
   31.37 +static inline void v2_print(FILE *fp, vec2_t v)
   31.38 +{
   31.39 +	fprintf(fp, "[ %.4f %.4f ]", v.x, v.y);
   31.40 +}
   31.41 +
   31.42 +static inline vec2_t v2_add(vec2_t v1, vec2_t v2)
   31.43 +{
   31.44 +	vec2_t res;
   31.45 +	res.x = v1.x + v2.x;
   31.46 +	res.y = v1.y + v2.y;
   31.47 +	return res;
   31.48 +}
   31.49 +
   31.50 +static inline vec2_t v2_sub(vec2_t v1, vec2_t v2)
   31.51 +{
   31.52 +	vec2_t res;
   31.53 +	res.x = v1.x - v2.x;
   31.54 +	res.y = v1.y - v2.y;
   31.55 +	return res;
   31.56 +}
   31.57 +
   31.58 +static inline vec2_t v2_scale(vec2_t v, scalar_t s)
   31.59 +{
   31.60 +	vec2_t res;
   31.61 +	res.x = v.x * s;
   31.62 +	res.y = v.y * s;
   31.63 +	return res;
   31.64 +}
   31.65 +
   31.66 +static inline scalar_t v2_dot(vec2_t v1, vec2_t v2)
   31.67 +{
   31.68 +	return v1.x * v2.x + v1.y * v2.y;
   31.69 +}
   31.70 +
   31.71 +static inline scalar_t v2_length(vec2_t v)
   31.72 +{
   31.73 +	return sqrt(v.x * v.x + v.y * v.y);
   31.74 +}
   31.75 +
   31.76 +static inline scalar_t v2_length_sq(vec2_t v)
   31.77 +{
   31.78 +	return v.x * v.x + v.y * v.y;
   31.79 +}
   31.80 +
   31.81 +static inline vec2_t v2_normalize(vec2_t v)
   31.82 +{
   31.83 +	scalar_t len = (scalar_t)sqrt(v.x * v.x + v.y * v.y);
   31.84 +	v.x /= len;
   31.85 +	v.y /= len;
   31.86 +	return v;
   31.87 +}
   31.88 +
   31.89 +static inline vec2_t v2_lerp(vec2_t v1, vec2_t v2, scalar_t t)
   31.90 +{
   31.91 +	vec2_t res;
   31.92 +	res.x = v1.x + (v2.x - v1.x) * t;
   31.93 +	res.y = v1.y + (v2.y - v1.y) * t;
   31.94 +	return res;
   31.95 +}
   31.96 +
   31.97 +
   31.98 +/* C 3D vector functions */
   31.99 +static inline vec3_t v3_cons(scalar_t x, scalar_t y, scalar_t z)
  31.100 +{
  31.101 +	vec3_t v;
  31.102 +	v.x = x;
  31.103 +	v.y = y;
  31.104 +	v.z = z;
  31.105 +	return v;
  31.106 +}
  31.107 +
  31.108 +static inline void v3_print(FILE *fp, vec3_t v)
  31.109 +{
  31.110 +	fprintf(fp, "[ %.4f %.4f %.4f ]", v.x, v.y, v.z);
  31.111 +}
  31.112 +
  31.113 +static inline vec3_t v3_add(vec3_t v1, vec3_t v2)
  31.114 +{
  31.115 +	v1.x += v2.x;
  31.116 +	v1.y += v2.y;
  31.117 +	v1.z += v2.z;
  31.118 +	return v1;
  31.119 +}
  31.120 +
  31.121 +static inline vec3_t v3_sub(vec3_t v1, vec3_t v2)
  31.122 +{
  31.123 +	v1.x -= v2.x;
  31.124 +	v1.y -= v2.y;
  31.125 +	v1.z -= v2.z;
  31.126 +	return v1;
  31.127 +}
  31.128 +
  31.129 +static inline vec3_t v3_neg(vec3_t v)
  31.130 +{
  31.131 +	v.x = -v.x;
  31.132 +	v.y = -v.y;
  31.133 +	v.z = -v.z;
  31.134 +	return v;
  31.135 +}
  31.136 +
  31.137 +static inline vec3_t v3_mul(vec3_t v1, vec3_t v2)
  31.138 +{
  31.139 +	v1.x *= v2.x;
  31.140 +	v1.y *= v2.y;
  31.141 +	v1.z *= v2.z;
  31.142 +	return v1;
  31.143 +}
  31.144 +
  31.145 +static inline vec3_t v3_scale(vec3_t v1, scalar_t s)
  31.146 +{
  31.147 +	v1.x *= s;
  31.148 +	v1.y *= s;
  31.149 +	v1.z *= s;
  31.150 +	return v1;
  31.151 +}
  31.152 +
  31.153 +static inline scalar_t v3_dot(vec3_t v1, vec3_t v2)
  31.154 +{
  31.155 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
  31.156 +}
  31.157 +
  31.158 +static inline vec3_t v3_cross(vec3_t v1, vec3_t v2)
  31.159 +{
  31.160 +	vec3_t v;
  31.161 +	v.x = v1.y * v2.z - v1.z * v2.y;
  31.162 +	v.y = v1.z * v2.x - v1.x * v2.z;
  31.163 +	v.z = v1.x * v2.y - v1.y * v2.x;
  31.164 +	return v;
  31.165 +}
  31.166 +
  31.167 +static inline scalar_t v3_length(vec3_t v)
  31.168 +{
  31.169 +	return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
  31.170 +}
  31.171 +
  31.172 +static inline scalar_t v3_length_sq(vec3_t v)
  31.173 +{
  31.174 +	return v.x * v.x + v.y * v.y + v.z * v.z;
  31.175 +}
  31.176 +
  31.177 +static inline vec3_t v3_normalize(vec3_t v)
  31.178 +{
  31.179 +	scalar_t len = sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
  31.180 +	v.x /= len;
  31.181 +	v.y /= len;
  31.182 +	v.z /= len;
  31.183 +	return v;
  31.184 +}
  31.185 +
  31.186 +static inline vec3_t v3_transform(vec3_t v, mat4_t m)
  31.187 +{
  31.188 +	vec3_t res;
  31.189 +	res.x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3];
  31.190 +	res.y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3];
  31.191 +	res.z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3];
  31.192 +	return res;
  31.193 +}
  31.194 +
  31.195 +static inline vec3_t v3_rotate(vec3_t v, scalar_t x, scalar_t y, scalar_t z)
  31.196 +{
  31.197 +	void m4_rotate(mat4_t, scalar_t, scalar_t, scalar_t);
  31.198 +
  31.199 +	mat4_t m = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
  31.200 +	m4_rotate(m, x, y, z);
  31.201 +	return v3_transform(v, m);
  31.202 +}
  31.203 +
  31.204 +static inline vec3_t v3_rotate_axis(vec3_t v, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
  31.205 +{
  31.206 +	void m4_rotate_axis(mat4_t, scalar_t, scalar_t, scalar_t, scalar_t);
  31.207 +
  31.208 +	mat4_t m = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
  31.209 +	m4_rotate_axis(m, angle, x, y, z);
  31.210 +	return v3_transform(v, m);
  31.211 +}
  31.212 +
  31.213 +static inline vec3_t v3_rotate_quat(vec3_t v, quat_t q)
  31.214 +{
  31.215 +	quat_t quat_rotate_quat(quat_t, quat_t);
  31.216 +
  31.217 +	quat_t vq = v4_cons(v.x, v.y, v.z, 0.0);
  31.218 +	quat_t res = quat_rotate_quat(vq, q);
  31.219 +	return v3_cons(res.x, res.y, res.z);
  31.220 +}
  31.221 +
  31.222 +static inline vec3_t v3_reflect(vec3_t v, vec3_t n)
  31.223 +{
  31.224 +	scalar_t dot = v3_dot(v, n);
  31.225 +	return v3_sub(v3_scale(n, dot * 2.0), v);
  31.226 +}
  31.227 +
  31.228 +static inline vec3_t v3_lerp(vec3_t v1, vec3_t v2, scalar_t t)
  31.229 +{
  31.230 +	v1.x += (v2.x - v1.x) * t;
  31.231 +	v1.y += (v2.y - v1.y) * t;
  31.232 +	v1.z += (v2.z - v1.z) * t;
  31.233 +	return v1;
  31.234 +}
  31.235 +
  31.236 +/* C 4D vector functions */
  31.237 +static inline vec4_t v4_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t w)
  31.238 +{
  31.239 +	vec4_t v;
  31.240 +	v.x = x;
  31.241 +	v.y = y;
  31.242 +	v.z = z;
  31.243 +	v.w = w;
  31.244 +	return v;
  31.245 +}
  31.246 +
  31.247 +static inline void v4_print(FILE *fp, vec4_t v)
  31.248 +{
  31.249 +	fprintf(fp, "[ %.4f %.4f %.4f %.4f ]", v.x, v.y, v.z, v.w);
  31.250 +}
  31.251 +
  31.252 +static inline vec4_t v4_add(vec4_t v1, vec4_t v2)
  31.253 +{
  31.254 +	v1.x += v2.x;
  31.255 +	v1.y += v2.y;
  31.256 +	v1.z += v2.z;
  31.257 +	v1.w += v2.w;
  31.258 +	return v1;
  31.259 +}
  31.260 +
  31.261 +static inline vec4_t v4_sub(vec4_t v1, vec4_t v2)
  31.262 +{
  31.263 +	v1.x -= v2.x;
  31.264 +	v1.y -= v2.y;
  31.265 +	v1.z -= v2.z;
  31.266 +	v1.w -= v2.w;
  31.267 +	return v1;
  31.268 +}
  31.269 +
  31.270 +static inline vec4_t v4_neg(vec4_t v)
  31.271 +{
  31.272 +	v.x = -v.x;
  31.273 +	v.y = -v.y;
  31.274 +	v.z = -v.z;
  31.275 +	v.w = -v.w;
  31.276 +	return v;
  31.277 +}
  31.278 +
  31.279 +static inline vec4_t v4_mul(vec4_t v1, vec4_t v2)
  31.280 +{
  31.281 +	v1.x *= v2.x;
  31.282 +	v1.y *= v2.y;
  31.283 +	v1.z *= v2.z;
  31.284 +	v1.w *= v2.w;
  31.285 +	return v1;
  31.286 +}
  31.287 +
  31.288 +static inline vec4_t v4_scale(vec4_t v, scalar_t s)
  31.289 +{
  31.290 +	v.x *= s;
  31.291 +	v.y *= s;
  31.292 +	v.z *= s;
  31.293 +	v.w *= s;
  31.294 +	return v;
  31.295 +}
  31.296 +
  31.297 +static inline scalar_t v4_dot(vec4_t v1, vec4_t v2)
  31.298 +{
  31.299 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
  31.300 +}
  31.301 +
  31.302 +static inline scalar_t v4_length(vec4_t v)
  31.303 +{
  31.304 +	return sqrt(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
  31.305 +}
  31.306 +
  31.307 +static inline scalar_t v4_length_sq(vec4_t v)
  31.308 +{
  31.309 +	return v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w;
  31.310 +}
  31.311 +
  31.312 +static inline vec4_t v4_normalize(vec4_t v)
  31.313 +{
  31.314 +	scalar_t len = sqrt(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
  31.315 +	v.x /= len;
  31.316 +	v.y /= len;
  31.317 +	v.z /= len;
  31.318 +	v.w /= len;
  31.319 +	return v;
  31.320 +}
  31.321 +
  31.322 +static inline vec4_t v4_transform(vec4_t v, mat4_t m)
  31.323 +{
  31.324 +	vec4_t res;
  31.325 +	res.x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w;
  31.326 +	res.y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w;
  31.327 +	res.z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w;
  31.328 +	res.w = m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w;
  31.329 +	return res;
  31.330 +}
  31.331 +
  31.332 +#ifdef __cplusplus
  31.333 +}	/* extern "C" */
  31.334 +
  31.335 +
  31.336 +/* --------------- C++ part -------------- */
  31.337 +
  31.338 +inline scalar_t &Vector2::operator [](int elem) {
  31.339 +	return elem ? y : x;
  31.340 +}
  31.341 +
  31.342 +inline const scalar_t &Vector2::operator [](int elem) const {
  31.343 +	return elem ? y : x;
  31.344 +}
  31.345 +
  31.346 +inline Vector2 operator -(const Vector2 &vec) {
  31.347 +	return Vector2(-vec.x, -vec.y);
  31.348 +}
  31.349 +
  31.350 +inline scalar_t dot_product(const Vector2 &v1, const Vector2 &v2) {
  31.351 +	return v1.x * v2.x + v1.y * v2.y;
  31.352 +}
  31.353 +
  31.354 +inline Vector2 operator +(const Vector2 &v1, const Vector2 &v2) {
  31.355 +	return Vector2(v1.x + v2.x, v1.y + v2.y);
  31.356 +}
  31.357 +
  31.358 +inline Vector2 operator -(const Vector2 &v1, const Vector2 &v2) {
  31.359 +	return Vector2(v1.x - v2.x, v1.y - v2.y);
  31.360 +}
  31.361 +
  31.362 +inline Vector2 operator *(const Vector2 &v1, const Vector2 &v2) {
  31.363 +	return Vector2(v1.x * v2.x, v1.y * v2.y);
  31.364 +}
  31.365 +
  31.366 +inline Vector2 operator /(const Vector2 &v1, const Vector2 &v2) {
  31.367 +	return Vector2(v1.x / v2.x, v1.y / v2.y);
  31.368 +}
  31.369 +
  31.370 +inline bool operator ==(const Vector2 &v1, const Vector2 &v2) {
  31.371 +	return (fabs(v1.x - v2.x) < XSMALL_NUMBER) && (fabs(v1.y - v2.x) < XSMALL_NUMBER);
  31.372 +}
  31.373 +
  31.374 +inline void operator +=(Vector2 &v1, const Vector2 &v2) {
  31.375 +	v1.x += v2.x;
  31.376 +	v1.y += v2.y;
  31.377 +}
  31.378 +
  31.379 +inline void operator -=(Vector2 &v1, const Vector2 &v2) {
  31.380 +	v1.x -= v2.x;
  31.381 +	v1.y -= v2.y;
  31.382 +}
  31.383 +
  31.384 +inline void operator *=(Vector2 &v1, const Vector2 &v2) {
  31.385 +	v1.x *= v2.x;
  31.386 +	v1.y *= v2.y;
  31.387 +}
  31.388 +
  31.389 +inline void operator /=(Vector2 &v1, const Vector2 &v2) {
  31.390 +	v1.x /= v2.x;
  31.391 +	v1.y /= v2.y;
  31.392 +}
  31.393 +
  31.394 +inline Vector2 operator +(const Vector2 &vec, scalar_t scalar) {
  31.395 +	return Vector2(vec.x + scalar, vec.y + scalar);
  31.396 +}
  31.397 +
  31.398 +inline Vector2 operator +(scalar_t scalar, const Vector2 &vec) {
  31.399 +	return Vector2(vec.x + scalar, vec.y + scalar);
  31.400 +}
  31.401 +
  31.402 +inline Vector2 operator -(scalar_t scalar, const Vector2 &vec) {
  31.403 +	return Vector2(vec.x - scalar, vec.y - scalar);
  31.404 +}
  31.405 +
  31.406 +inline Vector2 operator *(const Vector2 &vec, scalar_t scalar) {
  31.407 +	return Vector2(vec.x * scalar, vec.y * scalar);
  31.408 +}
  31.409 +
  31.410 +inline Vector2 operator *(scalar_t scalar, const Vector2 &vec) {
  31.411 +	return Vector2(vec.x * scalar, vec.y * scalar);
  31.412 +}
  31.413 +
  31.414 +inline Vector2 operator /(const Vector2 &vec, scalar_t scalar) {
  31.415 +	return Vector2(vec.x / scalar, vec.y / scalar);
  31.416 +}
  31.417 +
  31.418 +inline void operator +=(Vector2 &vec, scalar_t scalar) {
  31.419 +	vec.x += scalar;
  31.420 +	vec.y += scalar;
  31.421 +}
  31.422 +
  31.423 +inline void operator -=(Vector2 &vec, scalar_t scalar) {
  31.424 +	vec.x -= scalar;
  31.425 +	vec.y -= scalar;
  31.426 +}
  31.427 +
  31.428 +inline void operator *=(Vector2 &vec, scalar_t scalar) {
  31.429 +	vec.x *= scalar;
  31.430 +	vec.y *= scalar;
  31.431 +}
  31.432 +
  31.433 +inline void operator /=(Vector2 &vec, scalar_t scalar) {
  31.434 +	vec.x /= scalar;
  31.435 +	vec.y /= scalar;
  31.436 +}
  31.437 +
  31.438 +inline scalar_t Vector2::length() const {
  31.439 +	return sqrt(x*x + y*y);
  31.440 +}
  31.441 +
  31.442 +inline scalar_t Vector2::length_sq() const {
  31.443 +	return x*x + y*y;
  31.444 +}
  31.445 +
  31.446 +inline Vector2 lerp(const Vector2 &a, const Vector2 &b, scalar_t t)
  31.447 +{
  31.448 +	return a + (b - a) * t;
  31.449 +}
  31.450 +
  31.451 +inline Vector2 catmull_rom_spline(const Vector2 &v0, const Vector2 &v1,
  31.452 +		const Vector2 &v2, const Vector2 &v3, scalar_t t)
  31.453 +{
  31.454 +	scalar_t spline(scalar_t, scalar_t, scalar_t, scalar_t, scalar_t);
  31.455 +	scalar_t x = spline(v0.x, v1.x, v2.x, v3.x, t);
  31.456 +	scalar_t y = spline(v0.y, v1.y, v2.y, v3.y, t);
  31.457 +	return Vector2(x, y);
  31.458 +}
  31.459 +
  31.460 +
  31.461 +/* ------------- Vector3 -------------- */
  31.462 +
  31.463 +inline scalar_t &Vector3::operator [](int elem) {
  31.464 +	return elem ? (elem == 1 ? y : z) : x;
  31.465 +}
  31.466 +
  31.467 +inline const scalar_t &Vector3::operator [](int elem) const {
  31.468 +	return elem ? (elem == 1 ? y : z) : x;
  31.469 +}
  31.470 +
  31.471 +/* unary operations */
  31.472 +inline Vector3 operator -(const Vector3 &vec) {
  31.473 +	return Vector3(-vec.x, -vec.y, -vec.z);
  31.474 +}
  31.475 +
  31.476 +/* binary vector (op) vector operations */
  31.477 +inline scalar_t dot_product(const Vector3 &v1, const Vector3 &v2) {
  31.478 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
  31.479 +}
  31.480 +
  31.481 +inline Vector3 cross_product(const Vector3 &v1, const Vector3 &v2) {
  31.482 +	return Vector3(v1.y * v2.z - v1.z * v2.y,  v1.z * v2.x - v1.x * v2.z,  v1.x * v2.y - v1.y * v2.x);
  31.483 +}
  31.484 +
  31.485 +
  31.486 +inline Vector3 operator +(const Vector3 &v1, const Vector3 &v2) {
  31.487 +	return Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
  31.488 +}
  31.489 +
  31.490 +inline Vector3 operator -(const Vector3 &v1, const Vector3 &v2) {
  31.491 +	return Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
  31.492 +}
  31.493 +
  31.494 +inline Vector3 operator *(const Vector3 &v1, const Vector3 &v2) {
  31.495 +	return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
  31.496 +}
  31.497 +
  31.498 +inline Vector3 operator /(const Vector3 &v1, const Vector3 &v2) {
  31.499 +	return Vector3(v1.x / v2.x, v1.y / v2.y, v1.z / v2.z);
  31.500 +}
  31.501 +
  31.502 +inline bool operator ==(const Vector3 &v1, const Vector3 &v2) {
  31.503 +	return (fabs(v1.x - v2.x) < XSMALL_NUMBER) && (fabs(v1.y - v2.y) < XSMALL_NUMBER) && (fabs(v1.z - v2.z) < XSMALL_NUMBER);
  31.504 +}
  31.505 +
  31.506 +inline void operator +=(Vector3 &v1, const Vector3 &v2) {
  31.507 +	v1.x += v2.x;
  31.508 +	v1.y += v2.y;
  31.509 +	v1.z += v2.z;
  31.510 +}
  31.511 +
  31.512 +inline void operator -=(Vector3 &v1, const Vector3 &v2) {
  31.513 +	v1.x -= v2.x;
  31.514 +	v1.y -= v2.y;
  31.515 +	v1.z -= v2.z;
  31.516 +}
  31.517 +
  31.518 +inline void operator *=(Vector3 &v1, const Vector3 &v2) {
  31.519 +	v1.x *= v2.x;
  31.520 +	v1.y *= v2.y;
  31.521 +	v1.z *= v2.z;
  31.522 +}
  31.523 +
  31.524 +inline void operator /=(Vector3 &v1, const Vector3 &v2) {
  31.525 +	v1.x /= v2.x;
  31.526 +	v1.y /= v2.y;
  31.527 +	v1.z /= v2.z;
  31.528 +}
  31.529 +/* binary vector (op) scalar and scalar (op) vector operations */
  31.530 +inline Vector3 operator +(const Vector3 &vec, scalar_t scalar) {
  31.531 +	return Vector3(vec.x + scalar, vec.y + scalar, vec.z + scalar);
  31.532 +}
  31.533 +
  31.534 +inline Vector3 operator +(scalar_t scalar, const Vector3 &vec) {
  31.535 +	return Vector3(vec.x + scalar, vec.y + scalar, vec.z + scalar);
  31.536 +}
  31.537 +
  31.538 +inline Vector3 operator -(const Vector3 &vec, scalar_t scalar) {
  31.539 +	return Vector3(vec.x - scalar, vec.y - scalar, vec.z - scalar);
  31.540 +}
  31.541 +
  31.542 +inline Vector3 operator *(const Vector3 &vec, scalar_t scalar) {
  31.543 +	return Vector3(vec.x * scalar, vec.y * scalar, vec.z * scalar);
  31.544 +}
  31.545 +
  31.546 +inline Vector3 operator *(scalar_t scalar, const Vector3 &vec) {
  31.547 +	return Vector3(vec.x * scalar, vec.y * scalar, vec.z * scalar);
  31.548 +}
  31.549 +
  31.550 +inline Vector3 operator /(const Vector3 &vec, scalar_t scalar) {
  31.551 +	return Vector3(vec.x / scalar, vec.y / scalar, vec.z / scalar);
  31.552 +}
  31.553 +
  31.554 +inline void operator +=(Vector3 &vec, scalar_t scalar) {
  31.555 +	vec.x += scalar;
  31.556 +	vec.y += scalar;
  31.557 +	vec.z += scalar;
  31.558 +}
  31.559 +
  31.560 +inline void operator -=(Vector3 &vec, scalar_t scalar) {
  31.561 +	vec.x -= scalar;
  31.562 +	vec.y -= scalar;
  31.563 +	vec.z -= scalar;
  31.564 +}
  31.565 +
  31.566 +inline void operator *=(Vector3 &vec, scalar_t scalar) {
  31.567 +	vec.x *= scalar;
  31.568 +	vec.y *= scalar;
  31.569 +	vec.z *= scalar;
  31.570 +}
  31.571 +
  31.572 +inline void operator /=(Vector3 &vec, scalar_t scalar) {
  31.573 +	vec.x /= scalar;
  31.574 +	vec.y /= scalar;
  31.575 +	vec.z /= scalar;
  31.576 +}
  31.577 +
  31.578 +inline scalar_t Vector3::length() const {
  31.579 +	return sqrt(x*x + y*y + z*z);
  31.580 +}
  31.581 +inline scalar_t Vector3::length_sq() const {
  31.582 +	return x*x + y*y + z*z;
  31.583 +}
  31.584 +
  31.585 +inline Vector3 lerp(const Vector3 &a, const Vector3 &b, scalar_t t) {
  31.586 +	return a + (b - a) * t;
  31.587 +}
  31.588 +
  31.589 +inline Vector3 catmull_rom_spline(const Vector3 &v0, const Vector3 &v1,
  31.590 +		const Vector3 &v2, const Vector3 &v3, scalar_t t)
  31.591 +{
  31.592 +	scalar_t spline(scalar_t, scalar_t, scalar_t, scalar_t, scalar_t);
  31.593 +	scalar_t x = spline(v0.x, v1.x, v2.x, v3.x, t);
  31.594 +	scalar_t y = spline(v0.y, v1.y, v2.y, v3.y, t);
  31.595 +	scalar_t z = spline(v0.z, v1.z, v2.z, v3.z, t);
  31.596 +	return Vector3(x, y, z);
  31.597 +}
  31.598 +
  31.599 +/* ----------- Vector4 ----------------- */
  31.600 +
  31.601 +inline scalar_t &Vector4::operator [](int elem) {
  31.602 +	return elem ? (elem == 1 ? y : (elem == 2 ? z : w)) : x;
  31.603 +}
  31.604 +
  31.605 +inline const scalar_t &Vector4::operator [](int elem) const {
  31.606 +	return elem ? (elem == 1 ? y : (elem == 2 ? z : w)) : x;
  31.607 +}
  31.608 +
  31.609 +inline Vector4 operator -(const Vector4 &vec) {
  31.610 +	return Vector4(-vec.x, -vec.y, -vec.z, -vec.w);
  31.611 +}
  31.612 +
  31.613 +inline scalar_t dot_product(const Vector4 &v1, const Vector4 &v2) {
  31.614 +	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
  31.615 +}
  31.616 +
  31.617 +inline Vector4 cross_product(const Vector4 &v1, const Vector4 &v2, const Vector4 &v3) {
  31.618 +	scalar_t a, b, c, d, e, f;       /* Intermediate Values */
  31.619 +    Vector4 result;
  31.620 +
  31.621 +    /* Calculate intermediate values. */
  31.622 +    a = (v2.x * v3.y) - (v2.y * v3.x);
  31.623 +    b = (v2.x * v3.z) - (v2.z * v3.x);
  31.624 +    c = (v2.x * v3.w) - (v2.w * v3.x);
  31.625 +    d = (v2.y * v3.z) - (v2.z * v3.y);
  31.626 +    e = (v2.y * v3.w) - (v2.w * v3.y);
  31.627 +    f = (v2.z * v3.w) - (v2.w * v3.z);
  31.628 +
  31.629 +    /* Calculate the result-vector components. */
  31.630 +    result.x =   (v1.y * f) - (v1.z * e) + (v1.w * d);
  31.631 +    result.y = - (v1.x * f) + (v1.z * c) - (v1.w * b);
  31.632 +    result.z =   (v1.x * e) - (v1.y * c) + (v1.w * a);
  31.633 +    result.w = - (v1.x * d) + (v1.y * b) - (v1.z * a);
  31.634 +    return result;
  31.635 +}
  31.636 +
  31.637 +inline Vector4 operator +(const Vector4 &v1, const Vector4 &v2) {
  31.638 +	return Vector4(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z, v1.w + v2.w);
  31.639 +}
  31.640 +
  31.641 +inline Vector4 operator -(const Vector4 &v1, const Vector4 &v2) {
  31.642 +	return Vector4(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z, v1.w - v2.w);
  31.643 +}
  31.644 +
  31.645 +inline Vector4 operator *(const Vector4 &v1, const Vector4 &v2) {
  31.646 +	return Vector4(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z, v1.w * v2.w);
  31.647 +}
  31.648 +
  31.649 +inline Vector4 operator /(const Vector4 &v1, const Vector4 &v2) {
  31.650 +	return Vector4(v1.x / v2.x, v1.y / v2.y, v1.z / v2.z, v1.w / v2.w);
  31.651 +}
  31.652 +
  31.653 +inline bool operator ==(const Vector4 &v1, const Vector4 &v2) {
  31.654 +	return	(fabs(v1.x - v2.x) < XSMALL_NUMBER) &&
  31.655 +			(fabs(v1.y - v2.y) < XSMALL_NUMBER) &&
  31.656 +			(fabs(v1.z - v2.z) < XSMALL_NUMBER) &&
  31.657 +			(fabs(v1.w - v2.w) < XSMALL_NUMBER);
  31.658 +}
  31.659 +
  31.660 +inline void operator +=(Vector4 &v1, const Vector4 &v2) {
  31.661 +	v1.x += v2.x;
  31.662 +	v1.y += v2.y;
  31.663 +	v1.z += v2.z;
  31.664 +	v1.w += v2.w;
  31.665 +}
  31.666 +
  31.667 +inline void operator -=(Vector4 &v1, const Vector4 &v2) {
  31.668 +	v1.x -= v2.x;
  31.669 +	v1.y -= v2.y;
  31.670 +	v1.z -= v2.z;
  31.671 +	v1.w -= v2.w;
  31.672 +}
  31.673 +
  31.674 +inline void operator *=(Vector4 &v1, const Vector4 &v2) {
  31.675 +	v1.x *= v2.x;
  31.676 +	v1.y *= v2.y;
  31.677 +	v1.z *= v2.z;
  31.678 +	v1.w *= v2.w;
  31.679 +}
  31.680 +
  31.681 +inline void operator /=(Vector4 &v1, const Vector4 &v2) {
  31.682 +	v1.x /= v2.x;
  31.683 +	v1.y /= v2.y;
  31.684 +	v1.z /= v2.z;
  31.685 +	v1.w /= v2.w;
  31.686 +}
  31.687 +
  31.688 +/* binary vector (op) scalar and scalar (op) vector operations */
  31.689 +inline Vector4 operator +(const Vector4 &vec, scalar_t scalar) {
  31.690 +	return Vector4(vec.x + scalar, vec.y + scalar, vec.z + scalar, vec.w + scalar);
  31.691 +}
  31.692 +
  31.693 +inline Vector4 operator +(scalar_t scalar, const Vector4 &vec) {
  31.694 +	return Vector4(vec.x + scalar, vec.y + scalar, vec.z + scalar, vec.w + scalar);
  31.695 +}
  31.696 +
  31.697 +inline Vector4 operator -(const Vector4 &vec, scalar_t scalar) {
  31.698 +	return Vector4(vec.x - scalar, vec.y - scalar, vec.z - scalar, vec.w - scalar);
  31.699 +}
  31.700 +
  31.701 +inline Vector4 operator *(const Vector4 &vec, scalar_t scalar) {
  31.702 +	return Vector4(vec.x * scalar, vec.y * scalar, vec.z * scalar, vec.w * scalar);
  31.703 +}
  31.704 +
  31.705 +inline Vector4 operator *(scalar_t scalar, const Vector4 &vec) {
  31.706 +	return Vector4(vec.x * scalar, vec.y * scalar, vec.z * scalar, vec.w * scalar);
  31.707 +}
  31.708 +
  31.709 +inline Vector4 operator /(const Vector4 &vec, scalar_t scalar) {
  31.710 +	return Vector4(vec.x / scalar, vec.y / scalar, vec.z / scalar, vec.w / scalar);
  31.711 +}
  31.712 +
  31.713 +inline void operator +=(Vector4 &vec, scalar_t scalar) {
  31.714 +	vec.x += scalar;
  31.715 +	vec.y += scalar;
  31.716 +	vec.z += scalar;
  31.717 +	vec.w += scalar;
  31.718 +}
  31.719 +
  31.720 +inline void operator -=(Vector4 &vec, scalar_t scalar) {
  31.721 +	vec.x -= scalar;
  31.722 +	vec.y -= scalar;
  31.723 +	vec.z -= scalar;
  31.724 +	vec.w -= scalar;
  31.725 +}
  31.726 +
  31.727 +inline void operator *=(Vector4 &vec, scalar_t scalar) {
  31.728 +	vec.x *= scalar;
  31.729 +	vec.y *= scalar;
  31.730 +	vec.z *= scalar;
  31.731 +	vec.w *= scalar;
  31.732 +}
  31.733 +
  31.734 +inline void operator /=(Vector4 &vec, scalar_t scalar) {
  31.735 +	vec.x /= scalar;
  31.736 +	vec.y /= scalar;
  31.737 +	vec.z /= scalar;
  31.738 +	vec.w /= scalar;
  31.739 +}
  31.740 +
  31.741 +inline scalar_t Vector4::length() const {
  31.742 +	return sqrt(x*x + y*y + z*z + w*w);
  31.743 +}
  31.744 +inline scalar_t Vector4::length_sq() const {
  31.745 +	return x*x + y*y + z*z + w*w;
  31.746 +}
  31.747 +
  31.748 +inline Vector4 lerp(const Vector4 &v0, const Vector4 &v1, scalar_t t)
  31.749 +{
  31.750 +	return v0 + (v1 - v0) * t;
  31.751 +}
  31.752 +
  31.753 +inline Vector4 catmull_rom_spline(const Vector4 &v0, const Vector4 &v1,
  31.754 +		const Vector4 &v2, const Vector4 &v3, scalar_t t)
  31.755 +{
  31.756 +	scalar_t spline(scalar_t, scalar_t, scalar_t, scalar_t, scalar_t);
  31.757 +	scalar_t x = spline(v0.x, v1.x, v2.x, v3.x, t);
  31.758 +	scalar_t y = spline(v0.y, v1.y, v2.y, v3.y, t);
  31.759 +	scalar_t z = spline(v0.z, v1.z, v2.z, v3.z, t);
  31.760 +	scalar_t w = spline(v0.w, v1.w, v2.w, v3.w, t);
  31.761 +	return Vector4(x, y, z, w);
  31.762 +}
  31.763 +
  31.764 +#endif	/* __cplusplus */