/*
 *	clipmath.c
 *
 *	CLiP - Common Library for P/ECE
 *	Copyright (C) 2001 Naoyuki Sawa
 *
 *	* Sat Dec  1 13:20:55 JST 2001 Naoyuki Sawa
 *	- 作成開始。
 *	* Tue Oct 22 06:00:00 JST 2002 Naoyuki Sawa
 *	- sqrt32追加。
 *	* Tue Oct 23 12:00:00 JST 2002 Naoyuki Sawa
 *	- point_in_polygonのバグ修正。
 *	* Thu Oct 24 12:00:00 JST 2002 Naoyuki Sawa
 *	- cross_point追加。
 *	* Fri Oct 25 12:00:00 JST 2002 Naoyuki Sawa
 *	- ortho_point追加。
 *	* Sat Oct 26 12:59:00 JST 2002 Naoyuki Sawa
 *	- sqrt32をsqrt_32に名称変更。
 *	- point_in_polygon_32,cross_point_32,ortho_point_32を追加。
 *	* Mon Dec 2 20:00:00 JST 2002 Naoyuki Sawa
 *	- leapyear(),dayofweek()を追加しました。
 *	* Tue Dec 3 20:00:00 JST 2002 Naoyuki Sawa
 *	- x1n()を追加しました。
 *	* Sat Feb 22 07:25:00 JST 2003 Naoyuki Sawa
 *	- sin_16(),cos_16(),atan2_16()を追加しました。
 */
#include "clip.h"

/*****************************************************************************
 *	ベクトル
 *****************************************************************************/

/* 略記法の定義 */
#define Ax	a->x
#define Ay	a->y
#define Az	a->z
#define Bx	b->x
#define By	b->y
#define Bz	b->z

const VECTOR VECTOR_0 = { 0, 0, 0 };
const VECTOR VECTOR_X = { 1, 0, 0 };
const VECTOR VECTOR_Y = { 0, 1, 0 };
const VECTOR VECTOR_Z = { 0, 0, 1 };

VECTOR*
vector_init(VECTOR* o, float x, float y, float z)
{
	o->x = x;
	o->y = y;
	o->z = z;
	return o;
}

VECTOR*
vector_neg(VECTOR* o, const VECTOR* a)
{
	return vector_init(o,
		-Ax,
		-Ay,
		-Az);
}

VECTOR*
vector_add(VECTOR* o, const VECTOR* a, const VECTOR* b)
{
	return vector_init(o,
		Ax + Bx,
		Ay + By,
		Az + Bz);
}

VECTOR*
vector_sub(VECTOR* o, const VECTOR* a, const VECTOR* b)
{
	return vector_init(o,
		Ax - Bx,
		Ay - By,
		Az - Bz);
}

VECTOR*
vector_mul(VECTOR* o, const VECTOR* a, const VECTOR* b)
{
	return vector_init(o,
		Ax * Bx,
		Ay * By,
		Az * Bz);
}

VECTOR*
vector_div(VECTOR* o, const VECTOR* a, const VECTOR* b)
{
	return vector_init(o,
		Ax / Bx,
		Ay / By,
		Az / Bz);
}

VECTOR*
vector_mul1(VECTOR* o, const VECTOR* a, float s)
{
	return vector_init(o,
		Ax * s,
		Ay * s,
		Az * s);
}

VECTOR*
vector_div1(VECTOR* o, const VECTOR* a, float s)
{
	return vector_init(o,
		Ax / s,
		Ay / s,
		Az / s);
}

float
vector_dot(const VECTOR* a, const VECTOR* b)
{
	return Ax * Bx +
	       Ay * By +
	       Az * Bz;
}
VECTOR*
vector_cross(VECTOR* o, const VECTOR* a, const VECTOR* b)
{
	return vector_init(o,
		Ay * Bz - Az * By,
		Az * Bx - Ax * Bz,
		Ax * By - Ay * Bx);
}

VECTOR*
vector_normal(VECTOR* o, const VECTOR* a)
{
	return vector_div1(o, a, vector_mag(a));
}

float
vector_mag(const VECTOR* a)
{
	return sqrt(vector_mag2(a));
}

float
vector_mag2(const VECTOR* a)
{
	return vector_dot(a, a);
}

VECTOR*
vector_reflect(VECTOR* o, const VECTOR* N, const VECTOR* E)
{
	float s;
	s = 2 * vector_dot(E, N);
	return vector_init(o,
		s * N->x - E->x,
		s * N->y - E->y,
		s * N->z - E->z);
}

/*****************************************************************************
 *	行列
 *****************************************************************************/

/* 略記法の定義 */
#define A00	a->a00
#define A01	a->a01
#define A02	a->a02
#define A03	a->a03
#define A10	a->a10
#define A11	a->a11
#define A12	a->a12
#define A13	a->a13
#define A20	a->a20
#define A21	a->a21
#define A22	a->a22
#define A23	a->a23
#define B00	b->a00
#define B01	b->a01
#define B02	b->a02
#define B03	b->a03
#define B10	b->a10
#define B11	b->a11
#define B12	b->a12
#define B13	b->a13
#define B20	b->a20
#define B21	b->a21
#define B22	b->a22
#define B23	b->a23

const MATRIX MATRIX_I = {
	1, 0, 0, 0,
	0, 1, 0, 0,
	0, 0, 1, 0,
};

MATRIX*
matrix_init(MATRIX* o,
	float a00, float a01, float a02, float a03,
	float a10, float a11, float a12, float a13,
	float a20, float a21, float a22, float a23)
{
	o->a00 = a00; o->a01 = a01; o->a02 = a02; o->a03 = a03;
	o->a10 = a10; o->a11 = a11; o->a12 = a12; o->a13 = a13;
	o->a20 = a20; o->a21 = a21; o->a22 = a22; o->a23 = a23;
	return o;
}

MATRIX*
matrix_xpose(MATRIX* o, const MATRIX* a)
{
	return matrix_init(o,
		A00, A10, A20, 0,
		A01, A11, A21, 0,
		A02, A12, A22, 0);
}

MATRIX*
matrix_inv(MATRIX* o, const MATRIX* a)
{
	int i, j, k;
	float s, t;

	/* 4x4行列が横に2つ並んだ4x8行列を仮定し、
	 * 左半分を元行列、右半分を単位行列とします。
	 */
	float m[4][8] = {
		{ A00, A01, A02, A03, 1, 0, 0, 0 },
		{ A10, A11, A12, A13, 0, 1, 0, 0 },
		{ A20, A21, A22, A23, 0, 0, 1, 0 },
		{   0,   0,   0,   1, 0, 0, 0, 1 },
	};

	/* 行iを確定させる処理... */
	for(i = 0; i < 4; i++) {
		/* m[j=i〜3][i]の絶対値が一番大きい行jを探します。
		 * 理論上は、0以外ならどれでも構わないのですが、
		 * 誤差を小さくするために、一番大きい値を使います。
		 */
		j = i;
		s = fabs(m[j][i]);
		for(k = j + 1; k < 4; k++) {
			t = fabs(m[k][i]);
			if(t > s) {
				j = k;
				s = t;
			}
		}
		/* t=0なら不可逆ですが、強行します。 */

		/* 行iとjを交換します。 */
		if(i != j) {
			for(k = 0; k < 8; k++) {
				s = m[i][k];
				    m[i][k] = m[j][k];
				              m[j][k] = s;
			}
		}

		/* m[i][i]が1になるよう、行iをm[i][i]で割ります。 */
		s = 1.0F / m[i][i];
		for(j = 0; j < 8; j++) m[i][j] *= s;

		/* m[j=i以外][i]が0になるよう、行iをm[j][i]倍して引きます。 */
		for(j = 0; j < 4; j++) {
			if(j == i) continue;
			s = m[j][i];
			for(k = 0; k < 8; k++) m[j][k] -= m[i][k] * s;
		}
		/* これで行iが確定しました。 */
	}

	/* 右半分が元行列の逆行列になっています。 */
	return matrix_init(o,
		m[0][4], m[0][5], m[0][6], m[0][7],
		m[1][4], m[1][5], m[1][6], m[1][7],
		m[2][4], m[2][5], m[2][6], m[2][7]);
}

/* ※未確認 */
MATRIX*
matrix_invno(MATRIX* o, const MATRIX* a)
{
	/* 正規直交行列の逆変換は、次の通りです。
	 *	| R P |-1 = | RT RT*(-P) |
	 * ただし、
	 *	R  = 回転成分を表す3x3行列
	 *	P  = 平行移動成分を表す3次ベクトル
	 *	RT = Rの転置行列
	 * とします。
	 */
	return matrix_init(o,
		A00, A10, A20, -(A00 * A03 + A10 * A13 + A20 * A23),
		A01, A11, A21, -(A01 * A03 + A11 * A13 + A21 * A23),
		A02, A12, A22, -(A02 * A03 + A12 * A13 + A22 * A23));
}

