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 *
cpPolyShapeAlloc(void)26 cpPolyShapeAlloc(void)
27 {
28 return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape));
29 }
30
31 static void
cpPolyShapeDestroy(cpPolyShape * poly)32 cpPolyShapeDestroy(cpPolyShape *poly)
33 {
34 if(poly->count > CP_POLY_SHAPE_INLINE_ALLOC){
35 cpfree(poly->planes);
36 }
37 }
38
39 static cpBB
cpPolyShapeCacheData(cpPolyShape * poly,cpTransform transform)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
cpPolyShapePointQuery(cpPolyShape * poly,cpVect p,cpPointQueryInfo * info)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
cpPolyShapeSegmentQuery(cpPolyShape * poly,cpVect a,cpVect b,cpFloat r2,cpSegmentQueryInfo * info)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
SetVerts(cpPolyShape * poly,int count,const cpVect * verts)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
cpPolyShapeMassInfo(cpFloat mass,int count,const cpVect * verts,cpFloat radius)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 *
cpPolyShapeInit(cpPolyShape * poly,cpBody * body,int count,const cpVect * verts,cpTransform transform,cpFloat radius)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 *
cpPolyShapeInitRaw(cpPolyShape * poly,cpBody * body,int count,const cpVect * verts,cpFloat radius)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 *
cpPolyShapeNew(cpBody * body,int count,const cpVect * verts,cpTransform transform,cpFloat radius)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 *
cpPolyShapeNewRaw(cpBody * body,int count,const cpVect * verts,cpFloat radius)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 *
cpBoxShapeInit(cpPolyShape * poly,cpBody * body,cpFloat width,cpFloat height,cpFloat radius)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 *
cpBoxShapeInit2(cpPolyShape * poly,cpBody * body,cpBB box,cpFloat radius)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 *
cpBoxShapeNew(cpBody * body,cpFloat width,cpFloat height,cpFloat radius)247 cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height, cpFloat radius)
248 {
249 return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height, radius);
250 }
251
252 cpShape *
cpBoxShapeNew2(cpBody * body,cpBB box,cpFloat radius)253 cpBoxShapeNew2(cpBody *body, cpBB box, cpFloat radius)
254 {
255 return (cpShape *)cpBoxShapeInit2(cpPolyShapeAlloc(), body, box, radius);
256 }
257
258 int
cpPolyShapeGetCount(const cpShape * shape)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
cpPolyShapeGetVert(const cpShape * shape,int i)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
cpPolyShapeGetRadius(const cpShape * shape)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
cpPolyShapeSetVerts(cpShape * shape,int count,cpVect * verts,cpTransform transform)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
cpPolyShapeSetVertsRaw(cpShape * shape,int count,cpVect * verts)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
cpPolyShapeSetRadius(cpShape * shape,cpFloat radius)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 }
324