dchip.constraints_util 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.constraints_util;
23 
24 import dchip.chipmunk;
25 import dchip.chipmunk_types;
26 import dchip.cpBody;
27 import dchip.cpConstraint;
28 import dchip.cpVect;
29 
30 // These are utility routines to use when creating custom constraints.
31 // I'm not sure if this should be part of the private API or not.
32 // I should probably clean up the naming conventions if it is...
33 
34 cpVect relative_velocity(cpBody* a, cpBody* b, cpVect r1, cpVect r2)
35 {
36     cpVect v1_sum = cpvadd(a.v, cpvmult(cpvperp(r1), a.w));
37     cpVect v2_sum = cpvadd(b.v, cpvmult(cpvperp(r2), b.w));
38 
39     return cpvsub(v2_sum, v1_sum);
40 }
41 
42 cpFloat normal_relative_velocity(cpBody* a, cpBody* b, cpVect r1, cpVect r2, cpVect n)
43 {
44     return cpvdot(relative_velocity(a, b, r1, r2), n);
45 }
46 
47 void apply_impulse(cpBody* body_, cpVect j, cpVect r)
48 {
49     body_.v  = cpvadd(body_.v, cpvmult(j, body_.m_inv));
50     body_.w += body_.i_inv * cpvcross(r, j);
51 }
52 
53 void apply_impulses(cpBody* a, cpBody* b, cpVect r1, cpVect r2, cpVect j)
54 {
55     apply_impulse(a, cpvneg(j), r1);
56     apply_impulse(b, j, r2);
57 }
58 
59 void apply_bias_impulse(cpBody* body_, cpVect j, cpVect r)
60 {
61     body_.v_bias  = cpvadd(body_.v_bias, cpvmult(j, body_.m_inv));
62     body_.w_bias += body_.i_inv * cpvcross(r, j);
63 }
64 
65 void apply_bias_impulses(cpBody* a, cpBody* b, cpVect r1, cpVect r2, cpVect j)
66 {
67     apply_bias_impulse(a, cpvneg(j), r1);
68     apply_bias_impulse(b, j, r2);
69 }
70 
71 cpFloat k_scalar_body(cpBody* body_, cpVect r, cpVect n)
72 {
73     cpFloat rcn = cpvcross(r, n);
74     return body_.m_inv + body_.i_inv * rcn * rcn;
75 }
76 
77 cpFloat k_scalar(cpBody* a, cpBody* b, cpVect r1, cpVect r2, cpVect n)
78 {
79     cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n);
80     cpAssertSoft(value != 0.0, "Unsolvable collision or constraint.");
81 
82     return value;
83 }
84 
85 cpMat2x2 k_tensor(cpBody* a, cpBody* b, cpVect r1, cpVect r2)
86 {
87     cpFloat m_sum = a.m_inv + b.m_inv;
88 
89     // start with Identity*m_sum
90     cpFloat k11 = m_sum, k12 = 0.0f;
91     cpFloat k21 = 0.0f, k22 = m_sum;
92 
93     // add the influence from r1
94     cpFloat a_i_inv = a.i_inv;
95     cpFloat r1xsq   =  r1.x * r1.x * a_i_inv;
96     cpFloat r1ysq   =  r1.y * r1.y * a_i_inv;
97     cpFloat r1nxy   = -r1.x * r1.y * a_i_inv;
98     k11 += r1ysq;
99     k12 += r1nxy;
100     k21 += r1nxy;
101     k22 += r1xsq;
102 
103     // add the influnce from r2
104     cpFloat b_i_inv = b.i_inv;
105     cpFloat r2xsq   =  r2.x * r2.x * b_i_inv;
106     cpFloat r2ysq   =  r2.y * r2.y * b_i_inv;
107     cpFloat r2nxy   = -r2.x * r2.y * b_i_inv;
108     k11 += r2ysq;
109     k12 += r2nxy;
110     k21 += r2nxy;
111     k22 += r2xsq;
112 
113     // invert
114     cpFloat det = k11 * k22 - k12 * k21;
115     cpAssertSoft(det != 0.0, "Unsolvable constraint.");
116 
117     cpFloat det_inv = 1.0f / det;
118     return cpMat2x2New(
119         k22 * det_inv, -k12 * det_inv,
120         -k21 * det_inv, k11 * det_inv
121         );
122 }
123 
124 cpFloat bias_coef(cpFloat errorBias, cpFloat dt)
125 {
126     return 1.0f - cpfpow(errorBias, dt);
127 }