Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/util.d @ 4:7ebbd4d05553

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
initial commit
author Extrawurst
date Thu, 02 Dec 2010 02:11:26 +0100
parents
children df4ebc8add66
comparison
equal deleted inserted replaced
3:81145a61c2fe 4:7ebbd4d05553
1
2 // written in the D programming language
3
4 module chipmunkd.constraints.util;
5
6 import chipmunkd.cpBody;
7 import chipmunkd.chipmunk_types_h;
8 import chipmunkd.cpVect,chipmunkd.cpVect_h;
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 }