raycastlib.h

00001 #ifndef RAYCASTLIB_H
00002 #define RAYCASTLIB_H
00003 
00004 /**
00005   raycastlib - Small C header-only raycasting library for embedded and low
00006   performance computers, such as Arduino. Only uses integer math and stdint
00007   standard library.
00008 
00009   author: Miloslav "drummyfish" Ciz
00010   license: CC0
00011 
00012   - Game field's bottom left corner is at [0,0].
00013   - X axis goes right.
00014   - Y axis goes up.
00015   - Each game square is UNITS_PER_SQUARE * UNITS_PER_SQUARE.
00016   - Angles are in Units, 0 means pointing right (x+) and positively rotates
00017     clockwise, a full angle has UNITS_PER_SQUARE Units.
00018   */
00019 
00020 #include <stdio.h>
00021 
00022 #include <stdint.h>
00023 
00024 #define UNITS_PER_SQUARE 1024 ///< No. of Units in a side of a spatial square.
00025 
00026 typedef int32_t Unit;   /**< Smallest spatial unit, there is UNITS_PER_SQUARE
00027                              units in a square's length. This effectively
00028                              serves the purpose of a fixed-point arithmetic. */
00029 
00030 #define logVector2D(v)\
00031   printf("[%d,%d]\n",v.x,v.y);
00032 
00033 #define logRay(r){\
00034   printf("ray:\n");\
00035   printf("  start: ");\
00036   logVector2D(r.start);\
00037   printf("  dir: ");\
00038   logVector2D(r.direction);}\
00039 
00040 #define logHitResult(h){\
00041   printf("hit:\n");\
00042   printf("  sqaure: ");\
00043   logVector2D(h.square);\
00044   printf("  pos: ");\
00045   logVector2D(h.position);\
00046   printf("  dist: %d\n", h.distance);\
00047   printf("  texcoord: %d\n", h.textureCoord);}\
00048 
00049 /// Position in 2D space.
00050 typedef struct
00051 {
00052   int32_t y;
00053   int32_t x;
00054 } Vector2D;
00055 
00056 typedef struct
00057 {
00058   Vector2D start;
00059   Vector2D direction;
00060 } Ray;
00061 
00062 typedef struct
00063 {
00064   Vector2D square;     ///< Collided square coordinates.
00065   Vector2D position;   ///< Exact collision position in Units.
00066   Unit     distance;   /**< Euclidean distance to the hit position, or -1 if
00067                             no collision happened. */
00068   Unit     textureCoord;    /**< Normalized (0 to UNITS_PER_SQUARE - 1) texture
00069                                  coordinate. */
00070   uint8_t  direction;       ///< Direction of hit.
00071 } HitResult;
00072 
00073 typedef struct
00074 {
00075   Vector2D position;
00076   Unit direction;
00077   Vector2D resolution;
00078   Unit fovAngle;
00079   Unit height;
00080 } Camera;
00081 
00082 typedef struct
00083 {
00084   Vector2D position; ///< On-screen position.
00085   int8_t isWall;     ///< Whether the pixel is a wall or a floor(/ceiling).
00086   Unit depth;        ///< Corrected depth.
00087   HitResult hit;     ///< Corresponding ray hit.
00088 } PixelInfo;
00089 
00090 typedef struct
00091 {
00092   uint16_t maxHits;
00093   uint16_t maxSteps;
00094 } RayConstraints;
00095 
00096 /**
00097  Function used to retrieve the cells of the rendered scene. It should return
00098  a "type" of given square as an integer (e.g. square height) - between squares
00099  that return different numbers there is considered to be a collision.
00100 */ 
00101 typedef Unit (*ArrayFunction)(int16_t x, int16_t y);
00102 
00103 typedef void (*PixelFunction)(PixelInfo info);
00104 
00105 typedef void
00106   (*ColumnFunction)(HitResult *hits, uint16_t hitCount, uint16_t x, Ray ray);
00107 
00108 /**
00109  Simple-interface function to cast a single ray.
00110 
00111  @return          The first collision result.
00112  */
00113 HitResult castRay(Ray ray, ArrayFunction arrayFunc);
00114 
00115 /**
00116  Casts a single ray and returns a list of collisions.
00117  */
00118 void castRayMultiHit(Ray ray, ArrayFunction arrayFunc, HitResult *hitResults,
00119   uint16_t *hitResultsLen, RayConstraints constraints);
00120 
00121 Vector2D angleToDirection(Unit angle);
00122 Unit cosInt(Unit input);
00123 Unit sinInt(Unit input);
00124 
00125 /// Normalizes given vector to have UNITS_PER_SQUARE length.
00126 Vector2D normalize(Vector2D v);
00127 
00128 /// Computes a cos of an angle between two vectors.
00129 Unit vectorsAngleCos(Vector2D v1, Vector2D v2);
00130 
00131 uint16_t sqrtInt(uint32_t value);
00132 Unit dist(Vector2D p1, Vector2D p2);
00133 Unit len(Vector2D v);
00134 
00135 /**
00136  Converts an angle in whole degrees to an angle in Units that this library
00137  uses.
00138  */   
00139 Unit degreesToUnitsAngle(int16_t degrees);
00140 
00141 ///< Computes the change in size of an object due to perspective.
00142 Unit perspectiveScale(Unit originalSize, Unit distance, Unit fov);
00143 
00144 /**
00145  Casts rays for given camera view and for each hit calls a user provided
00146  function.
00147  */
00148 void castRaysMultiHit(Camera cam, ArrayFunction arrayFunc,
00149   ColumnFunction columnFunc, RayConstraints constraints);
00150 
00151 void render(Camera cam, ArrayFunction arrayFunc, PixelFunction pixelFunc,
00152   RayConstraints constraints);
00153 
00154 //=============================================================================
00155 // privates
00156 
00157 #ifdef RAYCASTLIB_PROFILE
00158   // function call counters for profiling
00159   uint32_t profile_sqrtInt = 0;
00160   uint32_t profile_clamp = 0;
00161   uint32_t profile_cosInt = 0;
00162   uint32_t profile_angleToDirection = 0;
00163   uint32_t profile_dist = 0;
00164   uint32_t profile_len = 0;
00165   uint32_t profile_pointIsLeftOfRay = 0;
00166   uint32_t profile_castRaySquare = 0;
00167   uint32_t profile_castRayMultiHit = 0; 
00168   uint32_t profile_castRay = 0;
00169   uint16_t profile_normalize = 0;
00170   uint16_t profile_vectorsAngleCos = 0;
00171 
00172   #define profileCall(c) profile_##c += 1
00173 
00174   #define printProfile() {\
00175     printf("profile:\n");\
00176     printf("  sqrtInt: %d\n",profile_sqrtInt);\
00177     printf("  clamp: %d\n",profile_clamp);\
00178     printf("  cosInt: %d\n",profile_cosInt);\
00179     printf("  angleToDirection: %d\n",profile_angleToDirection);\
00180     printf("  dist: %d\n",profile_dist);\
00181     printf("  len: %d\n",profile_len);\
00182     printf("  pointIsLeftOfRay: %d\n",profile_pointIsLeftOfRay);\
00183     printf("  castRaySquare: %d\n",profile_castRaySquare);\
00184     printf("  castRayMultiHit : %d\n",profile_castRayMultiHit);\
00185     printf("  castRay: %d\n",profile_castRay);\
00186     printf("  normalize: %d\n",profile_normalize);\
00187     printf("  vectorsAngleCos: %d\n",profile_vectorsAngleCos); }
00188 #else
00189   #define profileCall(c)
00190 #endif
00191 
00192 Unit clamp(Unit value, Unit valueMin, Unit valueMax)
00193 {
00194   profileCall(clamp);
00195 
00196   if (value < valueMin)
00197     return valueMin;
00198 
00199   if (value > valueMax)
00200     return valueMax;
00201 
00202   return value;
00203 }
00204 
00205 // Bhaskara's cosine approximation formula
00206 #define trigHelper(x) (UNITS_PER_SQUARE *\
00207   (UNITS_PER_SQUARE / 2 * UNITS_PER_SQUARE / 2 - 4 * (x) * (x)) /\
00208   (UNITS_PER_SQUARE / 2 * UNITS_PER_SQUARE / 2 + (x) * (x)))
00209 
00210 Unit cosInt(Unit input)
00211 {
00212   profileCall(cosInt);
00213 
00214   input = input % UNITS_PER_SQUARE;
00215 
00216   if (input < 0)
00217     input = UNITS_PER_SQUARE + input;
00218 
00219   if (input < UNITS_PER_SQUARE / 4)
00220     return trigHelper(input);
00221   else if (input < UNITS_PER_SQUARE / 2)
00222     return -1 * trigHelper(UNITS_PER_SQUARE / 2 - input);
00223   else if (input < 3 * UNITS_PER_SQUARE / 4)
00224     return -1 * trigHelper(input - UNITS_PER_SQUARE / 2);
00225   else
00226     return trigHelper(UNITS_PER_SQUARE - input);
00227 }
00228 
00229 #undef trigHelper
00230 
00231 Unit sinInt(Unit input)
00232 {
00233   return cosInt(input - UNITS_PER_SQUARE / 4);
00234 }
00235 
00236 Vector2D angleToDirection(Unit angle)
00237 {
00238   profileCall(angleToDirection);
00239 
00240   Vector2D result;
00241 
00242   result.x = cosInt(angle);
00243   result.y = -1 * sinInt(angle);
00244 
00245   return result;
00246 }
00247 
00248 uint16_t sqrtInt(uint32_t value)
00249 {
00250   profileCall(sqrtInt);
00251 
00252   uint32_t result = 0;
00253 
00254   uint32_t a  = value;
00255   uint32_t b = 1u << 30;
00256 
00257   while (b > a)
00258     b >>= 2;
00259 
00260   while (b != 0)
00261   {
00262     if (a >= result + b)
00263     {
00264       a -= result + b;
00265       result = result +  2 * b;
00266     }
00267 
00268     b >>= 2;
00269     result >>= 1;
00270   }
00271 
00272   return result;
00273 }
00274 
00275 Unit dist(Vector2D p1, Vector2D p2)
00276 {
00277   profileCall(dist);
00278 
00279   int32_t dx = p2.x - p1.x;
00280   int32_t dy = p2.y - p1.y;
00281 
00282   dx = dx * dx;
00283   dy = dy * dy;
00284 
00285   return sqrtInt((uint32_t) (dx + dy));
00286 }
00287 
00288 Unit len(Vector2D v)
00289 {
00290   profileCall(len);
00291 
00292   v.x *= v.x;
00293   v.y *= v.y;
00294 
00295   return sqrtInt(((uint32_t) v.x) + ((uint32_t) v.y));
00296 }
00297 
00298 int8_t pointIsLeftOfRay(Vector2D point, Ray ray)
00299 {
00300   profileCall(pointIsLeftOfRay);
00301 
00302   Unit dX    = point.x - ray.start.x;
00303   Unit dY    = point.y - ray.start.y;
00304   return (ray.direction.x * dY - ray.direction.y * dX) > 0;
00305          // ^ Z component of cross-product
00306 }
00307 
00308 /**
00309   Casts a ray within a single square, to collide with the square borders.  
00310  */
00311 void castRaySquare(Ray localRay, Vector2D *nextCellOff, Vector2D *collOff)
00312 {
00313   profileCall(castRaySquare);
00314 
00315   nextCellOff->x = 0;
00316   nextCellOff->y = 0;
00317 
00318   Ray criticalLine = localRay;
00319 
00320   #define helper(c1,c2,n)\
00321     {\
00322       nextCellOff->c1 = n;\
00323       collOff->c1 = criticalLine.start.c1 - localRay.start.c1;\
00324       collOff->c2 = \
00325         (((int32_t) collOff->c1) * localRay.direction.c2) /\
00326         ((localRay.direction.c1 == 0) ? 1 : localRay.direction.c1);\
00327     }
00328 
00329   #define helper2(n1,n2,c)\
00330     if (pointIsLeftOfRay(localRay.start,criticalLine) == c)\
00331       helper(y,x,n1)\
00332     else\
00333       helper(x,y,n2)
00334       
00335   if (localRay.direction.x > 0)
00336   {
00337     criticalLine.start.x = UNITS_PER_SQUARE - 1;
00338 
00339     if (localRay.direction.y > 0)
00340     {
00341       // top right
00342       criticalLine.start.y = UNITS_PER_SQUARE - 1;
00343       helper2(1,1,1)
00344     }
00345     else
00346     {
00347       // bottom right
00348       criticalLine.start.y = 0;
00349       helper2(-1,1,0)
00350     }
00351   }
00352   else
00353   {
00354     criticalLine.start.x = 0;
00355 
00356     if (localRay.direction.y > 0)
00357     {
00358       // top left
00359       criticalLine.start.y = UNITS_PER_SQUARE - 1;
00360       helper2(1,-1,0)
00361     }
00362     else
00363     {
00364       // bottom left
00365       criticalLine.start.y = 0;
00366       helper2(-1,-1,1)
00367     }
00368   }
00369 
00370   #undef helper2
00371   #undef helper
00372 
00373   collOff->x += nextCellOff->x;
00374   collOff->y += nextCellOff->y;
00375 }
00376 
00377 void castRayMultiHit(Ray ray, ArrayFunction arrayFunc, HitResult *hitResults,
00378   uint16_t *hitResultsLen, RayConstraints constraints)
00379 {
00380   profileCall(castRayMultiHit);
00381 
00382   Vector2D initialPos = ray.start;
00383   Vector2D currentPos = ray.start;
00384 
00385   Vector2D currentSquare;
00386 
00387   currentSquare.x = ray.start.x / UNITS_PER_SQUARE;
00388   currentSquare.y = ray.start.y / UNITS_PER_SQUARE;
00389 
00390   *hitResultsLen = 0;
00391 
00392   int16_t squareType = arrayFunc(currentSquare.x,currentSquare.y);
00393 
00394   Vector2D no, co; // next cell offset, collision offset
00395 
00396   no.x = 0;        // just to supress a warning
00397   no.y = 0;
00398 
00399   for (uint16_t i = 0; i < constraints.maxSteps; ++i)
00400   {
00401     int16_t currentType = arrayFunc(currentSquare.x,currentSquare.y);
00402 
00403     if (currentType != squareType)
00404     {
00405       // collision
00406 
00407       HitResult h;
00408 
00409       h.position = currentPos;
00410       h.square   = currentSquare;
00411       h.distance = dist(initialPos,currentPos);
00412 
00413       if (no.y > 0)
00414         h.direction = 0;
00415       else if (no.x > 0)
00416         h.direction = 1;
00417       else if (no.y < 0)
00418         h.direction = 2;
00419       else
00420         h.direction = 3;
00421 
00422       hitResults[*hitResultsLen] = h;
00423 
00424       *hitResultsLen += 1;
00425 
00426       squareType = currentType;
00427 
00428       if (*hitResultsLen >= constraints.maxHits)
00429         break;
00430     }
00431 
00432     ray.start.x = currentPos.x % UNITS_PER_SQUARE;
00433     ray.start.y = currentPos.y % UNITS_PER_SQUARE;
00434 
00435     castRaySquare(ray,&no,&co);
00436 
00437     currentSquare.x += no.x;
00438     currentSquare.y += no.y;
00439 
00440     // offset into the next cell
00441     currentPos.x += co.x;
00442     currentPos.y += co.y;
00443   }
00444 }
00445 
00446 HitResult castRay(Ray ray, ArrayFunction arrayFunc)
00447 {
00448   profileCall(castRay);
00449 
00450   HitResult result;
00451   uint16_t  len;
00452   RayConstraints c;
00453 
00454   c.maxSteps = 1000;
00455   c.maxHits = 1;
00456 
00457   castRayMultiHit(ray,arrayFunc,&result,&len,c);
00458 
00459   if (len == 0)
00460     result.distance = -1;
00461 
00462   return result;
00463 }
00464 
00465 void castRaysMultiHit(Camera cam, ArrayFunction arrayFunc,
00466   ColumnFunction columnFunc, RayConstraints constraints)
00467 {
00468   uint16_t fovHalf = cam.fovAngle / 2;
00469 
00470   Vector2D dir1 = angleToDirection(cam.direction - fovHalf);
00471   Vector2D dir2 = angleToDirection(cam.direction + fovHalf);
00472 
00473   Unit dX = dir2.x - dir1.x;
00474   Unit dY = dir2.y - dir1.y;
00475 
00476   HitResult hits[constraints.maxHits];
00477   Ray rays[constraints.maxHits];
00478   uint16_t hitCount;
00479 
00480   Ray r;
00481   r.start = cam.position;
00482 
00483   for (uint16_t i = 0; i < cam.resolution.x; ++i)
00484   {
00485     r.direction.x = dir1.x + (dX * i) / cam.resolution.x;
00486     r.direction.y = dir1.y + (dY * i) / cam.resolution.x;
00487 
00488     castRayMultiHit(r,arrayFunc,hits,&hitCount,constraints);
00489 
00490     columnFunc(hits,hitCount,i,r);
00491   }
00492 }
00493 
00494 PixelFunction _pixelFunction = 0;
00495 ArrayFunction _arrayFunction = 0;
00496 Camera _camera;
00497 Unit _floorDepthStep = 0; 
00498 
00499 Unit _startHeight = 0;
00500 
00501 void _columnFunction(HitResult *hits, uint16_t hitCount, uint16_t x, Ray ray)
00502 {
00503   int32_t y = _camera.resolution.y - 1; // on screen y, will only go upwards
00504 
00505   Unit worldZPrev = _startHeight;
00506 
00507   Unit previousDepth = 1;
00508 
00509   uint16_t middleRow = _camera.resolution.y / 2;
00510 
00511   PixelInfo p;
00512   p.position.x = x;
00513 
00514   for (uint32_t j = 0; j < hitCount; ++j)
00515   {
00516     HitResult hit = hits[j];
00517 
00518     /* FIXME/TODO: The adjusted (=orthogonal, camera-space) distance could
00519        possibly be computed more efficiently by not computing Euclidean
00520        distance at all, but rather compute the distance of the collision
00521        point from the projection plane (line). */
00522 
00523     Unit dist = // adjusted distance
00524       (hit.distance * vectorsAngleCos(angleToDirection(_camera.direction),
00525       ray.direction)) / UNITS_PER_SQUARE;
00526 
00527     dist = dist == 0 ? 1 : dist; // prevent division by zero
00528 
00529     Unit wallHeight = _arrayFunction(hit.square.x,hit.square.y);
00530 
00531     Unit worldZ2 = -1 * _camera.height + wallHeight;
00532 
00533     int16_t z1Screen = middleRow -
00534       perspectiveScale(
00535         (worldZPrev * _camera.resolution.y) / UNITS_PER_SQUARE,
00536         dist,1);
00537 
00538     z1Screen = clamp(z1Screen,0,_camera.resolution.y - 1);
00539 
00540     int16_t z2Screen = middleRow -
00541       perspectiveScale(
00542         (worldZ2 * _camera.resolution.y) / UNITS_PER_SQUARE,
00543         dist,1);
00544 
00545     z2Screen = clamp(z2Screen,0,_camera.resolution.y - 1);
00546 
00547     Unit zTop = z1Screen < z2Screen ? z1Screen : z2Screen;
00548 
00549     // draw floor until the wall
00550 
00551     p.isWall = 0;
00552     Unit depthDiff = dist - previousDepth;
00553 
00554     Unit floorCameraDiff = _camera.height - worldZPrev;
00555 
00556     for (int32_t i = y; i > zTop; --i)
00557     {
00558       p.position.y = i;
00559       p.depth = (_camera.resolution.y - i) * _floorDepthStep + floorCameraDiff;
00560       _pixelFunction(p);  
00561     }
00562 
00563     // draw the wall
00564 
00565     p.isWall = 1;
00566     p.depth = dist;
00567 
00568     for (int32_t i = z1Screen < y ? z1Screen : y; i > z2Screen; --i)
00569     {
00570       p.position.y = i;
00571       p.hit = hit;
00572       _pixelFunction(p);
00573     }
00574 
00575     y = y > zTop ? zTop : y;
00576     worldZPrev = worldZ2;
00577     previousDepth = dist;
00578   }
00579 
00580   // draw floor until horizon
00581 
00582   p.isWall = 0;
00583 
00584   Unit floorCameraDiff = _camera.height - worldZPrev;
00585 
00586   for (int32_t i = y; i >= middleRow; --i)
00587   {
00588     p.position.y = i;
00589     p.depth = (_camera.resolution.y - i) * _floorDepthStep + floorCameraDiff;
00590     _pixelFunction(p);
00591   }
00592 }
00593 
00594 void render(Camera cam, ArrayFunction arrayFunc, PixelFunction pixelFunc,
00595   RayConstraints constraints)
00596 {
00597   _pixelFunction = pixelFunc;
00598   _arrayFunction = arrayFunc;
00599   _camera = cam;
00600 
00601   _startHeight = arrayFunc(
00602     cam.position.x / UNITS_PER_SQUARE,
00603     cam.position.y / UNITS_PER_SQUARE) -1 * cam.height;
00604 
00605   // TODO
00606   _floorDepthStep = (16 * UNITS_PER_SQUARE) / cam.resolution.y; 
00607 
00608   castRaysMultiHit(cam,arrayFunc,_columnFunction,constraints);
00609 }
00610 
00611 Vector2D normalize(Vector2D v)
00612 {
00613   profileCall(normalize);
00614 
00615   Vector2D result;
00616 
00617   Unit l = len(v);
00618 
00619   result.x = (v.x * UNITS_PER_SQUARE) / l;
00620   result.y = (v.y * UNITS_PER_SQUARE) / l;
00621 
00622   return result;
00623 }
00624 
00625 Unit vectorsAngleCos(Vector2D v1, Vector2D v2)
00626 {
00627   profileCall(vectorsAngleCos);
00628 
00629   v1 = normalize(v1);
00630   v2 = normalize(v2);
00631 
00632   return (v1.x * v2.x + v1.y * v2.y) / UNITS_PER_SQUARE;
00633 }
00634 
00635 Unit degreesToUnitsAngle(int16_t degrees)
00636 {
00637   return (degrees * UNITS_PER_SQUARE) / 360;
00638 }
00639 
00640 Unit perspectiveScale(Unit originalSize, Unit distance, Unit fov)
00641 {
00642 return (originalSize * UNITS_PER_SQUARE) / distance;
00643 
00644   distance *= fov;
00645   distance = distance == 0 ? 1 : distance; // prevent division by zero
00646 
00647   return originalSize / distance; 
00648 }
00649 
00650 #endif