module coneneko.math;
import
	std.math,
	std.stream,
	std.conv,
	std.string,
	std.c.string,
	std.cstream,
	std.random;

///
class MathError : Error
{
	this(long line, string msg)
	{
		super(format("%s(%d): %s", __FILE__, line, msg));
	}
}

/// float x, y, z, w、Matrixにかける場合はw値に注意
struct Vector
{
	invariant()
	{
		assert(!isnan(x));
		assert(!isnan(y));
		assert(!isnan(z));
		assert(!isnan(w));
	}
	
	///
	float x = 0, y = 0, z = 0, w = 0;
	alias x r; ///
	alias y g; ///
	alias z b; ///
	alias w a; ///
	
	static Vector create() { Vector result; return result; } ///
	static Vector create(float x, float y) { return Vector.create(x, y, 0, 0); } ///
	static Vector create(float x, float y, float z) { return Vector.create(x, y, z, 0); } ///

	static Vector create(float x, float y, float z, float w) ///
	{
		Vector result;
		result.x = x;
		result.y = y;
		result.z = z;
		result.w = w;
		return result;
	}
	
	Vector opNeg() /// -
	{
		return create(-x, -y, -z, -w);
	}
	
	bool opEquals(Vector f) /// ==
	{
		return x == f.x && y == f.y && z == f.z && w == f.w ? true : false;
	}
	
	Vector opAdd(Vector f) /// +
	{
		return create(x + f.x, y + f.y, z + f.z, w + f.w);
	}
	
	Vector opSub(Vector f) /// -
	{
		return create(x - f.x, y - f.y, z - f.z, w - f.w);
	}
	
	Vector opMul(float f) /// *
	{
		return create(x * f, y * f, z * f, w * f);
	}
	
	Vector opDiv(float f) /// /
	{
		return create(x / f, y / f, z / f, w / f);
	}
	
	void opAddAssign(Vector f) /// +=
	{
		x += f.x;
		y += f.y;
		z += f.z;
		w += f.w;
	}
	
	void opSubAssign(Vector f) /// -=
	{
		x -= f.x;
		y -= f.y;
		z -= f.z;
		w -= f.w;
	}
	
	void opMulAssign(float f) /// *=
	{
		x *= f;
		y *= f;
		z *= f;
		w *= f;
	}
	
	void opDivAssign(float f) /// /=
	{
		x /= f;
		y /= f;
		z /= f;
		w /= f;
	}
	
	Vector opMul(Matrix m) /// wの値に注意、設定、除算、初期化
	{
		return Vector(
			x * m.m[0][0] + y * m.m[1][0] + z * m.m[2][0] + w * m.m[3][0],
			x * m.m[0][1] + y * m.m[1][1] + z * m.m[2][1] + w * m.m[3][1],
			x * m.m[0][2] + y * m.m[1][2] + z * m.m[2][2] + w * m.m[3][2],
			x * m.m[0][3] + y * m.m[1][3] + z * m.m[2][3] + w * m.m[3][3]
		);
	}
	
	void opMulAssign(Matrix m) /// ditto
	{
		Vector result = *this;
		*this = result * m;
	}
	
	string toString() ///
	{
		MemoryStream result = new MemoryStream();
		result.writef("{ %f %f %f %f }", cast(double)x, cast(double)y, cast(double)z, cast(double)w);
		return result.toString();
	}
	
	void print() ///
	{
		dout.writeString(toString());
	}
}

alias Vector.create vector; ///

Vector mulW(Vector a, Matrix b) /// a.w=1として計算、除算、result.w=0で返す
in
{
	assert(a.w == 0.0f);
}
out (result)
{
	assert(result.w  == 0.0f);
}
body
{
	auto result = Vector(a.x, a.y, a.z, 1) * b;
	if (result.w != 0) result /= result.w;
	result.w = 0;
	return result;
}

float scalar(Vector a) ///
{
	return sqrt(a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w);
}

float distance(Vector a, Vector b) /// 二点間の距離(差の長さ)
{
	return scalar(a - b);
}

unittest
{
	try
	{
		normalize(vector(0.0, 0.0, 0.0));
		assert(false);
	}
	catch (Error e) {}
}

Vector normalize(Vector a) /// 正規化
{
	float aLength = scalar(a);
	if (aLength == 0.0) throw new MathError(__LINE__, "normalize");
	return a / aLength;
}

float dot(Vector a, Vector b) /// 内積( length(a) * length(b) * cos(ab) )
{
	return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}

float cross2(Vector a, Vector b) /// 外積( length(a) * length(b) * sin(ab) )、二次元のは例外的に
{
	return a.x * b.y - a.y * b.x;
}

Vector cross(Vector a, Vector b) /// vec3
{
	return Vector.create(
		a.y * b.z - a.z * b.y,
		a.z * b.x - a.x * b.z,
		a.x * b.y - a.y * b.x
		);
}

