dali-physics/third-party/chipmunk2d/src/cpPolyShape.c

   1 /* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
   2  *
   3  * Permission is hereby granted, free of charge, to any person obtaining a copy
   4  * of this software and associated documentation files (the "Software"), to deal
   5  * in the Software without restriction, including without limitation the rights
   6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   7  * copies of the Software, and to permit persons to whom the Software is
   8  * furnished to do so, subject to the following conditions:
   9  *
  10  * The above copyright notice and this permission notice shall be included in
  11  * all copies or substantial portions of the Software.
  12  *
  13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  19  * SOFTWARE.
  20  */
  21
  22 #include "chipmunk/chipmunk_private.h"
  23 #include "chipmunk/chipmunk_unsafe.h"
  24
  25 cpPolyShape *
  26 cpPolyShapeAlloc(void)
  27 {
  28         return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape));
  29 }
  30
  31 static void
  32 cpPolyShapeDestroy(cpPolyShape *poly)
  33 {
  34         if(poly->count > CP_POLY_SHAPE_INLINE_ALLOC){
  35                 cpfree(poly->planes);
  36         }
  37 }
  38
  39 static cpBB
  40 cpPolyShapeCacheData(cpPolyShape *poly, cpTransform transform)
  41 {
  42         int count = poly->count;
  43         struct cpSplittingPlane *dst = poly->planes;
  44         struct cpSplittingPlane *src = dst + count;
  45
  46         cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
  47         cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
  48
  49         for(int i=0; i<count; i++){
  50                 cpVect v = cpTransformPoint(transform, src[i].v0);
  51                 cpVect n = cpTransformVect(transform, src[i].n);
  52
  53                 dst[i].v0 = v;
  54                 dst[i].n = n;
  55
  56                 l = cpfmin(l, v.x);
  57                 r = cpfmax(r, v.x);
  58                 b = cpfmin(b, v.y);
  59                 t = cpfmax(t, v.y);
  60         }
  61
  62         cpFloat radius = poly->r;
  63         return (poly->shape.bb = cpBBNew(l - radius, b - radius, r + radius, t + radius));
  64 }
  65
  66 static void
  67 cpPolyShapePointQuery(cpPolyShape *poly, cpVect p, cpPointQueryInfo *info){
  68         int count = poly->count;
  69         struct cpSplittingPlane *planes = poly->planes;
  70         cpFloat r = poly->r;
  71
  72         cpVect v0 = planes[count - 1].v0;
  73         cpFloat minDist = INFINITY;
  74         cpVect closestPoint = cpvzero;
  75         cpVect closestNormal = cpvzero;
  76         cpBool outside = cpFalse;
  77
  78         for(int i=0; i<count; i++){
  79                 cpVect v1 = planes[i].v0;
  80                 outside = outside || (cpvdot(planes[i].n, cpvsub(p,v1)) > 0.0f);
  81
  82                 cpVect closest = cpClosetPointOnSegment(p, v0, v1);
  83
  84                 cpFloat dist = cpvdist(p, closest);
  85                 if(dist < minDist){
  86                         minDist = dist;
  87                         closestPoint = closest;
  88                         closestNormal = planes[i].n;
  89                 }
  90
  91                 v0 = v1;
  92         }
  93
  94         cpFloat dist = (outside ? minDist : -minDist);
  95         cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist);
  96
  97         info->shape = (cpShape *)poly;
  98         info->point = cpvadd(closestPoint, cpvmult(g, r));
  99         info->distance = dist - r;
 100
 101         // Use the normal of the closest segment if the distance is small.
 102         info->gradient = (minDist > MAGIC_EPSILON ? g : closestNormal);
 103 }
 104
 105 static void
 106 cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
 107 {
 108         struct cpSplittingPlane *planes = poly->planes;
 109         int count = poly->count;
 110         cpFloat r = poly->r;
 111         cpFloat rsum = r + r2;
 112
 113         for(int i=0; i<count; i++){
 114                 cpVect n = planes[i].n;
 115                 cpFloat an = cpvdot(a, n);
 116                 cpFloat d =  an - cpvdot(planes[i].v0, n) - rsum;
 117                 if(d < 0.0f) continue;
 118
 119                 cpFloat bn = cpvdot(b, n);
 120                 cpFloat t = d/(an - bn);
 121                 if(t < 0.0f || 1.0f < t) continue;
 122
 123                 cpVect point = cpvlerp(a, b, t);
 124                 cpFloat dt = cpvcross(n, point);
 125                 cpFloat dtMin = cpvcross(n, planes[(i - 1 + count)%count].v0);
 126                 cpFloat dtMax = cpvcross(n, planes[i].v0);
 127
 128                 if(dtMin <= dt && dt <= dtMax){
 129                         info->shape = (cpShape *)poly;
 130                         info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2));
 131                         info->normal = n;
 132                         info->alpha = t;
 133                 }
 134         }
 135
 136         // Also check against the beveled vertexes.
 137         if(rsum > 0.0f){
 138                 for(int i=0; i<count; i++){
 139                         cpSegmentQueryInfo circle_info = {NULL, b, cpvzero, 1.0f};
 140                         CircleSegmentQuery(&poly->shape, planes[i].v0, r, a, b, r2, &circle_info);
 141                         if(circle_info.alpha < info->alpha) (*info) = circle_info;
 142                 }
 143         }
 144 }
 145
 146 static void
 147 SetVerts(cpPolyShape *poly, int count, const cpVect *verts)
 148 {
 149         poly->count = count;
 150         if(count <= CP_POLY_SHAPE_INLINE_ALLOC){
 151                 poly->planes = poly->_planes;
 152         } else {
 153                 poly->planes = (struct cpSplittingPlane *)cpcalloc(2*count, sizeof(struct cpSplittingPlane));
 154         }
 155
 156         for(int i=0; i<count; i++){
 157                 cpVect a = verts[(i - 1 + count)%count];
 158                 cpVect b = verts[i];
 159                 cpVect n = cpvnormalize(cpvrperp(cpvsub(b, a)));
 160
 161                 poly->planes[i + count].v0 = b;
 162                 poly->planes[i + count].n = n;
 163         }
 164 }
 165
 166 static struct cpShapeMassInfo
 167 cpPolyShapeMassInfo(cpFloat mass, int count, const cpVect *verts, cpFloat radius)
 168 {
 169         // TODO moment is approximate due to radius.
 170
 171         cpVect centroid = cpCentroidForPoly(count, verts);
 172         struct cpShapeMassInfo info = {
 173                 mass, cpMomentForPoly(1.0f, count, verts, cpvneg(centroid), radius),
 174                 centroid,
 175                 cpAreaForPoly(count, verts, radius),
 176         };
 177
 178         return info;
 179 }
 180
 181 static const cpShapeClass polyClass = {
 182         CP_POLY_SHAPE,
 183         (cpShapeCacheDataImpl)cpPolyShapeCacheData,
 184         (cpShapeDestroyImpl)cpPolyShapeDestroy,
 185         (cpShapePointQueryImpl)cpPolyShapePointQuery,
 186         (cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery,
 187 };
 188
 189 cpPolyShape *
 190 cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius)
 191 {
 192         cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect));
 193
 194         // Transform the verts before building the hull in case of a negative scale.
 195         for(int i=0; i<count; i++) hullVerts[i] = cpTransformPoint(transform, verts[i]);
 196
 197         unsigned int hullCount = cpConvexHull(count, hullVerts, hullVerts, NULL, 0.0);
 198         return cpPolyShapeInitRaw(poly, body, hullCount, hullVerts, radius);
 199 }
 200
 201 cpPolyShape *
 202 cpPolyShapeInitRaw(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpFloat radius)
 203 {
 204         cpShapeInit((cpShape *)poly, &polyClass, body, cpPolyShapeMassInfo(0.0f, count, verts, radius));
 205
 206         SetVerts(poly, count, verts);
 207         poly->r = radius;
 208
 209         return poly;
 210 }
 211
 212 cpShape *
 213 cpPolyShapeNew(cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius)
 214 {
 215         return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, count, verts, transform, radius);
 216 }
 217
 218 cpShape *
 219 cpPolyShapeNewRaw(cpBody *body, int count, const cpVect *verts, cpFloat radius)
 220 {
 221         return (cpShape *)cpPolyShapeInitRaw(cpPolyShapeAlloc(), body, count, verts, radius);
 222 }
 223
 224 cpPolyShape *
 225 cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height, cpFloat radius)
 226 {
 227         cpFloat hw = width/2.0f;
 228         cpFloat hh = height/2.0f;
 229
 230         return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh), radius);
 231 }
 232
 233 cpPolyShape *
 234 cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius)
 235 {
 236         cpVect verts[] = {
 237                 cpv(box.r, box.b),
 238                 cpv(box.r, box.t),
 239                 cpv(box.l, box.t),
 240                 cpv(box.l, box.b),
 241         };
 242
 243         return cpPolyShapeInitRaw(poly, body, 4, verts, radius);
 244 }
 245
 246 cpShape *
 247 cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height, cpFloat radius)
 248 {
 249         return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height, radius);
 250 }
 251
 252 cpShape *
 253 cpBoxShapeNew2(cpBody *body, cpBB box, cpFloat radius)
 254 {
 255         return (cpShape *)cpBoxShapeInit2(cpPolyShapeAlloc(), body, box, radius);
 256 }
 257
 258 int
 259 cpPolyShapeGetCount(const cpShape *shape)
 260 {
 261         cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
 262         return ((cpPolyShape *)shape)->count;
 263 }
 264
 265 cpVect
 266 cpPolyShapeGetVert(const cpShape *shape, int i)
 267 {
 268         cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
 269
 270         int count = cpPolyShapeGetCount(shape);
 271         cpAssertHard(0 <= i && i < count, "Index out of range.");
 272
 273         return ((cpPolyShape *)shape)->planes[i + count].v0;
 274 }
 275
 276 cpFloat
 277 cpPolyShapeGetRadius(const cpShape *shape)
 278 {
 279         cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
 280         return ((cpPolyShape *)shape)->r;
 281 }
 282
 283 // Unsafe API (chipmunk_unsafe.h)
 284
 285 void
 286 cpPolyShapeSetVerts(cpShape *shape, int count, cpVect *verts, cpTransform transform)
 287 {
 288         cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect));
 289
 290         // Transform the verts before building the hull in case of a negative scale.
 291         for(int i=0; i<count; i++) hullVerts[i] = cpTransformPoint(transform, verts[i]);
 292
 293         unsigned int hullCount = cpConvexHull(count, hullVerts, hullVerts, NULL, 0.0);
 294         cpPolyShapeSetVertsRaw(shape, hullCount, hullVerts);
 295 }
 296
 297 void
 298 cpPolyShapeSetVertsRaw(cpShape *shape, int count, cpVect *verts)
 299 {
 300         cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
 301         cpPolyShape *poly = (cpPolyShape *)shape;
 302         cpPolyShapeDestroy(poly);
 303
 304         SetVerts(poly, count, verts);
 305
 306         cpFloat mass = shape->massInfo.m;
 307         shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, count, verts, poly->r);
 308         if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
 309 }
 310
 311 void
 312 cpPolyShapeSetRadius(cpShape *shape, cpFloat radius)
 313 {
 314         cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
 315         cpPolyShape *poly = (cpPolyShape *)shape;
 316         poly->r = radius;
 317
 318
 319         // TODO radius is not handled by moment/area
 320 //      cpFloat mass = shape->massInfo.m;
 321 //      shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, poly->count, poly->verts, poly->r);
 322 //      if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
 323 }