VECTOR*
vector_xform(VECTOR* o, const MATRIX* a, const VECTOR* b)
{
	return vector_init(o,
		A00 * Bx + A01 * By + A02 * Bz + A03,
		A10 * Bx + A11 * By + A12 * Bz + A13,
		A20 * Bx + A21 * By + A22 * Bz + A23);
}

MATRIX*
matrix_xform(MATRIX* o, const MATRIX* a, const MATRIX* b)
{
	return matrix_init(o,
		A00 * B00 + A01 * B10 + A02 * B20,
		A00 * B01 + A01 * B11 + A02 * B21,
		A00 * B02 + A01 * B12 + A02 * B22,
		A00 * B03 + A01 * B13 + A02 * B23 + A03,
		A10 * B00 + A11 * B10 + A12 * B20,
		A10 * B01 + A11 * B11 + A12 * B21,
		A10 * B02 + A11 * B12 + A12 * B22,
		A10 * B03 + A11 * B13 + A12 * B23 + A13,
		A20 * B00 + A21 * B10 + A22 * B20,
		A20 * B01 + A21 * B11 + A22 * B21,
		A20 * B02 + A21 * B12 + A22 * B22,
		A20 * B03 + A21 * B13 + A22 * B23 + A23);
}

MATRIX*
matrix_xform12(MATRIX* o, const MATRIX* a,
	float b00, float b01, float b02, float b03,
	float b10, float b11, float b12, float b13,
	float b20, float b21, float b22, float b23)
{
	return matrix_init(o,
		A00 * b00 + A01 * b10 + A02 * b20,
		A00 * b01 + A01 * b11 + A02 * b21,
		A00 * b02 + A01 * b12 + A02 * b22,
		A00 * b03 + A01 * b13 + A02 * b23 + A03,
		A10 * b00 + A11 * b10 + A12 * b20,
		A10 * b01 + A11 * b11 + A12 * b21,
		A10 * b02 + A11 * b12 + A12 * b22,
		A10 * b03 + A11 * b13 + A12 * b23 + A13,
		A20 * b00 + A21 * b10 + A22 * b20,
		A20 * b01 + A21 * b11 + A22 * b21,
		A20 * b02 + A21 * b12 + A22 * b22,
		A20 * b03 + A21 * b13 + A22 * b23 + A23);
}

MATRIX*
matrix_xlate(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	return matrix_xform12(o, a,
		1, 0, 0, x,
		0, 1, 0, y,
		0, 0, 1, z);
}

MATRIX*
matrix_scale(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	return matrix_xform12(o, a,
		x, 0, 0, 0,
		0, y, 0, 0,
		0, 0, z, 0);
}

MATRIX*
matrix_scale1(MATRIX* o, const MATRIX* a, float s)
{
	return matrix_xform12(o, a,
		s, 0, 0, 0,
		0, s, 0, 0,
		0, 0, s, 0);
}

MATRIX*
matrix_rot(MATRIX* o, const MATRIX* a, float x, float y, float z, float d)
{
	VECTOR v;
	float s, c;
	float C;
	/* 回転軸ベクトルを正規化します。 */
	vector_init(&v, x, y, z);
	vector_normal(&v, &v);
	x = v.x, y = v.y, z = v.z;
	s = sin(d), c = cos(d);
	C = 1 - c;
	return matrix_xform12(o, a,
		x * x * C +     c, x * y * C - z * s, x * z * C + y * s, 0,
		y * x * C + z * s, y * y * C +     c, y * z * C - x * s, 0,
		z * x * C - y * s, z * y * C + x * s, z * z * C +     c, 0);
}

MATRIX*
matrix_rotx(MATRIX* o, const MATRIX* a, float d)
{
	/* X軸回転行列は、次の通りです。
	 *	|  1, 0, 0, 0 |
	 *	|  0, c,-s, 0 |
	 *	|  0, s, c, 0 |
	 * ただし、
	 *	c = cos(d)
	 *	s = sin(d)
	 *	d = 回転角度
	 * とします。
	 */
	float c, s;
	c = cos(d);
	s = sin(d);
	return matrix_xform12(o, a,
		 1, 0, 0, 0,
		 0, c,-s, 0,
		 0, s, c, 0);
}

MATRIX*
matrix_roty(MATRIX* o, const MATRIX* a, float d)
{
	/* Y軸回転行列は、次の通りです。
	 *	|  c, 0, s, 0 |
	 *	|  0, 1, 0, 0 |
	 *	| -s, 0, c, 0 |
	 * ただし、
	 *	c = cos(d)
	 *	s = sin(d)
	 *	d = 回転角度
	 * とします。
	 */
	float c, s;
	c = cos(d);
	s = sin(d);
	return matrix_xform12(o, a,
		 c, 0, s, 0,
		 0, 1, 0, 0,
		-s, 0, c, 0);
}

MATRIX*
matrix_rotz(MATRIX* o, const MATRIX* a, float d)
{
	/* Z軸回転行列は、次の通りです。
	 *	|  c,-s, 0, 0 |
	 *	|  s, c, 0, 0 |
	 *	|  0, 0, 1, 0 |
	 * ただし、
	 *	c = cos(d)
	 *	s = sin(d)
	 *	d = 回転角度
	 * とします。
	 */
	float c, s;
	c = cos(d);
	s = sin(d);
	return matrix_xform12(o, a,
		 c,-s, 0, 0,
		 s, c, 0, 0,
		 0, 0, 1, 0);
}

MATRIX*
matrix_rotxyx(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		  cB   ,          sA*sB     ,           cA*sB     , 0,
		-sB*-sC, (cA*cC + sA*cB*-sC), (-sA*cC + cA*cB*-sC), 0,
		-sB*cC , (cA*sC + sA*cB* cC), (-sA*sC + cA*cB* cC), 0);
}

MATRIX*
matrix_rotxzx(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		cB   ,  cA*-sB             ,  -sA*-sB             , 0,
		sB*cC, (cA* cB*cC + sA*-sC), (-sA* cB*cC + cA*-sC), 0,
		sB*sC, (cA* cB*sC + sA* cC), (-sA* cB*sC + cA* cC), 0);
}

MATRIX*
matrix_rotyxy(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		(cA* cC + -sA* cB*sC), sB*sC, (sA* cC + cA* cB*sC), 0,
		          -sA*-sB    , cB   ,           cA*-sB    , 0,
		(cA*-sC + -sA* cB*cC), sB*cC, (sA*-sC + cA* cB*cC), 0);
}

MATRIX*
matrix_rotyzy(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		(cA*cB* cC + -sA*sC), -sB *cC, (sA*cB* cC + cA*sC), 0,
		 cA*sB              ,  cB    ,  sA*sB             , 0,
		(cA*cB*-sC + -sA*cC), -sB*-sC, (sA*cB*-sC + cA*cC), 0);
}

MATRIX*
matrix_rotzxz(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		(cA*cC + sA*cB*-sC), (-sA*cC + cA*cB*-sC), -sB*-sC, 0,
		(cA*sC + sA*cB* cC), (-sA*sC + cA*cB* cC), -sB* cC, 0,
		         sA*sB     ,           cA*sB     ,  cB    , 0);
}

MATRIX*
matrix_rotzyz(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		(cA* cB*cC + sA*-sC), (-sA* cB*cC + cA*-sC), sB*cC, 0,
		(cA* cB*sC + sA* cC), (-sA* cB*sC + cA* cC), sB*sC, 0,
		 cA*-sB             ,  -sA*-sB             , cB   , 0);
}

MATRIX*
matrix_rotxyz(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		 cB*cC, (sA*sB*cC + cA*-sC), (cA*sB*cC + -sA*-sC), 0,
		 cB*sC, (sA*sB*sC + cA* cC), (cA*sB*sC + -sA* cC), 0,
		-sB   ,  sA*cB             ,  cA*cB              , 0);
}

MATRIX*
matrix_rotxzy(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		cB* cC, (cA*-sB* cC + sA*sC), (-sA*-sB* cC + cA*sC), 0,
		sB    ,  cA* cB             ,  -sA* cB             , 0,
		cB*-sC, (cA*-sB*-sC + sA*cC), (-sA*-sB*-sC + cA*cC), 0);
}

MATRIX*
matrix_rotyxz(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		(cA*cC + -sA*-sB*-sC), cB*-sC, (sA*cC + cA*-sB*-sC), 0,
		(cA*sC + -sA*-sB* cC), cB* cC, (sA*sC + cA*-sB* cC), 0,
		         -sA* cB     , sB    ,          cA* cB     , 0);
}

MATRIX*
matrix_rotyzx(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		 cA*cB              , -sB   ,  sA*cB             , 0,
		(cA*sB*cC + -sA*-sC),  cB*cC, (sA*sB*cC + cA*-sC), 0,
		(cA*sB*sC + -sA* cC),  cB*sC, (sA*sB*sC + cA* cC), 0);
}

MATRIX*
matrix_rotzxy(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		(cA* cC + sA*sB*sC), (-sA* cC + cA*sB*sC),  cB*sC, 0,
		          sA*cB    ,            cA*cB    , -sB   , 0,
		(cA*-sC + sA*sB*cC), (-sA*-sC + cA*sB*cC),  cB*cC, 0);
}

