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.
Files changed (71) hide show
  1. data/LICENSE +20 -0
  2. data/README +60 -0
  3. data/Rakefile +47 -40
  4. data/ext/chipmunk/chipmunk.c +39 -3
  5. data/ext/chipmunk/cpArbiter.c +91 -80
  6. data/ext/chipmunk/cpArray.c +24 -10
  7. data/ext/chipmunk/cpBB.c +5 -4
  8. data/ext/chipmunk/cpBody.c +30 -22
  9. data/ext/chipmunk/cpCollision.c +54 -53
  10. data/ext/chipmunk/cpConstraint.c +54 -0
  11. data/ext/chipmunk/cpDampedRotarySpring.c +106 -0
  12. data/ext/chipmunk/cpDampedSpring.c +117 -0
  13. data/ext/chipmunk/cpGearJoint.c +114 -0
  14. data/ext/chipmunk/cpGrooveJoint.c +138 -0
  15. data/ext/chipmunk/cpHashSet.c +74 -40
  16. data/ext/chipmunk/cpPinJoint.c +117 -0
  17. data/ext/chipmunk/cpPivotJoint.c +114 -0
  18. data/ext/chipmunk/cpPolyShape.c +117 -15
  19. data/ext/chipmunk/cpRatchetJoint.c +128 -0
  20. data/ext/chipmunk/cpRotaryLimitJoint.c +122 -0
  21. data/ext/chipmunk/cpShape.c +174 -18
  22. data/ext/chipmunk/cpSimpleMotor.c +99 -0
  23. data/ext/chipmunk/cpSlideJoint.c +131 -0
  24. data/ext/chipmunk/cpSpace.c +584 -215
  25. data/ext/chipmunk/cpSpaceHash.c +191 -105
  26. data/ext/chipmunk/cpVect.c +18 -10
  27. data/ext/chipmunk/extconf.rb +34 -4
  28. data/ext/chipmunk/{chipmunk.h → include/chipmunk/chipmunk.h} +63 -6
  29. data/ext/chipmunk/include/chipmunk/chipmunk_ffi.h +42 -0
  30. data/ext/chipmunk/include/chipmunk/chipmunk_types.h +80 -0
  31. data/ext/chipmunk/include/chipmunk/chipmunk_unsafe.h +54 -0
  32. data/ext/chipmunk/include/chipmunk/constraints/cpConstraint.h +92 -0
  33. data/ext/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h +46 -0
  34. data/ext/chipmunk/include/chipmunk/constraints/cpDampedSpring.h +53 -0
  35. data/ext/chipmunk/include/chipmunk/constraints/cpGearJoint.h +41 -0
  36. data/ext/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h +44 -0
  37. data/ext/chipmunk/include/chipmunk/constraints/cpPinJoint.h +43 -0
  38. data/ext/chipmunk/include/chipmunk/constraints/cpPivotJoint.h +42 -0
  39. data/ext/chipmunk/include/chipmunk/constraints/cpRatchetJoint.h +40 -0
  40. data/ext/chipmunk/include/chipmunk/constraints/cpRotaryLimitJoint.h +39 -0
  41. data/ext/chipmunk/include/chipmunk/constraints/cpSimpleMotor.h +37 -0
  42. data/ext/chipmunk/include/chipmunk/constraints/cpSlideJoint.h +44 -0
  43. data/ext/chipmunk/include/chipmunk/constraints/util.h +116 -0
  44. data/ext/chipmunk/{cpArbiter.h → include/chipmunk/cpArbiter.h} +66 -15
  45. data/ext/chipmunk/{cpArray.h → include/chipmunk/cpArray.h} +2 -1
  46. data/ext/chipmunk/{cpBB.h → include/chipmunk/cpBB.h} +21 -0
  47. data/ext/chipmunk/{cpBody.h → include/chipmunk/cpBody.h} +37 -9
  48. data/ext/chipmunk/{cpCollision.h → include/chipmunk/cpCollision.h} +1 -1
  49. data/ext/chipmunk/{cpHashSet.h → include/chipmunk/cpHashSet.h} +12 -9
  50. data/ext/chipmunk/{cpPolyShape.h → include/chipmunk/cpPolyShape.h} +13 -2
  51. data/ext/chipmunk/{cpShape.h → include/chipmunk/cpShape.h} +51 -18
  52. data/ext/chipmunk/include/chipmunk/cpSpace.h +180 -0
  53. data/ext/chipmunk/{cpSpaceHash.h → include/chipmunk/cpSpaceHash.h} +18 -9
  54. data/ext/chipmunk/{cpVect.h → include/chipmunk/cpVect.h} +61 -10
  55. data/ext/chipmunk/prime.h +32 -32
  56. data/ext/chipmunk/rb_chipmunk.c +125 -109
  57. data/ext/chipmunk/rb_chipmunk.h +96 -77
  58. data/ext/chipmunk/rb_cpArbiter.c +225 -0
  59. data/ext/chipmunk/rb_cpBB.c +174 -154
  60. data/ext/chipmunk/rb_cpBody.c +347 -239
  61. data/ext/chipmunk/rb_cpConstraint.c +346 -0
  62. data/ext/chipmunk/rb_cpShape.c +455 -292
  63. data/ext/chipmunk/rb_cpSpace.c +544 -330
  64. data/ext/chipmunk/rb_cpVect.c +321 -250
  65. data/lib/chipmunk.rb +28 -15
  66. data/lib/chipmunk/version.rb +3 -0
  67. metadata +74 -34
  68. data/ext/chipmunk/cpJoint.c +0 -553
  69. data/ext/chipmunk/cpJoint.h +0 -122
  70. data/ext/chipmunk/cpSpace.h +0 -120
  71. 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
