dchip.cpPivotJoint source code

1 /*
2  * Copyright (c) 2007-2013 Scott Lembcke and Howling Moon Software
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *
14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20  * SOFTWARE.
21  */
22 module dchip.cpPivotJoint;
23 
24 import std.string;
25 
26 import dchip.constraints_util;
27 import dchip.chipmunk;
28 import dchip.cpBody;
29 import dchip.cpConstraint;
30 import dchip.chipmunk_types;
31 import dchip.cpVect;
32 
33 //~ const cpConstraintClass* cpPivotJointGetClass();
34 
35 /// @private
36 struct cpPivotJoint
37 {
38     cpConstraint constraint;
39     cpVect anchr1, anchr2;
40 
41     cpVect r1, r2;
42     cpMat2x2 k;
43 
44     cpVect jAcc;
45     cpVect bias;
46 }
47 
48 mixin CP_DefineConstraintProperty!("cpPivotJoint", cpVect, "anchr1", "Anchr1");
49 mixin CP_DefineConstraintProperty!("cpPivotJoint", cpVect, "anchr2", "Anchr2");
50 
51 void preStep(cpPivotJoint* joint, cpFloat dt)
52 {
53     cpBody* a = joint.constraint.a;
54     cpBody* b = joint.constraint.b;
55 
56     joint.r1 = cpvrotate(joint.anchr1, a.rot);
57     joint.r2 = cpvrotate(joint.anchr2, b.rot);
58 
59     // Calculate mass tensor
60     joint.k = k_tensor(a, b, joint.r1, joint.r2);
61 
62     // calculate bias velocity
63     cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
64     joint.bias = cpvclamp(cpvmult(delta, -bias_coef(joint.constraint.errorBias, dt) / dt), joint.constraint.maxBias);
65 }
66 
67 void applyCachedImpulse(cpPivotJoint* joint, cpFloat dt_coef)
68 {
69     cpBody* a = joint.constraint.a;
70     cpBody* b = joint.constraint.b;
71 
72     apply_impulses(a, b, joint.r1, joint.r2, cpvmult(joint.jAcc, dt_coef));
73 }
74 
75 void applyImpulse(cpPivotJoint* joint, cpFloat dt)
76 {
77     cpBody* a = joint.constraint.a;
78     cpBody* b = joint.constraint.b;
79 
80     cpVect r1 = joint.r1;
81     cpVect r2 = joint.r2;
82 
83     // compute relative velocity
84     cpVect vr = relative_velocity(a, b, r1, r2);
85 
86     // compute normal impulse
87     cpVect j    = cpMat2x2Transform(joint.k, cpvsub(joint.bias, vr));
88     cpVect jOld = joint.jAcc;
89     joint.jAcc = cpvclamp(cpvadd(joint.jAcc, j), joint.constraint.maxForce * dt);
90     j = cpvsub(joint.jAcc, jOld);
91 
92     // apply impulse
93     apply_impulses(a, b, joint.r1, joint.r2, j);
94 }
95 
96 cpFloat getImpulse(cpConstraint* joint)
97 {
98     return cpvlength((cast(cpPivotJoint*)joint).jAcc);
99 }
100 
101 __gshared cpConstraintClass klass;
102 
103 void _initModuleCtor_cpPivotJoint()
104 {
105     klass = cpConstraintClass(
106         cast(cpConstraintPreStepImpl)&preStep,
107         cast(cpConstraintApplyCachedImpulseImpl)&applyCachedImpulse,
108         cast(cpConstraintApplyImpulseImpl)&applyImpulse,
109         cast(cpConstraintGetImpulseImpl)&getImpulse,
110     );
111 };
112 
113 const(cpConstraintClass *) cpPivotJointGetClass()
114 {
115     return cast(cpConstraintClass*)&klass;
116 }
117 
118 cpPivotJoint *
119 cpPivotJointAlloc()
120 {
121     return cast(cpPivotJoint*)cpcalloc(1, cpPivotJoint.sizeof);
122 }
123 
124 cpPivotJoint* cpPivotJointInit(cpPivotJoint* joint, cpBody* a, cpBody* b, cpVect anchr1, cpVect anchr2)
125 {
126     cpConstraintInit(cast(cpConstraint*)joint, &klass, a, b);
127 
128     joint.anchr1 = anchr1;
129     joint.anchr2 = anchr2;
130 
131     joint.jAcc = cpvzero;
132 
133     return joint;
134 }
135 
136 cpConstraint* cpPivotJointNew2(cpBody* a, cpBody* b, cpVect anchr1, cpVect anchr2)
137 {
138     return cast(cpConstraint*)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2);
139 }
140 
141 cpConstraint* cpPivotJointNew(cpBody* a, cpBody* b, cpVect pivot)
142 {
143     cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot);
144     cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot);
145     return cpPivotJointNew2(a, b, anchr1, anchr2);
146 }