MATRIX*
matrix_rotzyx(MATRIX* o, const MATRIX* a, float x, float y, float z)
{
	float cA = cos(x), sA = sin(x);
	float cB = cos(y), sB = sin(y);
	float cC = cos(z), sC = sin(z);
	return matrix_xform12(o, a,
		          cA* cB     ,          -sA* cB     , sB    , 0,
		(sA*cC +  cA*-sB*-sC), (cA*cC + -sA*-sB*-sC), cB*-sC, 0,
		(sA*sC +  cA*-sB* cC), (cA*sC + -sA*-sB* cC), cB* cC, 0);
}

/* ※未確認 */
float
matrix_rotget(VECTOR* o, const MATRIX* a)
{
	float s, t;
	s = acos((A00 + A11 + A22 - 1) / 2);
	if(s != 0) {
		t = 2 * sin(s);
		vector_init(o,
			(A21 - A12) / t,
			(A02 - A20) / t,
			(A10 - A01) / t);
	} else {
		*o = VECTOR_Z;
	}
	return s;
}

MATRIX*
matrix_orient(MATRIX* o, const MATRIX* a,
	float dx, float dy, float dz,
	float ux, float uy, float uz)
{
	/* 姿勢行列は、次の通りです。
	 *	| rx, ux, dx, 0 |
	 *	| ry, uy, dy, 0 |
	 *	| rz, uz, dz, 0 |
	 * ただし、
	 *	| rx, ry, rz |T = X軸姿勢ベクトル
	 *	| ux, uy, uz |T = Y軸姿勢ベクトル
	 *	| dx, dy, dz |T = Z軸姿勢ベクトル
	 * とします。
	 */
	VECTOR r, u, d;

	/* Z軸ベクトルを正規化します。 */
	vector_init(&d, dx, dy, dz);
	vector_normal(&d, &d);

	/* X軸ベクトルを求めて、正規化します。 
	 * X軸ベクトルは、Y軸->Z軸を左回りとする進行方向、
	 * すなわち外積です。
	 */
	vector_init(&u, ux, uy, uz);
	vector_cross(&r, &u, &d);
	vector_normal(&r, &r);

	/* Y軸ベクトルを求めます。
	 * Y軸ベクトルは、Z軸->X軸を左回りとする進行方向、
	 * すなわち外積です。
	 * Z軸およびX軸ベクトルが既に正規化されているので、
	 * Y軸ベクトルを明示的に正規化する必要はありません。
	 */
	vector_cross(&u, &d, &r);

	return matrix_xform12(o, a,
		r.x, u.x, d.x, 0,
		r.y, u.y, d.y, 0,
		r.z, u.z, d.z, 0);
}

MATRIX*
matrix_lookat(MATRIX* o, const MATRIX* a,
	float ex, float ey, float ez,
	float cx, float cy, float cz,
	float bank)
{
	/* カメラ自身の動きを表す変換行列は、次の通りです。
	 * 1. バンクする。
	 * 2. 視線の角度を、視点座標から注視点座標への向きとする。
	 * 3. 視点座標へ移動する。
	 * 視点変換行列は、カメラを固定して世界を動かすので、
	 * 1〜3の逆変換となります。
	 * ※行列を転置型にしたので、オブジェクトに近い変換が後ろになります。
	 *   よって、1〜3の処理は逆順で行います。(matrix_lookat()に限らず)
	 */
	MATRIX m;

	/* 視点変換行列を作成します。
	 * 注意! o==aの可能性があるので、oをワークとして使ってはいけません。
	 */
	matrix_xlate (&m, &MATRIX_I, ex, ey, ez);
	matrix_orient(&m, &m, cx - ex, cy - ey, cz - ez, 0, 1, 0);
	matrix_rotz  (&m, &m, bank);
	matrix_invno (&m, &m);

	/* 変換される行列に掛け合わせます。 */
	return matrix_xform(o, a, &m);
}

MATRIX*
matrix_frustum(MATRIX* o, const MATRIX* a, float width, float height, float front)
{
	/* 透視投影行列は、次の通りです。
	 *	| 2*front/width,              0,                 0,                        0 |
	 *	|             0, 2*front/height,                 0,                        0 |
	 *	|             0,              0, back/(back-front), -front*back/(back-front) |
	 *	|             0,              0,                 1,                        0 |
	 * しかし、ここではZ座標を正規化しないので、
	 *	| 2*front/width,              0, 0, 0 |
	 *	|             0, 2*front/height, 0, 0 |
	 *	|             0,              0, 1, 0 |
	 * とします。ただし、
	 *	width  = 投影面の幅
	 *	height = 投影面の高さ
	 *	front  = フロントクリッピング距離
	 *	back   = バックフリッピング距離
	 * とします。
	 */
	return matrix_xform12(o, a,
		2*front/width,              0, 0, 0,
		            0, 2*front/height, 0, 0,
		            0,              0, 1, 0);
}

MATRIX*
matrix_perspect(MATRIX* o, const MATRIX* a, float fovy, float aspect, float front)
{
	float width, height;
	height = front * tan(fovy / 2) * 2; /* 投影面の幅   */
	width = height * aspect;            /* 投影面の高さ */
	return matrix_frustum(o, a, width, height, front);
}

MATRIX*
matrix_viewport(MATRIX* o, const MATRIX* a, int left, int top, int width, int height)
{
	/* 正規化座標(x,y)
	 *	x = 左:-1〜1:右
	 *	y = 下:-1〜1:上
	 * から、ビューポート座標(X,Y)
	 *	X = 左:left      〜left+width:右
	 *	Y = 下:top+height〜       top:上
	 * への変換について、説明します。
	 *
	 * 1) モデル変換->視点変換->透視変換->x=x/z,y=y/z->ビューポート変換
	 *    という手順の場合...
	 * 変換式は、次の通りです。
	 *	X = left + (x + 1) *  width  / 2 = ( width  / 2) * x + (left + width  / 2)
	 *	Y = top  + (y - 1) * -height / 2 = (-height / 2) * y + (top  + height / 2)
	 * よって、ビューポート変換行列は、次の通りです。
	 *	| width/2,         0, 0, left+width/2 |
	 *	|       0, -height/2, 0, top+height/2 |
	 *	|       0,         0, 1,            0 |
	 *
	 * 2) モデル変換->視点変換->透視変換->ビューポート変換->x=x/z,y=y-z
	 *    という手順の場合...
	 * 定数部分にも1/zが影響してしまうことをあらかじめ考慮して、
	 *	X = ( width  / 2) * x + (left + width  / 2) * z
	 *	Y = (-height / 2) * y + (top  + height / 2) * z
	 * よって、ビューポート変換行列は、次の通りです。
	 *	| width/2,         0, left+width/2, 0 |
	 *	|       0, -height/2, top+height/2, 0 |
	 *	|       0,         0,            1, 0 |
	 */
	return matrix_xform12(o, a,
		width/2,         0, 0, left+width/2,
		      0, -height/2, 0, top+height/2,
		      0,         0, 1,            0);
}

MATRIX*
matrix_viewportz(MATRIX* o, const MATRIX* a, int left, int top, int width, int height)
{
	/* matrix_viewport()のコメントを参照してください。 */
	return matrix_xform12(o, a,
		width/2,         0, left+width/2, 0,
		      0, -height/2, top+height/2, 0,
		      0,         0,            1, 0);
}

MATRIX*
matrix_shadow(MATRIX* o, const MATRIX* a,
	float Lx, float Ly, float Lz,
	float Gx, float Gy, float Gz,
	float Nx, float Ny, float Nz)
{
	/*	P = 投影する座標
	 *	L = 光源ベクトル
	 *	G = 地面の一点
	 *	N = 地面の法線
	 * に対して、投影された座標Sを求める方法は、次の通りです。
	 * あらかじめ、
	 *	LN = L・N
	 *	GN = G・N
	 *	A  = Lx / LN
	 *	B  = Ly / LN
	 *	C  = Lz / LN
	 * を求めておき、
	 *	Sx = Px*(1 - Nx*A) + Py*(  - Ny*A) + Pz(  - Nz*A) + GN*A
	 *	Sy = Px*(  - Nx*B) + Py*(1 - Ny*B) + Pz(  - Nz*B) + GN*B
	 *	Sz = Px*(  - Nx*C) + Py*(  - Ny*C) + Pz(1 - Nz*C) + GN*C
	 * よって、影生成行列は
	 *	| 1-Nx*A,  -Ny*A,  -Nz*A, GN*A |
	 *	|  -Nx*B, 1-Ny*B,  -Nz*B, GN*B |
	 *	|  -Nx*C,  -Ny*C, 1-Nz*C, GN*C |
	 * となります。
	 */
	VECTOR L, G, N;
	float LN, GN;
	float A, B, C;
	vector_init(&L, Lx, Ly, Lz);
	vector_init(&G, Gx, Gy, Gz);
	vector_init(&N, Nx, Ny, Nz);
	LN = vector_dot(&L, &N);
	GN = vector_dot(&G, &N);
	A = L.x / LN;
	B = L.y / LN;
	C = L.z / LN;
	return matrix_xform12(o, a,
		1-Nx*A,  -Ny*A,  -Nz*A, GN*A,
		 -Nx*B, 1-Ny*B,  -Nz*B, GN*B,
		 -Nx*C,  -Ny*C, 1-Nz*C, GN*C);
}

