chipmunk 4.1.0 → 5.2.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- data/LICENSE +20 -0
- data/README +60 -0
- data/Rakefile +47 -40
- data/ext/chipmunk/chipmunk.c +39 -3
- data/ext/chipmunk/cpArbiter.c +91 -80
- data/ext/chipmunk/cpArray.c +24 -10
- data/ext/chipmunk/cpBB.c +5 -4
- data/ext/chipmunk/cpBody.c +30 -22
- data/ext/chipmunk/cpCollision.c +54 -53
- data/ext/chipmunk/cpConstraint.c +54 -0
- data/ext/chipmunk/cpDampedRotarySpring.c +106 -0
- data/ext/chipmunk/cpDampedSpring.c +117 -0
- data/ext/chipmunk/cpGearJoint.c +114 -0
- data/ext/chipmunk/cpGrooveJoint.c +138 -0
- data/ext/chipmunk/cpHashSet.c +74 -40
- data/ext/chipmunk/cpPinJoint.c +117 -0
- data/ext/chipmunk/cpPivotJoint.c +114 -0
- data/ext/chipmunk/cpPolyShape.c +117 -15
- data/ext/chipmunk/cpRatchetJoint.c +128 -0
- data/ext/chipmunk/cpRotaryLimitJoint.c +122 -0
- data/ext/chipmunk/cpShape.c +174 -18
- data/ext/chipmunk/cpSimpleMotor.c +99 -0
- data/ext/chipmunk/cpSlideJoint.c +131 -0
- data/ext/chipmunk/cpSpace.c +584 -215
- data/ext/chipmunk/cpSpaceHash.c +191 -105
- data/ext/chipmunk/cpVect.c +18 -10
- data/ext/chipmunk/extconf.rb +34 -4
- data/ext/chipmunk/{chipmunk.h → include/chipmunk/chipmunk.h} +63 -6
- data/ext/chipmunk/include/chipmunk/chipmunk_ffi.h +42 -0
- data/ext/chipmunk/include/chipmunk/chipmunk_types.h +80 -0
- data/ext/chipmunk/include/chipmunk/chipmunk_unsafe.h +54 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpConstraint.h +92 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h +46 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpDampedSpring.h +53 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpGearJoint.h +41 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h +44 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpPinJoint.h +43 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpPivotJoint.h +42 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpRatchetJoint.h +40 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpRotaryLimitJoint.h +39 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpSimpleMotor.h +37 -0
- data/ext/chipmunk/include/chipmunk/constraints/cpSlideJoint.h +44 -0
- data/ext/chipmunk/include/chipmunk/constraints/util.h +116 -0
- data/ext/chipmunk/{cpArbiter.h → include/chipmunk/cpArbiter.h} +66 -15
- data/ext/chipmunk/{cpArray.h → include/chipmunk/cpArray.h} +2 -1
- data/ext/chipmunk/{cpBB.h → include/chipmunk/cpBB.h} +21 -0
- data/ext/chipmunk/{cpBody.h → include/chipmunk/cpBody.h} +37 -9
- data/ext/chipmunk/{cpCollision.h → include/chipmunk/cpCollision.h} +1 -1
- data/ext/chipmunk/{cpHashSet.h → include/chipmunk/cpHashSet.h} +12 -9
- data/ext/chipmunk/{cpPolyShape.h → include/chipmunk/cpPolyShape.h} +13 -2
- data/ext/chipmunk/{cpShape.h → include/chipmunk/cpShape.h} +51 -18
- data/ext/chipmunk/include/chipmunk/cpSpace.h +180 -0
- data/ext/chipmunk/{cpSpaceHash.h → include/chipmunk/cpSpaceHash.h} +18 -9
- data/ext/chipmunk/{cpVect.h → include/chipmunk/cpVect.h} +61 -10
- data/ext/chipmunk/prime.h +32 -32
- data/ext/chipmunk/rb_chipmunk.c +125 -109
- data/ext/chipmunk/rb_chipmunk.h +96 -77
- data/ext/chipmunk/rb_cpArbiter.c +225 -0
- data/ext/chipmunk/rb_cpBB.c +174 -154
- data/ext/chipmunk/rb_cpBody.c +347 -239
- data/ext/chipmunk/rb_cpConstraint.c +346 -0
- data/ext/chipmunk/rb_cpShape.c +455 -292
- data/ext/chipmunk/rb_cpSpace.c +544 -330
- data/ext/chipmunk/rb_cpVect.c +321 -250
- data/lib/chipmunk.rb +28 -15
- data/lib/chipmunk/version.rb +3 -0
- metadata +74 -34
- data/ext/chipmunk/cpJoint.c +0 -553
- data/ext/chipmunk/cpJoint.h +0 -122
- data/ext/chipmunk/cpSpace.h +0 -120
- data/ext/chipmunk/rb_cpJoint.c +0 -136
data/ext/chipmunk/cpHashSet.c
CHANGED
@@ -25,35 +25,31 @@
|
|
25
25
|
#include "chipmunk.h"
|
26
26
|
#include "prime.h"
|
27
27
|
|
28
|
+
static void freeWrap(void *ptr, void *unused){cpfree(ptr);}
|
29
|
+
|
28
30
|
void
|
29
31
|
cpHashSetDestroy(cpHashSet *set)
|
30
32
|
{
|
31
|
-
// Free the chains.
|
32
|
-
for(int i=0; i<set->size; i++){
|
33
|
-
// Free the bins in the chain.
|
34
|
-
cpHashSetBin *bin = set->table[i];
|
35
|
-
while(bin){
|
36
|
-
cpHashSetBin *next = bin->next;
|
37
|
-
free(bin);
|
38
|
-
bin = next;
|
39
|
-
}
|
40
|
-
}
|
41
|
-
|
42
33
|
// Free the table.
|
43
|
-
|
34
|
+
cpfree(set->table);
|
35
|
+
|
36
|
+
cpArrayEach(set->allocatedBuffers, freeWrap, NULL);
|
37
|
+
cpArrayFree(set->allocatedBuffers);
|
44
38
|
}
|
45
39
|
|
46
40
|
void
|
47
41
|
cpHashSetFree(cpHashSet *set)
|
48
42
|
{
|
49
|
-
if(set)
|
50
|
-
|
43
|
+
if(set){
|
44
|
+
cpHashSetDestroy(set);
|
45
|
+
cpfree(set);
|
46
|
+
}
|
51
47
|
}
|
52
48
|
|
53
49
|
cpHashSet *
|
54
50
|
cpHashSetAlloc(void)
|
55
51
|
{
|
56
|
-
return (cpHashSet *)
|
52
|
+
return (cpHashSet *)cpcalloc(1, sizeof(cpHashSet));
|
57
53
|
}
|
58
54
|
|
59
55
|
cpHashSet *
|
@@ -67,7 +63,10 @@ cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTrans
|
|
67
63
|
|
68
64
|
set->default_value = NULL;
|
69
65
|
|
70
|
-
set->table = (cpHashSetBin **)
|
66
|
+
set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *));
|
67
|
+
set->pooledBins = NULL;
|
68
|
+
|
69
|
+
set->allocatedBuffers = cpArrayNew(0);
|
71
70
|
|
72
71
|
return set;
|
73
72
|
}
|
@@ -90,7 +89,7 @@ cpHashSetResize(cpHashSet *set)
|
|
90
89
|
// Get the next approximate doubled prime.
|
91
90
|
int newSize = next_prime(set->size + 1);
|
92
91
|
// Allocate a new table.
|
93
|
-
cpHashSetBin **newTable = (cpHashSetBin **)
|
92
|
+
cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *));
|
94
93
|
|
95
94
|
// Iterate over the chains.
|
96
95
|
for(int i=0; i<set->size; i++){
|
@@ -99,38 +98,68 @@ cpHashSetResize(cpHashSet *set)
|
|
99
98
|
while(bin){
|
100
99
|
cpHashSetBin *next = bin->next;
|
101
100
|
|
102
|
-
int
|
103
|
-
bin->next = newTable[
|
104
|
-
newTable[
|
101
|
+
int idx = bin->hash%newSize;
|
102
|
+
bin->next = newTable[idx];
|
103
|
+
newTable[idx] = bin;
|
105
104
|
|
106
105
|
bin = next;
|
107
106
|
}
|
108
107
|
}
|
109
108
|
|
110
|
-
|
109
|
+
cpfree(set->table);
|
111
110
|
|
112
111
|
set->table = newTable;
|
113
112
|
set->size = newSize;
|
114
113
|
}
|
115
114
|
|
115
|
+
static inline void
|
116
|
+
recycleBin(cpHashSet *set, cpHashSetBin *bin)
|
117
|
+
{
|
118
|
+
bin->next = set->pooledBins;
|
119
|
+
set->pooledBins = bin;
|
120
|
+
bin->elt = NULL;
|
121
|
+
}
|
122
|
+
|
123
|
+
static cpHashSetBin *
|
124
|
+
getUnusedBin(cpHashSet *set)
|
125
|
+
{
|
126
|
+
cpHashSetBin *bin = set->pooledBins;
|
127
|
+
|
128
|
+
if(bin){
|
129
|
+
set->pooledBins = bin->next;
|
130
|
+
return bin;
|
131
|
+
} else {
|
132
|
+
// Pool is exhausted, make more
|
133
|
+
int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin);
|
134
|
+
cpAssert(count, "Buffer size is too small.");
|
135
|
+
|
136
|
+
cpHashSetBin *buffer = (cpHashSetBin *)cpmalloc(CP_BUFFER_BYTES);
|
137
|
+
cpArrayPush(set->allocatedBuffers, buffer);
|
138
|
+
|
139
|
+
// push all but the first one, return the first instead
|
140
|
+
for(int i=1; i<count; i++) recycleBin(set, buffer + i);
|
141
|
+
return buffer;
|
142
|
+
}
|
143
|
+
}
|
144
|
+
|
116
145
|
void *
|
117
|
-
cpHashSetInsert(cpHashSet *set,
|
146
|
+
cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data)
|
118
147
|
{
|
119
|
-
int
|
148
|
+
int idx = hash%set->size;
|
120
149
|
|
121
150
|
// Find the bin with the matching element.
|
122
|
-
cpHashSetBin *bin = set->table[
|
151
|
+
cpHashSetBin *bin = set->table[idx];
|
123
152
|
while(bin && !set->eql(ptr, bin->elt))
|
124
153
|
bin = bin->next;
|
125
154
|
|
126
155
|
// Create it necessary.
|
127
156
|
if(!bin){
|
128
|
-
bin = (
|
157
|
+
bin = getUnusedBin(set);
|
129
158
|
bin->hash = hash;
|
130
159
|
bin->elt = set->trans(ptr, data); // Transform the pointer.
|
131
160
|
|
132
|
-
bin->next = set->table[
|
133
|
-
set->table[
|
161
|
+
bin->next = set->table[idx];
|
162
|
+
set->table[idx] = bin;
|
134
163
|
|
135
164
|
set->entries++;
|
136
165
|
|
@@ -143,14 +172,14 @@ cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data)
|
|
143
172
|
}
|
144
173
|
|
145
174
|
void *
|
146
|
-
cpHashSetRemove(cpHashSet *set,
|
175
|
+
cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr)
|
147
176
|
{
|
148
|
-
int
|
177
|
+
int idx = hash%set->size;
|
149
178
|
|
150
179
|
// Pointer to the previous bin pointer.
|
151
|
-
cpHashSetBin **prev_ptr = &set->table[
|
180
|
+
cpHashSetBin **prev_ptr = &set->table[idx];
|
152
181
|
// Pointer the the current bin.
|
153
|
-
cpHashSetBin *bin = set->table[
|
182
|
+
cpHashSetBin *bin = set->table[idx];
|
154
183
|
|
155
184
|
// Find the bin
|
156
185
|
while(bin && !set->eql(ptr, bin->elt)){
|
@@ -160,12 +189,14 @@ cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr)
|
|
160
189
|
|
161
190
|
// Remove it if it exists.
|
162
191
|
if(bin){
|
163
|
-
// Update the
|
192
|
+
// Update the previous bin pointer to point to the next bin.
|
164
193
|
(*prev_ptr) = bin->next;
|
165
194
|
set->entries--;
|
166
195
|
|
167
196
|
void *return_value = bin->elt;
|
168
|
-
|
197
|
+
|
198
|
+
recycleBin(set, bin);
|
199
|
+
|
169
200
|
return return_value;
|
170
201
|
}
|
171
202
|
|
@@ -173,10 +204,10 @@ cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr)
|
|
173
204
|
}
|
174
205
|
|
175
206
|
void *
|
176
|
-
cpHashSetFind(cpHashSet *set,
|
207
|
+
cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr)
|
177
208
|
{
|
178
|
-
int
|
179
|
-
cpHashSetBin *bin = set->table[
|
209
|
+
int idx = hash%set->size;
|
210
|
+
cpHashSetBin *bin = set->table[idx];
|
180
211
|
while(bin && !set->eql(ptr, bin->elt))
|
181
212
|
bin = bin->next;
|
182
213
|
|
@@ -187,14 +218,17 @@ void
|
|
187
218
|
cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data)
|
188
219
|
{
|
189
220
|
for(int i=0; i<set->size; i++){
|
190
|
-
cpHashSetBin *bin;
|
191
|
-
|
221
|
+
cpHashSetBin *bin = set->table[i];
|
222
|
+
while(bin){
|
223
|
+
cpHashSetBin *next = bin->next;
|
192
224
|
func(bin->elt, data);
|
225
|
+
bin = next;
|
226
|
+
}
|
193
227
|
}
|
194
228
|
}
|
195
229
|
|
196
230
|
void
|
197
|
-
|
231
|
+
cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data)
|
198
232
|
{
|
199
233
|
// Iterate over all the chains.
|
200
234
|
for(int i=0; i<set->size; i++){
|
@@ -210,7 +244,7 @@ cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data)
|
|
210
244
|
(*prev_ptr) = next;
|
211
245
|
|
212
246
|
set->entries--;
|
213
|
-
|
247
|
+
recycleBin(set, bin);
|
214
248
|
}
|
215
249
|
|
216
250
|
bin = next;
|
@@ -0,0 +1,117 @@
|
|
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
|
+
//#include <math.h>
|
24
|
+
|
25
|
+
#include "chipmunk.h"
|
26
|
+
#include "constraints/util.h"
|
27
|
+
|
28
|
+
static void
|
29
|
+
preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
|
30
|
+
{
|
31
|
+
cpBody *a = joint->constraint.a;
|
32
|
+
cpBody *b = joint->constraint.b;
|
33
|
+
|
34
|
+
joint->r1 = cpvrotate(joint->anchr1, a->rot);
|
35
|
+
joint->r2 = cpvrotate(joint->anchr2, b->rot);
|
36
|
+
|
37
|
+
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
|
38
|
+
cpFloat dist = cpvlength(delta);
|
39
|
+
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
|
40
|
+
|
41
|
+
// calculate mass normal
|
42
|
+
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
|
43
|
+
|
44
|
+
// calculate bias velocity
|
45
|
+
cpFloat maxBias = joint->constraint.maxBias;
|
46
|
+
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(dist - joint->dist), -maxBias, maxBias);
|
47
|
+
|
48
|
+
// compute max impulse
|
49
|
+
joint->jnMax = J_MAX(joint, dt);
|
50
|
+
|
51
|
+
// apply accumulated impulse
|
52
|
+
cpVect j = cpvmult(joint->n, joint->jnAcc);
|
53
|
+
apply_impulses(a, b, joint->r1, joint->r2, j);
|
54
|
+
}
|
55
|
+
|
56
|
+
static void
|
57
|
+
applyImpulse(cpPinJoint *joint)
|
58
|
+
{
|
59
|
+
cpBody *a = joint->constraint.a;
|
60
|
+
cpBody *b = joint->constraint.b;
|
61
|
+
cpVect n = joint->n;
|
62
|
+
|
63
|
+
// compute relative velocity
|
64
|
+
cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
|
65
|
+
|
66
|
+
// compute normal impulse
|
67
|
+
cpFloat jn = (joint->bias - vrn)*joint->nMass;
|
68
|
+
cpFloat jnOld = joint->jnAcc;
|
69
|
+
joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
|
70
|
+
jn = joint->jnAcc - jnOld;
|
71
|
+
|
72
|
+
// apply impulse
|
73
|
+
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
|
74
|
+
}
|
75
|
+
|
76
|
+
static cpFloat
|
77
|
+
getImpulse(cpPinJoint *joint)
|
78
|
+
{
|
79
|
+
return cpfabs(joint->jnAcc);
|
80
|
+
}
|
81
|
+
|
82
|
+
static const cpConstraintClass klass = {
|
83
|
+
(cpConstraintPreStepFunction)preStep,
|
84
|
+
(cpConstraintApplyImpulseFunction)applyImpulse,
|
85
|
+
(cpConstraintGetImpulseFunction)getImpulse,
|
86
|
+
};
|
87
|
+
CP_DefineClassGetter(cpPinJoint);
|
88
|
+
|
89
|
+
|
90
|
+
cpPinJoint *
|
91
|
+
cpPinJointAlloc(void)
|
92
|
+
{
|
93
|
+
return (cpPinJoint *)cpmalloc(sizeof(cpPinJoint));
|
94
|
+
}
|
95
|
+
|
96
|
+
cpPinJoint *
|
97
|
+
cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
|
98
|
+
{
|
99
|
+
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
100
|
+
|
101
|
+
joint->anchr1 = anchr1;
|
102
|
+
joint->anchr2 = anchr2;
|
103
|
+
|
104
|
+
cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot));
|
105
|
+
cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot));
|
106
|
+
joint->dist = cpvlength(cpvsub(p2, p1));
|
107
|
+
|
108
|
+
joint->jnAcc = 0.0f;
|
109
|
+
|
110
|
+
return joint;
|
111
|
+
}
|
112
|
+
|
113
|
+
cpConstraint *
|
114
|
+
cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
|
115
|
+
{
|
116
|
+
return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2);
|
117
|
+
}
|
@@ -0,0 +1,114 @@
|
|
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(cpPivotJoint *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
|
+
// Calculate mass tensor
|
37
|
+
k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);
|
38
|
+
|
39
|
+
// compute max impulse
|
40
|
+
joint->jMaxLen = J_MAX(joint, dt);
|
41
|
+
|
42
|
+
// calculate bias velocity
|
43
|
+
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
|
44
|
+
joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
|
45
|
+
|
46
|
+
// apply accumulated impulse
|
47
|
+
apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
|
48
|
+
}
|
49
|
+
|
50
|
+
static void
|
51
|
+
applyImpulse(cpPivotJoint *joint)
|
52
|
+
{
|
53
|
+
cpBody *a = joint->constraint.a;
|
54
|
+
cpBody *b = joint->constraint.b;
|
55
|
+
|
56
|
+
cpVect r1 = joint->r1;
|
57
|
+
cpVect r2 = joint->r2;
|
58
|
+
|
59
|
+
// compute relative velocity
|
60
|
+
cpVect vr = relative_velocity(a, b, r1, r2);
|
61
|
+
|
62
|
+
// compute normal impulse
|
63
|
+
cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
|
64
|
+
cpVect jOld = joint->jAcc;
|
65
|
+
joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen);
|
66
|
+
j = cpvsub(joint->jAcc, jOld);
|
67
|
+
|
68
|
+
// apply impulse
|
69
|
+
apply_impulses(a, b, joint->r1, joint->r2, j);
|
70
|
+
}
|
71
|
+
|
72
|
+
static cpFloat
|
73
|
+
getImpulse(cpConstraint *joint)
|
74
|
+
{
|
75
|
+
return cpvlength(((cpPivotJoint *)joint)->jAcc);
|
76
|
+
}
|
77
|
+
|
78
|
+
static const cpConstraintClass klass = {
|
79
|
+
(cpConstraintPreStepFunction)preStep,
|
80
|
+
(cpConstraintApplyImpulseFunction)applyImpulse,
|
81
|
+
(cpConstraintGetImpulseFunction)getImpulse,
|
82
|
+
};
|
83
|
+
CP_DefineClassGetter(cpPivotJoint)
|
84
|
+
|
85
|
+
cpPivotJoint *
|
86
|
+
cpPivotJointAlloc(void)
|
87
|
+
{
|
88
|
+
return (cpPivotJoint *)cpmalloc(sizeof(cpPivotJoint));
|
89
|
+
}
|
90
|
+
|
91
|
+
cpPivotJoint *
|
92
|
+
cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
|
93
|
+
{
|
94
|
+
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
95
|
+
|
96
|
+
joint->anchr1 = anchr1;
|
97
|
+
joint->anchr2 = anchr2;
|
98
|
+
|
99
|
+
joint->jAcc = cpvzero;
|
100
|
+
|
101
|
+
return joint;
|
102
|
+
}
|
103
|
+
|
104
|
+
cpConstraint *
|
105
|
+
cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
|
106
|
+
{
|
107
|
+
return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2);
|
108
|
+
}
|
109
|
+
|
110
|
+
cpConstraint *
|
111
|
+
cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
|
112
|
+
{
|
113
|
+
return cpPivotJointNew2(a, b, cpBodyWorld2Local(a, pivot), cpBodyWorld2Local(b, pivot));
|
114
|
+
}
|