+ }
@@ -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 = 3;
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(void *ptr, void *elt)
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
- cpArbiter *arb = (cpArbiter *)elt;
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(void *ptr, void *data)
45
+ contactSetTrans(cpShape **shapes, cpSpace *space)
47
46
  {
48
- cpShape **shapes = (cpShape **)ptr;
49
- cpShape *a = shapes[0];
50
- cpShape *b = shapes[1];
51
-
52
- cpSpace *space = (cpSpace *)data;
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 cpArbiterNew(a, b, space->stamp);
58
+ return cpArbiterInit(cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
55
59
  }
56
60
 
57
- // Collision pair function wrapper struct.
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(void *ptr, void *elt)
65
+ collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
66
66
  {
67
- unsigned int *ids = (unsigned int *)ptr;
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(void *ptr, void *data)
72
+ collFuncSetTrans(cpCollisionHandler *handler, void *unused)
79
73
  {
80
- unsigned int *ids = (unsigned int *)ptr;
81
- collFuncData *funcData = (collFuncData *)data;
74
+ cpCollisionHandler *copy = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler));
75
+ (*copy) = (*handler);
76
+
77
+ return copy;
78
+ }
82
79
 
83
- cpCollPairFunc *pair = (cpCollPairFunc *)malloc(sizeof(cpCollPairFunc));
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
- return pair;
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
- alwaysCollide(cpShape *a, cpShape *b, cpContact *arr, int numCon, cpFloat normal_coef, void *data)
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
- return 1;
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
- bbfunc(void *ptr)
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
- cpShape *shape = (cpShape *)ptr;
104
- return shape->bb;
128
+ cpContactBuffer *buffer = (cpContactBuffer *)malloc(sizeof(cpContactBuffer));
129
+ cpArrayPush(space->allocatedBuffers, buffer);
130
+ return (cpContactBufferHeader *)buffer;
105
131
  }
106
132
 
107
- // Iterator functions for destructors.
108
- static void freeWrap(void *ptr, void *unused){ free( ptr);}
109
- static void shapeFreeWrap(void *ptr, void *unused){ cpShapeFree((cpShape *) ptr);}
110
- static void arbiterFreeWrap(void *ptr, void *unused){ cpArbiterFree((cpArbiter *)ptr);}
111
- static void bodyFreeWrap(void *ptr, void *unused){ cpBodyFree((cpBody *) ptr);}
112
- static void jointFreeWrap(void *ptr, void *unused){ cpJointFree((cpJoint *) ptr);}
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 *)calloc(1, sizeof(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, &bbfunc);
138
- space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, &bbfunc);
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->contactSet = cpHashSetNew(0, contactSetEql, contactSetTrans);
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->joints = cpArrayNew(0);
185
+ space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans);
145
186
 
146
- cpCollPairFunc pairFunc = {0, 0, alwaysCollide, NULL};
147
- space->defaultPairFunc = pairFunc;
148
- space->collFuncSet = cpHashSetNew(0, collFuncSetEql, collFuncSetTrans);
149
- space->collFuncSet->default_value = &space->defaultPairFunc;
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->joints);
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->collFuncSet)
176
- cpHashSetEach(space->collFuncSet, &freeWrap, NULL);
177
- cpHashSetFree(space->collFuncSet);
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) cpSpaceDestroy(space);
184
- free(space);
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, &bodyFreeWrap, NULL);
193
- cpArrayEach(space->joints, &jointFreeWrap, NULL);
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
- void
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
- cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b)
211
- {
212
- unsigned int ids[] = {a, b};
213
- unsigned int hash = CP_HASH_PAIR(a, b);
214
- cpCollPairFunc *old_pair = (cpCollPairFunc *)cpHashSetRemove(space->collFuncSet, hash, ids);
215
- free(old_pair);
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
- cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data)
281
+ cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
220
282
  {
221
- cpCollPairFunc pairFunc = {0, 0, (func ? func : alwaysCollide), (func ? data : NULL)};
222
- space->defaultPairFunc = pairFunc;
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
- cpSpaceHashInsert(space->activeShapes, shape, shape->id, shape->bb);
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
- void
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->id, shape->bb);
338
+ cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
339
+
340
+ return shape;
236
341
  }
