chipmunk 4.1.0 → 5.2.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- data/LICENSE +20 -0
- data/README +60 -0
- data/Rakefile +47 -40
- data/ext/chipmunk/chipmunk.c +39 -3
- data/ext/chipmunk/cpArbiter.c +91 -80
- data/ext/chipmunk/cpArray.c +24 -10
- data/ext/chipmunk/cpBB.c +5 -4
- data/ext/chipmunk/cpBody.c +30 -22
- data/ext/chipmunk/cpCollision.c +54 -53
- data/ext/chipmunk/cpConstraint.c +54 -0
- data/ext/chipmunk/cpDampedRotarySpring.c +106 -0
- data/ext/chipmunk/cpDampedSpring.c +117 -0
- data/ext/chipmunk/cpGearJoint.c +114 -0
- data/ext/chipmunk/cpGrooveJoint.c +138 -0
- data/ext/chipmunk/cpHashSet.c +74 -40
- data/ext/chipmunk/cpPinJoint.c +117 -0
- data/ext/chipmunk/cpPivotJoint.c +114 -0
- data/ext/chipmunk/cpPolyShape.c +117 -15
- data/ext/chipmunk/cpRatchetJoint.c +128 -0
- data/ext/chipmunk/cpRotaryLimitJoint.c +122 -0
- data/ext/chipmunk/cpShape.c +174 -18
- data/ext/chipmunk/cpSimpleMotor.c +99 -0
- data/ext/chipmunk/cpSlideJoint.c +131 -0
- data/ext/chipmunk/cpSpace.c +584 -215
- data/ext/chipmunk/cpSpaceHash.c +191 -105
- data/ext/chipmunk/cpVect.c +18 -10
- data/ext/chipmunk/extconf.rb +34 -4
- data/ext/chipmunk/{chipmunk.h → include/chipmunk/chipmunk.h} +63 -6
- data/ext/chipmunk/include/chipmunk/chipmunk_ffi.h +42 -0
- data/ext/chipmunk/include/chipmunk/chipmunk_types.h +80 -0
- data/ext/chipmunk/include/chipmunk/chipmunk_unsafe.h +54 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpConstraint.h +92 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h +46 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpDampedSpring.h +53 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpGearJoint.h +41 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h +44 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpPinJoint.h +43 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpPivotJoint.h +42 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpRatchetJoint.h +40 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpRotaryLimitJoint.h +39 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpSimpleMotor.h +37 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpSlideJoint.h +44 -0
- data/ext/chipmunk/include/chipmunk/constraints/util.h +116 -0
- data/ext/chipmunk/{cpArbiter.h → include/chipmunk/cpArbiter.h} +66 -15
- data/ext/chipmunk/{cpArray.h → include/chipmunk/cpArray.h} +2 -1
- data/ext/chipmunk/{cpBB.h → include/chipmunk/cpBB.h} +21 -0
- data/ext/chipmunk/{cpBody.h → include/chipmunk/cpBody.h} +37 -9
- data/ext/chipmunk/{cpCollision.h → include/chipmunk/cpCollision.h} +1 -1
- data/ext/chipmunk/{cpHashSet.h → include/chipmunk/cpHashSet.h} +12 -9
- data/ext/chipmunk/{cpPolyShape.h → include/chipmunk/cpPolyShape.h} +13 -2
- data/ext/chipmunk/{cpShape.h → include/chipmunk/cpShape.h} +51 -18
- data/ext/chipmunk/include/chipmunk/cpSpace.h +180 -0
- data/ext/chipmunk/{cpSpaceHash.h → include/chipmunk/cpSpaceHash.h} +18 -9
- data/ext/chipmunk/{cpVect.h → include/chipmunk/cpVect.h} +61 -10
- data/ext/chipmunk/prime.h +32 -32
- data/ext/chipmunk/rb_chipmunk.c +125 -109
- data/ext/chipmunk/rb_chipmunk.h +96 -77
- data/ext/chipmunk/rb_cpArbiter.c +225 -0
- data/ext/chipmunk/rb_cpBB.c +174 -154
- data/ext/chipmunk/rb_cpBody.c +347 -239
- data/ext/chipmunk/rb_cpConstraint.c +346 -0
- data/ext/chipmunk/rb_cpShape.c +455 -292
- data/ext/chipmunk/rb_cpSpace.c +544 -330
- data/ext/chipmunk/rb_cpVect.c +321 -250
- data/lib/chipmunk.rb +28 -15
- data/lib/chipmunk/version.rb +3 -0
- metadata +74 -34
- data/ext/chipmunk/cpJoint.c +0 -553
- data/ext/chipmunk/cpJoint.h +0 -122
- data/ext/chipmunk/cpSpace.h +0 -120
- data/ext/chipmunk/rb_cpJoint.c +0 -136
data/ext/chipmunk/cpPolyShape.c
CHANGED
@@ -21,14 +21,14 @@
|
|
21
21
|
|
22
22
|
#include <stdlib.h>
|
23
23
|
#include <stdio.h>
|
24
|
-
#include <math.h>
|
25
24
|
|
26
25
|
#include "chipmunk.h"
|
26
|
+
#include "chipmunk_unsafe.h"
|
27
27
|
|
28
28
|
cpPolyShape *
|
29
29
|
cpPolyShapeAlloc(void)
|
30
30
|
{
|
31
|
-
return (cpPolyShape *)
|
31
|
+
return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape));
|
32
32
|
}
|
33
33
|
|
34
34
|
static void
|
@@ -87,17 +87,46 @@ cpPolyShapeDestroy(cpShape *shape)
|
|
87
87
|
{
|
88
88
|
cpPolyShape *poly = (cpPolyShape *)shape;
|
89
89
|
|
90
|
-
|
91
|
-
|
90
|
+
cpfree(poly->verts);
|
91
|
+
cpfree(poly->tVerts);
|
92
92
|
|
93
|
-
|
94
|
-
|
93
|
+
cpfree(poly->axes);
|
94
|
+
cpfree(poly->tAxes);
|
95
95
|
}
|
96
96
|
|
97
97
|
static int
|
98
98
|
cpPolyShapePointQuery(cpShape *shape, cpVect p){
|
99
|
-
|
100
|
-
|
99
|
+
return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p);
|
100
|
+
}
|
101
|
+
|
102
|
+
static void
|
103
|
+
cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
|
104
|
+
{
|
105
|
+
cpPolyShape *poly = (cpPolyShape *)shape;
|
106
|
+
cpPolyShapeAxis *axes = poly->tAxes;
|
107
|
+
cpVect *verts = poly->tVerts;
|
108
|
+
int numVerts = poly->numVerts;
|
109
|
+
|
110
|
+
for(int i=0; i<numVerts; i++){
|
111
|
+
cpVect n = axes[i].n;
|
112
|
+
cpFloat an = cpvdot(a, n);
|
113
|
+
if(axes[i].d > an) continue;
|
114
|
+
|
115
|
+
cpFloat bn = cpvdot(b, n);
|
116
|
+
cpFloat t = (axes[i].d - an)/(bn - an);
|
117
|
+
if(t < 0.0f || 1.0f < t) continue;
|
118
|
+
|
119
|
+
cpVect point = cpvlerp(a, b, t);
|
120
|
+
cpFloat dt = -cpvcross(n, point);
|
121
|
+
cpFloat dtMin = -cpvcross(n, verts[i]);
|
122
|
+
cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
|
123
|
+
|
124
|
+
if(dtMin <= dt && dt <= dtMax){
|
125
|
+
info->shape = shape;
|
126
|
+
info->t = t;
|
127
|
+
info->n = n;
|
128
|
+
}
|
129
|
+
}
|
101
130
|
}
|
102
131
|
|
103
132
|
static const cpShapeClass polyClass = {
|
@@ -105,17 +134,50 @@ static const cpShapeClass polyClass = {
|
|
105
134
|
cpPolyShapeCacheData,
|
106
135
|
cpPolyShapeDestroy,
|
107
136
|
cpPolyShapePointQuery,
|
137
|
+
cpPolyShapeSegmentQuery,
|
108
138
|
};
|
109
139
|
|
110
|
-
|
111
|
-
|
112
|
-
{
|
140
|
+
int
|
141
|
+
cpPolyValidate(cpVect *verts, int numVerts)
|
142
|
+
{
|
143
|
+
for(int i=0; i<numVerts; i++){
|
144
|
+
cpVect a = verts[i];
|
145
|
+
cpVect b = verts[(i+1)%numVerts];
|
146
|
+
cpVect c = verts[(i+2)%numVerts];
|
147
|
+
|
148
|
+
if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
|
149
|
+
return 0;
|
150
|
+
}
|
151
|
+
|
152
|
+
return 1;
|
153
|
+
}
|
154
|
+
|
155
|
+
int
|
156
|
+
cpPolyShapeGetNumVerts(cpShape *shape)
|
157
|
+
{
|
158
|
+
cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
|
159
|
+
return ((cpPolyShape *)shape)->numVerts;
|
160
|
+
}
|
161
|
+
|
162
|
+
cpVect
|
163
|
+
cpPolyShapeGetVert(cpShape *shape, int idx)
|
164
|
+
{
|
165
|
+
cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
|
166
|
+
cpAssert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");
|
167
|
+
|
168
|
+
return ((cpPolyShape *)shape)->verts[idx];
|
169
|
+
}
|
170
|
+
|
171
|
+
|
172
|
+
static void
|
173
|
+
setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset)
|
174
|
+
{
|
113
175
|
poly->numVerts = numVerts;
|
114
176
|
|
115
|
-
poly->verts = (cpVect *)
|
116
|
-
poly->tVerts = (cpVect *)
|
117
|
-
poly->axes = (cpPolyShapeAxis *)
|
118
|
-
poly->tAxes = (cpPolyShapeAxis *)
|
177
|
+
poly->verts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
|
178
|
+
poly->tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
|
179
|
+
poly->axes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
|
180
|
+
poly->tAxes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
|
119
181
|
|
120
182
|
for(int i=0; i<numVerts; i++){
|
121
183
|
cpVect a = cpvadd(offset, verts[i]);
|
@@ -126,7 +188,15 @@ cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cp
|
|
126
188
|
poly->axes[i].n = n;
|
127
189
|
poly->axes[i].d = cpvdot(n, a);
|
128
190
|
}
|
191
|
+
}
|
192
|
+
|
193
|
+
cpPolyShape *
|
194
|
+
cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset)
|
195
|
+
{
|
196
|
+
// Fail if the user attempts to pass a concave poly, or a bad winding.
|
197
|
+
cpAssert(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding.");
|
129
198
|
|
199
|
+
setUpVerts(poly, numVerts, verts, offset);
|
130
200
|
cpShapeInit((cpShape *)poly, &polyClass, body);
|
131
201
|
|
132
202
|
return poly;
|
@@ -137,3 +207,35 @@ cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset)
|
|
137
207
|
{
|
138
208
|
return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, numVerts, verts, offset);
|
139
209
|
}
|
210
|
+
|
211
|
+
cpPolyShape *
|
212
|
+
cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height)
|
213
|
+
{
|
214
|
+
cpFloat hw = width/2.0;
|
215
|
+
cpFloat hh = height/2.0;
|
216
|
+
|
217
|
+
cpVect verts[] = {
|
218
|
+
cpv(-hw,-hh),
|
219
|
+
cpv(-hw, hh),
|
220
|
+
cpv( hw, hh),
|
221
|
+
cpv( hw,-hh),
|
222
|
+
};
|
223
|
+
|
224
|
+
return cpPolyShapeInit(poly, body, 4, verts, cpvzero);
|
225
|
+
}
|
226
|
+
|
227
|
+
cpShape *
|
228
|
+
cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height)
|
229
|
+
{
|
230
|
+
return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height);
|
231
|
+
}
|
232
|
+
|
233
|
+
// Unsafe API (chipmunk_unsafe.h)
|
234
|
+
|
235
|
+
void
|
236
|
+
cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset)
|
237
|
+
{
|
238
|
+
cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
|
239
|
+
cpPolyShapeDestroy(shape);
|
240
|
+
setUpVerts((cpPolyShape *)shape, numVerts, verts, offset);
|
241
|
+
}
|
@@ -0,0 +1,128 @@
|
|
1
|
+
/* Copyright (c) 2007 Scott Lembcke
|
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 <stdio.h>
|
23
|
+
#include <stdlib.h>
|
24
|
+
#include <math.h>
|
25
|
+
|
26
|
+
#include "chipmunk.h"
|
27
|
+
#include "constraints/util.h"
|
28
|
+
|
29
|
+
static void
|
30
|
+
preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
|
31
|
+
{
|
32
|
+
cpBody *a = joint->constraint.a;
|
33
|
+
cpBody *b = joint->constraint.b;
|
34
|
+
|
35
|
+
cpFloat angle = joint->angle;
|
36
|
+
cpFloat phase = joint->phase;
|
37
|
+
cpFloat ratchet = joint->ratchet;
|
38
|
+
|
39
|
+
cpFloat delta = b->a - a->a;
|
40
|
+
cpFloat diff = angle - delta;
|
41
|
+
cpFloat pdist = 0.0f;
|
42
|
+
|
43
|
+
if(diff*ratchet > 0.0f){
|
44
|
+
pdist = diff;
|
45
|
+
} else {
|
46
|
+
joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase;
|
47
|
+
}
|
48
|
+
|
49
|
+
// calculate moment of inertia coefficient.
|
50
|
+
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
|
51
|
+
|
52
|
+
// calculate bias velocity
|
53
|
+
cpFloat maxBias = joint->constraint.maxBias;
|
54
|
+
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias);
|
55
|
+
|
56
|
+
// compute max impulse
|
57
|
+
joint->jMax = J_MAX(joint, dt);
|
58
|
+
|
59
|
+
// If the bias is 0, the joint is not at a limit. Reset the impulse.
|
60
|
+
if(!joint->bias)
|
61
|
+
joint->jAcc = 0.0f;
|
62
|
+
|
63
|
+
// apply joint torque
|
64
|
+
a->w -= joint->jAcc*a->i_inv;
|
65
|
+
b->w += joint->jAcc*b->i_inv;
|
66
|
+
}
|
67
|
+
|
68
|
+
static void
|
69
|
+
applyImpulse(cpRatchetJoint *joint)
|
70
|
+
{
|
71
|
+
if(!joint->bias) return; // early exit
|
72
|
+
|
73
|
+
cpBody *a = joint->constraint.a;
|
74
|
+
cpBody *b = joint->constraint.b;
|
75
|
+
|
76
|
+
// compute relative rotational velocity
|
77
|
+
cpFloat wr = b->w - a->w;
|
78
|
+
cpFloat ratchet = joint->ratchet;
|
79
|
+
|
80
|
+
// compute normal impulse
|
81
|
+
cpFloat j = -(joint->bias + wr)*joint->iSum;
|
82
|
+
cpFloat jOld = joint->jAcc;
|
83
|
+
joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint->jMax*cpfabs(ratchet))/ratchet;
|
84
|
+
j = joint->jAcc - jOld;
|
85
|
+
|
86
|
+
// apply impulse
|
87
|
+
a->w -= j*a->i_inv;
|
88
|
+
b->w += j*b->i_inv;
|
89
|
+
}
|
90
|
+
|
91
|
+
static cpFloat
|
92
|
+
getImpulse(cpRatchetJoint *joint)
|
93
|
+
{
|
94
|
+
return cpfabs(joint->jAcc);
|
95
|
+
}
|
96
|
+
|
97
|
+
static const cpConstraintClass klass = {
|
98
|
+
(cpConstraintPreStepFunction)preStep,
|
99
|
+
(cpConstraintApplyImpulseFunction)applyImpulse,
|
100
|
+
(cpConstraintGetImpulseFunction)getImpulse,
|
101
|
+
};
|
102
|
+
CP_DefineClassGetter(cpRatchetJoint)
|
103
|
+
|
104
|
+
cpRatchetJoint *
|
105
|
+
cpRatchetJointAlloc(void)
|
106
|
+
{
|
107
|
+
return (cpRatchetJoint *)cpmalloc(sizeof(cpRatchetJoint));
|
108
|
+
}
|
109
|
+
|
110
|
+
cpRatchetJoint *
|
111
|
+
cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
|
112
|
+
{
|
113
|
+
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
114
|
+
|
115
|
+
joint->angle = 0.0f;
|
116
|
+
joint->phase = phase;
|
117
|
+
joint->ratchet = ratchet;
|
118
|
+
|
119
|
+
joint->angle = b->a - a->a;
|
120
|
+
|
121
|
+
return joint;
|
122
|
+
}
|
123
|
+
|
124
|
+
cpConstraint *
|
125
|
+
cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
|
126
|
+
{
|
127
|
+
return (cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet);
|
128
|
+
}
|
@@ -0,0 +1,122 @@
|
|
1
|
+
/* Copyright (c) 2007 Scott Lembcke
|
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 <stdlib.h>
|
23
|
+
|
24
|
+
#include "chipmunk.h"
|
25
|
+
#include "constraints/util.h"
|
26
|
+
|
27
|
+
static void
|
28
|
+
preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
|
29
|
+
{
|
30
|
+
cpBody *a = joint->constraint.a;
|
31
|
+
cpBody *b = joint->constraint.b;
|
32
|
+
|
33
|
+
cpFloat dist = b->a - a->a;
|
34
|
+
cpFloat pdist = 0.0f;
|
35
|
+
if(dist > joint->max) {
|
36
|
+
pdist = joint->max - dist;
|
37
|
+
} else if(dist < joint->min) {
|
38
|
+
pdist = joint->min - dist;
|
39
|
+
}
|
40
|
+
|
41
|
+
// calculate moment of inertia coefficient.
|
42
|
+
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
|
43
|
+
|
44
|
+
// calculate bias velocity
|
45
|
+
cpFloat maxBias = joint->constraint.maxBias;
|
46
|
+
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
|
47
|
+
|
48
|
+
// compute max impulse
|
49
|
+
joint->jMax = J_MAX(joint, dt);
|
50
|
+
|
51
|
+
// If the bias is 0, the joint is not at a limit. Reset the impulse.
|
52
|
+
if(!joint->bias)
|
53
|
+
joint->jAcc = 0.0f;
|
54
|
+
|
55
|
+
// apply joint torque
|
56
|
+
a->w -= joint->jAcc*a->i_inv;
|
57
|
+
b->w += joint->jAcc*b->i_inv;
|
58
|
+
}
|
59
|
+
|
60
|
+
static void
|
61
|
+
applyImpulse(cpRotaryLimitJoint *joint)
|
62
|
+
{
|
63
|
+
if(!joint->bias) return; // early exit
|
64
|
+
|
65
|
+
cpBody *a = joint->constraint.a;
|
66
|
+
cpBody *b = joint->constraint.b;
|
67
|
+
|
68
|
+
// compute relative rotational velocity
|
69
|
+
cpFloat wr = b->w - a->w;
|
70
|
+
|
71
|
+
// compute normal impulse
|
72
|
+
cpFloat j = -(joint->bias + wr)*joint->iSum;
|
73
|
+
cpFloat jOld = joint->jAcc;
|
74
|
+
if(joint->bias < 0.0f){
|
75
|
+
joint->jAcc = cpfclamp(jOld + j, 0.0f, joint->jMax);
|
76
|
+
} else {
|
77
|
+
joint->jAcc = cpfclamp(jOld + j, -joint->jMax, 0.0f);
|
78
|
+
}
|
79
|
+
j = joint->jAcc - jOld;
|
80
|
+
|
81
|
+
// apply impulse
|
82
|
+
a->w -= j*a->i_inv;
|
83
|
+
b->w += j*b->i_inv;
|
84
|
+
}
|
85
|
+
|
86
|
+
static cpFloat
|
87
|
+
getImpulse(cpRotaryLimitJoint *joint)
|
88
|
+
{
|
89
|
+
return cpfabs(joint->jAcc);
|
90
|
+
}
|
91
|
+
|
92
|
+
static const cpConstraintClass klass = {
|
93
|
+
(cpConstraintPreStepFunction)preStep,
|
94
|
+
(cpConstraintApplyImpulseFunction)applyImpulse,
|
95
|
+
(cpConstraintGetImpulseFunction)getImpulse,
|
96
|
+
};
|
97
|
+
CP_DefineClassGetter(cpRotaryLimitJoint)
|
98
|
+
|
99
|
+
cpRotaryLimitJoint *
|
100
|
+
cpRotaryLimitJointAlloc(void)
|
101
|
+
{
|
102
|
+
return (cpRotaryLimitJoint *)cpmalloc(sizeof(cpRotaryLimitJoint));
|
103
|
+
}
|
104
|
+
|
105
|
+
cpRotaryLimitJoint *
|
106
|
+
cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max)
|
107
|
+
{
|
108
|
+
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
109
|
+
|
110
|
+
joint->min = min;
|
111
|
+
joint->max = max;
|
112
|
+
|
113
|
+
joint->jAcc = 0.0f;
|
114
|
+
|
115
|
+
return joint;
|
116
|
+
}
|
117
|
+
|
118
|
+
cpConstraint *
|
119
|
+
cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max)
|
120
|
+
{
|
121
|
+
return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max);
|
122
|
+
}
|
data/ext/chipmunk/cpShape.c
CHANGED
@@ -20,13 +20,18 @@
|
|
20
20
|
*/
|
21
21
|
|
22
22
|
#include <stdlib.h>
|
23
|
-
#include <assert.h>
|
24
23
|
#include <stdio.h>
|
24
|
+
#include <math.h>
|
25
25
|
|
26
26
|
#include "chipmunk.h"
|
27
|
-
#include "
|
27
|
+
#include "chipmunk_unsafe.h"
|
28
28
|
|
29
|
-
|
29
|
+
#define CP_DefineShapeGetter(struct, type, member, name) \
|
30
|
+
CP_DeclareShapeGetter(struct, type, name){ \
|
31
|
+
cpAssert(shape->klass == &struct##Class, "shape is not a "#struct); \
|
32
|
+
return ((struct *)shape)->member; \
|
33
|
+
}
|
34
|
+
cpHashValue SHAPE_ID_COUNTER = 0;
|
30
35
|
|
31
36
|
void
|
32
37
|
cpResetShapeIdCounter(void)
|
@@ -40,19 +45,19 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body)
|
|
40
45
|
{
|
41
46
|
shape->klass = klass;
|
42
47
|
|
43
|
-
shape->
|
48
|
+
shape->hashid = SHAPE_ID_COUNTER;
|
44
49
|
SHAPE_ID_COUNTER++;
|
45
50
|
|
46
|
-
assert(body != NULL);
|
47
51
|
shape->body = body;
|
52
|
+
shape->sensor = 0;
|
48
53
|
|
49
54
|
shape->e = 0.0f;
|
50
55
|
shape->u = 0.0f;
|
51
56
|
shape->surface_v = cpvzero;
|
52
57
|
|
53
58
|
shape->collision_type = 0;
|
54
|
-
shape->group =
|
55
|
-
shape->layers =
|
59
|
+
shape->group = CP_NO_GROUP;
|
60
|
+
shape->layers = CP_ALL_LAYERS;
|
56
61
|
|
57
62
|
shape->data = NULL;
|
58
63
|
|
@@ -70,8 +75,10 @@ cpShapeDestroy(cpShape *shape)
|
|
70
75
|
void
|
71
76
|
cpShapeFree(cpShape *shape)
|
72
77
|
{
|
73
|
-
if(shape)
|
74
|
-
|
78
|
+
if(shape){
|
79
|
+
cpShapeDestroy(shape);
|
80
|
+
cpfree(shape);
|
81
|
+
}
|
75
82
|
}
|
76
83
|
|
77
84
|
cpBB
|
@@ -88,12 +95,32 @@ cpShapePointQuery(cpShape *shape, cpVect p){
|
|
88
95
|
return shape->klass->pointQuery(shape, p);
|
89
96
|
}
|
90
97
|
|
98
|
+
int
|
99
|
+
cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
|
100
|
+
cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero};
|
101
|
+
(*info) = blank;
|
102
|
+
|
103
|
+
shape->klass->segmentQuery(shape, a, b, info);
|
104
|
+
return (info->shape != NULL);
|
105
|
+
}
|
106
|
+
|
107
|
+
void
|
108
|
+
cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info)
|
109
|
+
{
|
110
|
+
printf("Segment Query:\n");
|
111
|
+
printf("\tt: %f\n", info->t);
|
112
|
+
// printf("\tdist: %f\n", info->dist);
|
113
|
+
// printf("\tpoint: %s\n", cpvstr(info->point));
|
114
|
+
printf("\tn: %s\n", cpvstr(info->n));
|
115
|
+
}
|
116
|
+
|
117
|
+
|
91
118
|
|
92
119
|
|
93
120
|
cpCircleShape *
|
94
121
|
cpCircleShapeAlloc(void)
|
95
122
|
{
|
96
|
-
return (cpCircleShape *)
|
123
|
+
return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape));
|
97
124
|
}
|
98
125
|
|
99
126
|
static inline cpBB
|
@@ -114,16 +141,45 @@ cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
|
|
114
141
|
static int
|
115
142
|
cpCircleShapePointQuery(cpShape *shape, cpVect p){
|
116
143
|
cpCircleShape *circle = (cpCircleShape *)shape;
|
144
|
+
return cpvnear(circle->tc, p, circle->r);
|
145
|
+
}
|
146
|
+
|
147
|
+
static void
|
148
|
+
circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
|
149
|
+
{
|
150
|
+
// umm... gross I normally frown upon such things
|
151
|
+
a = cpvsub(a, center);
|
152
|
+
b = cpvsub(b, center);
|
153
|
+
|
154
|
+
cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b);
|
155
|
+
cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b);
|
156
|
+
cpFloat qc = cpvdot(a, a) - r*r;
|
117
157
|
|
118
|
-
cpFloat
|
119
|
-
|
158
|
+
cpFloat det = qb*qb - 4.0f*qa*qc;
|
159
|
+
|
160
|
+
if(det >= 0.0f){
|
161
|
+
cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa);
|
162
|
+
if(0.0f<= t && t <= 1.0f){
|
163
|
+
info->shape = shape;
|
164
|
+
info->t = t;
|
165
|
+
info->n = cpvnormalize(cpvlerp(a, b, t));
|
166
|
+
}
|
167
|
+
}
|
168
|
+
}
|
169
|
+
|
170
|
+
static void
|
171
|
+
cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
|
172
|
+
{
|
173
|
+
cpCircleShape *circle = (cpCircleShape *)shape;
|
174
|
+
circleSegmentQuery(shape, circle->tc, circle->r, a, b, info);
|
120
175
|
}
|
121
176
|
|
122
|
-
static const cpShapeClass
|
177
|
+
static const cpShapeClass cpCircleShapeClass = {
|
123
178
|
CP_CIRCLE_SHAPE,
|
124
179
|
cpCircleShapeCacheData,
|
125
180
|
NULL,
|
126
181
|
cpCircleShapePointQuery,
|
182
|
+
cpCircleShapeSegmentQuery,
|
127
183
|
};
|
128
184
|
|
129
185
|
cpCircleShape *
|
@@ -132,7 +188,7 @@ cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect of
|
|
132
188
|
circle->c = offset;
|
133
189
|
circle->r = radius;
|
134
190
|
|
135
|
-
cpShapeInit((cpShape *)circle, &
|
191
|
+
cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body);
|
136
192
|
|
137
193
|
return circle;
|
138
194
|
}
|
@@ -143,10 +199,13 @@ cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset)
|
|
143
199
|
return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset);
|
144
200
|
}
|
145
201
|
|
202
|
+
CP_DefineShapeGetter(cpCircleShape, cpVect, c, Offset)
|
203
|
+
CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius)
|
204
|
+
|
146
205
|
cpSegmentShape *
|
147
206
|
cpSegmentShapeAlloc(void)
|
148
207
|
{
|
149
|
-
return (cpSegmentShape *)
|
208
|
+
return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape));
|
150
209
|
}
|
151
210
|
|
152
211
|
static cpBB
|
@@ -182,11 +241,13 @@ cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
|
|
182
241
|
|
183
242
|
static int
|
184
243
|
cpSegmentShapePointQuery(cpShape *shape, cpVect p){
|
244
|
+
if(!cpBBcontainsVect(shape->bb, p)) return 0;
|
245
|
+
|
185
246
|
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
186
247
|
|
187
248
|
// Calculate normal distance from segment.
|
188
249
|
cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn);
|
189
|
-
cpFloat dist =
|
250
|
+
cpFloat dist = cpfabs(dn) - seg->r;
|
190
251
|
if(dist > 0.0f) return 0;
|
191
252
|
|
192
253
|
// Calculate tangential distance along segment.
|
@@ -216,11 +277,61 @@ cpSegmentShapePointQuery(cpShape *shape, cpVect p){
|
|
216
277
|
return 1;
|
217
278
|
}
|
218
279
|
|
219
|
-
static
|
280
|
+
static void
|
281
|
+
cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
|
282
|
+
{
|
283
|
+
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
284
|
+
cpVect n = seg->tn;
|
285
|
+
// flip n if a is behind the axis
|
286
|
+
if(cpvdot(a, n) < cpvdot(seg->ta, n))
|
287
|
+
n = cpvneg(n);
|
288
|
+
|
289
|
+
cpFloat an = cpvdot(a, n);
|
290
|
+
cpFloat bn = cpvdot(b, n);
|
291
|
+
cpFloat d = cpvdot(seg->ta, n) + seg->r;
|
292
|
+
|
293
|
+
cpFloat t = (d - an)/(bn - an);
|
294
|
+
if(0.0f < t && t < 1.0f){
|
295
|
+
cpVect point = cpvlerp(a, b, t);
|
296
|
+
cpFloat dt = -cpvcross(seg->tn, point);
|
297
|
+
cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
|
298
|
+
cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
|
299
|
+
|
300
|
+
if(dtMin < dt && dt < dtMax){
|
301
|
+
info->shape = shape;
|
302
|
+
info->t = t;
|
303
|
+
info->n = n;
|
304
|
+
|
305
|
+
return; // don't continue on and check endcaps
|
306
|
+
}
|
307
|
+
}
|
308
|
+
|
309
|
+
if(seg->r) {
|
310
|
+
cpSegmentQueryInfo info1; info1.shape = NULL;
|
311
|
+
cpSegmentQueryInfo info2; info2.shape = NULL;
|
312
|
+
circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1);
|
313
|
+
circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2);
|
314
|
+
|
315
|
+
if(info1.shape && !info2.shape){
|
316
|
+
(*info) = info1;
|
317
|
+
} else if(info2.shape && !info1.shape){
|
318
|
+
(*info) = info2;
|
319
|
+
} else if(info1.shape && info2.shape){
|
320
|
+
if(info1.t < info2.t){
|
321
|
+
(*info) = info1;
|
322
|
+
} else {
|
323
|
+
(*info) = info2;
|
324
|
+
}
|
325
|
+
}
|
326
|
+
}
|
327
|
+
}
|
328
|
+
|
329
|
+
static const cpShapeClass cpSegmentShapeClass = {
|
220
330
|
CP_SEGMENT_SHAPE,
|
221
331
|
cpSegmentShapeCacheData,
|
222
332
|
NULL,
|
223
333
|
cpSegmentShapePointQuery,
|
334
|
+
cpSegmentShapeSegmentQuery,
|
224
335
|
};
|
225
336
|
|
226
337
|
cpSegmentShape *
|
@@ -232,7 +343,7 @@ cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloa
|
|
232
343
|
|
233
344
|
seg->r = r;
|
234
345
|
|
235
|
-
cpShapeInit((cpShape *)seg, &
|
346
|
+
cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body);
|
236
347
|
|
237
348
|
return seg;
|
238
349
|
}
|
@@ -242,3 +353,48 @@ cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r)
|
|
242
353
|
{
|
243
354
|
return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r);
|
244
355
|
}
|
356
|
+
|
357
|
+
CP_DefineShapeGetter(cpSegmentShape, cpVect, a, A)
|
358
|
+
CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B)
|
359
|
+
CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal)
|
360
|
+
CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius)
|
361
|
+
|
362
|
+
// Unsafe API (chipmunk_unsafe.h)
|
363
|
+
|
364
|
+
void
|
365
|
+
cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
|
366
|
+
{
|
367
|
+
cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
|
368
|
+
cpCircleShape *circle = (cpCircleShape *)shape;
|
369
|
+
|
370
|
+
circle->r = radius;
|
371
|
+
}
|
372
|
+
|
373
|
+
void
|
374
|
+
cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
|
375
|
+
{
|
376
|
+
cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
|
377
|
+
cpCircleShape *circle = (cpCircleShape *)shape;
|
378
|
+
|
379
|
+
circle->c = offset;
|
380
|
+
}
|
381
|
+
|
382
|
+
void
|
383
|
+
cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
|
384
|
+
{
|
385
|
+
cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
386
|
+
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
387
|
+
|
388
|
+
seg->a = a;
|
389
|
+
seg->b = b;
|
390
|
+
seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
|
391
|
+
}
|
392
|
+
|
393
|
+
void
|
394
|
+
cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
|
395
|
+
{
|
396
|
+
cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
397
|
+
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
398
|
+
|
399
|
+
seg->r = radius;
|
400
|
+
}
|