/*****************************************************************************
 *	浮動小数演算ユーティリティ
 *****************************************************************************/

float
point_to_line_s(const VECTOR* p, const VECTOR* a, const VECTOR* da)
{
	/* 点 p から直線 a + daｓ へ垂線を下ろした位置を q とする。
	 * p から q へのベクトルと、da は直交するので、
	 *	(q - p)・da = 0		式1
	 * q は 直線上にあるので、
	 *	q = a + daｓ		式2
	 * 式1,2より
	 *	(a + daｓ - p)・da = 0
	 *	ｓ = da・(p - a) / da・da
	 * 注意: 分母分子に現れる da は、単純なかけ算ではなく内積なので、
	 *       分母分子を da で割って (p - a) / da にしてはいけない。
	 *	 これでは、ベクトル÷ベクトルになってしまう。
	 *	 分母分子に共通の内積を相殺することが正しいのかどうかわからないが、
	 *	 少なくとも僕はベクトル÷ベクトルの計算方法を知らない。
	 */
	VECTOR ap;
	vector_sub(&ap, p, a);
	return vector_dot(da, &ap) / vector_mag2(da);
}

float
bezier1(float t, float Q0, float Q1, float Q2)
{
	/* ベジェ曲線の定義は、次の通りです。
	 *	P(t) = |t^2 t 1|| 1 -2  1||Q0|
	 *			|-2  2  0||Q1|
	 *			| 1  0  0||Q2|
	 * ただし、
	 *	t: パラメトリック変数(0〜1)
	 *	Q0: コントロールポイント#0の座標
	 *	Ql:                     #1
	 *	Q2:                     #2
	 * とします。
	 */
	float t2 = t * t;
	return (t2 *  1 + t * -2 + 1 * 1) * Q0 +
	       (t2 * -2 + t *  2 + 1 * 0) * Q1 +
	       (t2 *  1 + t *  0 + 1 * 0) * Q2;
}

float
hermite1(float t, float Q0, float Q1, float Q0d, float Q1d)
{
	/* エルミート曲線の定義は、次の通りです。
	 *	P(t) = |t^3 t^2 t 1|| 2 -2  1  1||Q0 |
	 *			    |-3  3 -2 -1||Q1 |
	 *			    | 0  0  1  0||Q0'|
	 *			    | 1  0  0  0||Q1'|
	 * ただし、
	 *	t: パラメトリック変数(0〜1)
	 *	Q0: コントロールポイント#0の座標
	 *	Ql:                     #1
	 *	Q0': コントロールポイント#0における接点ベクトル
	 *	Ql':                     #1
	 * とします。
	 */
	float t2 = t  * t;
	float t3 = t2 * t;
	return (t3 *  2 + t2 * -3 + t * 0 + 1 * 1) * Q0  +
	       (t3 * -2 + t2 *  3 + t * 0 + 1 * 0) * Q1  +
	       (t3 *  1 + t2 * -2 + t * 1 + 1 * 0) * Q0d +
	       (t3 *  1 + t2 * -1 + t * 0 + 1 * 0) * Q1d;
}

