2 * Resources: Box2D - Erin Catto
14 #define k_rb_delta (1.0f/60.0f)
16 typedef struct rigidbody rigidbody
;
28 float bias
, norm_impulse
, tangent_impulse
[2];
33 v3f delta
; /* where is the origin of this in relation to a parent body */
34 m4x3f to_world
, to_local
;
37 static void rb_update_transform( rigidbody
*rb
)
40 q_m3x3( rb
->q
, rb
->to_world
);
41 v3_copy( rb
->co
, rb
->to_world
[3] );
43 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
46 static void rb_init( rigidbody
*rb
)
53 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
55 rb
->inv_mass
= 1.0f
/(8.0f
*dims
[0]*dims
[1]*dims
[2]);
57 rb_update_transform( rb
);
60 static void rb_iter( rigidbody
*rb
)
62 v3f gravity
= { 0.0f
, -9.6f
, 0.0f
};
63 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
65 /* intergrate velocity */
66 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
68 v3_lerp( rb
->I
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->I
);
70 /* inegrate inertia */
71 if( v3_length2( rb
->I
) > 0.0f
)
75 v3_copy( rb
->I
, axis
);
77 float mag
= v3_length( axis
);
78 v3_divs( axis
, mag
, axis
);
79 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
80 q_mul( rotation
, rb
->q
, rb
->q
);
84 static void rb_torque( rigidbody
*rb
, v3f axis
, float mag
)
86 v3_muladds( rb
->I
, axis
, mag
*k_rb_delta
, rb
->I
);
89 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
91 /* Compute tangent basis (box2d) */
92 if( fabsf( n
[0] ) >= 0.57735027f
)
106 v3_cross( n
, tx
, ty
);
109 static void rb_build_manifold( rigidbody
*rb
, scene
*sc
)
113 float *p000
= pts
[0], *p001
= pts
[1], *p010
= pts
[2], *p011
= pts
[3],
114 *p100
= pts
[4], *p101
= pts
[5], *p110
= pts
[6], *p111
= pts
[7];
116 p000
[0]=box
[0][0];p000
[1]=box
[0][1];p000
[2]=box
[0][2];
117 p001
[0]=box
[0][0];p001
[1]=box
[0][1];p001
[2]=box
[1][2];
118 p010
[0]=box
[0][0];p010
[1]=box
[1][1];p010
[2]=box
[0][2];
119 p011
[0]=box
[0][0];p011
[1]=box
[1][1];p011
[2]=box
[1][2];
121 p100
[0]=box
[1][0];p100
[1]=box
[0][1];p100
[2]=box
[0][2];
122 p101
[0]=box
[1][0];p101
[1]=box
[0][1];p101
[2]=box
[1][2];
123 p110
[0]=box
[1][0];p110
[1]=box
[1][1];p110
[2]=box
[0][2];
124 p111
[0]=box
[1][0];p111
[1]=box
[1][1];p111
[2]=box
[1][2];
126 m4x3_mulv( rb
->to_world
, p000
, p000
);
127 m4x3_mulv( rb
->to_world
, p001
, p001
);
128 m4x3_mulv( rb
->to_world
, p010
, p010
);
129 m4x3_mulv( rb
->to_world
, p011
, p011
);
130 m4x3_mulv( rb
->to_world
, p100
, p100
);
131 m4x3_mulv( rb
->to_world
, p101
, p101
);
132 m4x3_mulv( rb
->to_world
, p110
, p110
);
133 m4x3_mulv( rb
->to_world
, p111
, p111
);
135 rb
->manifold_count
= 0;
137 for( int i
=0; i
<8; i
++ )
139 float *point
= pts
[i
];
140 struct contact
*ct
= &rb
->manifold
[rb
->manifold_count
];
144 v3_copy( point
, surface
);
147 bvh_scene_sample( sc
, surface
, &hit
);
148 v3_copy( hit
.normal
, ct
->n
);
150 float p
= vg_minf( surface
[1] - point
[1], 1.0f
);
154 v3_add( point
, surface
, ct
->co
);
155 v3_muls( ct
->co
, 0.5f
, ct
->co
);
157 //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
159 v3_sub( ct
->co
, rb
->co
, ct
->delta
);
160 ct
->bias
= -0.2f
* (1.0f
/k_rb_delta
) * vg_minf( 0.0f
, -p
+0.04f
);
161 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
163 ct
->norm_impulse
= 0.0f
;
164 ct
->tangent_impulse
[0] = 0.0f
;
165 ct
->tangent_impulse
[1] = 0.0f
;
167 rb
->manifold_count
++;
168 if( rb
->manifold_count
== 4 )
174 static void rb_constraint_manifold( rigidbody
*rb
)
176 float k_friction
= 0.1f
;
178 /* Friction Impulse */
179 for( int i
=0; i
<rb
->manifold_count
; i
++ )
181 struct contact
*ct
= &rb
->manifold
[i
];
184 v3_cross( rb
->I
, ct
->delta
, dv
);
185 v3_add( rb
->v
, dv
, dv
);
187 for( int j
=0; j
<2; j
++ )
189 float vt
= vg_clampf( -v3_dot( dv
, ct
->t
[j
] ),
190 -k_friction
, k_friction
);
192 vt
= -v3_dot( dv
, ct
->t
[j
] );
194 float temp
= ct
->tangent_impulse
[j
];
195 ct
->tangent_impulse
[j
] = vg_clampf( temp
+vt
, -k_friction
, k_friction
);
196 vt
= ct
->tangent_impulse
[j
] - temp
;
200 v3_muls( ct
->t
[j
], vt
, impulse
);
201 v3_add( impulse
, rb
->v
, rb
->v
);
202 v3_cross( ct
->delta
, impulse
, impulse
);
203 v3_add( impulse
, rb
->I
, rb
->I
);
208 for( int i
=0; i
<rb
->manifold_count
; i
++ )
210 struct contact
*ct
= &rb
->manifold
[i
];
213 v3_cross( rb
->I
, ct
->delta
, dv
);
214 v3_add( rb
->v
, dv
, dv
);
216 float vn
= -v3_dot( dv
, ct
->n
);
219 float temp
= ct
->norm_impulse
;
220 ct
->norm_impulse
= vg_maxf( temp
+ vn
, 0.0f
);
221 vn
= ct
->norm_impulse
- temp
;
225 v3_muls( ct
->n
, vn
, impulse
);
226 v3_add( impulse
, rb
->v
, rb
->v
);
227 v3_cross( ct
->delta
, impulse
, impulse
);
228 v3_add( impulse
, rb
->I
, rb
->I
);
232 struct rb_angle_limit
234 rigidbody
*rba
, *rbb
;
239 static int rb_angle_limit_force(
240 rigidbody
*rba
, v3f va
,
241 rigidbody
*rbb
, v3f vb
,
245 m3x3_mulv( rba
->to_world
, va
, wva
);
246 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
248 float dt
= v3_dot(wva
,wvb
)*0.999f
,
253 float correction
= max
-ang
;
256 v3_cross( wva
, wvb
, axis
);
259 q_axis_angle( rotation
, axis
, -correction
*0.25f
);
260 q_mul( rotation
, rba
->q
, rba
->q
);
262 q_axis_angle( rotation
, axis
, correction
*0.25f
);
263 q_mul( rotation
, rbb
->q
, rbb
->q
);
271 static void rb_constraint_angle_limit( struct rb_angle_limit
*limit
)
278 static void rb_constraint_angle( rigidbody
*rba
, v3f va
,
279 rigidbody
*rbb
, v3f vb
,
280 float max
, float spring
)
283 m3x3_mulv( rba
->to_world
, va
, wva
);
284 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
286 float dt
= v3_dot(wva
,wvb
)*0.999f
,
290 v3_cross( wva
, wvb
, axis
);
291 v3_muladds( rba
->I
, axis
, ang
*spring
*0.5f
, rba
->I
);
292 v3_muladds( rbb
->I
, axis
, -ang
*spring
*0.5f
, rbb
->I
);
296 /* TODO: convert max into the dot product value so we dont have to always
297 * evaluate acosf, only if its greater than the angle specified */
301 float correction
= max
-ang
;
304 q_axis_angle( rotation
, axis
, -correction
*0.125f
);
305 q_mul( rotation
, rba
->q
, rba
->q
);
307 q_axis_angle( rotation
, axis
, correction
*0.125f
);
308 q_mul( rotation
, rbb
->q
, rbb
->q
);
312 static void rb_relative_velocity( rigidbody
*ra
, v3f lca
,
313 rigidbody
*rb
, v3f lcb
, v3f rcv
)
316 m3x3_mulv( ra
->to_world
, lca
, wca
);
317 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
319 v3_sub( ra
->v
, rb
->v
, rcv
);
322 v3_cross( ra
->I
, wca
, rcv_Ra
);
323 v3_cross( rb
->I
, wcb
, rcv_Rb
);
324 v3_add( rcv_Ra
, rcv
, rcv
);
325 v3_sub( rcv
, rcv_Rb
, rcv
);
328 static void rb_constraint_position( rigidbody
*ra
, v3f lca
,
329 rigidbody
*rb
, v3f lcb
)
331 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
333 m3x3_mulv( ra
->to_world
, lca
, wca
);
334 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
337 v3_add( wcb
, rb
->co
, delta
);
338 v3_sub( delta
, wca
, delta
);
339 v3_sub( delta
, ra
->co
, delta
);
341 v3_muladds( ra
->co
, delta
, 0.5f
, ra
->co
);
342 v3_muladds( rb
->co
, delta
, -0.5f
, rb
->co
);
345 v3_sub( ra
->v
, rb
->v
, rcv
);
348 v3_cross( ra
->I
, wca
, rcv_Ra
);
349 v3_cross( rb
->I
, wcb
, rcv_Rb
);
350 v3_add( rcv_Ra
, rcv
, rcv
);
351 v3_sub( rcv
, rcv_Rb
, rcv
);
353 float nm
= 0.5f
/(rb
->inv_mass
+ ra
->inv_mass
);
355 float mass_a
= 1.0f
/ra
->inv_mass
,
356 mass_b
= 1.0f
/rb
->inv_mass
,
357 total_mass
= mass_a
+mass_b
;
360 v3_muls( rcv
, 1.0f
, impulse
);
361 v3_muladds( rb
->v
, impulse
, mass_b
/total_mass
, rb
->v
);
362 v3_cross( wcb
, impulse
, impulse
);
363 v3_add( impulse
, rb
->I
, rb
->I
);
365 v3_muls( rcv
, -1.0f
, impulse
);
366 v3_muladds( ra
->v
, impulse
, mass_a
/total_mass
, ra
->v
);
367 v3_cross( wca
, impulse
, impulse
);
368 v3_add( impulse
, ra
->I
, ra
->I
);
372 v3_muls( delta
, 0.5f
*spring
, impulse
);
374 v3_add( impulse
, ra
->v
, ra
->v
);
375 v3_cross( wca
, impulse
, impulse
);
376 v3_add( impulse
, ra
->I
, ra
->I
);
378 v3_muls( delta
, -0.5f
*spring
, impulse
);
380 v3_add( impulse
, rb
->v
, rb
->v
);
381 v3_cross( wcb
, impulse
, impulse
);
382 v3_add( impulse
, rb
->I
, rb
->I
);
386 static void rb_debug( rigidbody
*rb
, u32 colour
)
389 v3f p000
, p001
, p010
, p011
, p100
, p101
, p110
, p111
;
391 p000
[0]=box
[0][0];p000
[1]=box
[0][1];p000
[2]=box
[0][2];
392 p001
[0]=box
[0][0];p001
[1]=box
[0][1];p001
[2]=box
[1][2];
393 p010
[0]=box
[0][0];p010
[1]=box
[1][1];p010
[2]=box
[0][2];
394 p011
[0]=box
[0][0];p011
[1]=box
[1][1];p011
[2]=box
[1][2];
396 p100
[0]=box
[1][0];p100
[1]=box
[0][1];p100
[2]=box
[0][2];
397 p101
[0]=box
[1][0];p101
[1]=box
[0][1];p101
[2]=box
[1][2];
398 p110
[0]=box
[1][0];p110
[1]=box
[1][1];p110
[2]=box
[0][2];
399 p111
[0]=box
[1][0];p111
[1]=box
[1][1];p111
[2]=box
[1][2];
401 m4x3_mulv( rb
->to_world
, p000
, p000
);
402 m4x3_mulv( rb
->to_world
, p001
, p001
);
403 m4x3_mulv( rb
->to_world
, p010
, p010
);
404 m4x3_mulv( rb
->to_world
, p011
, p011
);
405 m4x3_mulv( rb
->to_world
, p100
, p100
);
406 m4x3_mulv( rb
->to_world
, p101
, p101
);
407 m4x3_mulv( rb
->to_world
, p110
, p110
);
408 m4x3_mulv( rb
->to_world
, p111
, p111
);
410 vg_line( p000
, p001
, colour
);
411 vg_line( p001
, p011
, colour
);
412 vg_line( p011
, p010
, colour
);
413 vg_line( p010
, p000
, colour
);
415 vg_line( p100
, p101
, colour
);
416 vg_line( p101
, p111
, colour
);
417 vg_line( p111
, p110
, colour
);
418 vg_line( p110
, p100
, colour
);
420 vg_line( p100
, p000
, colour
);
421 vg_line( p101
, p001
, colour
);
422 vg_line( p110
, p010
, colour
);
423 vg_line( p111
, p011
, colour
);
425 vg_line( p000
, p110
, colour
);
426 vg_line( p100
, p010
, colour
);
429 #endif /* RIGIDBODY_H */