Vector cross(Vector a, Vector b, Vector c) /// vec4, 外積( 体積 )
{
	return Vector.create(
		  a.y * b.z * c.w  +  b.y * c.z * a.w  +  c.y * a.z * b.w
		- c.y * b.z * a.w  -  b.y * a.z * c.w  -  a.y * c.z * b.w,

		-(a.z * b.w * c.x  +  b.z * c.w * a.x  +  c.z * a.w * b.x
		- c.z * b.w * a.x  -  b.z * a.w * c.x  -  a.z * c.w * b.x),
		
		  a.w * b.x * c.y  +  b.w * c.x * a.y  +  c.w * a.x * b.y
		- c.w * b.x * a.y  -  b.w * a.x * c.y  -  a.w * c.x * b.y,
		
		-(a.x * b.y * c.z  +  b.x * c.y * a.z  +  c.x * a.y * b.z
		- c.x * b.y * a.z  -  b.x * a.y * c.z  -  a.x * c.y * b.z)
		);
}

Matrix matrix(float[] a ...) ///
{
	if (a.length != 4 * 4) throw new MathError(__LINE__, "matrix");
	return Matrix(a);
}

/// Direct3Dの方に合わせている、そのうちOpenGLに合わせる
struct Matrix
{
	/**
	 * 左上から[行][列]
	 * ---
	 * 00 01 02 03
	 * 10 11 12 13
	 * 20 21 22 23
	 * 30 31 32 33
	 * ---
	 */
	// TODO 混乱の元なのでabcd efgh ijkl mnopにする?
	// アクセスもopIndex[行, 列]で
	float m[4][4]; // [y][x]になる
	
	Matrix opMul(Matrix m2) /// *
	{
		Matrix result = *this;
		for (int i = 0; i < 4; i++)
		{
			result.m[i][0] = m[i][0] * m2.m[0][0] + m[i][1] * m2.m[1][0] + m[i][2] * m2.m[2][0] + m[i][3] * m2.m[3][0];
			result.m[i][1] = m[i][0] * m2.m[0][1] + m[i][1] * m2.m[1][1] + m[i][2] * m2.m[2][1] + m[i][3] * m2.m[3][1];
			result.m[i][2] = m[i][0] * m2.m[0][2] + m[i][1] * m2.m[1][2] + m[i][2] * m2.m[2][2] + m[i][3] * m2.m[3][2];
			result.m[i][3] = m[i][0] * m2.m[0][3] + m[i][1] * m2.m[1][3] + m[i][2] * m2.m[2][3] + m[i][3] * m2.m[3][3];
		}
		return result;
	}

	void opMulAssign(Matrix m2) /// *=
	{
		Matrix result = *this;
		*this = result * m2;
	}


	static Matrix identity() ///
	{
		Matrix result;
		for (int i = 0; i < 4; i++) result.m[i][] = 0.0;
		for (int i = 0; i < 4; i++) result.m[i][i] = 1.0;
		return result;
	}

	static Matrix zero() ///
	{
		Matrix result;
		for (int i = 0; i < 4; i++)
		{
			for (int j = 0; j < 4; j++) result.m[i][j] = 0;
		}
		return result;
	}

	static Matrix translation(float x, float y, float z) ///
	{
		Matrix result = identity;
		result.m[3][0] = x;
		result.m[3][1] = y;
		result.m[3][2] = z;
		return result;
	}
	
	static Matrix translation(Vector a) ///
	{
		return translation(a.x, a.y, a.z);
	}
	
	static Matrix scaling(float sx, float sy, float sz) ///
	{
		Matrix result = identity;
		result.m[0][0] = sx;
		result.m[1][1] = sy;
		result.m[2][2] = sz;
		return result;
	}
	
	static Matrix rotationX(float radian) ///
	{
		Matrix result = identity;
		result.m[1][1] = cos(radian);
		result.m[1][2] = sin(radian);
		result.m[2][1] = -sin(radian);
		result.m[2][2] = cos(radian);
		return result;
	}
	
	static Matrix rotationY(float radian) ///
	{
		Matrix result = identity;
		result.m[0][0] = cos(radian);
		result.m[0][2] = -sin(radian);
		result.m[2][0] = sin(radian);
		result.m[2][2] = cos(radian);
		return result;
	}
	
	static Matrix rotationZ(float radian) ///
	{
		Matrix result = identity;
		result.m[0][0] = cos(radian);
		result.m[0][1] = sin(radian);
		result.m[1][0] = -sin(radian);
		result.m[1][1] = cos(radian);
		return result;
	}