int
point_in_polygon(float px, float py, float points[/*n_points*/][2], int n_points)
{
	/* * 図形が凸型ならば、内積を使う方法もありますが、
	 *   今回は、図形が凸型の場合にも対応できる方法で実装します。
	 * * この関数はfloat用となっていますが、下の説明にも書いたように、
	 *   オーバーフロー対策をとってあるので、
	 *   そのまま仮引数とauto変数をfloat->intに変更すれば、
	 *   すぐにint用の関数を作ることができます。
	 *   現在のところ、まだint用関数が必要ないため、作っていません。
	 *
	 * [アルゴリズムＡ]--------------------------------------------------
	 *
	 * ある点(px,py)が、図形の内側にあるかどうかを調べる方法:
	 *
	 * (px,py)を始点とする半直線と、図形の各辺の交差を調べ、
	 * 交差する辺の数が奇数個ならば、(px,py)は図形の内側です。
	 *
	 * 半直線は、次の式で表されます。
	 *	x = px + At	| (A, B): 半直線の傾きを表すベクトル
	 *	y = py + Bt	| 0 <= t
	 * 図形のある辺は、次の式で表されます。
	 *	x = x0 + Cu	| C = x1 - x0, D = y1 - y0
	 *	y = y0 + Du	| 0 <= u < 1
	 *			| ※ 0<=u<=1としない理由は、
	 *			|    半直線がちょうど頂点を通る場合、
	 *			|    0<=u<=1では、隣り合う二つの辺の両方で
	 *			|    交差と判定されてしまうからです。
	 *			|    その場合、図形の内側からある頂点上を
	 *			|    通って外へ抜ける半直線だった場合に、
	 *			|    図形の内側であるにもかかわらず、
	 *			|    交差点は2となり、外側と判断されます。
	 *			|    この間違いを防ぐために、
	 *			|    0<=u<=1ではなく0<=u<1としてあります。
	 *			| ※Wed Oct 23 12:00:00 JST 2002 Naoyuki Sawa
	 *			| 　↑この部分のアルゴリズムに問題がありました。
	 *			| 　　アルゴリズムＢの説明を参照してください。
	 * 交差点を(x,y)とすると、
	 *	x = px + At = x0 + Cu
	 *	y = py + Bt = y0 + Du
	 * 連立方程式を解いて、t,uを求めます。
	 *	At - Cu = x0 - px
	 *	Bt - Du = y0 - py
	 *	t = (D(x0 - px) - C(y0 - py)) / (AD - BC)
	 *	u = (B(x0 - px) - A(y0 - py)) / (AD - BC)
	 *	t = (DE - CF) / (AD - BC)	| E = x0 - px
	 *	u = (BE - AF) / (AD - BC)	| F = y0 - py
	 * さて、半直線の傾きは任意に選ぶことができるので、
	 * 簡単のために水平線とします。
	 *	(A, B) = (1, 0)
	 *	t = (DE - CF) / D
	 *	u = ( 0 -  F) / D
	 * t,uの条件より、
	 *	0 <= (DE - CF) / D
	 *	0 <= ( 0 -  F) / D < 1
	 * Dが0に近いと、除算結果が非常に大きくなり、オーバーフロー発生の
	 * 危険があるため、両辺にDをかけて除算をなくします。
	 *	D >= 0の場合	| D < 0の場合
	 *	----------------+-------------
	 *	0 <= DE - CF	| 0 >= DE - CF	... 式1
	 *	0 <= -F < D	| 0 >= -F > D	... 式2
	 * 式1,2が共に成立すれば、半直線と辺は交差します。
	 * この判定を図形の辺の数だけ行い、交差する辺が奇数ならば
	 * 指定された点は図形の内側となります。
	 *
	 * [アルゴリズムＢ]--------------------------------------------------
	 *
	 * アルゴリズムＡには問題がありました。
	 * 多角形の頂点のうち上や下に尖った部分の頂点と同じラインの点が、
	 * 多角形外であっても誤って多角形内と判定されてしまいます。
	 *
	 * 次のような多角形を例にとって説明しましょう。
	 *
	 *　　　　　　　Ａ　　　　　
	 *　　　辺ＤＡ→／＼←辺ＡＢ
	 *　　　　　Ｄ／　／Ｂ　　　
	 *　　辺ＣＤ→＼／←辺ＢＣ　
	 *　　　　　　　Ｃ　　　　　
	 *
	 * アルゴリズムＡでは、各辺の終点側は交差判定に含めていませんでした。
	 * 即ち、多角形の一つの頂点は、水平線と一度しか交差しません。
	 * 終点側を交差判定に含めない理由は、アルゴリズムＡの説明のところにも書きました。
	 * 例えば、多角形内部・頂点Ｂの少し左にある点が多角形内にあるかどうか判定する場合、
	 * もしも頂点Ｂのところで、辺ＡＢの終点と辺ＢＣの始点の二回交差判定されてしまうと、
	 * 交差回数が偶数になって、多角形内部なのに外部と判断されてしまいます。
	 * このような問題を避けるために、各辺の終点側は交差判定に含めなかったのです。
	 *
	 * ところが、頂点ＡやＣについては、終点側も交差判定に含めなければいけません！
	 * 多角形外部・頂点Ａの少し左の点が多角形内にあるかどうか判定する場合、
	 * 頂点Ａのところで一回しか交差判定されないと、交差回数が奇数になって、
	 * 多角形外部であるにも関らず多角形内部と判断されてしまいます。
	 *
	 * 始点・終点とも交差判定に含めるかどうかの判断基準は、尖った頂点かどうか、
	 * すなわち、頂点の両側の辺が上向きから下向き、あるいはその逆に変っているか、です。
	 * 頂点Ａのところでは、辺ＤＡ（上向き）から辺ＡＢ（下向き）に変っているので、
	 * 始点・終点とも交差判定に含めます。
	 * 頂点Ｂのところでは、辺ＡＢ（下向き）・辺ＢＣ（下向き）で方向が変っていないので、
	 * 片方しか交差判定に含めません。
	 *
	 * アルゴリズムＡでは、各辺の終点側を交差判定から外していました。
	 * しかし、一つ前の辺の向きと今回の辺の向きの違いが判断基準になるのですから、
	 * 交差判定に含めるor外すのは、「今回の辺の始点」としたほうが実装が簡単です。
	 * よってアルゴリズムＢでは、前述の条件により、辺の始点を交差判定に含めるかどうかを
	 * 切り替えることにしました。
	 *
	 * 水平な辺は単に無視してしまって構いません。例えば次のような図形は：
	 *
	 *　　　　　　Ａ　　　　　　　
	 *　　　　　／＼　Ｂ　　　　　
	 *　　　　／　　＼＿＿Ｃ　　　
	 *　　　／　　＿　　／　　　　
	 *　　Ｉ＼＿／FE＼／　　　　　
	 *　　　Ｈ　Ｇ　Ｄ　　　　　　
	 *
	 * 水平な辺をを全て無視して、次のような図形と見なします。
	 *
	 *　　　　　　Ａ　　　　　　　
	 *　　　　　／＼　Ｂ　　　　　
	 *　　　　／　　＼　　Ｃ　　　
	 *　　　／　　　　　／　　　　
	 *　　Ｉ＼　／FE＼／　　　　　
	 *　　　Ｈ　Ｇ　Ｄ　　　　　　
	 *
	 * 辺ＣＤの始点（頂点Ｃ）を交差判定に含めるかどうかを考えてみると、
	 * 一つ前の辺ＡＢ（ＢＣではない！）が下向きで、辺ＣＤも下向きだから、
	 * 辺ＣＤの始点（頂点Ｃ）は交差判定に含めません。
	 *
	 * 辺ＨＩの始点（頂点Ｈ）を交差判定に含めるかどうかを考えてみると、
	 * 一つ前の辺ＦＧ（ＧＨではない！）が下向きで、辺ＨＩは上向きなので、
	 * 辺ＨＩの始点（頂点Ｈ）は交差判定に含めます。
	 *
	 * このように見ていくと、各辺の交差判定に始点側を含めるかどうかの一覧は、
	 * 次のようになります。
	 *
	 *	辺ＡＢの頂点Ａ　含める
	 *	辺ＢＣの頂点Ｂ　（水平な辺なので判定自体なし）
	 *	辺ＣＤの頂点Ｃ　含めない
	 *	辺ＤＥの頂点Ｄ　含める
	 *	辺ＥＦの頂点Ｅ　（水平な辺なので判定自体なし）
	 *	辺ＦＧの頂点Ｆ　含める
	 *	辺ＧＨの頂点Ｇ　（水平な辺なので判定自体なし）
	 *	辺ＨＩの頂点Ｈ　含める
	 *	辺ＩＡの頂点Ｉ　含めない
	 *
	 * さて、ある辺の始点側を交差判定に含めるかどうかの判断には、
	 * 一つ前の辺の傾き情報が必要です。
	 * 二つ目以降の辺ならば、一つ前の辺の傾きを覚えておけばいいのですが、
	 * 最初に処理する辺では、一つ前の辺の傾き情報がまだありません。
	 * わざわざ一つ前の辺を取ってきて傾きを調べるのはなんかイヤ(^^;なので、
	 * 次のように処理することにしました。
	 *
	 * 最初に処理する辺は、一番上（Y座標が最小）の頂点を始点とする辺を
	 * 採用することにします。
	 * こうすれば、一つ前の辺は（水平な辺を無視すれば）確実に上向きですし、
	 * 最初の辺は（水平な辺を無視すれば）確実に下向きです。
	 * すなわち、最初の辺の交差判定では、無条件に始点側も含めればいいことになります。
	 * （最初の辺が水平だった場合は、水平でない最初の辺で無条件に始点側を含めます。）
	 *
	 * 実はこの方法ですと、一番上の頂点を探すために全頂点を操作するので、
	 * 素直にリストの最初の頂点から始めて、一つ前の辺の傾きを調べた方が
	 * 処理速度的には速かったかもしれません。
	 * が、一つ前の辺が水平だった場合などの特別なケースに対応すると、
	 * 実装がややこしくなってしまうので、前述のような方法を採用しました。
	 *
	 * こちらのページを参考にさせて頂きました：
	 *	「Fussy's HOMEPAGE」
	 *	http://www.people.or.jp/~fussy/
	 *	http://www.people.or.jp/~fussy/algo/algo6-1.htm
	 */
#if 0 /*[アルゴリズムＡ]-----------------------------------------------------*/
//	※アルゴリズムＢが正しく動くことが確認できたら、こちらは消して下さい。
//
//	int n_cross, i;
//	float x0, y0, x1, y1, C, D, E, F;
//
//	if(n_points < 3) return 0;
//
//	/* まずクリア。 */
//	n_cross = 0;
//
//	/* 最初の辺の始点は、最後の頂点とします。 */
//	x0 = points[n_points - 1][0];
//	y0 = points[n_points - 1][1];
//	for(i = 0; i < n_points; i++) {
//		/* 今回の辺の終点を取得します。 */
//		x1 = points[i][0];
//		y1 = points[i][1];
//
//		/* 交差判定。 */
//		C = x1 - x0;
//		D = y1 - y0;
//		E = x0 - px;
//		F = y0 - py;
//		if(D >= 0) {
//			if(0 <= D * E - C * F &&
//			   0 <= -F && -F < D) n_cross++;
//		} else {
//			if(0 >= D * E - C * F &&
//			   0 >= -F && -F > D) n_cross++;
//		}
//
//		/* 次回の辺の始点は、今回の辺の終点です。 */
//		x0 = x1;
//		y0 = y1;
//	}
//
//	/* 奇数なら内側です。 */
//	return n_cross & 1;
//
#else /*[アルゴリズムＢ]-----------------------------------------------------*/
//※同一アルゴリズムの別実装で実験しましたが、本実装ではまだ検証していません。

	int n_cross, i, ii;
	float x0, y0, x1, y1, C, D, E, F, Dprev;

	if(n_points < 3) return 0;

	/* 一番上の頂点を探します。(複数ある場合はどれでも構いません) */
	ii = 0;
	y0 = points[ii][1];
	for(i = 1; i < n_points; i++) {
		if(points[i][1] < y0) {
			ii = i;
			y0 = points[ii][1];
		}
	}
	x0 = points[ii][0]; /* ii:(x0,y0)=初回の辺の始点 */

	n_cross = 0;
	Dprev = 0; /* 初回のDが＋でも−でも両端を含めるように */
	for(i = 0; i < n_points; i++) {
		/* 今回の辺の終点を取得します。 */
		ii++;
		if(ii == n_points) ii = 0;
		x1 = points[ii][0];
		y1 = points[ii][1];

		/* パラメータ計算。 */
		C = x1 - x0;
		D = y1 - y0;
		E = x0 - px;
		F = y0 - py;

		/* 始点→終点が上向きなら… */
		if(D > 0) {
			/* 前回も上向きなら、今回は始点を含めません。 */
			if(Dprev > 0) {
				if(0 <= D * E - C * F &&
				   0 <  -F && -F <= D) n_cross++;
			/* 下向きから上向きに変わる時は、両端を含めます。 */
			} else {
				if(0 <= D * E - C * F &&
				   0 <= -F && -F <= D) n_cross++;
			}
			Dprev = D; /* 向きを更新 */
		/* 始点→終点が下向きなら… */
		} else if(D < 0) {
			/* 前回も下向きなら、今回は始点を含めません。 */
			if(Dprev < 0) {
				if(0 >= D * E - C * F &&
				   0 >  -F && -F >= D) n_cross++;
			/* 上向きから下向きに変わる時は、両端を含めます。 */
			} else {
				if(0 >= D * E - C * F &&
				   0 >= -F && -F >= D) n_cross++;
			}
			Dprev = D; /* 向きを更新 */
		} /* 水平な辺は無視します。Dprevも更新しません。 */

		/* 今回の辺の終点が、次回の辺の始点になります。 */
		x0 = x1;
		y0 = y1;
	}

	/* 奇数なら内側です。 */
	return n_cross & 1;

#endif /*--------------------------------------------------------------------*/
}

