diff --git a/demo1.cpp b/demo1.cpp
index e9d16ea..7206ae9 100644
--- a/demo1.cpp
+++ b/demo1.cpp
@@ -727,16 +727,7 @@ inline void pixelFunc(PixelInfo *pixel)
           rgbToIndex(intensity/2,intensity,intensity/3) :
           rgbToIndex(intensity,intensity/2,0);
 
-  uint8_t *buf = pokitto.display.screenbuffer;
-
-  buf += pixel->position.x * SUBSAMPLE;
-  buf += pixel->position.y * SCREEN_WIDTH;
-
-  #pragma unroll
-  for (uint8_t i = 0; i < SUBSAMPLE - 1; ++i)
-    *buf++ = c;
-
-  *buf = c;
+  putSubsampledPixel // macro
 }
 
 void draw()
@@ -777,6 +768,20 @@ void draw()
 
     previousDepth = pos.depth;
   }
+
+  // uncomment for debuggin camera
+
+  /*
+  pokitto.display.setColor(255);
+  pokitto.display.setCursor(1,1);
+  pokitto.display.print(player.mCamera.position.x);
+  pokitto.display.print(" ");
+  pokitto.display.print(player.mCamera.position.y);
+  pokitto.display.print(" ");
+  pokitto.display.print(player.mCamera.height);
+  pokitto.display.print(" ");
+  pokitto.display.print(player.mCamera.direction);
+  */
 }
 
 bool runReleased = false; // helper for detecting switching between walk/run
@@ -786,7 +791,7 @@ int main()
   initGeneral();
 
   player.setPositionSquare(6,4);
-  player.mCamera.direction = 256;
+player.setPosition(7119,14343,5120,566);
 
   sprites[0] = Sprite(sprite1,10,5,1,100);
   sprites[1] = Sprite(sprite1,14,5,1,100);
diff --git a/demo2.cpp b/demo2.cpp
index 1cb6573..a146c67 100644
--- a/demo2.cpp
+++ b/demo2.cpp
@@ -798,16 +798,7 @@ inline void pixelFunc(PixelInfo *pixel)
     }
   }
 
-  uint8_t *buf = pokitto.display.screenbuffer;
-
-  buf += pixel->position.x * SUBSAMPLE;
-  buf += pixel->position.y * pokitto.display.width;
-
-  #pragma unroll
-  for (uint8_t i = 0; i < SUBSAMPLE - 1; ++i)
-    *buf++ = c;
-
-  *buf = c;
+  putSubsampledPixel // macro
 }
 
 void draw()
diff --git a/demo3.cpp b/demo3.cpp
index ae304a9..3bd2d05 100644
--- a/demo3.cpp
+++ b/demo3.cpp
@@ -230,16 +230,7 @@ inline void pixelFunc(PixelInfo *pixel)
   if (intensity != 0)
     c = addIntensity(c,intensity);
 
-  uint8_t *buf = pokitto.display.screenbuffer;
-
-  buf += pixel->position.x * SUBSAMPLE;
-  buf += pixel->position.y * SCREEN_WIDTH;
-
-  #pragma unroll
-  for (uint8_t i = 0; i < SUBSAMPLE - 1; ++i)
-    *buf++ = c;
-
-  *buf = c;
+  putSubsampledPixel // macro
 }
 
 bool flyBy = true;
diff --git a/general.hpp b/general.hpp
index 9052b75..fac169a 100644
--- a/general.hpp
+++ b/general.hpp
@@ -67,6 +67,22 @@ Unit zBuffer[SUBSAMPLED_WIDTH]; ///< 1D z-buffer for visibility determination.
 
 unsigned short palette[256];
 
+// helper macro for fast pixel drawing
+#ifdef POK_SIM
+  #define putSubsampledPixel\
+    pokitto.display.drawPixel(pixel->position.x * SUBSAMPLE,pixel->position.y,c);\
+    pokitto.display.drawPixel(pixel->position.x * SUBSAMPLE + 1,pixel->position.y,c);
+#else
+  // this code breaks the simulator
+  #define putSubsampledPixel\
+    uint8_t *buf = pokitto.display.screenbuffer;\
+    buf += pixel->position.x * SUBSAMPLE;\
+    buf += pixel->position.y * SCREEN_WIDTH;\
+    for (uint8_t i = 0; i < SUBSAMPLE - 1; ++i)\
+      *buf++ = c;\
+    *buf = c;
+#endif
+
 /**
   Gets (the index of) color by specified RGB components. 
 
@@ -250,6 +266,14 @@ public:
     mCamera.position.y = y;
   }
 
+  void setPosition(Unit x, Unit y, Unit z, Unit direction)
+  {
+    mCamera.position.x = x;
+    mCamera.position.y = y;
+    mCamera.height = z;
+    mCamera.direction = direction;
+  }
+
   void setPositionSquare(int16_t squareX, int16_t squareY)
   {
     setPosition(
@@ -295,7 +319,7 @@ public:
     Unit prevHeight = mCamera.height;
 
     moveCameraWithCollision(&mCamera,moveOffset,mVericalSpeed,
-        floorHeightFunction, ceilingHeightFunction, computeHeight ? 1 : 0);
+        floorHeightFunction, ceilingHeightFunction, computeHeight ? 1 : 0, 0);
 
     Unit heightDiff = mCamera.height - prevHeight;