	///
	static Matrix lookAtLH(
		float px, float py, float pz, float ax, float ay, float az, float ux, float uy, float uz)
	{
		Vector position = Vector.create(px, py, pz);
		Vector lookAtPoint = Vector.create(ax, ay, az);
		Vector upDirection = Vector.create(ux, uy, uz);
		
		Vector zaxis = lookAtPoint - position;
		assert(0.0 != scalar(zaxis));
		zaxis = normalize(zaxis);
		
		Vector xaxis = cross(upDirection, zaxis);
		assert(0.0 != scalar(xaxis));
		xaxis = normalize(xaxis);
		
		Vector yaxis = cross(zaxis, xaxis);
		Matrix result;
		result.m[0][0] = xaxis.x;
		result.m[0][1] = yaxis.x;
		result.m[0][2] = zaxis.x;
		result.m[0][3] = 0;
		
		result.m[1][0] = xaxis.y;
		result.m[1][1] = yaxis.y;
		result.m[1][2] = zaxis.y;
		result.m[1][3] = 0;
		
		result.m[2][0] = xaxis.z;
		result.m[2][1] = yaxis.z;
		result.m[2][2] = zaxis.z;
		result.m[2][3] = 0;
		
		result.m[3][0] = -dot(xaxis, position);
		result.m[3][1] = -dot(yaxis, position);
		result.m[3][2] = -dot(zaxis, position);
		result.m[3][3] = 1;
		return result;
	}
	
	static Matrix lookAtLH(Vector p, Vector a, Vector u) ///
	{
		return lookAtLH(p.x, p.y, p.z,  a.x, a.y, a.z,  u.x, u.y, u.z);
	}
	
	///
	static Matrix lookAtRH(Vector position, Vector at, Vector up = vector(0.0, 1.0, 0.0))
	{
		return lookAtRH(position.x, position.y, position.z, at.x, at.y, at.z, up.x, up.y, up.z);
	}
	
	///
	static Matrix lookAtRH(
		float px, float py, float pz,
		float ax, float ay, float az,
		float ux = 0.0, float uy = 1.0, float uz = 0.0)
	{
		Vector position = Vector.create(px, py, pz);
		Vector lookAtPoint = Vector.create(ax, ay, az);
		Vector upDirection = Vector.create(ux, uy, uz);
		
		Vector zaxis = position - lookAtPoint;
		assert(0.0 != scalar(zaxis));
		zaxis = normalize(zaxis);
		
		Vector xaxis = cross(upDirection, zaxis);
		assert(0.0 != scalar(xaxis));
		xaxis = normalize(xaxis);
		
		Vector yaxis = cross(zaxis, xaxis);
		Matrix result;
		result.m[0][0] = xaxis.x;
		result.m[0][1] = yaxis.x;
		result.m[0][2] = zaxis.x;
		result.m[0][3] = 0;
			
		result.m[1][0] = xaxis.y;
		result.m[1][1] = yaxis.y;
		result.m[1][2] = zaxis.y;
		result.m[1][3] = 0;
			
		result.m[2][0] = xaxis.z;
		result.m[2][1] = yaxis.z;
		result.m[2][2] = zaxis.z;
		result.m[2][3] = 0;
			
		result.m[3][0] = -dot(xaxis, position);
		result.m[3][1] = -dot(yaxis, position);
		result.m[3][2] = -dot(zaxis, position);
		result.m[3][3] = 1;
		return result;
	}
	alias lookAtRH lookAt; ///

	/// directxのを持ってきたのでz[0, 1]のまま、注意、TODO 混乱するので削除?
	static Matrix perspectiveFovLH(
		float fieldOfViewY, float aspectRatio, float nearPlaneZ, float farPlaneZ)
	{
		float cot(float a) { return 1 / tan(a); }
		Matrix result = zero;
		float h = cot(fieldOfViewY / 2);
		result.m[0][0] = h / aspectRatio;
		result.m[1][1] = h;
		result.m[2][2] = farPlaneZ / (farPlaneZ - nearPlaneZ);
		result.m[2][3] = 1;
		result.m[3][2] = -nearPlaneZ * farPlaneZ / (farPlaneZ - nearPlaneZ);
		return result;
	}
	
	/*
	projection変換
	direct3d z=0が手前、z=1が奥
	opengl z[-1.0, 1.0) z = -1.0が手前
	auto a = vector(x, y, z, 1.0) * projection;
	a /= a.w;
	*/
	
	/// opengl仕様 z[-1.0, 1.0]
	static Matrix perspectiveFovRH(
		float fieldOfViewY = PI / 8.0,
		float aspectRatio = 640.0 / 480.0,
		float nearPlaneZ = 1.0,
		float farPlaneZ = 1000.0)
	{
		float cot(float a) { return 1 / tan(a); }
		Matrix result = zero;
		float h = cot(fieldOfViewY / 2);
		result.m[0][0] = h / aspectRatio;
		result.m[1][1] = h;
		//result.m[2][2] = farPlaneZ / (nearPlaneZ - farPlaneZ); // d3d
		result.m[2][2] = (nearPlaneZ + farPlaneZ) / (nearPlaneZ - farPlaneZ);
		result.m[2][3] = -1;
		//result.m[3][2] = nearPlaneZ * farPlaneZ / (nearPlaneZ - farPlaneZ); // d3d
		result.m[3][2] = nearPlaneZ * farPlaneZ * 2.0 / (nearPlaneZ - farPlaneZ);
		return result;
	}
	alias perspectiveFovRH perspectiveFov; ///
	