/* ※整数バージョンでのみ確認済み */
int
cross_point(float x1, float y1, float x1end, float y1end,
	    float x2, float y2, float x2end, float y2end,
	    float* px, float* py)
{
	/* ２線分の交点を求めるアルゴリズムは次のとおりです。
	 * (ほとんどpoint_in_polygonのアルゴリズムと共通です)
	 * 
	 * 線分1(x1,y1)-(x1end,y1end)を次の式で表します：
	 * 	x = x1 + At	| A = x1end - x1
	 * 	y = y1 + Bt	| B = y1end - y1
	 * 線分2(x2,y2)-(x2end,y2end)を次の式で表します：
	 * 	x = x2 + Cu	| C = x2end - x2
	 * 	y = y2 + Du	| D = y2end - y2
	 * 交差点を(x,y)とすると：
	 * 	x = x1 + At = x2 + Cu
	 * 	y = y1 + Bt = y2 + Du
	 * 連立方程式を解いて、t,uを求めます：
	 * 	At - Cu = x2 - x1
	 * 	Bt - Du = y2 - y1
	 * 	t = (D(x2 - x1) - C(y2 - y1)) / (AD - BC)
	 * 	u = (B(x2 - x1) - A(y2 - y1)) / (AD - BC)
	 * 	t = (DE - CF) / (AD - BC)	| E = x2 - x1
	 * 	u = (BE - AF) / (AD - BC)	| F = y2 - y1
	 * 	t = (DE - CF) / G		| G = AD - BC
	 * 	u = (BE - AF) / G		|
	 * ２線分が交差するための条件は：
	 * 	0 <= (DE - CF) / G < 1
	 * 	0 <= (BE - AF) / G < 1
	 * 除算をなくすために、両辺にGをかけます：
	 * 	+--------------------+--------------------+--------------------+
	 * 	| G = 0 の場合       | G > 0 の場合       | G < 0 の場合       |
	 * 	+--------------------+--------------------+--------------------+
	 * 	| 交差していません。 | 0 <= (DE - CF) < G | 0 >= (DE - CF) > G |
	 * 	| (２線分は平行です) | 0 <= (BE - AF) < G | 0 >= (BE - AF) > G |
	 * 	+-----------------------------------------+--------------------+
	 * ２線分が交差していたら、交点を求めます。
	 * 線分1の式にtの式を代入して：
	 * 	x = x1 + A * (DE - CF) / G
	 * 	y = y1 + B * (DE - CF) / G
	 */
	float A, B, C, D, E, F, G, DE_CF, BE_AF;

	/* パラメータ計算。 */
	A = x1end - x1;
	B = y1end - y1;
	C = x2end - x2;
	D = y2end - y2;
	E = x2 - x1;
	F = y2 - y1;
	G = A * D - B * C;
	DE_CF = D * E - C * F;
	BE_AF = B * E - A * F;

	/* ２線分が交差しているか？ */
	if(G > 0) {
		if(0 > DE_CF || DE_CF >= G ||
		   0 > BE_AF || BE_AF >= G) return 0;
	} else if(G < 0) {
		if(0 < DE_CF || DE_CF <= G ||
		   0 < BE_AF || BE_AF <= G) return 0;
	} else {
		return 0;
	}

	/* 交点を求めます。 */
	if(px != NULL) *px = x1 + A * DE_CF / G;
	if(px != NULL) *py = y1 + B * DE_CF / G;
	return 1;
}

/* ※整数バージョンでのみ確認済み */
int
ortho_point(float x1, float y1,
	    float x2, float y2, float x2end, float y2end,
	    float*px, float* py)
{
	/* point_to_line_sとほとんど同じアルゴリズムです。
	 *
	 * 交点を求めるための直線パラメータ s は：
	 *	ｓ = da・(p - a) / da・da
	 * と表されます。(point_to_line_sの説明より)
	 *
	 * 垂線と線分の交点が、線分上にあるための条件は：
	 *	0 <= s < 1
	 *	0 <= da・(p - a) / da・da < 1
	 *
	 * da・da は明らかに0以上です。(da・da>=0)
	 * da・da=0 なら、線分は長さゼロなので、交点なしとします。
	 * da・da>0 なら、両辺に da・da をかけて：
	 *	0 <= da・(p - a) < da・da
	 * 上の式が成立すれば、交点は線分上にあります。
	 * （なお、da・da=0 の場合も上の式によって弾かれますので、
	 *   da・da=0 の場合だけを分けて判定する必要はありません）
	 *
	 * 交点が線分上にあったら、線分の式に s を代入して交点を求めます。
	 *	q = a + da * s
	 *	q = a + da * da・(p - a) / da・da
	 */
	float vx2, vy2, vx3, vy3, d1, d2;

	/* da */
	vx2 = x2end - x2;
	vy2 = y2end - y2;
	/* (p - a) */
	vx3 = x1 - x2;
	vy3 = y1 - y2;
	/* da・(p - a) */
	d1 = vx2 * vx3 + vy2 * vy3;
	/* da・da */
	d2 = vx2 * vx2 + vy2 * vy2;
	if(d1 < 0 || d2 <= d1) return 0; /* d2=0の場合も弾かれます */
	/* q = a + da * s */
	if(px != NULL) *px = x2 + vx2 * d1 / d2;
	if(py != NULL) *py = y2 + vy2 * d1 / d2;
	return 1;
}

int
polygon_orientation(float points[/*n_points*/][2], int n_points)
{
	/* 図形の記述を正方向と仮定して、角度の合計を取った場合、
	 * 正方向(図形内部の角度の合計)なら、(n_points-2)π となります。
	 * 逆方向(図形外部の角度の合計)なら、(n_points+2)π となります。
	 * 厳密には、合計が (n_points-2)π を超えた時点で逆方向と判断できるはずですが、
	 * 演算誤差のため、正方向でも合計が (n_point-2)π+α となる可能性があります。
	 * そこで、安全のために、両者の中間にしきい値を取って、
	 * n_pointsπ を超えた時点で逆方向と判断することにしました。 
	 *
	 * <<超重要!!>
	 * v01とv12の角度をはかるのではなく、v10とv12の角度をはかることに注意!
	 * 二つの辺の方角の差をはかるのではなく、頂点1のところの角度をはかるのです。
	 * もしもv01とv12の角度をはかってしまうと、例えば極端な例をあげるなら、
	 * 二つの隣り合った辺が一直線上にある場合、この間の角度は明らかに180度ですが、
	 * 間違って0度となってしまいます。
	 */
	float x0, y0, x1, y1, x2, y2, x10, x12, y10, y12, dx, dy;
	float threshold, d, d_sum;
	int i;

	if(n_points < 3) return 0;

	/* しきい値を求めます。 */
	threshold = n_points * PI;

	d_sum = 0;
	for(i = 0; i < n_points; i++) {
		/* v10=(x10,y10), v12=(x12,y12) を求めます。 */
		x0 = points[(i + 0) % n_points][0];
		y0 = points[(i + 0) % n_points][1];
		x1 = points[(i + 1) % n_points][0];
		y1 = points[(i + 1) % n_points][1];
		x2 = points[(i + 2) % n_points][0];
		y2 = points[(i + 2) % n_points][1];
		x10 = x0 - x1;
		y10 = y0 - y1;
		x12 = x2 - x1;
		y12 = y2 - y1;

		/* 内積: dx = |v10||v12|cosθ */
		dx = x10 * x12 + y10 * y12;

		/* 外積: dy = |v10||v12|sinθ */
		dy = x10 * y12 - x12 * y10;

		/* v10とv12の角度を 0〜2π の範囲で求めます。
		 * 注意: atan2()は -π〜π の範囲で返して来ます。
		 */
		d = atan2(dy, dx);
		if(d < 0) d += PI2;

		/* 角度を足して、しきい値を超えたら逆方向です。 */
		d_sum += d;
		if(d_sum > threshold) return 1;
	}

	/* 最後までしきい値を超えなければ、正方向です。 */
	return 0;
}

float
x1n(float x, float n)
{
	/* 考え方は次のとおりです：
	 *	y^n = x
	 * 両辺の自然対数を取って：
	 *	    ln(y^n) =     ln(x)
	 *	 n・ln(y)   =     ln(x)
	 *	    ln(y)   =     ln(x) / n
	 *	exp(ln(y))  = exp(ln(x) / n)
	 *	       y    = exp(ln(x) / n)
	 * 以上です。
	 */
	return exp(log(x) / n);
}

/*****************************************************************************
 *	整数演算ユーティリティ
 *****************************************************************************/

int
sqrt_32(int a)
{
	/* S1C33では32x32bit(5cycle)よりも16x16bit(1cycle)の方が速いので、
	 * 変数iやbをunsigned shortにした方が一見速くなるように見えます。
	 * が、実際にはさほど効果がないようです。
	 * 比較やシフト時に16→32bitへの昇格処理が増えてしまうからです。
	 * 全てアセンブラで書けば昇格処理を省いて16bitに最適化できますが、
	 * 今回はとりあえずこのままで行くことにしました。
	 */
	int i, b, c;

	b = 0; /* aが0なら、初回でa==b*bとなり、すぐにループ終了します */
	for(i = 1 << 15; i != 0; i >>= 1) {
		c = b * b;
		if(c < a) {
			b += i;
		} else if(c > a) {
			b -= i;
		} else {
			break;
		}
	}

	return b;
}

