--- a/Renderer/Engine/matrix_calc.cc	Wed Nov 16 12:39:07 2011 +0900
+++ b/Renderer/Engine/matrix_calc.cc	Fri Nov 18 16:56:11 2011 +0900
@@ -5,12 +5,6 @@
 #include "matrix_calc.h"
 using namespace std;
 
-void noMoreMemory()
-{
-  cout << "can't allocate memory\n";
-  exit(1);
-}
-
 void
 transMatrix(float *m0, float *m1, float *v)
 {
@@ -40,17 +34,16 @@
 
     for (int i = 0; i < 4; i++) {
 	for (int j = 0; j < 4; j++) {
-	    m[i*4+j] = m1[j*4+i];
+	    m0[i*4+j] = m[i*4+j] = m1[j*4+i];
 	}
     }
 
-    m[12] = -(m1[12]*m[0] + m1[13]*m[1] + m1[14]*m[2]);
-    m[13] = -(m1[12]*m[4] + m1[13]*m[5] + m1[14]*m[6]);
-    m[14] = -(m1[12]*m[8] + m1[13]*m[9] + m1[14]*m[10]);
-    m[3] = m[7] = m[11] = 0.0f;
-    m[15] = 1.0f;
+    m0[12] = -(m1[12]*m[0] + m1[13]*m[1] + m1[14]*m[2]);
+    m0[13] = -(m1[12]*m[4] + m1[13]*m[5] + m1[14]*m[6]);
+    m0[14] = -(m1[12]*m[8] + m1[13]*m[9] + m1[14]*m[10]);
+    m0[3] = m0[7] = m0[11] = 0.0f;
+    m0[15] = 1.0f;
 
-    memcpy(m0, m, sizeof(float)*16);
 }
 
 /**
@@ -67,6 +60,7 @@
     }
 }
 
+
 /**
  * ベクトルの正規化
  *
@@ -117,13 +111,14 @@
 {
     float t[16];
 
+    // こういう小細工よりベクタ使った方が良いんだが
     for (int i = 0; i < 4; i++) {
-	for (int j = 0; j < 4; j++) {
+	for (int j = i+1; j < 4; j++) {
+	    float tmp = t[j*4+i];
 	    t[i*4+j] = m1[j*4+i];
+	    m1[j*4+i] = tmp;
 	}
     }
-
-    memcpy(m0, t, sizeof(float)*16);
 }
 
 /**
@@ -149,6 +144,17 @@
     }
 }
 
+void matrix4x4R(float &xyz[16], float &xyz1[16], float &xyz2[16]) 
+{
+  for(int t=0; t<16; t+=4)
+    {
+      for(int i=0; i<4; i++)
+	{
+	  xyz[t+i] = xyz1[t]*xyz2[i] + xyz1[t+1]*xyz2[4+i] + xyz1[t+2]*xyz2[8+i] + xyz1[t+3]*xyz2[12+i];
+	}
+    }
+}
+
 /**
  *   c_xyz を中心に sacle 倍する
  */
@@ -185,9 +191,9 @@
 get_matrix( float *matrix, float *rxyz, float *txyz, float *stack)
 {
   float radx,rady,radz;
-  radx = rxyz[0]*3.141562/180;
-  rady = rxyz[1]*3.141562/180;
-  radz = rxyz[2]*3.141562/180;
+  radx = rxyz[0]*M_PI/180;
+  rady = rxyz[1]*M_PI/180;
+  radz = rxyz[2]*M_PI/180;
 
   float sinx = sin(radx);
   float cosx = cos(radx);
@@ -228,9 +234,9 @@
 get_matrix_scale( float *matrix, float *rxyz, float *txyz, float *scale, float *stack)
 {
   float radx,rady,radz;
-  radx = rxyz[0]*3.14/180;
-  rady = rxyz[1]*3.14/180;
-  radz = rxyz[2]*3.14/180;
+  radx = rxyz[0]*M_PI/180;
+  rady = rxyz[1]*M_PI/180;
+  radz = rxyz[2]*M_PI/180;
 
   float sinx = sin(radx)*scale[0];
   float cosx = cos(radx)*scale[0];
@@ -267,9 +273,43 @@
 
 }
 
+void
+rotate_matrix( float &m[16], float &rxyz[4] )
+{
+  float radx,rady,radz;
+  radx = rxyz[0]*M_PI/180;
+  rady = rxyz[1]*M_PI/180;
+  radz = rxyz[2]*M_PI/180;
+
+  float sinx = sin(radx);
+  float cosx = cos(radx);
+  float siny = sin(rady);
+  float cosy = cos(rady);
+  float sinz = sin(radz);
+  float cosz = cos(radz);
+
+  /* View Transform */
+  m[0] = cosz*cosy+sinz*sinx*siny;
+  m[1] = sinz*cosx;
+  m[2] = -cosz*siny+sinz*sinx*cosy;
+  m[3] = 0;
+  m[4] = -sinz*cosy+cosz*sinx*siny;
+  m[5] = cosz*cosx;
+  m[6] = sinz*siny+cosz*sinx*cosy;
+  m[7] = 0;
+  m[8] = cosx*siny;
+  m[9] = -sinx;
+  m[10] = cosx*cosy;
+  m[11] = 0;
+  m[12] = 0;
+  m[13] = 0;
+  m[14] = 0;
+  m[15] = 1;
+}
+
 void rotate_x(float *xyz, float r)
 {
-  float rad = r*3.14/180;
+  float rad = r*M_PI/180;
 
   xyz[0] = xyz[0];
   xyz[1] = xyz[1]*cos(rad) - xyz[2]*sin(rad);
@@ -278,7 +318,7 @@
 
 void rotate_y(float *xyz, float r)
 {
-  float rad = r*3.14/180;
+  float rad = r*M_PI/180;
 
   xyz[0] = xyz[0]*cos(rad) + xyz[2]*sin(rad);
   xyz[1] = xyz[1];
@@ -287,7 +327,7 @@
 
 void rotate_z(float *xyz, float r)
 {
-  float rad = r*3.14/180;
+  float rad = r*M_PI/180;
 
   xyz[0] = xyz[0]*cos(rad) - xyz[1]*sin(rad);
   xyz[1] = xyz[0]*sin(rad) + xyz[1]*cos(rad);