	///
	static Matrix ortho(
		float left = -1.0, float right = 1.0,
		float bottom = -1.0, float top = 1.0,
		float zNear = -1.0, float zFar = 1.0
	)
	{
		Matrix result = zero;
		result[0, 0] = 2.0 / (right - left);
		result[1, 1] = 2.0 / (top - bottom);
		result[2, 2] = -2.0 / (zFar - zNear);
		result[0, 3] = -((right + left) / (right - left));
		result[1, 3] = -((top + bottom) / (top - bottom));
		result[2, 3] = -((zFar + zNear) / (zFar - zNear));
		result[3, 3] = 1;
		return result;
	}
	
	string toString() ///
	{
		MemoryStream result = new MemoryStream();
		result.writef("{\n");
		for (int i = 0; i < 4; i++)
		{
			result.writef("  %f %f %f %f\n", cast(double)m[i][0], cast(double)m[i][1], cast(double)m[i][2], cast(double)m[i][3]);
		}
		result.writef("}");
		return result.toString();
	}
	
	void print() ///
	{
		dout.writeLine(toString());
	}

/+
	static Matrix rotation(float vx, float vy, float vz, float radian) // Quaternion
	{
		Vector v = vector(vx, vy, vz);
		assert(0.0 != scalar(v));
		v = normalize(v);
		Matrix result;

		float w = (float)cos(radian / 2.0);
		float w2 = w * w;
		float s = (float)sin(radian / 2.0);
		float x = v.x * s;
		float y = v.y * s;
		float z = v.z * s;
		float x2 = x * x;
		float y2 = y * y;
		float z2 = z * z;

		result.m[0][0] = w2 + x2 - y2 - z2;
		result.m[1][0] = 2 * ((x * y) - (w * z));
		result.m[2][0] = 2 * ((x * z) + (w * y));
		result.m[3][0] = 0;

		result.m[0][1] = 2 * ( ( x * y ) + ( w * z ) );
		result.m[1][1] = w2 - x2 + y2 - z2;
		result.m[2][1] = 2 * ( ( y * z ) - ( w * x ) );
		result.m[3][1] = 0;

		result.m[0][2] = 2 * ( ( x * z ) - ( w * y ) );
		result.m[1][2] = 2 * ( ( y * z ) + ( w * x ) );
		result.m[2][2] = w2 - x2 - y2 + z2;
		result.m[3][2] = 0;

		result.m[0][3] = 0;
		result.m[1][3] = 0;
		result.m[2][3] = 0;
		result.m[3][3] = 1;

		return result;
	}
+/

	static Matrix opCall(string a) { return Matrix(toFloatArray(a)); } ///
	
	static Matrix opCall(float[] a) ///
	{
		if (a.length != 4 * 4) throw new Error("Matrix.opCall");
		Matrix result;
		memcpy(&result, a.ptr, float.sizeof * a.length);
		return result;
	}
	
	void opIndexAssign(float value, uint x, uint y) ///
	{
		m[y][x] = value;
	}
	
	
	static Matrix modelLookAt(Vector position, Vector at) /// モデルデータ用、upは(0, 1, 0)
	{
		Vector p = position;
		Vector a = at;
		if (p == a) a.z -= 1.0;
		Vector pa = a - p;
		return inverse(Matrix.lookAtLH(0, 0, 0,  pa.x, pa.y, pa.z,  0, 1, 0))
			* Matrix.translation(p.x, p.y, p.z);
	}
}

unittest
{
	assert(Matrix.identity == Matrix("1 0 0 0  0 1 0 0  0 0 1 0  0 0 0 1"));
}

float[] toFloatArray(string a) ///
{
	alias atof toFloat;
	float[] result;
	foreach (string c; split(a)) result ~= toFloat(c);
	return result;
}