int
leapyear(int year)
{
	/* 閏年の定義（グレゴリオ暦）は
	 *	年数が4の倍数の年をうるう年とする。
	 *	ただし，100の倍数の年で400の倍数でない年は，平年とする
	 * となっています。
	 * したがって、1600年や2000年はうるう年だが，
	 * 1700年，1800年，1900年，2100年などは平年。
	 * うるう年は400年に97回で，1年の平均日数は365.2422日です。
	 * (http://www.ht-net21.ne.jp/~tomokazu/pclab/days/days.htmlより)
	 */
	return ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0);
}

int
dayofweek(int year, int month, int day)
{
	/* 「ツェラー(Zeller)の公式」：
	 *	西暦y年m月d日の曜日は、
	 *	(y + [y/4] - [y/100] + [y/400] + [2.6m+1.6] + d) mod 7
	 *	この値が0なら日曜日、1なら月曜日…6なら土曜日である。
	 *	ただし、１，２月は前年の１３，１４月とする。
	 *	＝[x]:ガウス記号・xを超えない最大の整数＝
	 *	＝この公式の有効範囲は1583年※２から3999年まで)＝
	 * (http://www.ht-net21.ne.jp/~tomokazu/pclab/days/days.htmlより)
	 */
	if(month == 1 || month == 2) {
		year--;
		month += 12;
	}
	return (year + year/4 - year/100 + year/400 + (13*month+8)/5 + day) % 7;
}

/*****************************************************************************
 *	浮動小数演算ユーティリティの整数バージョン
 *****************************************************************************/

/* 浮動小数バージョンをコピーして、float->intに置き換えただけです。
 * 浮動小数バージョンを修正したときは、こちらも追従させてください。
 */
int
point_in_polygon_32(int px, int py, int points[/*n_points*/][2], int n_points)
{
	int n_cross, i, ii;
	int x0, y0, x1, y1, C, D, E, F, Dprev;

	if(n_points < 3) return 0;

	/* 一番上の頂点を探します。(複数ある場合はどれでも構いません) */
	ii = 0;
	y0 = points[ii][1];
	for(i = 1; i < n_points; i++) {
		if(points[i][1] < y0) {
			ii = i;
			y0 = points[ii][1];
		}
	}
	x0 = points[ii][0]; /* ii:(x0,y0)=初回の辺の始点 */

	n_cross = 0;
	Dprev = 0; /* 初回のDが＋でも−でも両端を含めるように */
	for(i = 0; i < n_points; i++) {
		/* 今回の辺の終点を取得します。 */
		ii++;
		if(ii == n_points) ii = 0;
		x1 = points[ii][0];
		y1 = points[ii][1];

		/* パラメータ計算。 */
		C = x1 - x0;
		D = y1 - y0;
		E = x0 - px;
		F = y0 - py;

		/* 始点→終点が上向きなら… */
		if(D > 0) {
			/* 前回も上向きなら、今回は始点を含めません。 */
			if(Dprev > 0) {
				if(0 <= D * E - C * F &&
				   0 <  -F && -F <= D) n_cross++;
			/* 下向きから上向きに変わる時は、両端を含めます。 */
			} else {
				if(0 <= D * E - C * F &&
				   0 <= -F && -F <= D) n_cross++;
			}
			Dprev = D; /* 向きを更新 */
		/* 始点→終点が下向きなら… */
		} else if(D < 0) {
			/* 前回も下向きなら、今回は始点を含めません。 */
			if(Dprev < 0) {
				if(0 >= D * E - C * F &&
				   0 >  -F && -F >= D) n_cross++;
			/* 上向きから下向きに変わる時は、両端を含めます。 */
			} else {
				if(0 >= D * E - C * F &&
				   0 >= -F && -F >= D) n_cross++;
			}
			Dprev = D; /* 向きを更新 */
		} /* 水平な辺は無視します。Dprevも更新しません。 */

		/* 今回の辺の終点が、次回の辺の始点になります。 */
		x0 = x1;
		y0 = y1;
	}

	/* 奇数なら内側です。 */
	return n_cross & 1;
}

/* 浮動小数バージョンをコピーして、int->intに置き換えただけです。
 * 浮動小数バージョンを修正したときは、こちらも追従させてください。
 */
int
cross_point_32(int x1, int y1, int x1end, int y1end,
	       int x2, int y2, int x2end, int y2end,
	       int* px, int* py)
{
	int A, B, C, D, E, F, G, DE_CF, BE_AF;

	/* パラメータ計算。 */
	A = x1end - x1;
	B = y1end - y1;
	C = x2end - x2;
	D = y2end - y2;
	E = x2 - x1;
	F = y2 - y1;
	G = A * D - B * C;
	DE_CF = D * E - C * F;
	BE_AF = B * E - A * F;

	/* ２線分が交差しているか？ */
	if(G > 0) {
		if(0 > DE_CF || DE_CF >= G ||
		   0 > BE_AF || BE_AF >= G) return 0;
	} else if(G < 0) {
		if(0 < DE_CF || DE_CF <= G ||
		   0 < BE_AF || BE_AF <= G) return 0;
	} else {
		return 0;
	}

	/* 交点を求めます。 */
	if(px != NULL) *px = x1 + A * DE_CF / G;
	if(px != NULL) *py = y1 + B * DE_CF / G;
	return 1;
}

/* 浮動小数バージョンをコピーして、int->intに置き換えただけです。
 * 浮動小数バージョンを修正したときは、こちらも追従させてください。
 */
int
ortho_point_32(int x1, int y1,
	       int x2, int y2, int x2end, int y2end,
	       int*px, int* py)
{
	int vx2, vy2, vx3, vy3, d1, d2;

	/* da */
	vx2 = x2end - x2;
	vy2 = y2end - y2;
	/* (p - a) */
	vx3 = x1 - x2;
	vy3 = y1 - y2;
	/* da・(p - a) */
	d1 = vx2 * vx3 + vy2 * vy3;
	/* da・da */
	d2 = vx2 * vx2 + vy2 * vy2;
	if(d1 < 0 || d2 <= d1) return 0; /* d2=0の場合も弾かれます */
	/* q = a + da * s */
	if(px != NULL) *px = x2 + vx2 * d1 / d2;
	if(py != NULL) *py = y2 + vy2 * d1 / d2;
	return 1;
}

/****************************************************************************
 *	三角関数の固定小数版（8.8ビット）
 ****************************************************************************/

//////////////////////////////////////////////////////////////////////////////
//	●sin/tan_tbl_16[]は、以下のプログラムを用いて、PC上で生成しました。
//
//	#include <stdio.h>
//	#include <math.h>
//	
//	int
//	main()
//	{
//		int i, v;
//		double pi, d, r, s;
//	
//		pi = atan2(0, -1);
//	
//		printf("static const short sin_tbl_16[64 + 1] = {\n");
//		for(i = 0; i < 64 + 1; i++) {
//			d = 180.0 * i / 128; /* 表示用の角度 */
//			r = pi    * i / 128; /* 計算用の角度 */
//			s = sin(r);
//			v = (int)((s * (1 << 8)) + 0.5);
//			printf("\t/* %2d:sin(%5.2f)=%5.2f */ 0x%04x,\n", i, d, s, v);
//		}
//		printf("};\n");
//	
//		printf("static const short tan_tbl_16[32] = {\n");
//		for(i = 0; i < 32; i++) {
//			d = 180.0 * (i * 2 + 1) / 256; /* 表示用の角度 */
//			r = pi    * (i * 2 + 1) / 256; /* 計算用の角度 */
//			s = tan(r);
//			v = (int)((s * (1 << 8)) + 0.5);
//			printf("\t/* %2d:tan(%5.2f)=%5.2f */ 0x%04x,\n", i, d, s, v);
//		}
//		printf("};\n");
//	
//		return 0;
//	}
//////////////////////////////////////////////////////////////////////////////