237
342
 
238
- void
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
- void
245
- cpSpaceAddJoint(cpSpace *space, cpJoint *joint)
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
- cpArrayPush(space->joints, joint);
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
- cpSpaceHashRemove(space->activeShapes, shape, shape->id);
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
- cpSpaceHashRemove(space->staticShapes, shape, shape->id);
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
- cpSpaceRemoveJoint(cpSpace *space, cpJoint *joint)
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
- cpArrayDeleteObj(space->joints, joint);
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(void *ptr, void *unused)
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
- typedef struct pointQueryFuncPair {
312
- cpSpacePointQueryFunc func;
313
- void *data;
314
- } pointQueryFuncPair;
652
+ #pragma mark Collision Detection Functions
315
653
 
316
- static int
317
- pointQueryHelper(void *point, void *obj, void *data)
654
+ static cpContactBufferHeader *
655
+ cpSpaceGetFreeContactBuffer(cpSpace *space)
318
656
  {
319
- cpShape *shape = (cpShape *)obj;
320
- pointQueryFuncPair *pair = (pointQueryFuncPair *)data;
321
-
322
- if(cpShapePointQuery(shape, *((cpVect *)point)))
323
- pair->func(shape, pair->data);
324
-
325
- return 1; // return value needed for historical reasons (value is ignored)
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
- pointQuery(cpSpaceHash *hash, cpVect point, cpSpacePointQueryFunc func, void *data)
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
- pointQuery(space->staticShapes, point, func, data);
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
- // TODO: Refactor this into separate functions?
363
- static int
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 0;
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
- // Find the collision pair function for the shapes.
382
- unsigned int ids[] = {a->collision_type, b->collision_type};
383
- unsigned int hash = CP_HASH_PAIR(a->collision_type, b->collision_type);
384
- cpCollPairFunc *pairFunc = (cpCollPairFunc *)cpHashSetFind(space->collFuncSet, hash, ids);
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 = NULL;
389
- int numContacts = cpCollideShapes(a, b, &contacts);
390
- if(!numContacts) return 0; // Shapes are not colliding.
391
-
392
- // The collision pair function requires objects to be ordered by their collision types.
393
- cpShape *pair_a = a;
394
- cpShape *pair_b = b;
395
- cpFloat normal_coef = 1.0f;
396
-
397
- // Swap them if necessary.
398
- if(pair_a->collision_type != pairFunc->a){
399
- cpShape *temp = pair_a;
400
- pair_a = pair_b;
401
- pair_b = temp;
402
- normal_coef = -1.0f;
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(pairFunc->func(pair_a, pair_b, contacts, numContacts, normal_coef, pairFunc->data)){
406
- // The collision pair function OKed the collision. Record the contact information.
407
-
408
- // Get an arbiter from space->contactSet for the two shapes.
409
- // This is where the persistant contact magic comes from.
410
- cpShape *shape_pair[] = {a, b};
411
- cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, CP_HASH_PAIR(a, b), shape_pair, space);
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
- // The collision pair function rejected the collision.
425
-
426
- free(contacts);
427
- return 0;
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(void *ptr, void *data)
762
+ active2staticIter(cpShape *shape, cpSpace *space)
434
763
  {
435
- cpShape *shape = (cpShape *)ptr;
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 reject func to throw away old arbiters.
767
+ // Hashset filter func to throw away old arbiters.
441
768
  static int
442
- contactSetReject(void *ptr, void *data)
769
+ contactSetFilter(cpArbiter *arb, cpSpace *space)
443
770
  {
444
- cpArbiter *arb = (cpArbiter *)ptr;
445
- cpSpace *space = (cpSpace *)data;
771
+ int ticks = space->stamp - arb->stamp;
446
772
 
447
- if((space->stamp - arb->stamp) > cp_contact_persistence){
448
- cpArbiterFree(arb);
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; // prevents div by zero.
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 *arbiters = space->arbiters;
463
- cpArray *joints = space->joints;
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, &updateBBCache, NULL);
821
+ cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
477
822
 
478
823
  // Collide!
479
- cpSpaceHashEach(space->activeShapes, &active2staticIter, space);
480
- cpSpaceHashQueryRehash(space->activeShapes, &queryFunc, space);
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 joints.
487
- for(int i=0; i<joints->num; i++){
488
- cpJoint *joint = (cpJoint *)joints->arr[i];
489
- joint->klass->preStep(joint, dt_inv);
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<joints->num; j++){
497
- cpJoint *joint = (cpJoint *)joints->arr[j];
498
- joint->klass->applyImpulse(joint);
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 = pow(1.0f/space->damping, -dt);
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], 0.0f);
868
+ cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], elasticCoef);
516
869
 
517
- for(int j=0; j<joints->num; j++){
518
- cpJoint *joint = (cpJoint *)joints->arr[j];
519
- joint->klass->applyImpulse(joint);
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++)