Matrix inverse(Matrix m) ///
{
	Matrix result = Matrix.zero;
	result.m[0][0] = m.m[0][0];
	result.m[0][1] = m.m[1][0];
	result.m[0][2] = m.m[2][0];
	result.m[1][0] = m.m[0][1];
	result.m[1][1] = m.m[1][1];
	result.m[1][2] = m.m[2][1];
	result.m[2][0] = m.m[0][2];
	result.m[2][1] = m.m[1][2];
	result.m[2][2] = m.m[2][2];
	result.m[3][0] = -(m.m[0][0] * m.m[3][0] + m.m[1][0] * m.m[3][1] + m.m[2][0] * m.m[3][2]);
	result.m[3][1] = -(m.m[0][1] * m.m[3][0] + m.m[1][1] * m.m[3][1] + m.m[2][1] * m.m[3][2]);
	result.m[3][2] = -(m.m[0][2] * m.m[3][0] + m.m[1][2] * m.m[3][1] + m.m[2][2] * m.m[3][2]);
	result.m[3][3] = 1;
	return result;
}

Matrix createMatrixFromQuaternion(Vector quaternion) ///
{
	float x = quaternion.x;
	float y = quaternion.y;
	float z = quaternion.z;
	float w = quaternion.w;
	Matrix result = Matrix.identity;
	result.m[0][0] = 1 - 2 * (y * y + z * z);
	result.m[1][1] = 1 - 2 * (x * x + z * z);
	result.m[2][2] = 1 - 2 * (x * x + y * y);
	result.m[0][1] = 2 * (x * y + z * w);
	result.m[0][2] = 2 * (x * z - y * w);
	result.m[1][0] = 2 * (x * y - z * w);
	result.m[1][2] = 2 * (y * z + x * w);
	result.m[2][0] = 2 * (x * z + y * w);
	result.m[2][1] = 2 * (y * z - x * w);
	return result;
}

Vector createSlerpQuaternion(Vector quaternion0, Vector quaternion1, float t) ///
{
	const float DELTA = 1e-6;
	float cosom = dot(quaternion0, quaternion1);
	if (cosom < 0)
	{
		cosom = -cosom;
		quaternion1 = -quaternion1;
	}
	float scale0, scale1;
	if (1 - cosom > DELTA)
	{
		float omega = acos(cosom);
		float sinom = sin(omega);
		scale0 = sin((1 - t) * omega) / sinom;
		scale1 = sin(t * omega) / sinom;
	}
	else
	{
		scale0 = 1 - t;
		scale1 = t;
	}
	return quaternion0 * scale0 + quaternion1 * scale1;
}




bool triangleIntersectRay(
	Vector t0, Vector t1, Vector t2, Vector rayPosition, Vector rayDirection) ///
{
	float distance;
	return triangleIntersectRay2(t0, t1, t2, rayPosition, rayDirection, distance);
}

float distanceOfTriangleAndRay(
	Vector t0, Vector t1, Vector t2, Vector rayPosition, Vector rayDirection) ///
in
{
	assert(triangleIntersectRay(t0, t1, t2, rayPosition, rayDirection));
}
body
{
	float distance;
	triangleIntersectRay2(t0, t1, t2, rayPosition, rayDirection, distance);
	return distance;
}

bool triangleIntersectRay2(
	Vector t0, Vector t1, Vector t2, Vector rayPosition, Vector rayDirection,
	inout float distance) ///
{
	const float EPSILON = 0.000001;
	Vector edge1 = t1 - t0;
	Vector edge2 = t2 - t0;
	Vector pvec = cross(rayDirection, edge2);
	float det = dot(edge1, pvec);
	if (det > -EPSILON && det < EPSILON) return false;
	float inv_det = 1.0 / det;
	Vector tvec = rayPosition - t0;
	float u = dot(tvec, pvec) * inv_det;
	if (u < 0.0 || u > 1.0) return false;
	Vector qvec = cross(tvec, edge1);
	float v = dot(rayDirection, qvec) * inv_det;
	if (v < 0.0 || u + v > 1.0) return false;
	distance = dot(edge2, qvec) * inv_det;
	return true;
}

float distanceOfPointAndLine(Vector p, Vector l0, Vector l1) ///
{
	return scalar(cross(l1 - l0, p - l0)) / scalar(l1 - l0);
}

float distanceOfPointAndLineSegment(Vector p, Vector ls0, Vector ls1) ///
{
	return scalar(lineSegmentToPoint(ls0, ls1, p));
}

Vector lineSegmentToPoint(Vector ls0, Vector ls1, Vector p) ///
{
	if (dot(p - ls0, ls0 - ls1) > 0.0) return p - ls0;
	else if (dot(p - ls1, ls1 - ls0) > 0.0) return p - ls1;
	Vector a = cross(cross(ls1 - ls0, p - ls0), ls1 - ls0);
	assert(0.0 != scalar(a));
	a = normalize(a);
	return a * distanceOfPointAndLine(p, ls0, ls1);
}