static const short sin_tbl_16[64 + 1] = {
	/*  0:sin( 0.00)= 0.00 */ 0x0000,
	/*  1:sin( 1.41)= 0.02 */ 0x0006,
	/*  2:sin( 2.81)= 0.05 */ 0x000d,
	/*  3:sin( 4.22)= 0.07 */ 0x0013,
	/*  4:sin( 5.62)= 0.10 */ 0x0019,
	/*  5:sin( 7.03)= 0.12 */ 0x001f,
	/*  6:sin( 8.44)= 0.15 */ 0x0026,
	/*  7:sin( 9.84)= 0.17 */ 0x002c,
	/*  8:sin(11.25)= 0.20 */ 0x0032,
	/*  9:sin(12.66)= 0.22 */ 0x0038,
	/* 10:sin(14.06)= 0.24 */ 0x003e,
	/* 11:sin(15.47)= 0.27 */ 0x0044,
	/* 12:sin(16.88)= 0.29 */ 0x004a,
	/* 13:sin(18.28)= 0.31 */ 0x0050,
	/* 14:sin(19.69)= 0.34 */ 0x0056,
	/* 15:sin(21.09)= 0.36 */ 0x005c,
	/* 16:sin(22.50)= 0.38 */ 0x0062,
	/* 17:sin(23.91)= 0.41 */ 0x0068,
	/* 18:sin(25.31)= 0.43 */ 0x006d,
	/* 19:sin(26.72)= 0.45 */ 0x0073,
	/* 20:sin(28.12)= 0.47 */ 0x0079,
	/* 21:sin(29.53)= 0.49 */ 0x007e,
	/* 22:sin(30.94)= 0.51 */ 0x0084,
	/* 23:sin(32.34)= 0.53 */ 0x0089,
	/* 24:sin(33.75)= 0.56 */ 0x008e,
	/* 25:sin(35.16)= 0.58 */ 0x0093,
	/* 26:sin(36.56)= 0.60 */ 0x0098,
	/* 27:sin(37.97)= 0.62 */ 0x009d,
	/* 28:sin(39.38)= 0.63 */ 0x00a2,
	/* 29:sin(40.78)= 0.65 */ 0x00a7,
	/* 30:sin(42.19)= 0.67 */ 0x00ac,
	/* 31:sin(43.59)= 0.69 */ 0x00b1,
	/* 32:sin(45.00)= 0.71 */ 0x00b5,
	/* 33:sin(46.41)= 0.72 */ 0x00b9,
	/* 34:sin(47.81)= 0.74 */ 0x00be,
	/* 35:sin(49.22)= 0.76 */ 0x00c2,
	/* 36:sin(50.62)= 0.77 */ 0x00c6,
	/* 37:sin(52.03)= 0.79 */ 0x00ca,
	/* 38:sin(53.44)= 0.80 */ 0x00ce,
	/* 39:sin(54.84)= 0.82 */ 0x00d1,
	/* 40:sin(56.25)= 0.83 */ 0x00d5,
	/* 41:sin(57.66)= 0.84 */ 0x00d8,
	/* 42:sin(59.06)= 0.86 */ 0x00dc,
	/* 43:sin(60.47)= 0.87 */ 0x00df,
	/* 44:sin(61.88)= 0.88 */ 0x00e2,
	/* 45:sin(63.28)= 0.89 */ 0x00e5,
	/* 46:sin(64.69)= 0.90 */ 0x00e7,
	/* 47:sin(66.09)= 0.91 */ 0x00ea,
	/* 48:sin(67.50)= 0.92 */ 0x00ed,
	/* 49:sin(68.91)= 0.93 */ 0x00ef,
	/* 50:sin(70.31)= 0.94 */ 0x00f1,
	/* 51:sin(71.72)= 0.95 */ 0x00f3,
	/* 52:sin(73.12)= 0.96 */ 0x00f5,
	/* 53:sin(74.53)= 0.96 */ 0x00f7,
	/* 54:sin(75.94)= 0.97 */ 0x00f8,
	/* 55:sin(77.34)= 0.98 */ 0x00fa,
	/* 56:sin(78.75)= 0.98 */ 0x00fb,
	/* 57:sin(80.16)= 0.99 */ 0x00fc,
	/* 58:sin(81.56)= 0.99 */ 0x00fd,
	/* 59:sin(82.97)= 0.99 */ 0x00fe,
	/* 60:sin(84.38)= 1.00 */ 0x00ff,
	/* 61:sin(85.78)= 1.00 */ 0x00ff,
	/* 62:sin(87.19)= 1.00 */ 0x0100,
	/* 63:sin(88.59)= 1.00 */ 0x0100,
	/* 64:sin(90.00)= 1.00 */ 0x0100,
};
static const short tan_tbl_16[32] = {
	/*  0:tan( 0.70)= 0.01 */ 0x0003,
	/*  1:tan( 2.11)= 0.04 */ 0x0009,
	/*  2:tan( 3.52)= 0.06 */ 0x0010,
	/*  3:tan( 4.92)= 0.09 */ 0x0016,
	/*  4:tan( 6.33)= 0.11 */ 0x001c,
	/*  5:tan( 7.73)= 0.14 */ 0x0023,
	/*  6:tan( 9.14)= 0.16 */ 0x0029,
	/*  7:tan(10.55)= 0.19 */ 0x0030,
	/*  8:tan(11.95)= 0.21 */ 0x0036,
	/*  9:tan(13.36)= 0.24 */ 0x003d,
	/* 10:tan(14.77)= 0.26 */ 0x0043,
	/* 11:tan(16.17)= 0.29 */ 0x004a,
	/* 12:tan(17.58)= 0.32 */ 0x0051,
	/* 13:tan(18.98)= 0.34 */ 0x0058,
	/* 14:tan(20.39)= 0.37 */ 0x005f,
	/* 15:tan(21.80)= 0.40 */ 0x0066,
	/* 16:tan(23.20)= 0.43 */ 0x006e,
	/* 17:tan(24.61)= 0.46 */ 0x0075,
	/* 18:tan(26.02)= 0.49 */ 0x007d,
	/* 19:tan(27.42)= 0.52 */ 0x0085,
	/* 20:tan(28.83)= 0.55 */ 0x008d,
	/* 21:tan(30.23)= 0.58 */ 0x0095,
	/* 22:tan(31.64)= 0.62 */ 0x009e,
	/* 23:tan(33.05)= 0.65 */ 0x00a7,
	/* 24:tan(34.45)= 0.69 */ 0x00b0,
	/* 25:tan(35.86)= 0.72 */ 0x00b9,
	/* 26:tan(37.27)= 0.76 */ 0x00c3,
	/* 27:tan(38.67)= 0.80 */ 0x00cd,
	/* 28:tan(40.08)= 0.84 */ 0x00d7,
	/* 29:tan(41.48)= 0.88 */ 0x00e2,
	/* 30:tan(42.89)= 0.93 */ 0x00ee,
	/* 31:tan(44.30)= 0.98 */ 0x00fa,
};

int
sin_16(int dir)
{
	short result;

	dir &= 0xff;
	       if(dir <= 0x40) {	/*   0〜 90度 */
		result = sin_tbl_16[dir];
	} else if(dir <= 0x80) {	/*  90〜180度 */
		result = sin_tbl_16[0x80 - dir];
	} else if(dir <= 0xc0) {	/* 180〜270度 */
		result = -sin_tbl_16[dir - 0x80];
	} else {			/* 270〜360度 */
		result = -sin_tbl_16[0x100 - dir];
	}

	return result;
}

int
cos_16(int dir)
{
	return sin_16(dir + 0x40);
}

int
atan2_16(int y, int x)
{
	int octant, a, dir;

	/* 八分円の場合分け。
	 *
	 * Ｙ  [101][100]
	 * ↑[001]    [000]
	 *   [011]    [010]
	 *     [111][110]
	 *             →Ｘ
	 *
	 * bit0: x>=0なら0、x<0なら1
	 * bit1: y>=0なら0、y<0なら1
	 * bit2: abs(x)>=abs(y)なら0、abs(x)<abs(y)なら1
	 */
	octant = 0;
	if(x < 0) { /* x>=0に補正 */
		octant |= 1 << 0;
		x = -x;
	}
	if(y < 0) { /* y>=0に補正 */
		octant |= 1 << 1;
		y = -y;
	}
	if(x < y) { /* x>=yに補正。既にx,yは正数なので、abs()不要 */
		octant |= 1 << 2;
		a = x;
		    x = y;
		        y = a;
	}

	/* この時点で、(x,y)は0〜45度の範囲に補正されています。
	 * (x,y)=(0,0)なら処理しません。
	 * x>=0,y>=0,x>=yに補正されているので、x=0なら明らかにy=0です。
	 */
	if(x == 0) return 0;

	/* tan値をテーブル比較して、0〜45度で角度を求めます。
	 * ※tanテーブルの各要素は、一周を1/256した角度よりも、
	 *   それぞれ1/512づつ先の角度のtan値を保持しています。
	 * ※期待する結果が0x00〜0x1fではなく、0x00〜0x20なので、
	 *   バイナリサーチできない？？？
 	 */
	a = (y << 8) / x;
	for(dir = 0; dir < 0x20; dir++) {
		if(a < tan_tbl_16[dir]) break;
	}
	/* この時点で、0x00<=dir<=0x20になっています。 */

	/* 八分円のいずれかによって、結果を補正します。 */
	if(octant & (1 << 2)) dir = 0x40 - dir;	/*  0x00<=dir<=0x40に拡張 */
	if(octant & (1 << 0)) dir = 0x80 - dir;	/*  0x00<=dir<=0x80に拡張 */
	if(octant & (1 << 1)) dir = 0x00 - dir;	/* -0x80<=dir<=0x80に拡張 */

	/* この時点で、-0x80<=dir<=0x80になっています。
	 * 最後に、dir=0x80（π）の場合のみ、-0x80（-π）に補正します。
	 * 0x80（π）でも間違いではありませんが、-0x80〜0x7fとして返した方が
	 * 符号付き1バイトの範囲に収まるので、自然だと思うからです。
	 * 最初からdirをchar型で取り扱うこともできますが、計算のたびに
	 * char→intへのキャストが入るため、逆に遅くなってしまいます。
	 */
	return (char)dir;
}

