raycastlib.h

   1 #ifndef RAYCASTLIB_H
   2 #define RAYCASTLIB_H
   3
   4 /**
   5   raycastlib - Small C header-only raycasting library for embedded and low
   6   performance computers, such as Arduino. Only uses integer math and stdint
   7   standard library.
   8
   9   author: Miloslav "drummyfish" Ciz
  10   license: CC0
  11   version: 0.1
  12
  13   - Game field's bottom left corner is at [0,0].
  14   - X axis goes right in the ground plane.
  15   - Y axis goes up in the ground plane.
  16   - Height means the Z (vertical) coordinate.
  17   - Each game square is UNITS_PER_SQUARE * UNITS_PER_SQUARE points.
  18   - Angles are in Units, 0 means pointing right (x+) and positively rotates
  19     clockwise. A full angle has UNITS_PER_SQUARE Units.
  20 */
  21
  22 #include <stdint.h>
  23
  24 #ifndef RAYCAST_TINY
  25   #define UNITS_PER_SQUARE 1024 ///< N. of Units in a side of a spatial square.
  26   typedef int32_t Unit; /**< Smallest spatial unit, there is UNITS_PER_SQUARE
  27                              units in a square's length. This effectively
  28                              serves the purpose of a fixed-point arithmetic. */
  29   #define UNIT_INFINITY 5000000;
  30
  31   typedef int32_t int_maybe32_t;
  32   typedef uint32_t uint_maybe32_t;
  33 #else
  34   #define UNITS_PER_SQUARE 64
  35   typedef int16_t Unit;
  36   #define UNIT_INFINITY 32767;
  37   typedef int16_t int_maybe32_t;
  38   typedef uint16_t uint_maybe32_t;
  39 #endif
  40
  41 #ifndef USE_COS_LUT
  42 #define USE_COS_LUT 0 /**< type of look up table for cos function:
  43                            0: none (compute)
  44                            1: 64 items
  45                            2: 128 items
  46                         */
  47 #endif
  48
  49 #ifndef VERTICAL_FOV
  50 #define VERTICAL_FOV (UNITS_PER_SQUARE / 2)
  51 #endif
  52
  53 #ifndef HORIZONTAL_FOV
  54 #define HORIZONTAL_FOV (UNITS_PER_SQUARE / 4)
  55 #endif
  56
  57 #define HORIZONTAL_FOV_HALF (HORIZONTAL_FOV / 2)
  58
  59 #ifndef CAMERA_COLL_RADIUS
  60 #define CAMERA_COLL_RADIUS UNITS_PER_SQUARE / 4
  61 #endif
  62
  63 #ifndef CAMERA_COLL_HEIGHT_BELOW
  64 #define CAMERA_COLL_HEIGHT_BELOW UNITS_PER_SQUARE
  65 #endif
  66
  67 #ifndef CAMERA_COLL_HEIGHT_ABOVE
  68 #define CAMERA_COLL_HEIGHT_ABOVE (UNITS_PER_SQUARE / 3)
  69 #endif
  70
  71 #ifndef CAMERA_COLL_STEP_HEIGHT
  72 #define CAMERA_COLL_STEP_HEIGHT (UNITS_PER_SQUARE / 2)
  73 #endif
  74
  75 #define logVector2D(v)\
  76   printf("[%d,%d]\n",v.x,v.y);
  77
  78 #define logRay(r){\
  79   printf("ray:\n");\
  80   printf("  start: ");\
  81   logVector2D(r.start);\
  82   printf("  dir: ");\
  83   logVector2D(r.direction);}\
  84
  85 #define logHitResult(h){\
  86   printf("hit:\n");\
  87   printf("  sqaure: ");\
  88   logVector2D(h.square);\
  89   printf("  pos: ");\
  90   logVector2D(h.position);\
  91   printf("  dist: %d\n", h.distance);\
  92   printf("  texcoord: %d\n", h.textureCoord);}\
  93
  94 #define logPixelInfo(p){\
  95   printf("pixel:\n");\
  96   printf("  position: ");\
  97   logVector2D(p.position);\
  98   printf("  depth: %d\n", p.depth);\
  99   printf("  wall: %d\n", p.isWall);\
 100   printf("  textCoordY: %d\n", p.textureCoordY);\
 101   printf("  hit: ");\
 102   logHitResult(p.hit);\
 103   }\
 104
 105 /// Position in 2D space.
 106 typedef struct
 107 {
 108   Unit y;
 109   Unit x;
 110 } Vector2D;
 111
 112 typedef struct
 113 {
 114   Vector2D start;
 115   Vector2D direction;
 116 } Ray;
 117
 118 typedef struct
 119 {
 120   Vector2D square;       ///< Collided square coordinates.
 121   Vector2D position;     ///< Exact collision position in Units.
 122   Unit     distance;     /**< Euclidean distance to the hit position, or -1 if
 123                             no collision happened. */
 124   Unit     textureCoord; ///< Normalized (0 to UNITS_PER_SQUARE - 1) tex coord.
 125   Unit     type;         ///< Integer identifying type of square.
 126   uint8_t  direction;    ///< Direction of hit.
 127 } HitResult;
 128
 129 // TODO: things like FOV could be constants to make them precomp. and faster?
 130
 131 typedef struct
 132 {
 133   Vector2D position;
 134   Unit direction;
 135   Vector2D resolution;
 136   int16_t shear; /* Shear offset in pixels (0 => no shear), can simulate
 137                     looking up/down. */
 138   Unit height;
 139 } Camera;
 140
 141 /**
 142   Holds an information about a single rendered pixel (for a pixel function
 143   that works as a fragment shader.
 144 */
 145 typedef struct
 146 {
 147   Vector2D position;  ///< On-screen position.
 148   int8_t isWall;      ///< Whether the pixel is a wall or a floor/ceiling.
 149   int8_t isFloor;     ///< Whether the pixel is floor or ceiling.
 150   int8_t isHorizon;   ///< Whether the pixel is floor going towards horizon.
 151   Unit depth;         ///< Corrected depth.
 152   HitResult hit;      ///< Corresponding ray hit.
 153   Unit textureCoordY; ///< Normalized (0 to UNITS_PER_SQUARE - 1) tex coord.
 154 } PixelInfo;
 155
 156 typedef struct
 157 {
 158   uint16_t maxHits;
 159   uint16_t maxSteps;
 160   uint8_t computeTextureCoords; ///< Turns texture coords on/off.
 161 } RayConstraints;
 162
 163 /**
 164   Function used to retrieve some information about cells of the rendered scene.
 165   It should return a characteristic of given square as an integer (e.g. square
 166   height, texture index, ...) - between squares that return different numbers
 167   there is considered to be a collision.
 168
 169   This function should be as fast as possible as it will typically be called
 170   very often.
 171 */
 172 typedef Unit (*ArrayFunction)(int16_t x, int16_t y);
 173
 174 /**
 175   Function that renders a single pixel at the display. It is handed an info
 176   about the pixel it should draw.
 177
 178   This function should be as fast as possible as it will typically be called
 179   very often.
 180 */
 181 typedef void (*PixelFunction)(PixelInfo *info);
 182
 183 typedef void
 184   (*ColumnFunction)(HitResult *hits, uint16_t hitCount, uint16_t x, Ray ray);
 185
 186 /**
 187   Simple-interface function to cast a single ray.
 188   @return          The first collision result.
 189 */
 190 HitResult castRay(Ray ray, ArrayFunction arrayFunc);
 191
 192 /**
 193   Maps a single point in the world to the screen (2D position + depth).
 194 */
 195 PixelInfo mapToScreen(Vector2D worldPosition, Unit height, Camera camera);
 196
 197 /**
 198   Casts a single ray and returns a list of collisions.
 199 */
 200 void castRayMultiHit(Ray ray, ArrayFunction arrayFunc, ArrayFunction typeFunc,
 201   HitResult *hitResults, uint16_t *hitResultsLen, RayConstraints constraints);
 202
 203 Vector2D angleToDirection(Unit angle);
 204
 205 /**
 206 Cos function.
 207
 208 @param input to cos in Units (UNITS_PER_SQUARE = 2 * pi = 360 degrees)
 209 @return normalized output in Units (from -UNITS_PER_SQUARE to UNITS_PER_SQUARE)
 210 */
 211 Unit cosInt(Unit input);
 212
 213 Unit sinInt(Unit input);
 214
 215 /// Normalizes given vector to have UNITS_PER_SQUARE length.
 216 Vector2D normalize(Vector2D v);
 217
 218 /// Computes a cos of an angle between two vectors.
 219 Unit vectorsAngleCos(Vector2D v1, Vector2D v2);
 220
 221 uint16_t sqrtInt(uint_maybe32_t value);
 222 Unit dist(Vector2D p1, Vector2D p2);
 223 Unit len(Vector2D v);
 224
 225 /**
 226   Converts an angle in whole degrees to an angle in Units that this library
 227   uses.
 228 */
 229 Unit degreesToUnitsAngle(int16_t degrees);
 230
 231 ///< Computes the change in size of an object due to perspective.
 232 Unit perspectiveScale(Unit originalSize, Unit distance);
 233
 234 /**
 235   Casts rays for given camera view and for each hit calls a user provided
 236   function.
 237 */
 238 void castRaysMultiHit(Camera cam, ArrayFunction arrayFunc,
 239   ArrayFunction typeFunction, ColumnFunction columnFunc,
 240   RayConstraints constraints);
 241
 242 /**
 243   Using provided functions, renders a complete complex camera view.
 244
 245   @param cam camera whose view to render
 246   @param floorHeightFunc function that returns floor height (in Units)
 247   @param ceilingHeightFunc same as floorHeightFunc but for ceiling, can also be
 248                            0 (no ceiling will be rendered)
 249   @param typeFunction function that says a type of square (e.g. its texture
 250                      index), can be 0 (no type in hit result)
 251   @param pixelFunc callback function to draw a single pixel on screen
 252   @param constraints constraints for each cast ray
 253 */
 254 void render(Camera cam, ArrayFunction floorHeightFunc,
 255   ArrayFunction ceilingHeightFunc, ArrayFunction typeFunction,
 256   PixelFunction pixelFunc, RayConstraints constraints);
 257
 258 /**
 259   Renders given camera view, with help of provided functions. This function is
 260   simpler and faster than render(...) and is meant to be rendering scenes
 261   with simple 1-intersection raycasting. The render(...) function can give more
 262   accurate results than this function, so it's to be considered even for simple
 263   scenes.
 264 */
 265 void renderSimple(Camera cam, ArrayFunction floorHeightFunc,
 266   ArrayFunction typeFunc, PixelFunction pixelFunc, RayConstraints constraints);
 267
 268 /**
 269   Function that moves given camera and makes it collide with walls and
 270   potentially also floor and ceilings. It's meant to help implement player
 271   movement.
 272
 273   @param camera camera to move
 274   @param planeOffset offset to move the camera in
 275   @param heightOffset height offset to move the camera in
 276   @param floorHeightFunc function used to retrieve the floor height
 277   @param ceilingHeightFunc function for retrieving ceiling height, can be 0
 278                            (camera won't collide with ceiling)
 279   @param computeHeight whether to compute height - if false (0), floor and
 280                        ceiling functions won't be used and the camera will
 281                        only collide horizontally with walls (good for simpler
 282                        game, also faster)
 283   @param force if true, forces to recompute collision even if position doesn't
 284                change
 285 */
 286 void moveCameraWithCollision(Camera *camera, Vector2D planeOffset,
 287   Unit heightOffset, ArrayFunction floorHeightFunc,
 288   ArrayFunction ceilingHeightFunc, int8_t computeHeight, int8_t force);
 289
 290 //=============================================================================
 291 // privates
 292
 293 #ifdef RAYCASTLIB_PROFILE
 294   // function call counters for profiling
 295   uint32_t profile_sqrtInt = 0;
 296   uint32_t profile_clamp = 0;
 297   uint32_t profile_cosInt = 0;
 298   uint32_t profile_angleToDirection = 0;
 299   uint32_t profile_dist = 0;
 300   uint32_t profile_len = 0;
 301   uint32_t profile_pointIsLeftOfRay = 0;
 302   uint32_t profile_castRaySquare = 0;
 303   uint32_t profile_castRayMultiHit = 0;
 304   uint32_t profile_castRay = 0;
 305   uint32_t profile_absVal = 0;
 306   uint32_t profile_normalize = 0;
 307   uint32_t profile_vectorsAngleCos = 0;
 308   uint32_t profile_perspectiveScale = 0;
 309   uint32_t profile_wrap = 0;
 310   uint32_t profile_divRoundDown = 0;
 311   #define profileCall(c) profile_##c += 1
 312
 313   #define printProfile() {\
 314     printf("profile:\n");\
 315     printf("  sqrtInt: %d\n",profile_sqrtInt);\
 316     printf("  clamp: %d\n",profile_clamp);\
 317     printf("  cosInt: %d\n",profile_cosInt);\
 318     printf("  angleToDirection: %d\n",profile_angleToDirection);\
 319     printf("  dist: %d\n",profile_dist);\
 320     printf("  len: %d\n",profile_len);\
 321     printf("  pointIsLeftOfRay: %d\n",profile_pointIsLeftOfRay);\
 322     printf("  castRaySquare: %d\n",profile_castRaySquare);\
 323     printf("  castRayMultiHit : %d\n",profile_castRayMultiHit);\
 324     printf("  castRay: %d\n",profile_castRay);\
 325     printf("  normalize: %d\n",profile_normalize);\
 326     printf("  vectorsAngleCos: %d\n",profile_vectorsAngleCos);\
 327     printf("  absVal: %d\n",profile_absVal);\
 328     printf("  perspectiveScale: %d\n",profile_perspectiveScale);\
 329     printf("  wrap: %d\n",profile_wrap);\
 330     printf("  divRoundDown: %d\n",profile_divRoundDown); }
 331 #else
 332   #define profileCall(c)
 333 #endif
 334
 335 Unit clamp(Unit value, Unit valueMin, Unit valueMax)
 336 {
 337   profileCall(clamp);
 338
 339   if (value >= valueMin)
 340   {
 341     if (value <= valueMax)
 342       return value;
 343     else
 344       return valueMax;
 345   }
 346   else
 347     return valueMin;
 348 }
 349
 350 Unit absVal(Unit value)
 351 {
 352   profileCall(absVal);
 353
 354   return value >= 0 ? value: -1 * value;
 355 }
 356
 357 /// Like mod, but behaves differently for negative values.
 358 Unit wrap(Unit value, Unit mod)
 359 {
 360   profileCall(wrap);
 361
 362   return value >= 0 ? (value % mod) : (mod + (value % mod) - 1);
 363 }
 364
 365 /// Performs division, rounding down, NOT towards zero.
 366 Unit divRoundDown(Unit value, Unit divisor)
 367 {
 368   profileCall(divRoundDown);
 369
 370   return value / divisor - ((value >= 0) ? 0 : 1);
 371 }
 372
 373 // Bhaskara's cosine approximation formula
 374 #define trigHelper(x) (((Unit) UNITS_PER_SQUARE) *\
 375   (UNITS_PER_SQUARE / 2 * UNITS_PER_SQUARE / 2 - 4 * (x) * (x)) /\
 376   (UNITS_PER_SQUARE / 2 * UNITS_PER_SQUARE / 2 + (x) * (x)))
 377
 378
 379 #if USE_COS_LUT == 1
 380 const Unit cosLUT[64] =
 381 {
 382   1024,1019,1004,979,946,903,851,791,724,649,568,482,391,297,199,100,0,-100,
 383   -199,-297,-391,-482,-568,-649,-724,-791,-851,-903,-946,-979,-1004,-1019,
 384   -1023,-1019,-1004,-979,-946,-903,-851,-791,-724,-649,-568,-482,-391,-297,
 385   -199,-100,0,100,199,297,391,482,568,649,724,791,851,903,946,979,1004,1019
 386 };
 387 #elif USE_COS_LUT == 2
 388 const Unit cosLUT[128] =
 389 {
 390   1024,1022,1019,1012,1004,993,979,964,946,925,903,878,851,822,791,758,724,
 391   687,649,609,568,526,482,437,391,344,297,248,199,150,100,50,0,-50,-100,-150,
 392   -199,-248,-297,-344,-391,-437,-482,-526,-568,-609,-649,-687,-724,-758,-791,
 393   -822,-851,-878,-903,-925,-946,-964,-979,-993,-1004,-1012,-1019,-1022,-1023,
 394   -1022,-1019,-1012,-1004,-993,-979,-964,-946,-925,-903,-878,-851,-822,-791,
 395   -758,-724,-687,-649,-609,-568,-526,-482,-437,-391,-344,-297,-248,-199,-150,
 396   -100,-50,0,50,100,150,199,248,297,344,391,437,482,526,568,609,649,687,724,
 397   758,791,822,851,878,903,925,946,964,979,993,1004,1012,1019,1022
 398 };
 399 #endif
 400
 401 Unit cosInt(Unit input)
 402 {
 403   profileCall(cosInt);
 404
 405   // TODO: could be optimized with LUT
 406
 407   input = wrap(input,UNITS_PER_SQUARE);
 408
 409 #if USE_COS_LUT == 1
 410   return cosLUT[input / 16];
 411 #elif USE_COS_LUT == 2
 412   return cosLUT[input / 8];
 413 #else
 414   if (input < UNITS_PER_SQUARE / 4)
 415     return trigHelper(input);
 416   else if (input < UNITS_PER_SQUARE / 2)
 417     return -1 * trigHelper(UNITS_PER_SQUARE / 2 - input);
 418   else if (input < 3 * UNITS_PER_SQUARE / 4)
 419     return -1 * trigHelper(input - UNITS_PER_SQUARE / 2);
 420   else
 421     return trigHelper(UNITS_PER_SQUARE - input);
 422 #endif
 423 }
 424
 425 #undef trigHelper
 426
 427 Unit sinInt(Unit input)
 428 {
 429   return cosInt(input - UNITS_PER_SQUARE / 4);
 430 }
 431
 432 Vector2D angleToDirection(Unit angle)
 433 {
 434   profileCall(angleToDirection);
 435
 436   Vector2D result;
 437
 438   result.x = cosInt(angle);
 439   result.y = -1 * sinInt(angle);
 440
 441   return result;
 442 }
 443
 444 uint16_t sqrtInt(uint_maybe32_t value)
 445 {
 446   profileCall(sqrtInt);
 447
 448   uint_maybe32_t result = 0;
 449   uint_maybe32_t a = value;
 450
 451 #ifdef RAYCAST_TINY
 452   uint_maybe32_t b = 1u << 14;
 453 #else
 454   uint_maybe32_t b = 1u << 30;
 455 #endif
 456
 457   while (b > a)
 458     b >>= 2;
 459
 460   while (b != 0)
 461   {
 462     if (a >= result + b)
 463     {
 464       a -= result + b;
 465       result = result +  2 * b;
 466     }
 467
 468     b >>= 2;
 469     result >>= 1;
 470   }
 471
 472   return result;
 473 }
 474
 475 Unit dist(Vector2D p1, Vector2D p2)
 476 {
 477   profileCall(dist);
 478
 479   int_maybe32_t dx = p2.x - p1.x;
 480   int_maybe32_t dy = p2.y - p1.y;
 481
 482   dx = dx * dx;
 483   dy = dy * dy;
 484
 485   return sqrtInt((uint_maybe32_t) (dx + dy));
 486 }
 487
 488 Unit len(Vector2D v)
 489 {
 490   profileCall(len);
 491
 492   v.x *= v.x;
 493   v.y *= v.y;
 494
 495   return sqrtInt(((uint_maybe32_t) v.x) + ((uint_maybe32_t) v.y));
 496 }
 497
 498 int8_t pointIsLeftOfRay(Vector2D point, Ray ray)
 499 {
 500   profileCall(pointIsLeftOfRay);
 501
 502   Unit dX    = point.x - ray.start.x;
 503   Unit dY    = point.y - ray.start.y;
 504   return (ray.direction.x * dY - ray.direction.y * dX) > 0;
 505          // ^ Z component of cross-product
 506 }
 507
 508 /**
 509   Casts a ray within a single square, to collide with the square borders.
 510 */
 511 void castRaySquare(Ray *localRay, Vector2D *nextCellOff, Vector2D *collOff)
 512 {
 513   profileCall(castRaySquare);
 514
 515   nextCellOff->x = 0;
 516   nextCellOff->y = 0;
 517
 518   Ray criticalLine;
 519   criticalLine.start = localRay->start;
 520   criticalLine.direction = localRay->direction;
 521
 522   #define helper(c1,c2,n)\
 523     {\
 524       nextCellOff->c1 = n;\
 525       collOff->c1 = criticalLine.start.c1 - localRay->start.c1;\
 526       collOff->c2 = \
 527         (((int_maybe32_t) collOff->c1) * localRay->direction.c2) /\
 528         ((localRay->direction.c1 == 0) ? 1 : localRay->direction.c1);\
 529     }
 530
 531   #define helper2(n1,n2,c)\
 532     if (pointIsLeftOfRay(localRay->start,criticalLine) == c)\
 533       helper(y,x,n1)\
 534     else\
 535       helper(x,y,n2)
 536
 537   if (localRay->direction.x > 0)
 538   {
 539     criticalLine.start.x = UNITS_PER_SQUARE - 1;
 540
 541     if (localRay->direction.y > 0)
 542     {
 543       // top right
 544       criticalLine.start.y = UNITS_PER_SQUARE - 1;
 545       helper2(1,1,1)
 546     }
 547     else
 548     {
 549       // bottom right
 550       criticalLine.start.y = 0;
 551       helper2(-1,1,0)
 552     }
 553   }
 554   else
 555   {
 556     criticalLine.start.x = 0;
 557
 558     if (localRay->direction.y > 0)
 559     {
 560       // top left
 561       criticalLine.start.y = UNITS_PER_SQUARE - 1;
 562       helper2(1,-1,0)
 563     }
 564     else
 565     {
 566       // bottom left
 567       criticalLine.start.y = 0;
 568       helper2(-1,-1,1)
 569     }
 570   }
 571
 572   #undef helper2
 573   #undef helper
 574
 575   collOff->x += nextCellOff->x;
 576   collOff->y += nextCellOff->y;
 577 }
 578
 579 void castRayMultiHit(Ray ray, ArrayFunction arrayFunc, ArrayFunction typeFunc,
 580   HitResult *hitResults, uint16_t *hitResultsLen, RayConstraints constraints)
 581 {
 582   profileCall(castRayMultiHit);
 583
 584   Vector2D initialPos = ray.start;
 585   Vector2D currentPos = ray.start;
 586
 587   Vector2D currentSquare;
 588
 589   currentSquare.x = divRoundDown(ray.start.x, UNITS_PER_SQUARE);
 590   currentSquare.y = divRoundDown(ray.start.y,UNITS_PER_SQUARE);
 591
 592   *hitResultsLen = 0;
 593
 594   Unit squareType = arrayFunc(currentSquare.x,currentSquare.y);
 595
 596   Vector2D no, co; // next cell offset, collision offset
 597
 598   no.x = 0;        // just to supress a warning
 599   no.y = 0;
 600   co.x = 0;
 601   co.y = 0;
 602
 603   for (uint16_t i = 0; i < constraints.maxSteps; ++i)
 604   {
 605     Unit currentType = arrayFunc(currentSquare.x,currentSquare.y);
 606
 607     if (currentType != squareType)
 608     {
 609       // collision
 610
 611       HitResult h;
 612
 613       h.position = currentPos;
 614       h.square   = currentSquare;
 615       h.distance = dist(initialPos,currentPos);
 616
 617       if (typeFunc != 0)
 618         h.type = typeFunc(currentSquare.x,currentSquare.y);
 619
 620       if (no.y > 0)
 621       {
 622         h.direction = 0;
 623         h.textureCoord = constraints.computeTextureCoords ?
 624           wrap(currentPos.x,UNITS_PER_SQUARE) : 0;
 625       }
 626       else if (no.x > 0)
 627       {
 628         h.direction = 1;
 629         h.textureCoord = constraints.computeTextureCoords ?
 630           wrap(UNITS_PER_SQUARE - currentPos.y,UNITS_PER_SQUARE) : 0;
 631       }
 632       else if (no.y < 0)
 633       {
 634         h.direction = 2;
 635         h.textureCoord = constraints.computeTextureCoords ?
 636           wrap(UNITS_PER_SQUARE - currentPos.x,UNITS_PER_SQUARE) : 0;
 637       }
 638       else
 639       {
 640         h.direction = 3;
 641         h.textureCoord = constraints.computeTextureCoords ?
 642           wrap(currentPos.y,UNITS_PER_SQUARE) : 0;
 643       }
 644
 645       hitResults[*hitResultsLen] = h;
 646
 647       *hitResultsLen += 1;
 648
 649       squareType = currentType;
 650
 651       if (*hitResultsLen >= constraints.maxHits)
 652         break;
 653     }
 654
 655     ray.start.x = wrap(currentPos.x,UNITS_PER_SQUARE);
 656     ray.start.y = wrap(currentPos.y,UNITS_PER_SQUARE);
 657
 658     castRaySquare(&ray,&no,&co);
 659
 660     currentSquare.x += no.x;
 661     currentSquare.y += no.y;
 662
 663     // offset into the next cell
 664     currentPos.x += co.x;
 665     currentPos.y += co.y;
 666   }
 667 }
 668
 669 HitResult castRay(Ray ray, ArrayFunction arrayFunc)
 670 {
 671   profileCall(castRay);
 672
 673   HitResult result;
 674   uint16_t  len;
 675   RayConstraints c;
 676
 677   c.maxSteps = 1000;
 678   c.maxHits = 1;
 679
 680   castRayMultiHit(ray,arrayFunc,0,&result,&len,c);
 681
 682   if (len == 0)
 683     result.distance = -1;
 684
 685   return result;
 686 }
 687
 688 void castRaysMultiHit(Camera cam, ArrayFunction arrayFunc,
 689   ArrayFunction typeFunction, ColumnFunction columnFunc,
 690   RayConstraints constraints)
 691 {
 692   Vector2D dir1 = angleToDirection(cam.direction - HORIZONTAL_FOV_HALF);
 693   Vector2D dir2 = angleToDirection(cam.direction + HORIZONTAL_FOV_HALF);
 694
 695   Unit dX = dir2.x - dir1.x;
 696   Unit dY = dir2.y - dir1.y;
 697
 698   HitResult hits[constraints.maxHits];
 699   uint16_t hitCount;
 700
 701   Ray r;
 702   r.start = cam.position;
 703
 704   for (uint16_t i = 0; i < cam.resolution.x; ++i)
 705   {
 706     r.direction.x = dir1.x + (dX * i) / cam.resolution.x;
 707     r.direction.y = dir1.y + (dY * i) / cam.resolution.x;
 708
 709     castRayMultiHit(r,arrayFunc,typeFunction,hits,&hitCount,constraints);
 710
 711     columnFunc(hits,hitCount,i,r);
 712   }
 713 }
 714
 715 // global helper variables, for precomputing stuff etc.
 716 PixelFunction _pixelFunction = 0;
 717 Camera _camera;
 718 Unit _floorDepthStep = 0;
 719 Unit _startFloorHeight = 0;
 720 Unit _startCeilHeight = 0;
 721 int_maybe32_t _camResYLimit = 0;
 722 Unit _middleRow = 0;
 723 ArrayFunction _floorFunction = 0;
 724 ArrayFunction _ceilFunction = 0;
 725 uint8_t _computeTextureCoords = 0;
 726 Unit _fogStartYBottom = 0;
 727 Unit _fogStartYTop = 0;
 728 int16_t _cameraHeightScreen = 0;
 729
 730 /**
 731   Helper function that determines intersection with both ceiling and floor.
 732 */
 733 Unit _floorCeilFunction(int16_t x, int16_t y)
 734 {
 735   // TODO: adjust also for RAYCAST_TINY
 736
 737   Unit f = _floorFunction(x,y);
 738
 739   if (_ceilFunction == 0)
 740     return f;
 741
 742   Unit c = _ceilFunction(x,y);
 743
 744   return ((f & 0x0000ffff) << 16) | (c & 0x0000ffff);
 745 }
 746
 747 Unit adjustDistance(Unit distance, Camera *camera, Ray *ray)
 748 {
 749   /* FIXME/TODO: The adjusted (=orthogonal, camera-space) distance could
 750      possibly be computed more efficiently by not computing Euclidean
 751      distance at all, but rather compute the distance of the collision
 752      point from the projection plane (line). */
 753
 754   Unit result =
 755     (distance *
 756      vectorsAngleCos(angleToDirection(camera->direction),ray->direction)) /
 757      UNITS_PER_SQUARE;
 758
 759   return result == 0 ? 1 : result;
 760                 // ^ prevent division by zero
 761 }
 762
 763 void _columnFunction(HitResult *hits, uint16_t hitCount, uint16_t x, Ray ray)
 764 {
 765   int_maybe32_t y = _camResYLimit; // screen y (for floor), will only go up
 766   int_maybe32_t y2 = 0;            // screen y (for ceil), will only fo down
 767
 768   Unit worldZPrev = _startFloorHeight;
 769   Unit worldZPrevCeil = _startCeilHeight;
 770
 771   PixelInfo p;
 772   p.position.x = x;
 773
 774   #define VERTICAL_DEPTH_MULTIPLY 2
 775
 776   // we'll be simulatenously drawing the floor and the ceiling now
 777   for (uint_maybe32_t j = 0; j < hitCount; ++j)
 778   {
 779     HitResult hit = hits[j];
 780
 781     Unit dist = adjustDistance(hit.distance,&_camera,&ray);
 782
 783     Unit wallHeight = _floorFunction(hit.square.x,hit.square.y);
 784
 785     Unit worldZ2 = wallHeight - _camera.height;
 786
 787     int_maybe32_t z1Screen = _middleRow - perspectiveScale(
 788       (worldZPrev * _camera.resolution.y) / UNITS_PER_SQUARE,dist);
 789
 790     int_maybe32_t z2Screen = _middleRow - perspectiveScale(
 791       (worldZ2 * _camera.resolution.y) / UNITS_PER_SQUARE,dist);
 792
 793     int8_t skipFloorWall = ((z1Screen < 0 && z2Screen < 0) ||
 794       (z1Screen > _camResYLimit && z2Screen > _camResYLimit));
 795
 796     int_maybe32_t z1ScreenNoClamp = z1Screen;
 797
 798     z1Screen = clamp(z1Screen,0,_camResYLimit);
 799     z1Screen = z1Screen > y2 ? z1Screen : y2;
 800
 801     int_maybe32_t wallScreenHeightNoClamp = z1ScreenNoClamp - z2Screen + 1;
 802     wallScreenHeightNoClamp = wallScreenHeightNoClamp == 0 ? 1 :
 803       wallScreenHeightNoClamp;
 804
 805     z2Screen = clamp(z2Screen,0,_camResYLimit);
 806     z2Screen = z2Screen > y2 ? z2Screen : y2;
 807
 808     int_maybe32_t zTop = z1Screen < z2Screen ? z1Screen : z2Screen;
 809
 810     // make the same variables for ceiling
 811
 812     Unit wallHeightCeil = 0;
 813     Unit worldZ2Ceil = 0;
 814     int_maybe32_t z1ScreenCeil = 0;
 815     int_maybe32_t z1ScreenCeilNoClamp = 0;
 816     int_maybe32_t z2ScreenCeil = 0;
 817     int_maybe32_t wallScreenHeightCeilNoClamp = 0;
 818     int_maybe32_t zBottomCeil = y2;
 819     int8_t skipCeilingWall = 1;
 820
 821     if (_ceilFunction != 0)
 822     {
 823       wallHeightCeil = _ceilFunction != 0 ?
 824         _ceilFunction(hit.square.x,hit.square.y) : 0;
 825
 826       worldZ2Ceil = wallHeightCeil - _camera.height;
 827
 828       z1ScreenCeil = _middleRow - perspectiveScale(
 829         (worldZPrevCeil * _camera.resolution.y) / UNITS_PER_SQUARE,dist);
 830
 831       z2ScreenCeil = _middleRow - perspectiveScale(
 832         (worldZ2Ceil * _camera.resolution.y) / UNITS_PER_SQUARE,dist);
 833
 834       skipCeilingWall = ((z1ScreenCeil < 0 && z2ScreenCeil < 0) ||
 835         (z1ScreenCeil > _camResYLimit && z2ScreenCeil > _camResYLimit));
 836
 837       z1ScreenCeilNoClamp = z1ScreenCeil;
 838
 839       z1ScreenCeil = clamp(z1ScreenCeil,0,_camResYLimit);
 840       z1ScreenCeil = z1ScreenCeil < y ? z1ScreenCeil : y;
 841
 842       wallScreenHeightCeilNoClamp =
 843         z2ScreenCeil - z1ScreenCeilNoClamp;
 844
 845       wallScreenHeightCeilNoClamp = wallScreenHeightCeilNoClamp != 0 ?
 846         wallScreenHeightCeilNoClamp : 1;
 847
 848       z2ScreenCeil = clamp(z2ScreenCeil,0,_camResYLimit);
 849       z2ScreenCeil = z2ScreenCeil < y ? z2ScreenCeil : y;
 850
 851       zBottomCeil = z1ScreenCeil > z2ScreenCeil ?
 852         z1ScreenCeil : z2ScreenCeil;
 853     }
 854
 855     if (zTop <= zBottomCeil)
 856       zBottomCeil = zTop; // walls on ceiling and floor met
 857
 858     // draw floor until wall
 859
 860     p.isWall = 0;
 861     p.isFloor = 1;
 862     p.isHorizon = 0;
 863
 864     Unit floorCameraDiff = absVal(worldZPrev) * VERTICAL_DEPTH_MULTIPLY;
 865
 866     for (int_maybe32_t i = y; i > z1Screen; --i)
 867     {
 868       p.position.y = i;
 869       p.depth = (_fogStartYBottom - i) * _floorDepthStep + floorCameraDiff;
 870       _pixelFunction(&p);
 871     }
 872
 873     if (z1Screen < y)
 874       y = z1Screen;
 875
 876     // draw ceiling until wall
 877
 878     p.isFloor = 0;
 879
 880     if (_ceilFunction != 0)
 881     {
 882       Unit ceilCameraDiff = absVal(worldZPrevCeil) * VERTICAL_DEPTH_MULTIPLY;
 883
 884       for (int_maybe32_t i = y2; i < z1ScreenCeil; ++i)
 885       {
 886         p.position.y = i;
 887         p.depth = (i - _fogStartYTop) * _floorDepthStep + ceilCameraDiff;
 888         _pixelFunction(&p);
 889       }
 890     }
 891
 892     if (z1ScreenCeil > y2)
 893       y2 = z1ScreenCeil;
 894
 895     // draw floor wall
 896
 897     p.isWall = 1;
 898     p.depth = dist;
 899     p.isFloor = 1;
 900
 901     int_maybe32_t iTo;
 902
 903     if (!skipFloorWall)
 904     {
 905       iTo = y2 < zTop ? zTop : y2;
 906
 907       for (int_maybe32_t i = y; i >= iTo; --i)
 908       {
 909         p.position.y = i;
 910         p.hit = hit;
 911
 912         if (_computeTextureCoords)
 913           p.textureCoordY = UNITS_PER_SQUARE - 1 - ((z1ScreenNoClamp - i) *
 914             UNITS_PER_SQUARE) / wallScreenHeightNoClamp;
 915
 916         _pixelFunction(&p);
 917       }
 918     }
 919
 920     // draw ceiling wall
 921
 922     p.isFloor = 0;
 923
 924     if (!skipCeilingWall)
 925     {
 926       iTo = y > zBottomCeil ? zBottomCeil : y;
 927
 928       for (int_maybe32_t i = y2; i < iTo; ++i)
 929       {
 930         p.position.y = i;
 931         p.hit = hit;
 932
 933         if (_computeTextureCoords)
 934           p.textureCoordY = ((i - z1ScreenCeilNoClamp) *
 935             UNITS_PER_SQUARE) / wallScreenHeightCeilNoClamp;
 936
 937         _pixelFunction(&p);
 938       }
 939     }
 940
 941     y = y > zTop ? zTop : y;
 942     worldZPrev = worldZ2;
 943
 944     y2 = y2 < zBottomCeil ? zBottomCeil : y2;
 945     worldZPrevCeil = worldZ2Ceil;
 946
 947     if (y <= y2)
 948       break; // walls on ceiling and floor met
 949   }
 950
 951   // draw floor until horizon
 952
 953   p.isWall = 0;
 954   p.isFloor = 1;
 955   p.isHorizon = 1;
 956
 957   Unit floorCameraDiff = absVal(worldZPrev) * VERTICAL_DEPTH_MULTIPLY;
 958   Unit horizon = (y2 < _middleRow || _ceilFunction == 0) ? _middleRow : y2;
 959   horizon = clamp(horizon,0,_camera.resolution.y);
 960
 961   horizon += horizon > y2 ? 0 : 1; // bug workaround
 962
 963   for (int_maybe32_t i = y; i >= horizon; --i)
 964   {
 965     p.position.y = i;
 966     p.depth = (_fogStartYBottom - i) * _floorDepthStep + floorCameraDiff;
 967     _pixelFunction(&p);
 968   }
 969
 970   y = horizon < y ? horizon : y;
 971
 972   // draw ceiling until horizon
 973
 974   p.isFloor = 0;
 975
 976   Unit ceilCameraDiff =
 977     _ceilFunction != 0 ?
 978     absVal(worldZPrevCeil) * VERTICAL_DEPTH_MULTIPLY : UNITS_PER_SQUARE;
 979
 980   for (int_maybe32_t i = y2; i < y; ++i)
 981   {
 982     p.position.y = i;
 983     p.depth = (i - _fogStartYTop) * _floorDepthStep + ceilCameraDiff;
 984     _pixelFunction(&p);
 985   }
 986
 987   #undef VERTICAL_DEPTH_MULTIPLY
 988 }
 989
 990 void _columnFunctionSimple(HitResult *hits, uint16_t hitCount, uint16_t x,
 991   Ray ray)
 992 {
 993   int16_t y = 0;
 994   int16_t wallScreenHeight = 0;
 995   int16_t coordHelper = 0;
 996   int16_t wallStart = _middleRow;
 997   int16_t wallEnd = _middleRow;
 998   int16_t heightOffset = 0;
 999
1000   Unit dist = 1;
1001
1002   PixelInfo p;
1003   p.position.x = x;
1004
1005   if (hitCount > 0)
1006   {
1007     HitResult hit = hits[0];
1008     p.hit = hit;
1009     dist = adjustDistance(hit.distance,&_camera,&ray);
1010     int16_t wallHeightWorld = _floorFunction(hit.square.x,hit.square.y);
1011     wallScreenHeight = perspectiveScale((wallHeightWorld *
1012       _camera.resolution.y) / UNITS_PER_SQUARE,dist);
1013
1014     heightOffset = perspectiveScale(_cameraHeightScreen,dist);
1015
1016     wallStart = _middleRow - wallScreenHeight / 2 + heightOffset;
1017
1018     coordHelper = -1 * wallStart;
1019     coordHelper = coordHelper >= 0 ? coordHelper : 0;
1020
1021     wallStart = clamp(wallStart,0,_camResYLimit);
1022     wallEnd = clamp(wallStart + wallScreenHeight,0,_camResYLimit);
1023   }
1024
1025   // draw ceiling
1026
1027   p.isWall = 0;
1028   p.isFloor = 0;
1029   p.isHorizon = 1;
1030   p.depth = 1;
1031
1032   while (y < wallStart)
1033   {
1034     p.position.y = y;
1035     _pixelFunction(&p);
1036     ++y;
1037     p.depth += _floorDepthStep;
1038   }
1039
1040   // draw wall
1041
1042   p.isWall = 1;
1043   p.isFloor = 1;
1044   p.depth = dist;
1045
1046   while (y < wallEnd)
1047   {
1048     p.position.y = y;
1049
1050     if (_computeTextureCoords)
1051       p.textureCoordY = (coordHelper * UNITS_PER_SQUARE) / wallScreenHeight;
1052
1053     _pixelFunction(&p);
1054     ++y;
1055     ++coordHelper;
1056   }
1057
1058   // draw floor
1059
1060   p.isWall = 0;
1061   p.depth = _middleRow * _floorDepthStep;
1062
1063   while (y < _camera.resolution.y)
1064   {
1065     p.position.y = y;
1066     _pixelFunction(&p);
1067     ++y;
1068     p.depth -= _floorDepthStep;
1069   }
1070 }
1071
1072 void render(Camera cam, ArrayFunction floorHeightFunc,
1073   ArrayFunction ceilingHeightFunc, ArrayFunction typeFunction,
1074   PixelFunction pixelFunc, RayConstraints constraints)
1075 {
1076   _pixelFunction = pixelFunc;
1077   _floorFunction = floorHeightFunc;
1078   _ceilFunction = ceilingHeightFunc;
1079   _camera = cam;
1080   _camResYLimit = cam.resolution.y - 1;
1081
1082   int16_t halfResY = cam.resolution.y / 2;
1083
1084   _middleRow = halfResY + cam.shear;
1085
1086   _fogStartYBottom = _middleRow + halfResY;
1087   _fogStartYTop = _middleRow - halfResY;
1088
1089   _computeTextureCoords = constraints.computeTextureCoords;
1090
1091   _startFloorHeight = floorHeightFunc(
1092     divRoundDown(cam.position.x,UNITS_PER_SQUARE),
1093     divRoundDown(cam.position.y,UNITS_PER_SQUARE)) -1 * cam.height;
1094
1095   _startCeilHeight =
1096     ceilingHeightFunc != 0 ?
1097       ceilingHeightFunc(
1098         divRoundDown(cam.position.x,UNITS_PER_SQUARE),
1099         divRoundDown(cam.position.y,UNITS_PER_SQUARE)) -1 * cam.height
1100       : UNIT_INFINITY;
1101
1102   // TODO
1103   _floorDepthStep = (12 * UNITS_PER_SQUARE) / cam.resolution.y;
1104
1105   castRaysMultiHit(cam,_floorCeilFunction,typeFunction,
1106     _columnFunction,constraints);
1107 }
1108
1109 void renderSimple(Camera cam, ArrayFunction floorHeightFunc,
1110   ArrayFunction typeFunc, PixelFunction pixelFunc, RayConstraints constraints)
1111 {
1112   _pixelFunction = pixelFunc;
1113   _floorFunction = floorHeightFunc;
1114   _camera = cam;
1115   _camResYLimit = cam.resolution.y - 1;
1116   _middleRow = cam.resolution.y / 2;
1117   _computeTextureCoords = constraints.computeTextureCoords;
1118
1119   _cameraHeightScreen =
1120     (_camera.resolution.y * (_camera.height - UNITS_PER_SQUARE)) /
1121     UNITS_PER_SQUARE;
1122
1123   // TODO
1124   _floorDepthStep = (12 * UNITS_PER_SQUARE) / cam.resolution.y;
1125
1126   constraints.maxHits = 1;
1127
1128   castRaysMultiHit(cam,_floorFunction,typeFunc,_columnFunctionSimple,
1129     constraints);
1130 }
1131
1132 Vector2D normalize(Vector2D v)
1133 {
1134   profileCall(normalize);
1135
1136   Vector2D result;
1137
1138   Unit l = len(v);
1139
1140   l = l != 0 ? l : 1;
1141
1142   result.x = (v.x * UNITS_PER_SQUARE) / l;
1143   result.y = (v.y * UNITS_PER_SQUARE) / l;
1144
1145   return result;
1146 }
1147
1148 Unit vectorsAngleCos(Vector2D v1, Vector2D v2)
1149 {
1150   profileCall(vectorsAngleCos);
1151
1152   v1 = normalize(v1);
1153   v2 = normalize(v2);
1154
1155   return (v1.x * v2.x + v1.y * v2.y) / UNITS_PER_SQUARE;
1156 }
1157
1158 PixelInfo mapToScreen(Vector2D worldPosition, Unit height, Camera camera)
1159 {
1160   // TODO: precompute some stuff that's constant in the frame
1161
1162   PixelInfo result;
1163
1164   Unit d = dist(worldPosition,camera.position);
1165
1166   Vector2D toPoint;
1167
1168   toPoint.x = worldPosition.x - camera.position.x;
1169   toPoint.y = worldPosition.y - camera.position.y;
1170
1171   Vector2D cameraDir = angleToDirection(camera.direction);
1172
1173   result.depth = // adjusted distance
1174     (d * vectorsAngleCos(cameraDir,toPoint)) / UNITS_PER_SQUARE;
1175
1176   result.position.y = camera.resolution.y / 2 -
1177     (camera.resolution.y *
1178      perspectiveScale(height - camera.height,result.depth)) / UNITS_PER_SQUARE
1179     + camera.shear;
1180
1181   Unit middleColumn = camera.resolution.x / 2;
1182
1183   Unit a = sqrtInt(d * d - result.depth * result.depth);
1184
1185   Ray r;
1186   r.start = camera.position;
1187   r.direction = cameraDir;
1188
1189   if (!pointIsLeftOfRay(worldPosition,r))
1190     a *= -1;
1191
1192   Unit cos = cosInt(HORIZONTAL_FOV_HALF);
1193
1194   Unit b =
1195     (result.depth * sinInt(HORIZONTAL_FOV_HALF)) / (cos == 0 ? 1 : cos);
1196     // sin/cos = tan
1197
1198   result.position.x = (a * middleColumn) / (b == 0 ? 1 : b);
1199   result.position.x = middleColumn - result.position.x;
1200
1201   return result;
1202 }
1203
1204 Unit degreesToUnitsAngle(int16_t degrees)
1205 {
1206   return (degrees * UNITS_PER_SQUARE) / 360;
1207 }
1208
1209 Unit perspectiveScale(Unit originalSize, Unit distance)
1210 {
1211   profileCall(perspectiveScale);
1212
1213   return distance != 0 ?
1214    (originalSize * UNITS_PER_SQUARE) /
1215       ((VERTICAL_FOV * 2 * distance) / UNITS_PER_SQUARE)
1216    : 0;
1217 }
1218
1219 void moveCameraWithCollision(Camera *camera, Vector2D planeOffset,
1220   Unit heightOffset, ArrayFunction floorHeightFunc,
1221   ArrayFunction ceilingHeightFunc, int8_t computeHeight, int8_t force)
1222 {
1223   // TODO: have the cam coll parameters precomputed as macros? => faster
1224
1225   int8_t movesInPlane = planeOffset.x != 0 || planeOffset.y != 0;
1226   int16_t xSquareNew, ySquareNew;
1227
1228   if (movesInPlane || force)
1229   {
1230     Vector2D corner; // BBox corner in the movement direction
1231     Vector2D cornerNew;
1232
1233     int16_t xDir = planeOffset.x > 0 ? 1 : (planeOffset.x < 0 ? -1 : 0);
1234     int16_t yDir = planeOffset.y > 0 ? 1 : (planeOffset.y < 0 ? -1 : 0);
1235
1236     corner.x = camera->position.x + xDir * CAMERA_COLL_RADIUS;
1237     corner.y = camera->position.y + yDir * CAMERA_COLL_RADIUS;
1238
1239     int16_t xSquare = divRoundDown(corner.x,UNITS_PER_SQUARE);
1240     int16_t ySquare = divRoundDown(corner.y,UNITS_PER_SQUARE);
1241
1242     cornerNew.x = corner.x + planeOffset.x;
1243     cornerNew.y = corner.y + planeOffset.y;
1244
1245     xSquareNew = divRoundDown(cornerNew.x,UNITS_PER_SQUARE);
1246     ySquareNew = divRoundDown(cornerNew.y,UNITS_PER_SQUARE);
1247
1248     Unit bottomLimit = camera->height - CAMERA_COLL_HEIGHT_BELOW +
1249       CAMERA_COLL_STEP_HEIGHT;
1250     Unit topLimit = camera->height + CAMERA_COLL_HEIGHT_ABOVE;
1251
1252     // checks a single square for collision against the camera
1253     #define collCheck(dir,s1,s2)\
1254     if (computeHeight)\
1255     {\
1256       Unit height = floorHeightFunc(s1,s2);\
1257       if (height > bottomLimit)\
1258         dir##Collides = 1;\
1259       else if (ceilingHeightFunc != 0)\
1260       {\
1261         height = ceilingHeightFunc(s1,s2);\
1262         if (height < topLimit)\
1263           dir##Collides = 1;\
1264       }\
1265     }\
1266     else\
1267       dir##Collides = floorHeightFunc(s1,s2) > CAMERA_COLL_STEP_HEIGHT;
1268
1269     // check a collision against non-diagonal square
1270     #define collCheckOrtho(dir,dir2,s1,s2,x)\
1271     if (dir##SquareNew != dir##Square)\
1272       collCheck(dir,s1,s2)\
1273     if (!dir##Collides)\
1274     { /* now also check for coll on the neighbouring square */ \
1275       int16_t dir2##Square2 = divRoundDown(corner.dir2 - dir2##Dir *\
1276         CAMERA_COLL_RADIUS * 2,UNITS_PER_SQUARE);\
1277       if (dir2##Square2 != dir2##Square)\
1278       {\
1279         if (x)\
1280           collCheck(dir,dir##SquareNew,dir2##Square2)\
1281         else\
1282           collCheck(dir,dir2##Square2,dir##SquareNew)\
1283       }\
1284     }
1285
1286     int8_t xCollides = 0;
1287     collCheckOrtho(x,y,xSquareNew,ySquare,1)
1288
1289     int8_t yCollides = 0;
1290     collCheckOrtho(y,x,xSquare,ySquareNew,0)
1291
1292     #define collHandle(dir)\
1293     if (dir##Collides)\
1294       cornerNew.dir = (dir##Square) * UNITS_PER_SQUARE + UNITS_PER_SQUARE / 2\
1295       + dir##Dir * (UNITS_PER_SQUARE / 2) - dir##Dir;\
1296
1297     if (!xCollides && !yCollides) /* if non-diagonal collision happend, corner
1298                                      collision can't happen */
1299     {
1300       if (xSquare != xSquareNew && ySquare != ySquareNew) // corner?
1301       {
1302         int8_t xyCollides = 0;
1303         collCheck(xy,xSquareNew,ySquareNew)
1304
1305         if (xyCollides)
1306         {
1307           // normally should slide, but let's KISS
1308           cornerNew = corner;
1309         }
1310       }
1311     }
1312
1313     collHandle(x)
1314     collHandle(y)
1315
1316     #undef collCheck
1317     #undef collHandle
1318
1319     camera->position.x = cornerNew.x - xDir * CAMERA_COLL_RADIUS;
1320     camera->position.y = cornerNew.y - yDir * CAMERA_COLL_RADIUS;
1321   }
1322
1323   if (computeHeight && (movesInPlane || heightOffset != 0 || force))
1324   {
1325     camera->height += heightOffset;
1326
1327     int16_t xSquare1 =
1328       divRoundDown(camera->position.x - CAMERA_COLL_RADIUS,UNITS_PER_SQUARE);
1329     int16_t xSquare2 =
1330       divRoundDown(camera->position.x + CAMERA_COLL_RADIUS,UNITS_PER_SQUARE);
1331     int16_t ySquare1 =
1332       divRoundDown(camera->position.y - CAMERA_COLL_RADIUS,UNITS_PER_SQUARE);
1333     int16_t ySquare2 =
1334       divRoundDown(camera->position.y + CAMERA_COLL_RADIUS,UNITS_PER_SQUARE);
1335
1336     Unit bottomLimit = floorHeightFunc(xSquare1,ySquare1);
1337     Unit topLimit = ceilingHeightFunc != 0 ?
1338       ceilingHeightFunc(xSquare1,ySquare1) : UNIT_INFINITY;
1339
1340     Unit height;
1341
1342     #define checkSquares(s1,s2)\
1343     {\
1344       height = floorHeightFunc(xSquare##s1,ySquare##s2);\
1345       bottomLimit = bottomLimit < height ? height : bottomLimit;\
1346       height = ceilingHeightFunc != 0 ?\
1347         ceilingHeightFunc(xSquare##s1,ySquare##s2) : UNIT_INFINITY;\
1348       topLimit = topLimit > height ? height : topLimit;\
1349     }
1350
1351     if (xSquare2 != xSquare1)
1352       checkSquares(2,1)
1353
1354     if (ySquare2 != ySquare1)
1355       checkSquares(1,2)
1356
1357     if (xSquare2 != xSquare1 && ySquare2 != ySquare1)
1358       checkSquares(2,2)
1359
1360     camera->height = clamp(camera->height,
1361       bottomLimit + CAMERA_COLL_HEIGHT_BELOW,
1362       topLimit - CAMERA_COLL_HEIGHT_ABOVE);
1363     #undef checkSquares
1364   }
1365 }
1366
1367 #endif