Update

main.c

@@ -2,6 +2,7 @@
 
 #include "tinyphysicsengine.h"
 #include <SDL2/SDL.h>
+#include <math.h>
 
 #define S3L_RESOLUTION_X 640
 #define S3L_RESOLUTION_Y 480
@@ -317,6 +318,23 @@ const S3L_Index sphereTriangleIndices[SPHERE_TRIANGLE_COUNT * 3] = {
 
 int main(void)
 {
+
+/*
+TPE_Vec3 rrr = 
+TPE_orientationFromVecs(
+
+  TPE_vec3(0,0,100),
+  TPE_vec3(100,100,0)
+
+);
+
+printf("result:\n");
+TPE_PRINTF_VEC3(rrr);
+
+
+return 0;
+*/
+
   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);
@@ -462,6 +480,14 @@ for (int i = 0; i < world.bodyCount; ++i)
 
     S3L_rotationToDirections(sphereScene.camera.transform.rotation,SHIFT_STEP,&camF,&camR,0);
 
+TPE_Vec3 rrr = TPE_orientationFromVecs(
+  TPE_vec3(camF.x,camF.y,camF.z),
+  TPE_vec3(camR.x,camR.y,camR.z)
+);
+
+TPE_PRINTF_VEC3(rrr);
+putchar('\n');
+
     if (state[SDL_SCANCODE_LSHIFT])
     {
       if (state[SDL_SCANCODE_UP])
@@ -483,6 +509,10 @@ for (int i = 0; i < world.bodyCount; ++i)
         sphereScene.camera.transform.rotation.y += ROT_STEP;
       else if (state[SDL_SCANCODE_RIGHT])
         sphereScene.camera.transform.rotation.y -= ROT_STEP;
+      else if (state[SDL_SCANCODE_K])
+        sphereScene.camera.transform.rotation.z += ROT_STEP;
+      else if (state[SDL_SCANCODE_L])
+        sphereScene.camera.transform.rotation.z -= ROT_STEP;
     }
 
 if (state[SDL_SCANCODE_M])

tinyphysicsengine.h

@@ -1,6 +1,15 @@
 #ifndef _TINYPHYSICSENGINE_H
 #define _TINYPHYSICSENGINE_H
 
+/**
+
+  Orientations/rotations are in extrinsic Euler angles in the ZXY order (by Z,
+  then by X, then by Y).
+
+  Where it matters (e.g. rotations about axes) we consider a left-handed coord.
+  system (x right, y up, z forward).
+*/
+
 #include <stdint.h>
 
 typedef int32_t TPE_Unit;
@@ -113,6 +122,11 @@ TPE_Vec3 TPE_vec3Times(TPE_Vec3 v, TPE_Unit units);
 TPE_Vec3 TPE_vec3TimesNonNormalized(TPE_Vec3 v, TPE_Unit q);
 TPE_Vec3 TPE_vec3Normalized(TPE_Vec3 v);
 
+/* Computes orientation/rotation (see docs for orientation format) from two
+  vectors (which should be at least a close to being perpensicular and do NOT
+  need to be normalized). */
+TPE_Vec3 TPE_orientationFromVecs(TPE_Vec3 forward, TPE_Vec3 right);
+
 TPE_Unit TPE_vec3Dot(TPE_Vec3 v1, TPE_Vec3 v2);
 
 TPE_Unit TPE_sqrt(TPE_Unit value);
@@ -204,9 +218,15 @@ void TPE_bodyRotate(TPE_Body *body, TPE_Vec3 rotation);
 /** Compute the center of mass of a soft body. */
 TPE_Vec3 TPE_bodyGetCenter(const TPE_Body *body);
 
-TPE_Unit TPE_cos(TPE_Unit x);
+/** Compute sine, TPE_FRACTIONS_PER_UNIT as argument corresponds to 2 * PI
+  radians. Returns a number from -TPE_FRACTIONS_PER_UNIT to
+  TPE_FRACTIONS_PER_UNIT. */
 TPE_Unit TPE_sin(TPE_Unit x);
 
+TPE_Unit TPE_cos(TPE_Unit x);
+
+TPE_Unit TPE_atan(TPE_Unit x);
+
 //------------------------------------------------------------------------------
 
 static inline TPE_Unit TPE_nonZero(TPE_Unit x)
@@ -1110,4 +1130,99 @@ TPE_Vec3 TPE_vec3Normalized(TPE_Vec3 v)
   return v;
 }
 
+TPE_Unit TPE_atan(TPE_Unit x)
+{
+  /* atan approximation by polynomial 
+     WARNING: this will break with different value of TPE_FRACTIONS_PER_UNIT */
+
+  TPE_Unit sign = 1, x2 = x * x;
+
+  if (x < 0)
+  {
+    x *= -1;
+    sign = -1;
+  }
+
+  return sign *
+    (307 * x + x2) / ((267026 + 633 * x + x2) / 128);
+}
+
+void _TPE_vec2Rotate(TPE_Unit *x, TPE_Unit *y, TPE_Unit angle)
+{
+  TPE_Unit tmp = *x;
+
+  TPE_Unit s = TPE_sin(angle);
+  TPE_Unit c = TPE_cos(angle);
+
+  *x = (c * *x - s * *y) / TPE_FRACTIONS_PER_UNIT;
+  *y = (s * tmp + c * *y) / TPE_FRACTIONS_PER_UNIT;
+}
+
+TPE_Unit _TPE_vec2Angle(TPE_Unit x, TPE_Unit y)
+{
+  TPE_Unit r = 0;
+
+  if (x != 0)
+  {
+    r = TPE_atan((y * TPE_FRACTIONS_PER_UNIT) / x);
+
+    if (x < 0)
+      r += TPE_FRACTIONS_PER_UNIT / 2;
+    else if (r < 0)
+      r = TPE_FRACTIONS_PER_UNIT + r;
+  }
+  else
+  {
+    if (y < 0)
+      r = (3 * TPE_FRACTIONS_PER_UNIT) / 4;
+    else if (y > 0)
+      r = TPE_FRACTIONS_PER_UNIT / 4;
+    // else (y == 0) r stays 0
+  }
+
+  return r;
+}
+
+TPE_Vec3 TPE_orientationFromVecs(TPE_Vec3 forward, TPE_Vec3 right)
+{
+  TPE_Vec3 result;
+
+  // get rotation around Y:
+
+  result.y = 
+    (TPE_FRACTIONS_PER_UNIT - _TPE_vec2Angle(forward.z,forward.x)) %
+    TPE_FRACTIONS_PER_UNIT;
+
+  // now rotate back by this angle to align with x = 0 plane:
+
+  _TPE_vec2Rotate(&forward.z,&forward.x,result.y);
+  _TPE_vec2Rotate(&right.z,&right.x,result.y);
+
+  // now do the same for the second axis:
+
+  result.x = 
+    (TPE_FRACTIONS_PER_UNIT - _TPE_vec2Angle(forward.z,forward.y)) %
+    TPE_FRACTIONS_PER_UNIT;
+
+_TPE_vec2Rotate(&forward.z,&forward.y,result.x);
+
+
+printf("aaa:");
+TPE_PRINTF_VEC3(forward);
+TPE_PRINTF_VEC3(right);
+printf("\n");
+
+  _TPE_vec2Rotate(&right.z,&right.y,result.x);
+
+
+  result.z = 
+    (TPE_FRACTIONS_PER_UNIT - _TPE_vec2Angle(right.x,right.y)) %
+    TPE_FRACTIONS_PER_UNIT;
+
+
+
+  return result;
+
+}
+
 #endif // guard