Continue quat

make.sh

@@ -1,4 +1,4 @@
 #!/bin/sh
 
 rm test_sdl
-g++ -x c -std=c99 -Wall -Wextra -pedantic -O3 -Wall -Wextra -Wno-unused-parameter -Wno-missing-field-initializers -lSDL2 -o test test.c && ./test
+g++ -x c -std=c99 -Wall -Wextra -pedantic -O3 -Wall -Wextra -Wno-unused-parameter -Wno-missing-field-initializers -lSDL2 -o test_sdl test_sdl.c && ./test_sdl

test.c

@@ -39,18 +39,23 @@ int testQuatToEuler(TPE_Unit yaw, TPE_Unit pitch, TPE_Unit roll)
 
 
 
-  TPE_setVec4(&axis,0,0,F,0);
+  TPE_setVec4(&axis,F,0,0,0);
 
   TPE_rotationToQuaternion(axis,pitch,&q);
 
+TPE_PRINTF_VEC4(q);
   
 
   TPE_Unit y,p,r;
 
   TPE_quaternionToEulerAngles(q,&y,&p,&r);
 
-  printf("%d %d %d\n",y,p,r);
-  
+
+
+  //printf("%d %d %d\n",y,p,r);
+ 
+
+ 
 }
 
 int main(void)
@@ -63,8 +68,7 @@ int main(void)
   testRotToQuat(0,0,F,    F/2,  0,0,F,0);
   testRotToQuat(-F,F,F,   -F/8, 195,-195,-195,472);
 
-
-  testQuatToEuler(0,F/2,0);
+  testQuatToEuler(0,128,0);
 
   return 0;
 }

test_sdl.c

