|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.constraints.util;
|
|
|
5
|
|
|
6 import chipmunkd.cpBody;
|
|
15
|
7 import chipmunkd.chipmunk_types;
|
|
|
8 import chipmunkd.cpVect;
|
|
4
|
9
|
|
|
10 template CP_DefineClassGetter(string t)
|
|
|
11 {
|
|
|
12 enum CP_DefineClassGetter =
|
|
|
13 "cpConstraintClass* "~t~"GetClass(){return cast(cpConstraintClass *)&klass;}";
|
|
|
14 }
|
|
|
15
|
|
|
16 template J_MAX(string constraint,string dt)
|
|
|
17 {
|
|
|
18 enum J_MAX = "((cast(cpConstraint *)"~constraint~").maxForce*("~dt~"))";
|
|
|
19 }
|
|
|
20
|
|
|
21 template CONSTRAINT_BEGIN(string constraint,string a_var,string b_var)
|
|
|
22 {
|
|
|
23 enum CONSTRAINT_BEGIN = "cpBody *"~a_var~"; cpBody *"~b_var~";
|
|
|
24 "~a_var~" = (cast(cpConstraint *)"~constraint~")."~a_var~";
|
|
|
25 "~b_var~" = (cast(cpConstraint *)"~constraint~")."~b_var~";
|
|
|
26 if(
|
|
|
27 (cpBodyIsSleeping("~a_var~") || cpBodyIsStatic("~a_var~")) &&
|
|
|
28 (cpBodyIsSleeping("~b_var~") || cpBodyIsStatic("~b_var~"))
|
|
|
29 ) return;
|
|
|
30 ";
|
|
|
31 }
|
|
|
32
|
|
|
33 static cpVect
|
|
|
34 relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
|
|
|
35 cpVect v1_sum = cpvadd(a.v, cpvmult(cpvperp(r1), a.w));
|
|
|
36 cpVect v2_sum = cpvadd(b.v, cpvmult(cpvperp(r2), b.w));
|
|
|
37
|
|
|
38 return cpvsub(v2_sum, v1_sum);
|
|
|
39 }
|
|
|
40
|
|
|
41 static cpFloat
|
|
|
42 normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){
|
|
|
43 return cpvdot(relative_velocity(a, b, r1, r2), n);
|
|
|
44 }
|
|
|
45
|
|
|
46 static void
|
|
|
47 apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
|
|
|
48 {
|
|
|
49 cpBodyApplyImpulse(a, cpvneg(j), r1);
|
|
|
50 cpBodyApplyImpulse(b, j, r2);
|
|
|
51 }
|
|
|
52
|
|
|
53 static void
|
|
|
54 apply_bias_impulse(cpBody *_body, cpVect j, cpVect r)
|
|
|
55 {
|
|
|
56 _body.v_bias = cpvadd(_body.v_bias, cpvmult(j, _body.m_inv));
|
|
|
57 _body.w_bias += _body.i_inv*cpvcross(r, j);
|
|
|
58 }
|
|
|
59
|
|
|
60 static void
|
|
|
61 apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
|
|
|
62 {
|
|
|
63 apply_bias_impulse(a, cpvneg(j), r1);
|
|
|
64 apply_bias_impulse(b, j, r2);
|
|
|
65 }
|
|
|
66
|
|
|
67 static cpVect
|
|
|
68 clamp_vect(cpVect v, cpFloat len)
|
|
|
69 {
|
|
|
70 return cpvclamp(v, len);
|
|
|
71 // return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
|
|
|
72 }
|
|
|
73
|
|
|
74 static cpFloat
|
|
|
75 k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
|
|
|
76 {
|
|
|
77 cpFloat mass_sum = a.m_inv + b.m_inv;
|
|
|
78 cpFloat r1cn = cpvcross(r1, n);
|
|
|
79 cpFloat r2cn = cpvcross(r2, n);
|
|
|
80
|
|
|
81 cpFloat value = mass_sum + a.i_inv*r1cn*r1cn + b.i_inv*r2cn*r2cn;
|
|
|
82 assert(value != 0.0, "Unsolvable collision or constraint.");
|
|
|
83
|
|
|
84 return value;
|
|
|
85 }
|
|
|
86
|
|
|
87 static void
|
|
|
88 k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2)
|
|
|
89 {
|
|
|
90 // calculate mass matrix
|
|
|
91 // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
|
|
|
92 cpFloat k11, k12, k21, k22;
|
|
|
93 cpFloat m_sum = a.m_inv + b.m_inv;
|
|
|
94
|
|
|
95 // start with I*m_sum
|
|
|
96 k11 = m_sum; k12 = 0.0f;
|
|
|
97 k21 = 0.0f; k22 = m_sum;
|
|
|
98
|
|
|
99 // add the influence from r1
|
|
|
100 cpFloat a_i_inv = a.i_inv;
|
|
|
101 cpFloat r1xsq = r1.x * r1.x * a_i_inv;
|
|
|
102 cpFloat r1ysq = r1.y * r1.y * a_i_inv;
|
|
|
103 cpFloat r1nxy = -r1.x * r1.y * a_i_inv;
|
|
|
104 k11 += r1ysq; k12 += r1nxy;
|
|
|
105 k21 += r1nxy; k22 += r1xsq;
|
|
|
106
|
|
|
107 // add the influnce from r2
|
|
|
108 cpFloat b_i_inv = b.i_inv;
|
|
|
109 cpFloat r2xsq = r2.x * r2.x * b_i_inv;
|
|
|
110 cpFloat r2ysq = r2.y * r2.y * b_i_inv;
|
|
|
111 cpFloat r2nxy = -r2.x * r2.y * b_i_inv;
|
|
|
112 k11 += r2ysq; k12 += r2nxy;
|
|
|
113 k21 += r2nxy; k22 += r2xsq;
|
|
|
114
|
|
|
115 // invert
|
|
|
116 cpFloat determinant = k11*k22 - k12*k21;
|
|
|
117 assert(determinant != 0.0, "Unsolvable constraint.");
|
|
|
118
|
|
|
119 cpFloat det_inv = 1.0f/determinant;
|
|
|
120 *k1 = cpv( k22*det_inv, -k12*det_inv);
|
|
|
121 *k2 = cpv(-k21*det_inv, k11*det_inv);
|
|
|
122 }
|
|
|
123
|
|
|
124 static cpVect
|
|
|
125 mult_k(cpVect vr, cpVect k1, cpVect k2)
|
|
|
126 {
|
|
|
127 return cpv(cpvdot(vr, k1), cpvdot(vr, k2));
|
|
|
128 }
|
