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,54 @@
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
+ // TODO: Comment me!
28
+
29
+ cpFloat cp_constraint_bias_coef = 0.1f;
30
+
31
+ void cpConstraintDestroy(cpConstraint *constraint){}
32
+
33
+ void
34
+ cpConstraintFree(cpConstraint *constraint)
35
+ {
36
+ if(constraint){
37
+ cpConstraintDestroy(constraint);
38
+ cpfree(constraint);
39
+ }
40
+ }
41
+
42
+ // *** defined in util.h
43
+
44
+ void
45
+ cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b)
46
+ {
47
+ constraint->klass = klass;
48
+ constraint->a = a;
49
+ constraint->b = b;
50
+
51
+ constraint->maxForce = (cpFloat)INFINITY;
52
+ constraint->biasCoef = cp_constraint_bias_coef;
53
+ constraint->maxBias = (cpFloat)INFINITY;
54
+ }
@@ -0,0 +1,106 @@
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 cpFloat
29
+ defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
30
+ return (relativeAngle - spring->restAngle)*spring->stiffness;
31
+ }
32
+
33
+ static void
34
+ preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
35
+ {
36
+ cpBody *a = spring->constraint.a;
37
+ cpBody *b = spring->constraint.b;
38
+
39
+ spring->iSum = 1.0f/(a->i_inv + b->i_inv);
40
+
41
+ spring->dt = dt;
42
+ spring->target_wrn = 0.0f;
43
+
44
+ // apply spring torque
45
+ cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt;
46
+ a->w -= j_spring*a->i_inv;
47
+ b->w += j_spring*b->i_inv;
48
+ }
49
+
50
+ static void
51
+ applyImpulse(cpDampedRotarySpring *spring)
52
+ {
53
+ cpBody *a = spring->constraint.a;
54
+ cpBody *b = spring->constraint.b;
55
+
56
+ // compute relative velocity
57
+ cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
58
+
59
+ // compute velocity loss from drag
60
+ // not 100% certain this is derived correctly, though it makes sense
61
+ cpFloat w_damp = wrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->iSum));
62
+ spring->target_wrn = wrn - w_damp;
63
+
64
+ //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
65
+ cpFloat j_damp = w_damp*spring->iSum;
66
+ a->w -= j_damp*a->i_inv;
67
+ b->w += j_damp*b->i_inv;
68
+ }
69
+
70
+ static cpFloat
71
+ getImpulse(cpConstraint *constraint)
72
+ {
73
+ return 0.0f;
74
+ }
75
+
76
+ static const cpConstraintClass klass = {
77
+ (cpConstraintPreStepFunction)preStep,
78
+ (cpConstraintApplyImpulseFunction)applyImpulse,
79
+ (cpConstraintGetImpulseFunction)getImpulse,
80
+ };
81
+ CP_DefineClassGetter(cpDampedRotarySpring)
82
+
83
+ cpDampedRotarySpring *
84
+ cpDampedRotarySpringAlloc(void)
85
+ {
86
+ return (cpDampedRotarySpring *)cpmalloc(sizeof(cpDampedRotarySpring));
87
+ }
88
+
89
+ cpDampedRotarySpring *
90
+ cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
91
+ {
92
+ cpConstraintInit((cpConstraint *)spring, &klass, a, b);
93
+
94
+ spring->restAngle = restAngle;
95
+ spring->stiffness = stiffness;
96
+ spring->damping = damping;
97
+ spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque;
98
+
99
+ return spring;
100
+ }
101
+
102
+ cpConstraint *
103
+ cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
104
+ {
105
+ return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping);
106
+ }
@@ -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 cpFloat
29
+ defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
30
+ return (spring->restLength - dist)*spring->stiffness;
31
+ }
32
+
33
+ static void
34
+ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
35
+ {
36
+ cpBody *a = spring->constraint.a;
37
+ cpBody *b = spring->constraint.b;
38
+
39
+ spring->r1 = cpvrotate(spring->anchr1, a->rot);
40
+ spring->r2 = cpvrotate(spring->anchr2, b->rot);
41
+
42
+ cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
43
+ cpFloat dist = cpvlength(delta);
44
+ spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
45
+
46
+ // calculate mass normal
47
+ spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n);
48
+
49
+ spring->dt = dt;
50
+ spring->target_vrn = 0.0f;
51
+
52
+ // apply spring force
53
+ cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
54
+ apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
55
+ }
56
+
57
+ static void
58
+ applyImpulse(cpDampedSpring *spring)
59
+ {
60
+ cpBody *a = spring->constraint.a;
61
+ cpBody *b = spring->constraint.b;
62
+
63
+ cpVect n = spring->n;
64
+ cpVect r1 = spring->r1;
65
+ cpVect r2 = spring->r2;
66
+
67
+ // compute relative velocity
68
+ cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
69
+
70
+ // compute velocity loss from drag
71
+ // not 100% certain this is derived correctly, though it makes sense
72
+ cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass));
73
+ spring->target_vrn = vrn + v_damp;
74
+
75
+ apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
76
+ }
77
+
78
+ static cpFloat
79
+ getImpulse(cpConstraint *constraint)
80
+ {
81
+ return 0.0f;
82
+ }
83
+
84
+ static const cpConstraintClass klass = {
85
+ (cpConstraintPreStepFunction)preStep,
86
+ (cpConstraintApplyImpulseFunction)applyImpulse,
87
+ (cpConstraintGetImpulseFunction)getImpulse,
88
+ };
89
+ CP_DefineClassGetter(cpDampedSpring)
90
+
91
+ cpDampedSpring *
92
+ cpDampedSpringAlloc(void)
93
+ {
94
+ return (cpDampedSpring *)cpmalloc(sizeof(cpDampedSpring));
95
+ }
96
+
97
+ cpDampedSpring *
98
+ cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
99
+ {
100
+ cpConstraintInit((cpConstraint *)spring, cpDampedSpringGetClass(), a, b);
101
+
102
+ spring->anchr1 = anchr1;
103
+ spring->anchr2 = anchr2;
104
+
105
+ spring->restLength = restLength;
106
+ spring->stiffness = stiffness;
107
+ spring->damping = damping;
108
+ spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce;
109
+
110
+ return spring;
111
+ }
112
+
113
+ cpConstraint *
114
+ cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
115
+ {
116
+ return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping);
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(cpGearJoint *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*joint->ratio_inv + joint->ratio*b->i_inv);
35
+
36
+ // calculate bias velocity
37
+ cpFloat maxBias = joint->constraint.maxBias;
38
+ joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(b->a*joint->ratio - a->a - joint->phase), -maxBias, maxBias);
39
+
40
+ // compute max impulse
41
+ joint->jMax = J_MAX(joint, dt);
42
+
43
+ // apply joint torque
44
+ cpFloat j = joint->jAcc;
45
+ a->w -= j*a->i_inv*joint->ratio_inv;
46
+ b->w += j*b->i_inv;
47
+ }
48
+
49
+ static void
50
+ applyImpulse(cpGearJoint *joint)
51
+ {
52
+ cpBody *a = joint->constraint.a;
53
+ cpBody *b = joint->constraint.b;
54
+
55
+ // compute relative rotational velocity
56
+ cpFloat wr = b->w*joint->ratio - a->w;
57
+
58
+ // compute normal impulse
59
+ cpFloat j = (joint->bias - wr)*joint->iSum;
60
+ cpFloat jOld = joint->jAcc;
61
+ joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax);
62
+ j = joint->jAcc - jOld;
63
+
64
+ // apply impulse
65
+ a->w -= j*a->i_inv*joint->ratio_inv;
66
+ b->w += j*b->i_inv;
67
+ }
68
+
69
+ static cpFloat
70
+ getImpulse(cpGearJoint *joint)
71
+ {
72
+ return cpfabs(joint->jAcc);
73
+ }
74
+
75
+ static const cpConstraintClass klass = {
76
+ (cpConstraintPreStepFunction)preStep,
77
+ (cpConstraintApplyImpulseFunction)applyImpulse,
78
+ (cpConstraintGetImpulseFunction)getImpulse,
79
+ };
80
+ CP_DefineClassGetter(cpGearJoint)
81
+
82
+ cpGearJoint *
83
+ cpGearJointAlloc(void)
84
+ {
85
+ return (cpGearJoint *)cpmalloc(sizeof(cpGearJoint));
86
+ }
87
+
88
+ cpGearJoint *
89
+ cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
90
+ {
91
+ cpConstraintInit((cpConstraint *)joint, &klass, a, b);
92
+
93
+ joint->phase = phase;
94
+ joint->ratio = ratio;
95
+ joint->ratio_inv = 1.0f/ratio;
96
+
97
+ joint->jAcc = 0.0f;
98
+
99
+ return joint;
100
+ }
101
+
102
+ cpConstraint *
103
+ cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
104
+ {
105
+ return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio);
106
+ }
107
+
108
+ void
109
+ cpGearJointSetRatio(cpConstraint *constraint, cpFloat value)
110
+ {
111
+ cpConstraintCheckCast(constraint, cpGearJoint);
112
+ ((cpGearJoint *)constraint)->ratio = value;
113
+ ((cpGearJoint *)constraint)->ratio_inv = 1.0f/value;
114
+ }
@@ -0,0 +1,138 @@
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(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
29
+ {
30
+ cpBody *a = joint->constraint.a;
31
+ cpBody *b = joint->constraint.b;
32
+
33
+ // calculate endpoints in worldspace
34
+ cpVect ta = cpBodyLocal2World(a, joint->grv_a);
35
+ cpVect tb = cpBodyLocal2World(a, joint->grv_b);
36
+
37
+ // calculate axis
38
+ cpVect n = cpvrotate(joint->grv_n, a->rot);
39
+ cpFloat d = cpvdot(ta, n);
40
+
41
+ joint->grv_tn = n;
42
+ joint->r2 = cpvrotate(joint->anchr2, b->rot);
43
+
44
+ // calculate tangential distance along the axis of r2
45
+ cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
46
+ // calculate clamping factor and r2
47
+ if(td <= cpvcross(ta, n)){
48
+ joint->clamp = 1.0f;
49
+ joint->r1 = cpvsub(ta, a->p);
50
+ } else if(td >= cpvcross(tb, n)){
51
+ joint->clamp = -1.0f;
52
+ joint->r1 = cpvsub(tb, a->p);
53
+ } else {
54
+ joint->clamp = 0.0f;
55
+ joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
56
+ }
57
+
58
+ // Calculate mass tensor
59
+ k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);
60
+
61
+ // compute max impulse
62
+ joint->jMaxLen = J_MAX(joint, dt);
63
+
64
+ // calculate bias velocity
65
+ cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
66
+ joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
67
+
68
+ // apply accumulated impulse
69
+ apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
70
+ }
71
+
72
+ static inline cpVect
73
+ grooveConstrain(cpGrooveJoint *joint, cpVect j){
74
+ cpVect n = joint->grv_tn;
75
+ cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
76
+ return cpvclamp(jClamp, joint->jMaxLen);
77
+ }
78
+
79
+ static void
80
+ applyImpulse(cpGrooveJoint *joint)
81
+ {
82
+ cpBody *a = joint->constraint.a;
83
+ cpBody *b = joint->constraint.b;
84
+
85
+ cpVect r1 = joint->r1;
86
+ cpVect r2 = joint->r2;
87
+
88
+ // compute impulse
89
+ cpVect vr = relative_velocity(a, b, r1, r2);
90
+
91
+ cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
92
+ cpVect jOld = joint->jAcc;
93
+ joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j));
94
+ j = cpvsub(joint->jAcc, jOld);
95
+
96
+ // apply impulse
97
+ apply_impulses(a, b, joint->r1, joint->r2, j);
98
+ }
99
+
100
+ static cpFloat
101
+ getImpulse(cpGrooveJoint *joint)
102
+ {
103
+ return cpvlength(joint->jAcc);
104
+ }
105
+
106
+ static const cpConstraintClass klass = {
107
+ (cpConstraintPreStepFunction)preStep,
108
+ (cpConstraintApplyImpulseFunction)applyImpulse,
109
+ (cpConstraintGetImpulseFunction)getImpulse,
110
+ };
111
+ CP_DefineClassGetter(cpGrooveJoint)
112
+
113
+ cpGrooveJoint *
114
+ cpGrooveJointAlloc(void)
115
+ {
116
+ return (cpGrooveJoint *)cpmalloc(sizeof(cpGrooveJoint));
117
+ }
118
+
119
+ cpGrooveJoint *
120
+ cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
121
+ {
122
+ cpConstraintInit((cpConstraint *)joint, &klass, a, b);
123
+
124
+ joint->grv_a = groove_a;
125
+ joint->grv_b = groove_b;
126
+ joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
127
+ joint->anchr2 = anchr2;
128
+
129
+ joint->jAcc = cpvzero;
130
+
131
+ return joint;
132
+ }
133
+
134
+ cpConstraint *
135
+ cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
136
+ {
137
+ return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
138
+ }