dali-physics/third-party/chipmunk2d/objectivec/src/ChipmunkPointCloudSampler.m

   1 // Copyright 2013 Howling Moon Software. All rights reserved.
   2 // See http://chipmunk2d.net/legal.php for more information.
   3
   4 #import "ChipmunkPointCloudSampler.h"
   5
   6
   7 typedef struct DeformPoint {
   8         cpVect pos;
   9         cpFloat radius;
  10         cpFloat fuzz;
  11 } DeformPoint;
  12
  13 static cpBB
  14 PointBB(DeformPoint *point)
  15 {
  16         cpVect v = point->pos;
  17         cpFloat r = point->radius;
  18
  19         return cpBBNew(v.x - r, v.y - r, v.x + r, v.y + r);
  20 }
  21
  22
  23
  24 @implementation ChipmunkPointCloudSampler
  25
  26 static inline cpFloat
  27 fuzz(cpVect v, cpVect c, cpFloat r, cpFloat softness)
  28 {
  29         cpFloat distsq = cpvdistsq(v, c);
  30         return (distsq < r*r ? 1.0f - cpfclamp01((r - cpfsqrt(distsq))/(softness*r)) : 1.0f);
  31 }
  32
  33 static void
  34 PointQuery(cpVect *v, DeformPoint *point, cpCollisionID id, cpFloat *density)
  35 {
  36         (*density) *= fuzz(*v, point->pos, point->radius, point->fuzz);
  37 }
  38
  39 static cpFloat
  40 PointCloudSample(ChipmunkPointCloudSampler *cloud, cpVect pos)
  41 {
  42         cpFloat density = 1.0f;
  43         cpSpatialIndexQuery(cloud->_index, &pos, cpBBNewForCircle(pos, 0.0f), (cpSpatialIndexQueryFunc)PointQuery, &density);
  44
  45         return density;
  46 }
  47
  48 - (id)initWithCellSize:(cpFloat)cellSize
  49 {
  50         if((self = [super initWithSamplingFunction:(cpMarchSampleFunc)PointCloudSample])){
  51                 _cellSize = cellSize;
  52                 // TODO table size
  53                 _index = cpSpaceHashNew(cellSize, 1000, (cpSpatialIndexBBFunc)PointBB, NULL);
  54         }
  55
  56         return self;
  57 }
  58
  59 static void freeWrap(void *ptr, void *unused){cpfree(ptr);}
  60
  61 - (void)dealloc
  62 {
  63         cpSpatialIndexEach(_index, (cpSpatialIndexIteratorFunc)freeWrap, NULL);
  64         cpSpatialIndexFree(_index);
  65
  66         [super dealloc];
  67 }
  68
  69 -(cpBB)addPoint:(cpVect)pos radius:(cpFloat)radius fuzz:(cpFloat)fuzz
  70 {
  71         DeformPoint *point = (DeformPoint *)cpcalloc(1, sizeof(DeformPoint));
  72         point->pos = pos;
  73         point->radius = radius;
  74         point->fuzz = fuzz;
  75
  76         cpSpatialIndexInsert(_index, point, (cpHashValue)point);
  77
  78         return PointBB(point);
  79 }
  80
  81 @end