unittest // Vectorをfloat2として使う
{
	Vector f;
	f = -Vector.create(1, 2);
	assert(-1 == f.x && -2 == f.y);
	assert(Vector.create(-1, -2) == f);
	f = Vector.create(1, 2);
	assert(Vector.create(2, 4) == f + f);
	assert(Vector.create(0, 0) == f - f);
	assert(Vector.create(3, 6) == f * 3);
	assert(Vector.create(3, 6) == 3 * f);
	assert(Vector.create(0.5, 1) == f / 2);
	f += Vector.create(1, 2);
	assert(Vector.create(2, 4) == f);
	f -= Vector.create(1, 2);
	assert(Vector.create(1, 2) == f);
	f *= 3;
	assert(Vector.create(3, 6) == f);
	f /= 3;
	assert(Vector.create(1, 2) == f);
	assert(5 == scalar(Vector.create(3, 4)));
	assert(5 == distance(Vector.create(1, 1), Vector.create(4, 5)));
	bool nearlyOne(float f) { return fabs(f - 1) < 0.00000001 ? true : false; }
	assert(nearlyOne(scalar(normalize(Vector.create(3, 4)))));
	assert(0 == dot(Vector.create(1, 0), Vector.create(0, 1)));
	assert(0 == cross2(Vector.create(1, 0), Vector.create(-1, 0)));
}

unittest // Vectorをfloat3として使う
{
	Vector f;
	f = -Vector.create(1, 2, 3);
	assert(-1 == f.x && -2 == f.y && -3 == f.z);
	assert(Vector.create(-1, -2, -3) == f);
	f = Vector.create(1, 2, 3);
	assert(Vector.create(2, 4, 6) == f + f);
	assert(Vector.create(0, 0, 0) == f - f);
	assert(Vector.create(3, 6, 9) == f * 3);
	assert(Vector.create(0.5, 1, 1.5) == f / 2);
	f += Vector.create(1, 2, 3);
	assert(Vector.create(2, 4, 6) == f);
	f -= Vector.create(1, 2, 3);
	assert(Vector.create(1, 2, 3) == f);
	f *= 3;
	assert(Vector.create(3, 6, 9) == f);
	f /= 3;
	assert(Vector.create(1, 2, 3) == f);
	bool nearly(float a, float b) { return fabs(a - b) < 0.000001 ? true : false; }
	assert(nearly(sqrt(3.0), scalar(Vector.create(1, 1, 1))));
	assert(nearly(sqrt(3.0),
		distance(Vector.create(1, 1, 1), Vector.create(2, 2, 2))
		));
	assert(nearly(1.0, scalar(normalize(Vector.create(1, 2, 3)))));
	assert(0 == dot(Vector.create(1, 0, 0), Vector.create(0, 1, 0)));
	assert(Vector.create(0, 0, 0) == cross(Vector.create(1, 0, 0), Vector.create(-1, 0, 0)));
}

unittest
{
	assert(16 == Vector.sizeof);
	Vector f;
	f = -Vector.create(1, 2, 3, 4);
	assert(-1 == f.x && -2 == f.y && -3 == f.z && -4 == f.w);
	assert(Vector.create(-1, -2, -3, -4) == f);
	f = Vector.create(1, 2, 3, 4);
	assert(Vector.create(2, 4, 6, 8) == f + f);
	assert(Vector.create(0, 0, 0, 0) == f - f);
	assert(Vector.create(3, 6, 9, 12) == f * 3);
	assert(Vector.create(0.5, 1, 1.5, 2) == f / 2);
	f += Vector.create(1, 2, 3, 4);
	assert(Vector.create(2, 4, 6, 8) == f);
	f -= Vector.create(1, 2, 3, 4);
	assert(Vector.create(1, 2, 3, 4) == f);
	f *= 3;
	assert(Vector.create(3, 6, 9, 12) == f);
	f /= 3;
	assert(Vector.create(1, 2, 3, 4) == f);
	bool nearly(float a, float b) { return fabs(a - b) < 0.000001 ? true : false; }
	assert(nearly(sqrt(4.0), scalar(Vector.create(1, 1, 1, 1))));
	assert(nearly(sqrt(4.0),
		distance(Vector.create(1, 1, 1, 1), Vector.create(2, 2, 2, 2))
		));
	assert(nearly(1.0, scalar(normalize(Vector.create(1, 2, 3, 4)))));
	assert(0 == dot(Vector.create(1, 0, 0, 0), Vector.create(0, 1, 0, 0)));
	assert(Vector.create(0, 0, 0, 0) == cross(
		Vector.create(1, 0, 0, 0),
		Vector.create(-1, 0, 0, 0),
		Vector.create(0, 0, 0, 0)
		));
}

float toDegree(float radian) { return radian / PI * 180.0; } ///
float toRadian(float degree) { return degree / 180.0 * PI; } ///

Vector clampVector(Vector a, float min, float max) ///
{
	Vector result;
	result.x = clamp(a.x, min, max);
	result.y = clamp(a.y, min, max);
	result.z = clamp(a.z, min, max);
	result.w = clamp(a.w, min, max);
	return result;
}

