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
@@ -0,0 +1,99 @@
|
|
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(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
|
29
|
+
{
|
30
|
+
cpBody *a = joint->constraint.a;
|
31
|
+
cpBody *b = joint->constraint.b;
|
32
|
+
|
33
|
+
// calculate moment of inertia coefficient.
|
34
|
+
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
|
35
|
+
|
36
|
+
// compute max impulse
|
37
|
+
joint->jMax = J_MAX(joint, dt);
|
38
|
+
|
39
|
+
// apply joint torque
|
40
|
+
a->w -= joint->jAcc*a->i_inv;
|
41
|
+
b->w += joint->jAcc*b->i_inv;
|
42
|
+
}
|
43
|
+
|
44
|
+
static void
|
45
|
+
applyImpulse(cpSimpleMotor *joint)
|
46
|
+
{
|
47
|
+
cpBody *a = joint->constraint.a;
|
48
|
+
cpBody *b = joint->constraint.b;
|
49
|
+
|
50
|
+
// compute relative rotational velocity
|
51
|
+
cpFloat wr = b->w - a->w + joint->rate;
|
52
|
+
|
53
|
+
// compute normal impulse
|
54
|
+
cpFloat j = -wr*joint->iSum;
|
55
|
+
cpFloat jOld = joint->jAcc;
|
56
|
+
joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax);
|
57
|
+
j = joint->jAcc - jOld;
|
58
|
+
|
59
|
+
// apply impulse
|
60
|
+
a->w -= j*a->i_inv;
|
61
|
+
b->w += j*b->i_inv;
|
62
|
+
}
|
63
|
+
|
64
|
+
static cpFloat
|
65
|
+
getImpulse(cpSimpleMotor *joint)
|
66
|
+
{
|
67
|
+
return cpfabs(joint->jAcc);
|
68
|
+
}
|
69
|
+
|
70
|
+
static const cpConstraintClass klass = {
|
71
|
+
(cpConstraintPreStepFunction)preStep,
|
72
|
+
(cpConstraintApplyImpulseFunction)applyImpulse,
|
73
|
+
(cpConstraintGetImpulseFunction)getImpulse,
|
74
|
+
};
|
75
|
+
CP_DefineClassGetter(cpSimpleMotor)
|
76
|
+
|
77
|
+
cpSimpleMotor *
|
78
|
+
cpSimpleMotorAlloc(void)
|
79
|
+
{
|
80
|
+
return (cpSimpleMotor *)cpmalloc(sizeof(cpSimpleMotor));
|
81
|
+
}
|
82
|
+
|
83
|
+
cpSimpleMotor *
|
84
|
+
cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate)
|
85
|
+
{
|
86
|
+
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
87
|
+
|
88
|
+
joint->rate = rate;
|
89
|
+
|
90
|
+
joint->jAcc = 0.0f;
|
91
|
+
|
92
|
+
return joint;
|
93
|
+
}
|
94
|
+
|
95
|
+
cpConstraint *
|
96
|
+
cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate)
|
97
|
+
{
|
98
|
+
return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate);
|
99
|
+
}
|
@@ -0,0 +1,131 @@
|
|
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(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
|
29
|
+
{
|
30
|
+
cpBody *a = joint->constraint.a;
|
31
|
+
cpBody *b = joint->constraint.b;
|
32
|
+
|
33
|
+
joint->r1 = cpvrotate(joint->anchr1, a->rot);
|
34
|
+
joint->r2 = cpvrotate(joint->anchr2, b->rot);
|
35
|
+
|
36
|
+
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
|
37
|
+
cpFloat dist = cpvlength(delta);
|
38
|
+
cpFloat pdist = 0.0f;
|
39
|
+
if(dist > joint->max) {
|
40
|
+
pdist = dist - joint->max;
|
41
|
+
} else if(dist < joint->min) {
|
42
|
+
pdist = joint->min - dist;
|
43
|
+
dist = -dist;
|
44
|
+
}
|
45
|
+
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
|
46
|
+
|
47
|
+
// calculate mass normal
|
48
|
+
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
|
49
|
+
|
50
|
+
// calculate bias velocity
|
51
|
+
cpFloat maxBias = joint->constraint.maxBias;
|
52
|
+
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
|
53
|
+
|
54
|
+
// compute max impulse
|
55
|
+
joint->jnMax = J_MAX(joint, dt);
|
56
|
+
|
57
|
+
// apply accumulated impulse
|
58
|
+
if(!joint->bias) //{
|
59
|
+
// if bias is 0, then the joint is not at a limit.
|
60
|
+
joint->jnAcc = 0.0f;
|
61
|
+
// } else {
|
62
|
+
cpVect j = cpvmult(joint->n, joint->jnAcc);
|
63
|
+
apply_impulses(a, b, joint->r1, joint->r2, j);
|
64
|
+
// }
|
65
|
+
}
|
66
|
+
|
67
|
+
static void
|
68
|
+
applyImpulse(cpSlideJoint *joint)
|
69
|
+
{
|
70
|
+
if(!joint->bias) return; // early exit
|
71
|
+
|
72
|
+
cpBody *a = joint->constraint.a;
|
73
|
+
cpBody *b = joint->constraint.b;
|
74
|
+
|
75
|
+
cpVect n = joint->n;
|
76
|
+
cpVect r1 = joint->r1;
|
77
|
+
cpVect r2 = joint->r2;
|
78
|
+
|
79
|
+
// compute relative velocity
|
80
|
+
cpVect vr = relative_velocity(a, b, r1, r2);
|
81
|
+
cpFloat vrn = cpvdot(vr, n);
|
82
|
+
|
83
|
+
// compute normal impulse
|
84
|
+
cpFloat jn = (joint->bias - vrn)*joint->nMass;
|
85
|
+
cpFloat jnOld = joint->jnAcc;
|
86
|
+
joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f);
|
87
|
+
jn = joint->jnAcc - jnOld;
|
88
|
+
|
89
|
+
// apply impulse
|
90
|
+
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
|
91
|
+
}
|
92
|
+
|
93
|
+
static cpFloat
|
94
|
+
getImpulse(cpConstraint *joint)
|
95
|
+
{
|
96
|
+
return cpfabs(((cpSlideJoint *)joint)->jnAcc);
|
97
|
+
}
|
98
|
+
|
99
|
+
static const cpConstraintClass klass = {
|
100
|
+
(cpConstraintPreStepFunction)preStep,
|
101
|
+
(cpConstraintApplyImpulseFunction)applyImpulse,
|
102
|
+
(cpConstraintGetImpulseFunction)getImpulse,
|
103
|
+
};
|
104
|
+
CP_DefineClassGetter(cpSlideJoint)
|
105
|
+
|
106
|
+
cpSlideJoint *
|
107
|
+
cpSlideJointAlloc(void)
|
108
|
+
{
|
109
|
+
return (cpSlideJoint *)cpmalloc(sizeof(cpSlideJoint));
|
110
|
+
}
|
111
|
+
|
112
|
+
cpSlideJoint *
|
113
|
+
cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
|
114
|
+
{
|
115
|
+
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
116
|
+
|
117
|
+
joint->anchr1 = anchr1;
|
118
|
+
joint->anchr2 = anchr2;
|
119
|
+
joint->min = min;
|
120
|
+
joint->max = max;
|
121
|
+
|
122
|
+
joint->jnAcc = 0.0f;
|
123
|
+
|
124
|
+
return joint;
|
125
|
+
}
|
126
|
+
|
127
|
+
cpConstraint *
|
128
|
+
cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
|
129
|
+
{
|
130
|
+
return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max);
|
131
|
+
}
|
data/ext/chipmunk/cpSpace.c
CHANGED
@@ -21,100 +21,129 @@
|
|
21
21
|
|
22
22
|
#include <stdlib.h>
|
23
23
|
#include <stdio.h>
|
24
|
+
#include <string.h>
|
24
25
|
#include <math.h>
|
25
|
-
#include <assert.h>
|
26
26
|
|
27
27
|
#include "chipmunk.h"
|
28
28
|
|
29
|
-
int cp_contact_persistence =
|
29
|
+
int cp_contact_persistence = 1;
|
30
|
+
|
31
|
+
#pragma mark Contact Set Helpers
|
30
32
|
|
31
33
|
// Equal function for contactSet.
|
32
34
|
static int
|
33
|
-
contactSetEql(
|
35
|
+
contactSetEql(cpShape **shapes, cpArbiter *arb)
|
34
36
|
{
|
35
|
-
cpShape **shapes = (cpShape **)ptr;
|
36
37
|
cpShape *a = shapes[0];
|
37
38
|
cpShape *b = shapes[1];
|
38
39
|
|
39
|
-
|
40
|
-
|
41
|
-
return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b));
|
40
|
+
return ((a == arb->private_a && b == arb->private_b) || (b == arb->private_a && a == arb->private_b));
|
42
41
|
}
|
43
42
|
|
44
43
|
// Transformation function for contactSet.
|
45
44
|
static void *
|
46
|
-
contactSetTrans(
|
45
|
+
contactSetTrans(cpShape **shapes, cpSpace *space)
|
47
46
|
{
|
48
|
-
|
49
|
-
|
50
|
-
|
51
|
-
|
52
|
-
|
47
|
+
if(space->pooledArbiters->num == 0){
|
48
|
+
// arbiter pool is exhausted, make more
|
49
|
+
int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
|
50
|
+
cpAssert(count, "Buffer size too small.");
|
51
|
+
|
52
|
+
cpArbiter *buffer = (cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
|
53
|
+
cpArrayPush(space->allocatedBuffers, buffer);
|
54
|
+
|
55
|
+
for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
|
56
|
+
}
|
53
57
|
|
54
|
-
return
|
58
|
+
return cpArbiterInit(cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
|
55
59
|
}
|
56
60
|
|
57
|
-
|
58
|
-
typedef struct collFuncData {
|
59
|
-
cpCollFunc func;
|
60
|
-
void *data;
|
61
|
-
} collFuncData;
|
61
|
+
#pragma mark Collision Pair Function Helpers
|
62
62
|
|
63
63
|
// Equals function for collFuncSet.
|
64
64
|
static int
|
65
|
-
collFuncSetEql(
|
65
|
+
collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
|
66
66
|
{
|
67
|
-
|
68
|
-
unsigned int a = ids[0];
|
69
|
-
unsigned int b = ids[1];
|
70
|
-
|
71
|
-
cpCollPairFunc *pair = (cpCollPairFunc *)elt;
|
72
|
-
|
73
|
-
return ((a == pair->a && b == pair->b) || (b == pair->a && a == pair->b));
|
67
|
+
return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b));
|
74
68
|
}
|
75
69
|
|
76
70
|
// Transformation function for collFuncSet.
|
77
71
|
static void *
|
78
|
-
collFuncSetTrans(
|
72
|
+
collFuncSetTrans(cpCollisionHandler *handler, void *unused)
|
79
73
|
{
|
80
|
-
|
81
|
-
|
74
|
+
cpCollisionHandler *copy = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler));
|
75
|
+
(*copy) = (*handler);
|
76
|
+
|
77
|
+
return copy;
|
78
|
+
}
|
82
79
|
|
83
|
-
|
84
|
-
pair->a = ids[0];
|
85
|
-
pair->b = ids[1];
|
86
|
-
pair->func = funcData->func;
|
87
|
-
pair->data = funcData->data;
|
80
|
+
#pragma mark Post Step Function Helpers
|
88
81
|
|
89
|
-
|
90
|
-
|
82
|
+
typedef struct postStepCallback {
|
83
|
+
cpPostStepFunc func;
|
84
|
+
void *obj;
|
85
|
+
void *data;
|
86
|
+
} postStepCallback;
|
91
87
|
|
92
|
-
// Default collision pair function.
|
93
88
|
static int
|
94
|
-
|
89
|
+
postStepFuncSetEql(postStepCallback *a, postStepCallback *b){
|
90
|
+
return a->obj == b->obj;
|
91
|
+
}
|
92
|
+
|
93
|
+
static void *
|
94
|
+
postStepFuncSetTrans(postStepCallback *callback, void *ignored)
|
95
95
|
{
|
96
|
-
|
96
|
+
postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback));
|
97
|
+
(*value) = (*callback);
|
98
|
+
|
99
|
+
return value;
|
97
100
|
}
|
98
101
|
|
102
|
+
#pragma mark Misc Helper Funcs
|
103
|
+
|
104
|
+
// Default collision functions.
|
105
|
+
static int alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
|
106
|
+
static void nothing(cpArbiter *arb, cpSpace *space, void *data){}
|
107
|
+
|
99
108
|
// BBfunc callback for the spatial hash.
|
100
|
-
static cpBB
|
101
|
-
|
109
|
+
static cpBB shapeBBFunc(cpShape *shape){return shape->bb;}
|
110
|
+
|
111
|
+
// Iterator functions for destructors.
|
112
|
+
static void freeWrap(void *ptr, void *unused){ cpfree(ptr);}
|
113
|
+
static void shapeFreeWrap(cpShape *ptr, void *unused){ cpShapeFree(ptr);}
|
114
|
+
static void bodyFreeWrap(cpBody *ptr, void *unused){ cpBodyFree(ptr);}
|
115
|
+
static void constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFree(ptr);}
|
116
|
+
|
117
|
+
#pragma mark Memory Management Functions
|
118
|
+
|
119
|
+
#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact))
|
120
|
+
typedef struct cpContactBuffer {
|
121
|
+
cpContactBufferHeader header;
|
122
|
+
cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
|
123
|
+
} cpContactBuffer;
|
124
|
+
|
125
|
+
static cpContactBufferHeader *
|
126
|
+
cpSpaceAllocContactBuffer(cpSpace *space)
|
102
127
|
{
|
103
|
-
|
104
|
-
|
128
|
+
cpContactBuffer *buffer = (cpContactBuffer *)malloc(sizeof(cpContactBuffer));
|
129
|
+
cpArrayPush(space->allocatedBuffers, buffer);
|
130
|
+
return (cpContactBufferHeader *)buffer;
|
105
131
|
}
|
106
132
|
|
107
|
-
|
108
|
-
|
109
|
-
|
110
|
-
|
111
|
-
|
112
|
-
|
133
|
+
static cpContactBufferHeader *
|
134
|
+
cpContactBufferHeaderInit(cpContactBufferHeader *header, cpSpace *space)
|
135
|
+
{
|
136
|
+
header->stamp = space->stamp;
|
137
|
+
header->next = space->contactBuffersTail;
|
138
|
+
header->numContacts = 0;
|
139
|
+
|
140
|
+
return header;
|
141
|
+
}
|
113
142
|
|
114
|
-
cpSpace*
|
143
|
+
cpSpace *
|
115
144
|
cpSpaceAlloc(void)
|
116
145
|
{
|
117
|
-
return (cpSpace *)
|
146
|
+
return (cpSpace *)cpcalloc(1, sizeof(cpSpace));
|
118
147
|
}
|
119
148
|
|
120
149
|
#define DEFAULT_DIM_SIZE 100.0f
|
@@ -122,6 +151,10 @@ cpSpaceAlloc(void)
|
|
122
151
|
#define DEFAULT_ITERATIONS 10
|
123
152
|
#define DEFAULT_ELASTIC_ITERATIONS 0
|
124
153
|
|
154
|
+
#define MAX_CONTACTS 10000
|
155
|
+
|
156
|
+
cpCollisionHandler defaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL};
|
157
|
+
|
125
158
|
cpSpace*
|
126
159
|
cpSpaceInit(cpSpace *space)
|
127
160
|
{
|
@@ -132,21 +165,32 @@ cpSpaceInit(cpSpace *space)
|
|
132
165
|
space->gravity = cpvzero;
|
133
166
|
space->damping = 1.0f;
|
134
167
|
|
168
|
+
space->locked = 0;
|
135
169
|
space->stamp = 0;
|
136
170
|
|
137
|
-
space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT,
|
138
|
-
space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT,
|
171
|
+
space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpaceHashBBFunc)shapeBBFunc);
|
172
|
+
space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpaceHashBBFunc)shapeBBFunc);
|
173
|
+
|
174
|
+
space->allocatedBuffers = cpArrayNew(0);
|
139
175
|
|
140
176
|
space->bodies = cpArrayNew(0);
|
141
177
|
space->arbiters = cpArrayNew(0);
|
142
|
-
space->
|
178
|
+
space->pooledArbiters = cpArrayNew(0);
|
179
|
+
|
180
|
+
cpContactBufferHeader *header = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), space);
|
181
|
+
space->contactBuffersHead = header;
|
182
|
+
space->contactBuffersTail = header;
|
183
|
+
header->next = header; // Buffers will form a ring, start the ring explicitly
|
143
184
|
|
144
|
-
space->
|
185
|
+
space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans);
|
145
186
|
|
146
|
-
|
147
|
-
|
148
|
-
space->
|
149
|
-
space->collFuncSet
|
187
|
+
space->constraints = cpArrayNew(0);
|
188
|
+
|
189
|
+
space->defaultHandler = defaultHandler;
|
190
|
+
space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, (cpHashSetTransFunc)collFuncSetTrans);
|
191
|
+
space->collFuncSet->default_value = &space->defaultHandler;
|
192
|
+
|
193
|
+
space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, (cpHashSetTransFunc)postStepFuncSetTrans);
|
150
194
|
|
151
195
|
return space;
|
152
196
|
}
|
@@ -165,110 +209,284 @@ cpSpaceDestroy(cpSpace *space)
|
|
165
209
|
|
166
210
|
cpArrayFree(space->bodies);
|
167
211
|
|
168
|
-
cpArrayFree(space->
|
212
|
+
cpArrayFree(space->constraints);
|
169
213
|
|
170
|
-
if(space->contactSet)
|
171
|
-
cpHashSetEach(space->contactSet, &arbiterFreeWrap, NULL);
|
172
214
|
cpHashSetFree(space->contactSet);
|
215
|
+
|
173
216
|
cpArrayFree(space->arbiters);
|
217
|
+
cpArrayFree(space->pooledArbiters);
|
174
218
|
|
175
|
-
if(space->
|
176
|
-
|
177
|
-
|
219
|
+
if(space->allocatedBuffers){
|
220
|
+
cpArrayEach(space->allocatedBuffers, freeWrap, NULL);
|
221
|
+
cpArrayFree(space->allocatedBuffers);
|
222
|
+
}
|
223
|
+
|
224
|
+
if(space->postStepCallbacks){
|
225
|
+
cpHashSetEach(space->postStepCallbacks, freeWrap, NULL);
|
226
|
+
cpHashSetFree(space->postStepCallbacks);
|
227
|
+
}
|
228
|
+
|
229
|
+
if(space->collFuncSet){
|
230
|
+
cpHashSetEach(space->collFuncSet, freeWrap, NULL);
|
231
|
+
cpHashSetFree(space->collFuncSet);
|
232
|
+
}
|
178
233
|
}
|
179
234
|
|
180
235
|
void
|
181
236
|
cpSpaceFree(cpSpace *space)
|
182
237
|
{
|
183
|
-
if(space)
|
184
|
-
|
238
|
+
if(space){
|
239
|
+
cpSpaceDestroy(space);
|
240
|
+
cpfree(space);
|
241
|
+
}
|
185
242
|
}
|
186
243
|
|
187
244
|
void
|
188
245
|
cpSpaceFreeChildren(cpSpace *space)
|
189
246
|
{
|
190
|
-
cpSpaceHashEach(space->staticShapes, &shapeFreeWrap, NULL);
|
191
|
-
cpSpaceHashEach(space->activeShapes, &shapeFreeWrap, NULL);
|
192
|
-
cpArrayEach(space->bodies,
|
193
|
-
cpArrayEach(space->
|
247
|
+
cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
|
248
|
+
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
|
249
|
+
cpArrayEach(space->bodies, (cpArrayIter)&bodyFreeWrap, NULL);
|
250
|
+
cpArrayEach(space->constraints, (cpArrayIter)&constraintFreeWrap, NULL);
|
194
251
|
}
|
195
252
|
|
196
|
-
|
197
|
-
cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b,
|
198
|
-
cpCollFunc func, void *data)
|
199
|
-
{
|
200
|
-
unsigned int ids[] = {a, b};
|
201
|
-
unsigned int hash = CP_HASH_PAIR(a, b);
|
202
|
-
// Remove any old function so the new one will get added.
|
203
|
-
cpSpaceRemoveCollisionPairFunc(space, a, b);
|
204
|
-
|
205
|
-
collFuncData funcData = {func, data};
|
206
|
-
cpHashSetInsert(space->collFuncSet, hash, ids, &funcData);
|
207
|
-
}
|
253
|
+
#pragma mark Collision Handler Function Management
|
208
254
|
|
209
255
|
void
|
210
|
-
|
211
|
-
|
212
|
-
|
213
|
-
|
214
|
-
|
215
|
-
|
256
|
+
cpSpaceAddCollisionHandler(
|
257
|
+
cpSpace *space,
|
258
|
+
cpCollisionType a, cpCollisionType b,
|
259
|
+
cpCollisionBeginFunc begin,
|
260
|
+
cpCollisionPreSolveFunc preSolve,
|
261
|
+
cpCollisionPostSolveFunc postSolve,
|
262
|
+
cpCollisionSeparateFunc separate,
|
263
|
+
void *data
|
264
|
+
){
|
265
|
+
// Remove any old function so the new one will get added.
|
266
|
+
cpSpaceRemoveCollisionHandler(space, a, b);
|
267
|
+
|
268
|
+
cpCollisionHandler handler = {
|
269
|
+
a, b,
|
270
|
+
begin ? begin : alwaysCollide,
|
271
|
+
preSolve ? preSolve : alwaysCollide,
|
272
|
+
postSolve ? postSolve : nothing,
|
273
|
+
separate ? separate : nothing,
|
274
|
+
data
|
275
|
+
};
|
276
|
+
|
277
|
+
cpHashSetInsert(space->collFuncSet, CP_HASH_PAIR(a, b), &handler, NULL);
|
216
278
|
}
|
217
279
|
|
218
280
|
void
|
219
|
-
|
281
|
+
cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
|
220
282
|
{
|
221
|
-
|
222
|
-
space->
|
283
|
+
struct{cpCollisionType a, b;} ids = {a, b};
|
284
|
+
cpCollisionHandler *old_handler = cpHashSetRemove(space->collFuncSet, CP_HASH_PAIR(a, b), &ids);
|
285
|
+
cpfree(old_handler);
|
223
286
|
}
|
224
287
|
|
225
288
|
void
|
289
|
+
cpSpaceSetDefaultCollisionHandler(
|
290
|
+
cpSpace *space,
|
291
|
+
cpCollisionBeginFunc begin,
|
292
|
+
cpCollisionPreSolveFunc preSolve,
|
293
|
+
cpCollisionPostSolveFunc postSolve,
|
294
|
+
cpCollisionSeparateFunc separate,
|
295
|
+
void *data
|
296
|
+
){
|
297
|
+
cpCollisionHandler handler = {
|
298
|
+
0, 0,
|
299
|
+
begin ? begin : alwaysCollide,
|
300
|
+
preSolve ? preSolve : alwaysCollide,
|
301
|
+
postSolve ? postSolve : nothing,
|
302
|
+
separate ? separate : nothing,
|
303
|
+
data
|
304
|
+
};
|
305
|
+
|
306
|
+
space->defaultHandler = handler;
|
307
|
+
}
|
308
|
+
|
309
|
+
#pragma mark Body, Shape, and Joint Management
|
310
|
+
|
311
|
+
#define cpAssertSpaceUnlocked(space) \
|
312
|
+
cpAssert(!space->locked, \
|
313
|
+
"This addition/removal cannot be done safely during a call to cpSpaceStep(). " \
|
314
|
+
"Put these calls into a Post Step Callback." \
|
315
|
+
);
|
316
|
+
|
317
|
+
cpShape *
|
226
318
|
cpSpaceAddShape(cpSpace *space, cpShape *shape)
|
227
319
|
{
|
228
|
-
|
320
|
+
cpAssert(shape->body, "Cannot add a shape with a NULL body.");
|
321
|
+
cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
|
322
|
+
"Cannot add the same shape more than once.");
|
323
|
+
cpAssertSpaceUnlocked(space);
|
324
|
+
|
325
|
+
cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
|
326
|
+
return shape;
|
229
327
|
}
|
230
328
|
|
231
|
-
|
329
|
+
cpShape *
|
232
330
|
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
|
233
331
|
{
|
332
|
+
cpAssert(shape->body, "Cannot add a static shape with a NULL body.");
|
333
|
+
cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
|
334
|
+
"Cannot add the same static shape more than once.");
|
335
|
+
cpAssertSpaceUnlocked(space);
|
336
|
+
|
234
337
|
cpShapeCacheBB(shape);
|
235
|
-
cpSpaceHashInsert(space->staticShapes, shape, shape->
|
338
|
+
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
|
339
|
+
|
340
|
+
return shape;
|
236
341
|
}
|
237
342
|
|
238
|
-
|
343
|
+
cpBody *
|
239
344
|
cpSpaceAddBody(cpSpace *space, cpBody *body)
|
240
345
|
{
|
346
|
+
cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once.");
|
347
|
+
// cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
|
348
|
+
|
241
349
|
cpArrayPush(space->bodies, body);
|
350
|
+
|
351
|
+
return body;
|
242
352
|
}
|
243
353
|
|
244
|
-
|
245
|
-
|
354
|
+
cpConstraint *
|
355
|
+
cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
|
356
|
+
{
|
357
|
+
cpAssert(!cpArrayContains(space->constraints, constraint), "Cannot add the same constraint more than once.");
|
358
|
+
// cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback.
|
359
|
+
|
360
|
+
cpArrayPush(space->constraints, constraint);
|
361
|
+
|
362
|
+
return constraint;
|
363
|
+
}
|
364
|
+
|
365
|
+
typedef struct removalContext {
|
366
|
+
cpSpace *space;
|
367
|
+
cpShape *shape;
|
368
|
+
} removalContext;
|
369
|
+
|
370
|
+
// Hashset filter func to throw away old arbiters.
|
371
|
+
static int
|
372
|
+
contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
|
246
373
|
{
|
247
|
-
|
374
|
+
if(context->shape == arb->private_a || context->shape == arb->private_b){
|
375
|
+
arb->handler->separate(arb, context->space, arb->handler->data);
|
376
|
+
cpArrayPush(context->space->pooledArbiters, arb);
|
377
|
+
return 0;
|
378
|
+
}
|
379
|
+
|
380
|
+
return 1;
|
248
381
|
}
|
249
382
|
|
250
383
|
void
|
251
384
|
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
|
252
385
|
{
|
253
|
-
|
386
|
+
cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
|
387
|
+
"Cannot remove a shape that was never added to the space. (Removed twice maybe?)");
|
388
|
+
cpAssertSpaceUnlocked(space);
|
389
|
+
|
390
|
+
removalContext context = {space, shape};
|
391
|
+
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
|
392
|
+
cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
|
254
393
|
}
|
255
394
|
|
256
395
|
void
|
257
396
|
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
|
258
397
|
{
|
259
|
-
|
398
|
+
cpAssertWarn(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
|
399
|
+
"Cannot remove a static shape that was never added to the space. (Removed twice maybe?)");
|
400
|
+
cpAssertSpaceUnlocked(space);
|
401
|
+
|
402
|
+
removalContext context = {space, shape};
|
403
|
+
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
|
404
|
+
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
|
260
405
|
}
|
261
406
|
|
262
407
|
void
|
263
408
|
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
|
264
409
|
{
|
410
|
+
cpAssertWarn(cpArrayContains(space->bodies, body),
|
411
|
+
"Cannot remove a body that was never added to the space. (Removed twice maybe?)");
|
412
|
+
cpAssertSpaceUnlocked(space);
|
413
|
+
|
265
414
|
cpArrayDeleteObj(space->bodies, body);
|
266
415
|
}
|
267
416
|
|
268
417
|
void
|
269
|
-
|
418
|
+
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
|
419
|
+
{
|
420
|
+
cpAssertWarn(cpArrayContains(space->constraints, constraint),
|
421
|
+
"Cannot remove a constraint that was never added to the space. (Removed twice maybe?)");
|
422
|
+
// cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
|
423
|
+
|
424
|
+
cpArrayDeleteObj(space->constraints, constraint);
|
425
|
+
}
|
426
|
+
|
427
|
+
#pragma mark Post Step Functions
|
428
|
+
|
429
|
+
void
|
430
|
+
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
|
431
|
+
{
|
432
|
+
postStepCallback callback = {func, obj, data};
|
433
|
+
cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL);
|
434
|
+
}
|
435
|
+
|
436
|
+
static void
|
437
|
+
removeAndFreeShapeAndBody(cpShape *shape, cpSpace *space)
|
438
|
+
{
|
439
|
+
cpSpaceRemoveShape(space, shape);
|
440
|
+
cpShapeFree(shape);
|
441
|
+
}
|
442
|
+
|
443
|
+
void
|
444
|
+
cpSpacePostStepRemoveAndFreeShapeAndBody(cpSpace *space, cpShape *shape)
|
445
|
+
{
|
446
|
+
cpSpaceAddPostStepCallback(space, (cpPostStepFunc)removeAndFreeShapeAndBody, shape, space);
|
447
|
+
}
|
448
|
+
|
449
|
+
#pragma mark Point Query Functions
|
450
|
+
|
451
|
+
typedef struct pointQueryContext {
|
452
|
+
cpLayers layers;
|
453
|
+
cpGroup group;
|
454
|
+
cpSpacePointQueryFunc func;
|
455
|
+
void *data;
|
456
|
+
} pointQueryContext;
|
457
|
+
|
458
|
+
static void
|
459
|
+
pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context)
|
460
|
+
{
|
461
|
+
if(
|
462
|
+
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
|
463
|
+
cpShapePointQuery(shape, *point)
|
464
|
+
){
|
465
|
+
context->func(shape, context->data);
|
466
|
+
}
|
467
|
+
}
|
468
|
+
|
469
|
+
void
|
470
|
+
cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
|
471
|
+
{
|
472
|
+
pointQueryContext context = {layers, group, func, data};
|
473
|
+
cpSpaceHashPointQuery(space->activeShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
|
474
|
+
cpSpaceHashPointQuery(space->staticShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
|
475
|
+
}
|
476
|
+
|
477
|
+
static void
|
478
|
+
rememberLastPointQuery(cpShape *shape, cpShape **outShape)
|
270
479
|
{
|
271
|
-
|
480
|
+
(*outShape) = shape;
|
481
|
+
}
|
482
|
+
|
483
|
+
cpShape *
|
484
|
+
cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group)
|
485
|
+
{
|
486
|
+
cpShape *shape = NULL;
|
487
|
+
cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape);
|
488
|
+
|
489
|
+
return shape;
|
272
490
|
}
|
273
491
|
|
274
492
|
void
|
@@ -280,11 +498,134 @@ cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
|
|
280
498
|
func((cpBody *)bodies->arr[i], data);
|
281
499
|
}
|
282
500
|
|
501
|
+
#pragma mark Segment Query Functions
|
502
|
+
|
503
|
+
typedef struct segQueryContext {
|
504
|
+
cpVect start, end;
|
505
|
+
cpLayers layers;
|
506
|
+
cpGroup group;
|
507
|
+
cpSpaceSegmentQueryFunc func;
|
508
|
+
int anyCollision;
|
509
|
+
} segQueryContext;
|
510
|
+
|
511
|
+
static cpFloat
|
512
|
+
segQueryFunc(segQueryContext *context, cpShape *shape, void *data)
|
513
|
+
{
|
514
|
+
cpSegmentQueryInfo info;
|
515
|
+
|
516
|
+
if(
|
517
|
+
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
|
518
|
+
cpShapeSegmentQuery(shape, context->start, context->end, &info)
|
519
|
+
){
|
520
|
+
if(context->func){
|
521
|
+
context->func(shape, info.t, info.n, data);
|
522
|
+
}
|
523
|
+
|
524
|
+
context->anyCollision = 1;
|
525
|
+
}
|
526
|
+
|
527
|
+
return 1.0f;
|
528
|
+
}
|
529
|
+
|
530
|
+
int
|
531
|
+
cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
|
532
|
+
{
|
533
|
+
segQueryContext context = {
|
534
|
+
start, end,
|
535
|
+
layers, group,
|
536
|
+
func,
|
537
|
+
0,
|
538
|
+
};
|
539
|
+
|
540
|
+
cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
|
541
|
+
cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
|
542
|
+
|
543
|
+
return context.anyCollision;
|
544
|
+
}
|
545
|
+
|
546
|
+
typedef struct segQueryFirstContext {
|
547
|
+
cpVect start, end;
|
548
|
+
cpLayers layers;
|
549
|
+
cpGroup group;
|
550
|
+
} segQueryFirstContext;
|
551
|
+
|
552
|
+
static cpFloat
|
553
|
+
segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out)
|
554
|
+
{
|
555
|
+
cpSegmentQueryInfo info;// = {NULL, 1.0f, cpvzero};
|
556
|
+
if(
|
557
|
+
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
|
558
|
+
cpShapeSegmentQuery(shape, context->start, context->end, &info)
|
559
|
+
){
|
560
|
+
if(info.t < out->t){
|
561
|
+
out->shape = info.shape;
|
562
|
+
out->t = info.t;
|
563
|
+
out->n = info.n;
|
564
|
+
}
|
565
|
+
|
566
|
+
return info.t;
|
567
|
+
}
|
568
|
+
|
569
|
+
return 1.0f;
|
570
|
+
}
|
571
|
+
|
572
|
+
cpShape *
|
573
|
+
cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out)
|
574
|
+
{
|
575
|
+
cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero};
|
576
|
+
if(out){
|
577
|
+
(*out) = info;
|
578
|
+
} else {
|
579
|
+
out = &info;
|
580
|
+
}
|
581
|
+
|
582
|
+
out->t = 1.0f;
|
583
|
+
|
584
|
+
segQueryFirstContext context = {
|
585
|
+
start, end,
|
586
|
+
layers, group
|
587
|
+
};
|
588
|
+
|
589
|
+
cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
|
590
|
+
cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
|
591
|
+
|
592
|
+
return out->shape;
|
593
|
+
}
|
594
|
+
|
595
|
+
#pragma mark BB Query functions
|
596
|
+
|
597
|
+
typedef struct bbQueryContext {
|
598
|
+
cpLayers layers;
|
599
|
+
cpGroup group;
|
600
|
+
cpSpaceBBQueryFunc func;
|
601
|
+
void *data;
|
602
|
+
} bbQueryContext;
|
603
|
+
|
604
|
+
static void
|
605
|
+
bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context)
|
606
|
+
{
|
607
|
+
if(
|
608
|
+
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
|
609
|
+
cpBBintersects(*bb, shape->bb)
|
610
|
+
){
|
611
|
+
context->func(shape, context->data);
|
612
|
+
}
|
613
|
+
}
|
614
|
+
|
615
|
+
void
|
616
|
+
cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
|
617
|
+
{
|
618
|
+
bbQueryContext context = {layers, group, func, data};
|
619
|
+
cpSpaceHashQuery(space->activeShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
|
620
|
+
cpSpaceHashQuery(space->staticShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
|
621
|
+
}
|
622
|
+
|
623
|
+
#pragma mark Spatial Hash Management
|
624
|
+
|
283
625
|
// Iterator function used for updating shape BBoxes.
|
284
626
|
static void
|
285
|
-
updateBBCache(
|
627
|
+
updateBBCache(cpShape *shape, void *unused)
|
286
628
|
{
|
287
|
-
cpShape *shape = (cpShape *)ptr;
|
288
629
|
cpShapeCacheBB(shape);
|
289
630
|
}
|
290
631
|
|
@@ -304,44 +645,37 @@ cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
|
|
304
645
|
void
|
305
646
|
cpSpaceRehashStatic(cpSpace *space)
|
306
647
|
{
|
307
|
-
cpSpaceHashEach(space->staticShapes, &updateBBCache, NULL);
|
648
|
+
cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&updateBBCache, NULL);
|
308
649
|
cpSpaceHashRehash(space->staticShapes);
|
309
650
|
}
|
310
651
|
|
311
|
-
|
312
|
-
cpSpacePointQueryFunc func;
|
313
|
-
void *data;
|
314
|
-
} pointQueryFuncPair;
|
652
|
+
#pragma mark Collision Detection Functions
|
315
653
|
|
316
|
-
static
|
317
|
-
|
654
|
+
static cpContactBufferHeader *
|
655
|
+
cpSpaceGetFreeContactBuffer(cpSpace *space)
|
318
656
|
{
|
319
|
-
|
320
|
-
|
321
|
-
|
322
|
-
|
323
|
-
|
324
|
-
|
325
|
-
|
657
|
+
if(space->stamp - space->contactBuffersTail->stamp > cp_contact_persistence){
|
658
|
+
cpContactBufferHeader *header = space->contactBuffersTail;
|
659
|
+
space->contactBuffersTail = header->next;
|
660
|
+
|
661
|
+
return cpContactBufferHeaderInit(header, space);
|
662
|
+
} else {
|
663
|
+
cpContactBufferHeader *header = cpSpaceAllocContactBuffer(space);
|
664
|
+
return cpContactBufferHeaderInit(header, space);
|
665
|
+
}
|
326
666
|
}
|
327
667
|
|
328
668
|
static void
|
329
|
-
|
330
|
-
{
|
331
|
-
pointQueryFuncPair pair = {func, data};
|
332
|
-
cpSpaceHashPointQuery(hash, point, pointQueryHelper, &pair);
|
333
|
-
}
|
334
|
-
|
335
|
-
void
|
336
|
-
cpSpaceShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data)
|
337
|
-
{
|
338
|
-
pointQuery(space->activeShapes, point, func, data);
|
339
|
-
}
|
340
|
-
|
341
|
-
void
|
342
|
-
cpSpaceStaticShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data)
|
669
|
+
cpSpacePushNewContactBuffer(cpSpace *space)
|
343
670
|
{
|
344
|
-
|
671
|
+
// for(cpContactBuffer *buffer = space->contactBuffersTail; buffer != space->contactBuffersHead; buffer = buffer->next){
|
672
|
+
// printf("%p -> ", buffer);
|
673
|
+
// }
|
674
|
+
// printf("%p (head)\n", space->contactBuffersHead);
|
675
|
+
|
676
|
+
cpContactBufferHeader *buffer = cpSpaceGetFreeContactBuffer(space);
|
677
|
+
space->contactBuffersHead->next = buffer;
|
678
|
+
space->contactBuffersHead = buffer;
|
345
679
|
}
|
346
680
|
|
347
681
|
static inline int
|
@@ -359,17 +693,19 @@ queryReject(cpShape *a, cpShape *b)
|
|
359
693
|
}
|
360
694
|
|
361
695
|
// Callback from the spatial hash.
|
362
|
-
|
363
|
-
|
364
|
-
queryFunc(void *p1, void *p2, void *data)
|
696
|
+
static void
|
697
|
+
queryFunc(cpShape *a, cpShape *b, cpSpace *space)
|
365
698
|
{
|
366
|
-
// Cast the generic pointers from the spatial hash back to usefull types
|
367
|
-
cpShape *a = (cpShape *)p1;
|
368
|
-
cpShape *b = (cpShape *)p2;
|
369
|
-
cpSpace *space = (cpSpace *)data;
|
370
|
-
|
371
699
|
// Reject any of the simple cases
|
372
|
-
if(queryReject(a,b)) return
|
700
|
+
if(queryReject(a,b)) return;
|
701
|
+
|
702
|
+
// Find the collision pair function for the shapes.
|
703
|
+
struct{cpCollisionType a, b;} ids = {a->collision_type, b->collision_type};
|
704
|
+
cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type);
|
705
|
+
cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, &ids);
|
706
|
+
|
707
|
+
int sensor = a->sensor || b->sensor;
|
708
|
+
if(sensor && handler == &space->defaultHandler) return;
|
373
709
|
|
374
710
|
// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
|
375
711
|
if(a->klass->type > b->klass->type){
|
@@ -378,92 +714,101 @@ queryFunc(void *p1, void *p2, void *data)
|
|
378
714
|
b = temp;
|
379
715
|
}
|
380
716
|
|
381
|
-
|
382
|
-
|
383
|
-
|
384
|
-
|
385
|
-
if(!pairFunc->func) return 0; // A NULL pair function means don't collide at all.
|
717
|
+
if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
|
718
|
+
// contact buffer could overflow on the next collision, push a fresh one.
|
719
|
+
cpSpacePushNewContactBuffer(space);
|
720
|
+
}
|
386
721
|
|
387
722
|
// Narrow-phase collision detection.
|
388
|
-
cpContact *contacts =
|
389
|
-
int numContacts = cpCollideShapes(a, b,
|
390
|
-
if(!numContacts) return
|
391
|
-
|
392
|
-
|
393
|
-
|
394
|
-
|
395
|
-
|
396
|
-
|
397
|
-
|
398
|
-
|
399
|
-
|
400
|
-
|
401
|
-
|
402
|
-
|
723
|
+
cpContact *contacts = ((cpContactBuffer *)(space->contactBuffersHead))->contacts + space->contactBuffersHead->numContacts;
|
724
|
+
int numContacts = cpCollideShapes(a, b, contacts);
|
725
|
+
if(!numContacts) return; // Shapes are not colliding.
|
726
|
+
space->contactBuffersHead->numContacts += numContacts;
|
727
|
+
|
728
|
+
// Get an arbiter from space->contactSet for the two shapes.
|
729
|
+
// This is where the persistant contact magic comes from.
|
730
|
+
cpShape *shape_pair[] = {a, b};
|
731
|
+
cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b);
|
732
|
+
cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, space);
|
733
|
+
cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array
|
734
|
+
|
735
|
+
// Call the begin function first if it's the first step
|
736
|
+
if(arb->stamp == -1 && !handler->begin(arb, space, handler->data)){
|
737
|
+
cpArbiterIgnore(arb); // permanently ignore the collision until separation
|
403
738
|
}
|
404
739
|
|
405
|
-
if(
|
406
|
-
//
|
407
|
-
|
408
|
-
//
|
409
|
-
|
410
|
-
|
411
|
-
|
412
|
-
|
413
|
-
// Timestamp the arbiter.
|
414
|
-
arb->stamp = space->stamp;
|
415
|
-
arb->a = a; arb->b = b; // TODO: Investigate why this is still necessary?
|
416
|
-
// Inject the new contact points into the arbiter.
|
417
|
-
cpArbiterInject(arb, contacts, numContacts);
|
418
|
-
|
419
|
-
// Add the arbiter to the list of active arbiters.
|
740
|
+
if(
|
741
|
+
// Ignore the arbiter if it has been flagged
|
742
|
+
(arb->state != cpArbiterStateIgnore) &&
|
743
|
+
// Call preSolve
|
744
|
+
handler->preSolve(arb, space, handler->data) &&
|
745
|
+
// Process, but don't add collisions for sensors.
|
746
|
+
!sensor
|
747
|
+
){
|
420
748
|
cpArrayPush(space->arbiters, arb);
|
421
|
-
|
422
|
-
return numContacts;
|
423
749
|
} else {
|
424
|
-
|
425
|
-
|
426
|
-
|
427
|
-
|
750
|
+
// cpfree(arb->contacts);
|
751
|
+
space->contactBuffersHead->numContacts -= numContacts;
|
752
|
+
arb->contacts = NULL;
|
753
|
+
arb->numContacts = 0;
|
428
754
|
}
|
755
|
+
|
756
|
+
// Time stamp the arbiter so we know it was used recently.
|
757
|
+
arb->stamp = space->stamp;
|
429
758
|
}
|
430
759
|
|
431
760
|
// Iterator for active/static hash collisions.
|
432
761
|
static void
|
433
|
-
active2staticIter(
|
762
|
+
active2staticIter(cpShape *shape, cpSpace *space)
|
434
763
|
{
|
435
|
-
|
436
|
-
cpSpace *space = (cpSpace *)data;
|
437
|
-
cpSpaceHashQuery(space->staticShapes, shape, shape->bb, &queryFunc, space);
|
764
|
+
cpSpaceHashQuery(space->staticShapes, shape, shape->bb, (cpSpaceHashQueryFunc)queryFunc, space);
|
438
765
|
}
|
439
766
|
|
440
|
-
// Hashset
|
767
|
+
// Hashset filter func to throw away old arbiters.
|
441
768
|
static int
|
442
|
-
|
769
|
+
contactSetFilter(cpArbiter *arb, cpSpace *space)
|
443
770
|
{
|
444
|
-
|
445
|
-
cpSpace *space = (cpSpace *)data;
|
771
|
+
int ticks = space->stamp - arb->stamp;
|
446
772
|
|
447
|
-
|
448
|
-
|
773
|
+
// was used last frame, but not this one
|
774
|
+
if(ticks == 1){
|
775
|
+
arb->handler->separate(arb, space, arb->handler->data);
|
776
|
+
arb->stamp = -1; // mark it as a new pair again.
|
777
|
+
}
|
778
|
+
|
779
|
+
if(ticks >= cp_contact_persistence){
|
780
|
+
cpArrayPush(space->pooledArbiters, arb);
|
449
781
|
return 0;
|
450
782
|
}
|
451
783
|
|
452
784
|
return 1;
|
453
785
|
}
|
454
786
|
|
787
|
+
// Hashset filter func to call and throw away post step callbacks.
|
788
|
+
static int
|
789
|
+
postStepCallbackSetFilter(postStepCallback *callback, cpSpace *space)
|
790
|
+
{
|
791
|
+
callback->func(space, callback->obj, callback->data);
|
792
|
+
cpfree(callback);
|
793
|
+
|
794
|
+
return 0;
|
795
|
+
}
|
796
|
+
|
797
|
+
#pragma mark All Important cpSpaceStep() Function
|
798
|
+
|
455
799
|
void
|
456
800
|
cpSpaceStep(cpSpace *space, cpFloat dt)
|
457
801
|
{
|
458
|
-
if(!dt) return; //
|
802
|
+
if(!dt) return; // don't step if the timestep is 0!
|
803
|
+
|
459
804
|
cpFloat dt_inv = 1.0f/dt;
|
460
805
|
|
461
806
|
cpArray *bodies = space->bodies;
|
462
|
-
cpArray *
|
463
|
-
|
807
|
+
cpArray *constraints = space->constraints;
|
808
|
+
|
809
|
+
space->locked = 1;
|
464
810
|
|
465
811
|
// Empty the arbiter list.
|
466
|
-
cpHashSetReject(space->contactSet, &contactSetReject, space);
|
467
812
|
space->arbiters->num = 0;
|
468
813
|
|
469
814
|
// Integrate positions.
|
@@ -473,34 +818,39 @@ cpSpaceStep(cpSpace *space, cpFloat dt)
|
|
473
818
|
}
|
474
819
|
|
475
820
|
// Pre-cache BBoxes and shape data.
|
476
|
-
cpSpaceHashEach(space->activeShapes,
|
821
|
+
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
|
477
822
|
|
478
823
|
// Collide!
|
479
|
-
|
480
|
-
|
824
|
+
cpSpacePushNewContactBuffer(space);
|
825
|
+
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
|
826
|
+
cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
|
827
|
+
|
828
|
+
// Clear out old cached arbiters and dispatch untouch functions
|
829
|
+
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
|
481
830
|
|
482
831
|
// Prestep the arbiters.
|
832
|
+
cpArray *arbiters = space->arbiters;
|
483
833
|
for(int i=0; i<arbiters->num; i++)
|
484
834
|
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
|
485
835
|
|
486
|
-
// Prestep the
|
487
|
-
for(int i=0; i<
|
488
|
-
|
489
|
-
|
836
|
+
// Prestep the constraints.
|
837
|
+
for(int i=0; i<constraints->num; i++){
|
838
|
+
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
839
|
+
constraint->klass->preStep(constraint, dt, dt_inv);
|
490
840
|
}
|
491
841
|
|
492
842
|
for(int i=0; i<space->elasticIterations; i++){
|
493
843
|
for(int j=0; j<arbiters->num; j++)
|
494
844
|
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);
|
495
845
|
|
496
|
-
for(int j=0; j<
|
497
|
-
|
498
|
-
|
846
|
+
for(int j=0; j<constraints->num; j++){
|
847
|
+
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
|
848
|
+
constraint->klass->applyImpulse(constraint);
|
499
849
|
}
|
500
850
|
}
|
501
851
|
|
502
852
|
// Integrate velocities.
|
503
|
-
cpFloat damping =
|
853
|
+
cpFloat damping = cpfpow(1.0f/space->damping, -dt);
|
504
854
|
for(int i=0; i<bodies->num; i++){
|
505
855
|
cpBody *body = (cpBody *)bodies->arr[i];
|
506
856
|
body->velocity_func(body, space->gravity, damping, dt);
|
@@ -509,17 +859,36 @@ cpSpaceStep(cpSpace *space, cpFloat dt)
|
|
509
859
|
for(int i=0; i<arbiters->num; i++)
|
510
860
|
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
|
511
861
|
|
862
|
+
// run the old-style elastic solver if elastic iterations are disabled
|
863
|
+
cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f);
|
864
|
+
|
512
865
|
// Run the impulse solver.
|
513
866
|
for(int i=0; i<space->iterations; i++){
|
514
867
|
for(int j=0; j<arbiters->num; j++)
|
515
|
-
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j],
|
868
|
+
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], elasticCoef);
|
516
869
|
|
517
|
-
for(int j=0; j<
|
518
|
-
|
519
|
-
|
870
|
+
for(int j=0; j<constraints->num; j++){
|
871
|
+
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
|
872
|
+
constraint->klass->applyImpulse(constraint);
|
520
873
|
}
|
521
874
|
}
|
522
|
-
|
875
|
+
|
876
|
+
space->locked = 0;
|
877
|
+
|
878
|
+
// run the post solve callbacks
|
879
|
+
for(int i=0; i<arbiters->num; i++){
|
880
|
+
cpArbiter *arb = arbiters->arr[i];
|
881
|
+
|
882
|
+
cpCollisionHandler *handler = arb->handler;
|
883
|
+
handler->postSolve(arb, space, handler->data);
|
884
|
+
|
885
|
+
arb->state = cpArbiterStateNormal;
|
886
|
+
}
|
887
|
+
|
888
|
+
// Run the post step callbacks
|
889
|
+
// Use filter as an easy way to clear out the queue as it runs
|
890
|
+
cpHashSetFilter(space->postStepCallbacks, (cpHashSetFilterFunc)postStepCallbackSetFilter, space);
|
891
|
+
|
523
892
|
// cpFloat dvsq = cpvdot(space->gravity, space->gravity);
|
524
893
|
// dvsq *= dt*dt * space->damping*space->damping;
|
525
894
|
// for(int i=0; i<bodies->num; i++)
|