diff --git a/small3dlib.h b/small3dlib.h
index 485f0e7..74e09c2 100644
--- a/small3dlib.h
+++ b/small3dlib.h
@@ -276,9 +276,10 @@ typedef struct
 } S3L_Vec4;
 
 static inline void S3L_initVec4(S3L_Vec4 *v);
-
 static inline void S3L_vec3Add(S3L_Vec4 *result, S3L_Vec4 added);
 static inline void S3L_vec3Sub(S3L_Vec4 *result, S3L_Vec4 substracted);
+S3L_Unit S3L_vec3Length(S3L_Vec4 v);
+void S3L_normalizeVec3(S3L_Vec4 *v);
 
 #define S3L_logVec4(v)\
   printf("Vec4: %d %d %d %d\n",((v).x),((v).y),((v).z),((v).w))
@@ -444,6 +445,9 @@ static inline S3L_Unit S3L_nonZero(S3L_Unit value);
 S3L_Unit S3L_sin(S3L_Unit x);
 static inline S3L_Unit S3L_cos(S3L_Unit x);
 
+S3L_Unit S3L_vec3Length(S3L_Vec4 v);
+S3L_Unit S3L_sqrt(S3L_Unit value);
+
 /** Interpolated between two values, v1 and v2, in the same ratio as t is to
   tMax. Does NOT prevent zero division. */
 static inline S3L_Unit S3L_interpolate(
@@ -964,6 +968,55 @@ void S3L_makeRotationMatrixZXY(
   #undef S 
 }
 
+S3L_Unit S3L_sqrt(S3L_Unit value)
+{
+  int8_t sign = 1;
+
+  if (value < 0)
+  {
+    sign = -1;
+    value *= -1;
+  }
+
+  uint32_t result = 0;
+  uint32_t a = value;
+  uint32_t b = 1u << 30;
+
+  while (b > a)
+    b >>= 2;
+
+  while (b != 0)
+  {
+    if (a >= result + b)
+    {
+      a -= result + b;
+      result = result +  2 * b;
+    }
+
+    b >>= 2;
+    result >>= 1;
+  }
+
+  return result * sign;
+}
+
+S3L_Unit S3L_vec3Length(S3L_Vec4 v)
+{
+  return S3L_sqrt(v.x * v.x + v.y * v.y + v.z * v.z);  
+}
+
+void S3L_normalizeVec3(S3L_Vec4 *v)
+{
+  S3L_Unit l = S3L_vec3Length(*v);
+
+  if (l == 0)
+    return;
+
+  v->x = (v->x * S3L_FRACTIONS_PER_UNIT) / l;
+  v->y = (v->y * S3L_FRACTIONS_PER_UNIT) / l;
+  v->z = (v->z * S3L_FRACTIONS_PER_UNIT) / l;
+}
+
 void S3L_initTransoform3D(S3L_Transform3D *t)
 {
   S3L_initVec4(&(t->translation));
diff --git a/testSDL.c b/testSDL.c
index 7c0c946..67e748d 100644
--- a/testSDL.c
+++ b/testSDL.c
@@ -266,26 +266,7 @@ void recomputeLight()
  
   for (int i = 0; i < 127; ++i)
   {
-    S3L_Vec4 v;
-
-    int index = i * 3;
-
-    v.x = houseVertices[index];
-    v.y = houseVertices[index + 1];
-    v.z = houseVertices[index + 2];
-  
-    S3L_vec3Xmat4(&v,m);
-
-    S3L_Unit d = S3L_distanceManhattan(v,scene.camera.transform.translation);
-
-    d = radius - d;
-
-    int l = 0;
-
-    if (d >= 0)
-      l = 255 - S3L_interpolateFrom0(255,d,radius);
-
-    houseVertexLighting[i] = l;
+    houseVertexLighting[i] = 255;
   }
 }
 
@@ -302,8 +283,7 @@ void draw()
   if (f % 16 == 0)
     recomputeLight();
 
-  //scene.models[0].transform.rotation.z = f * 0.1;
-  //scene.models[0].transform.rotation.x = f * 0.3;
+  scene.models[0].transform.translation.x = sin(f / 16.0) * S3L_FRACTIONS_PER_UNIT;
 
   S3L_drawScene(scene);