@@ -26,42 +26,6 @@ S3L_Index cubeTriangles[] = { S3L_CUBE_TRIANGLES };
 
 int main()
 {
-
-
-TPE_Vec4 a, b, r, r2, r3, axis;
-TPE_Unit yaw, pitch, roll;
-
-TPE_setVec4(&axis,0,512,0,0);
-
-TPE_rotationToQuaternion(axis,TPE_FRACTIONS_PER_UNIT / 4,&r);
-
-TPE_PRINTF_VEC4(r)
-
-TPE_quaternionToEulerAngles(r,&yaw,&pitch,&roll);
-
-printf("%d %d %d\n",yaw,pitch,roll);
-
-/*
-TPE_setVec4(&axis,512,0,0,0);
-TPE_rotationToQuaternion(axis,-128,&r);
-
-TPE_setVec4(&axis,0,512,0,0);
-TPE_rotationToQuaternion(axis,-128,&r2);
-
-TPE_quaternionMultiply(r,r2,&r3);
-
-TPE_setVec4(&axis,512,0,0,0);
-TPE_rotationToQuaternion(axis,-128,&r);
-TPE_quaternionMultiply(r3,r,&r2);
-
-TPE_PRINTF_VEC4(r2);
-
-TPE_setVec4(&axis,0,0,512,0);
-TPE_rotationToQuaternion(axis,128,&r);
-
-TPE_PRINTF_VEC4(r);
-*/
-
   SDL_Window *window = SDL_CreateWindow("test", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, S3L_RESOLUTION_X, S3L_RESOLUTION_Y, SDL_WINDOW_SHOWN); 
   SDL_Renderer *renderer = SDL_CreateRenderer(window,-1,0);
   SDL_Texture *textureSDL = SDL_CreateTexture(renderer,SDL_PIXELFORMAT_RGBX8888, SDL_TEXTUREACCESS_STATIC, S3L_RESOLUTION_X, S3L_RESOLUTION_Y);
@@ -74,14 +38,38 @@ TPE_PRINTF_VEC4(r);
 
   S3L_initModel3D(cubeVertices,S3L_CUBE_VERTEX_COUNT,cubeTriangles,S3L_CUBE_TRIANGLE_COUNT,&cubeModel);
 
-  cubeModel.transform.translation.z = 3 * S3L_FRACTIONS_PER_UNIT;
  
   S3L_Scene scene;
 
   S3L_initScene(&cubeModel,1,&scene);
+
+  scene.camera.transform.translation.z = -3 * S3L_FRACTIONS_PER_UNIT;
+
+S3L_Mat4 m;
+cubeModel.customTransformMatrix = &m;
  
+TPE_Vec4 quat, axis;
+
+TPE_setVec4(&axis,512,0,0,0);
+
+  TPE_Unit frame = 0;
+
   while (running)
   {
+
+
+TPE_rotationToQuaternion(axis,frame,&quat);
+TPE_quaternionToRotationMatrix(quat,m);
+/*
+S3L_logMat4(m);
+
+S3L_makeRotationMatrixZXY(40,0,0,&m);
+
+S3L_logMat4(m);
+
+break;
+*/
+
     for (uint32_t i = 0; i < PIXELS_SIZE; ++i)
       pixels[i] = 0;
 
@@ -105,6 +93,8 @@ TPE_PRINTF_VEC4(r);
     SDL_RenderPresent(renderer);
 
     usleep(20000);
+
+    frame++;
   }
 
   return 0;

tinyphysicsengine.h

@@ -417,50 +417,68 @@ void TPE_quaternionToEulerAngles(TPE_Vec4 quaternion, TPE_Unit *yaw,
 {
   *roll = 
      TPE_atan2(
-     (  (2 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z)) 
+     (  (2 * (quaternion.w * quaternion.x - quaternion.y * quaternion.z)) 
         / TPE_FRACTIONS_PER_UNIT 
      ),
      (
        TPE_FRACTIONS_PER_UNIT - 
-       ((2 * (quaternion.x * quaternion.x + 
+       ((2 * (quaternion.x * quaternion.x - 
          quaternion.y * quaternion.y))  / TPE_FRACTIONS_PER_UNIT))
      );
 
   *pitch =
      TPE_asin(
-       (2 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x)) / TPE_FRACTIONS_PER_UNIT
+       (2 * (quaternion.w * quaternion.y + quaternion.z * quaternion.x)) / TPE_FRACTIONS_PER_UNIT
     );
 
   *yaw =
      TPE_atan2(
-     (  (2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y))
+     (  (2 * (quaternion.w * quaternion.z - quaternion.x * quaternion.y))
         / TPE_FRACTIONS_PER_UNIT
      ),
      (  
        TPE_FRACTIONS_PER_UNIT - 
-        ((2 * (quaternion.y * quaternion.y + 
+        ((2 * (quaternion.y * quaternion.y - 
           quaternion.z * quaternion.z))  / TPE_FRACTIONS_PER_UNIT))
      );
 }
 
-void TPE_quaternionToRotationMatrix(TPE_Vec4 quaternion, TPE_Unit matrix[16])
+void TPE_quaternionToRotationMatrix(TPE_Vec4 quaternion, TPE_Unit matrix[4][4])
 {
-/*
   TPE_Unit 
-    n2y2 = (2 * quaternion.y * quaternion.y) / TPE_FRACTIONS_PER_UNIT,
-    n2z2 = (2 * quaternion.z * quaternion.z) / TPE_FRACTIONS_PER_UNIT;
-    n2xy
+    _2x2 = (2 * quaternion.x * quaternion.x) / TPE_FRACTIONS_PER_UNIT,
+    _2y2 = (2 * quaternion.y * quaternion.y) / TPE_FRACTIONS_PER_UNIT,
+    _2z2 = (2 * quaternion.z * quaternion.z) / TPE_FRACTIONS_PER_UNIT,
+    _2xy = (2 * quaternion.x * quaternion.y) / TPE_FRACTIONS_PER_UNIT,
+    _2xw = (2 * quaternion.x * quaternion.w) / TPE_FRACTIONS_PER_UNIT,
+    _2zw = (2 * quaternion.z * quaternion.w) / TPE_FRACTIONS_PER_UNIT,
+    _2xz = (2 * quaternion.x * quaternion.z) / TPE_FRACTIONS_PER_UNIT,
+    _2yw = (2 * quaternion.y * quaternion.w) / TPE_FRACTIONS_PER_UNIT,
+    _2yz = (2 * quaternion.y * quaternion.z) / TPE_FRACTIONS_PER_UNIT;
 
   #define ONE TPE_FRACTIONS_PER_UNIT
 
-  matrix[0] = ONE - n2y2 - n2z2;
-  matrix[1] = ONE - 
-  matrix[2] =
-  matrix[3] = 0;
+  matrix[0][0]  = ONE - _2y2 - _2z2;
+  matrix[1][0]  = _2xy - _2zw;
+  matrix[2][0]  = _2xz + _2yw;
+  matrix[3][0]  = 0;
+
+  matrix[0][1]  = _2xy + _2zw;
+  matrix[1][1]  = ONE - _2x2 - _2z2;
+  matrix[2][1]  = _2yz - _2xw;
+  matrix[3][1]  = 0;
 
+  matrix[0][2]  = _2xz - _2yw;
+  matrix[1][2]  = _2yz + _2xw;
+  matrix[2][2] = ONE - _2x2 - _2y2;
+  matrix[3][2] = 0;
+
+  matrix[0][3] = 0;
+  matrix[1][3] = 0;
+  matrix[2][3] = 0;
+  matrix[3][3] = ONE;
 
   #undef ONE
-*/
 }
 
 void TPE_vec3Add(const TPE_Vec4 a, const TPE_Vec4 b, TPE_Vec4 *result)