T clamp(T, U)(T x, U min, U max) ///
{
	if (x < min) return min;
	if (x > max) return max;
	return x;
}

bool nearly(float a, float b) /// ほぼ等しい
{
	return fabs(a - b) < 0.001; // || feqrel(a, b) > 10;
}

bool nearly(Vector a, Vector b) /// ほぼ等しい
{
	return nearly(a.x, b.x) && nearly(a.y, b.y) && nearly(a.z, b.z) && nearly(a.w, b.w);
}

unittest
{
	assert(nearly(123.4, 123.4));
	assert(10.0 == clamp(5.0, 10.0, 20.0));
	assert(20.0 == clamp(111.1, 10.0, 20.0));
	assert(nearly(12.3, clamp(12.3, 10.0, 20.0)));
}

float lerp(float left, float right, float interpolater) ///
{
	return left * (1.0 - interpolater) + right * interpolater;
}

Vector lerp(Vector left, Vector right, float interpolater) ///
{
	return left * (1.0 - interpolater) + right * interpolater;
}

Vector[] lerp(Vector[] left, Vector[] right, float interpolater) ///
in
{
	assert(left.length == right.length);
}
body
{
	Vector[] result = new Vector[left.length];
	for (int i = 0; i < result.length; i++) result[i] = lerp(left[i], right[i], interpolater);
	return result;
}

unittest
{
	float frand()
	out (result)
	{
		assert(-1 <= result && result <= 1);
	}
	body
	{
		return cast(float)rand() / cast(float)uint.max;
	}
	
	assert(isInnerPoint(Matrix.identity, 0.5, 0.5));
	
	for (int i = 0; i < 1000; i++)
	{
		float x = frand() * 2 - 1, y = frand() * 2 - 1;
		if (isInnerPoint(Matrix.identity, x, y))
		{
			assert(-0.5 <= x && x <= 0.5
				&& -0.5 <= y && y <= 0.5);
		}
		else
		{
			assert(x < -0.5 || 0.5 < x
				|| y < -0.5 || 0.5 < y);
		}
	}
	
	assert(isInnerPoint(Matrix.translation(0.5, 0.5, 0), 0, 0));
	assert(!isInnerPoint(Matrix.translation(0.5, 0.5, 0), -0.1, -0.1));
	assert(!isInnerPoint(Matrix.scaling(0.5, 0.5, 1), 0.5, 0.5));
}

/// 基本ビルボード (0.5, 0.5) (0.5, -0.5) (-0.5, -0.5) (-0.5, 0.5) 内部の点であるかどうか
bool isInnerPoint(Matrix m, float x, float y)
{
	alias Vector.create vector;
	Vector[] plist;
	plist ~= vector(0.5, 0.5);
	plist ~= vector(0.5, -0.5);
	plist ~= vector(-0.5, -0.5);
	plist ~= vector(-0.5, 0.5);
	foreach (ref Vector a; plist) a = mulW(a, m);
	Vector p = vector(x, y);
	
	plist ~= plist[0];
	for (int i = 0; i < plist.length - 1; i++)
	{
		if (cross(plist[i + 1] - plist[i], p - plist[i + 1]).z > 0) return false;
	}
	return true;
}

T min(T)(T a, T b) ///
{
	return a < b ? a : b;
}

T max(T)(T a, T b) ///
{
	return a > b ? a : b;
}

unittest
{
	assert(1 == min(1, 10));
	assert(2.0 == min(11.1, 2.0));
	assert(10 == max(1, 10));
	assert(11.1 == max(11.1, 2.0));
}

unittest
{
	Vector[] list;
	list ~= vector(0, 1, 0);
	list ~= vector(1, -1, 1);
	list ~= vector(-1, -1, 1);
	list ~= vector(0, -1, -1);
	Vector[] area;
	foreach (v; [0, 1, 2,  0, 3, 1,  0, 2, 3,  1, 2, 3]) area ~= list[v];
	assert(innerArea(area, vector(0, 0, 0)));
	assert(!innerArea(area, vector(0, 2, 0)));
}

bool innerArea(Vector[] areaTriangles, Vector point) /// 六方向にrayを飛ばして全て衝突しているなら
{
	Vector[] dir;
	dir ~= vector(1, 0, 0);
	dir ~= vector(0, 1, 0);
	dir ~= vector(0, 0, 1);
	bool[] bits = new bool[6];
	assert(false == bits[0]);
	for (int i = 0; i < areaTriangles.length; i += 3)
	{
		auto p0 = areaTriangles[i];
		auto p1 = areaTriangles[i + 1];
		auto p2 = areaTriangles[i + 2];
		float dist;
		if (!(bits[0] && bits[1])
			&& triangleIntersectRay2(p0, p1, p2, point, dir[0], dist))
		{
			if (0 <= dist) bits[0] = true;
			if (dist <= 0) bits[1] = true;
		}
		if (!(bits[2] && bits[3])
			&& triangleIntersectRay2(p0, p1, p2, point, dir[1], dist))
		{
			if (0 <= dist) bits[2] = true;
			if (dist <= 0) bits[3] = true;
		}
		if (!(bits[4] && bits[5])
			&& triangleIntersectRay2(p0, p1, p2, point, dir[2], dist))
		{
			if (0 <= dist) bits[4] = true;
			if (dist <= 0) bits[5] = true;
		}
	}
	return bits[0] && bits[1] && bits[2] && bits[3] && bits[4] && bits[5] ? true : false;
}

Vector linear(Vector previous, Vector next, float t) // 線形補間
in
{
	assert(0.0 <= t && t <= 1.0);
}
body
{
	return previous * (1 - t) + next * t;
}

/**
	A-B-C... span(最初から最後までの所要時間)を設定して、任意のtから
	[A, B) A-Bの位置[0.0, 1.0)
	[B, C) B-Cの位置[0.0, 1.0)
	を取り出せる
*/
class AnyRoute(T)
{
	protected T[] nodes;
	void opAssign(T[] nodes) { this.nodes = nodes; t = 0; }
	uint span, t;
	T current() { return nodes[cast(size_t)trunc(position)]; }
	T next() { return nodes[cast(size_t)trunc(position) + 1]; }
	float interpolater() { real y = 1.0; return modf(position, y); }
	bool valid() { return t < span && 2 <= nodes.length; }
	
	protected float position()
	in
	{
		assert(valid);
	}
	out (result)
	{
		assert(0.0 <= result && result < nodes.length - 1);
	}
	body
	{
		auto result = cast(float)t / span; // [0, 1)
		result *= nodes.length - 1;
		return result;
	}
}

private class AnyRouteTest
{
	unittest
	{
		auto r = new AnyRoute!(int)();
		r = [3, 1, 2, 0];
		r.span = 3;
		assert(r.current == 3 && r.next == 1 && r.interpolater == 0.0);
		assert(r.valid);
		r.t++;
		assert(r.current == 1 && r.next == 2 && r.interpolater == 0.0);
		assert(r.valid);
		r.t++;
		assert(r.current == 2 && r.next == 0 && r.interpolater == 0.0);
		assert(r.valid);
		r.t++;
		assert(!r.valid);
	}
	
	unittest
	{
		auto r = new AnyRoute!(string)();
		r = ["hoge", "foo", "end"];
		r.span = 20;
		r.t = 15;
		assert(r.current == "foo" && r.next == "end" && r.interpolater == 0.5);
	}
	
	unittest
	{
		struct Pair { int first, second; }
		auto r = new AnyRoute!(Pair)();
		r = [Pair(1, 1), Pair(1, 2), Pair(1, 3)];
		r.span = 10;
		r.t = 5;
		assert(r.current == Pair(1, 2) && r.next == Pair(1, 3) && r.interpolater == 0.0);
	}
}

struct MotionKeys
{
	uint motionIndex, keyFrame;
}

/// 隣り合うnodeのkeyFrameの差をrangeとして扱う
class MotionKeysRoute : AnyRoute!(MotionKeys)
{
	override bool valid()
	{
		return super.valid && 1.0 <= rangeFrom0(nodes.length - 1);
	}
	
	protected override float position()
	in
	{
		assert(valid);
	}
	out (result)
	{
		assert(0.0 <= result && result < nodes.length - 1);
	}
	body
	{
		auto position = cast(float)t / span; // [0, 1)
		auto totalRange = rangeFrom0(nodes.length - 1);
		position *= totalRange;
		
		size_t currentIndex;
		for (int i = 0; true; i++)
		{
			assert(i < nodes.length);
			if (position < rangeFrom0(i))
			{
				currentIndex = i - 1;
				break;
			}
		}
		
		float currentPosition = rangeFrom0(currentIndex);
		float nextPosition = rangeFrom0(currentIndex + 1);
		assert(currentPosition != nextPosition);
		
		float ip = (position - currentPosition) / (nextPosition - currentPosition);
		
		return cast(float)currentIndex + ip;
	}
	
	private float rangeFrom0(size_t nodesIndex)
	{
		float result = 0.0;
		for (int i = 0; i < nodesIndex; i++)
		{
			result += fabs(nodes[i + 1].keyFrame - nodes[i].keyFrame);
		}
		return result;
	}
}

class MotionKeysRouteTest
{
	unittest
	{
		auto r = new MotionKeysRoute();
		r = [MotionKeys(0, 0), MotionKeys(0, 5), MotionKeys(0, 6)];
		r.span = 6;
		r.t = 5;
		assert(r.current.keyFrame == 5);
		assert(r.next.keyFrame == 6);
		assert(r.interpolater == 0.0);
	}
}
