2 * Resources: Box2D - Erin Catto
7 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
);
16 #define k_rb_delta (1.0f/60.0f)
18 typedef struct rigidbody rigidbody
;
30 float bias
, norm_impulse
, tangent_impulse
[2];
35 v3f delta
; /* where is the origin of this in relation to a parent body */
36 m4x3f to_world
, to_local
;
39 static void rb_update_transform( rigidbody
*rb
)
42 q_m3x3( rb
->q
, rb
->to_world
);
43 v3_copy( rb
->co
, rb
->to_world
[3] );
45 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
47 box_copy( rb
->bbx
, rb
->bbx_world
);
48 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
51 static void rb_init( rigidbody
*rb
)
58 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
60 rb
->inv_mass
= 1.0f
/(8.0f
*dims
[0]*dims
[1]*dims
[2]);
62 rb_update_transform( rb
);
65 static void rb_iter( rigidbody
*rb
)
67 v3f gravity
= { 0.0f
, -9.6f
, 0.0f
};
68 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
70 /* intergrate velocity */
71 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
73 v3_lerp( rb
->I
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->I
);
75 /* inegrate inertia */
76 if( v3_length2( rb
->I
) > 0.0f
)
80 v3_copy( rb
->I
, axis
);
82 float mag
= v3_length( axis
);
83 v3_divs( axis
, mag
, axis
);
84 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
85 q_mul( rotation
, rb
->q
, rb
->q
);
89 static void rb_torque( rigidbody
*rb
, v3f axis
, float mag
)
91 v3_muladds( rb
->I
, axis
, mag
*k_rb_delta
, rb
->I
);
94 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
96 /* Compute tangent basis (box2d) */
97 if( fabsf( n
[0] ) >= 0.57735027f
)
111 v3_cross( n
, tx
, ty
);
116 static void rb_manifold_reset( rigidbody
*rb
)
118 rb
->manifold_count
= 0;
121 static void rb_build_manifold_terrain( rigidbody
*rb
)
125 float *p000
= pts
[0], *p001
= pts
[1], *p010
= pts
[2], *p011
= pts
[3],
126 *p100
= pts
[4], *p101
= pts
[5], *p110
= pts
[6], *p111
= pts
[7];
128 p000
[0]=box
[0][0];p000
[1]=box
[0][1];p000
[2]=box
[0][2];
129 p001
[0]=box
[0][0];p001
[1]=box
[0][1];p001
[2]=box
[1][2];
130 p010
[0]=box
[0][0];p010
[1]=box
[1][1];p010
[2]=box
[0][2];
131 p011
[0]=box
[0][0];p011
[1]=box
[1][1];p011
[2]=box
[1][2];
133 p100
[0]=box
[1][0];p100
[1]=box
[0][1];p100
[2]=box
[0][2];
134 p101
[0]=box
[1][0];p101
[1]=box
[0][1];p101
[2]=box
[1][2];
135 p110
[0]=box
[1][0];p110
[1]=box
[1][1];p110
[2]=box
[0][2];
136 p111
[0]=box
[1][0];p111
[1]=box
[1][1];p111
[2]=box
[1][2];
138 m4x3_mulv( rb
->to_world
, p000
, p000
);
139 m4x3_mulv( rb
->to_world
, p001
, p001
);
140 m4x3_mulv( rb
->to_world
, p010
, p010
);
141 m4x3_mulv( rb
->to_world
, p011
, p011
);
142 m4x3_mulv( rb
->to_world
, p100
, p100
);
143 m4x3_mulv( rb
->to_world
, p101
, p101
);
144 m4x3_mulv( rb
->to_world
, p110
, p110
);
145 m4x3_mulv( rb
->to_world
, p111
, p111
);
149 for( int i
=0; i
<8; i
++ )
151 float *point
= pts
[i
];
152 struct contact
*ct
= &rb
->manifold
[rb
->manifold_count
];
155 v3_copy( point
, surface
);
160 if( !ray_world( surface
, (v3f
){0.0f
,-1.0f
,0.0f
}, &hit
))
163 v3_copy( hit
.normal
, ct
->n
);
164 v3_copy( hit
.pos
, surface
);
166 float p
= vg_minf( surface
[1] - point
[1], 1.0f
);
170 v3_add( point
, surface
, ct
->co
);
171 v3_muls( ct
->co
, 0.5f
, ct
->co
);
173 //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
175 v3_sub( ct
->co
, rb
->co
, ct
->delta
);
176 ct
->bias
= -0.2f
* (1.0f
/k_rb_delta
) * vg_minf( 0.0f
, -p
+0.04f
);
177 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
179 ct
->norm_impulse
= 0.0f
;
180 ct
->tangent_impulse
[0] = 0.0f
;
181 ct
->tangent_impulse
[1] = 0.0f
;
183 rb
->manifold_count
++;
191 static void rb_constraint_manifold( rigidbody
*rb
)
193 float k_friction
= 0.1f
;
195 /* Friction Impulse */
196 for( int i
=0; i
<rb
->manifold_count
; i
++ )
198 struct contact
*ct
= &rb
->manifold
[i
];
201 v3_cross( rb
->I
, ct
->delta
, dv
);
202 v3_add( rb
->v
, dv
, dv
);
204 for( int j
=0; j
<2; j
++ )
206 float vt
= vg_clampf( -v3_dot( dv
, ct
->t
[j
] ),
207 -k_friction
, k_friction
);
209 vt
= -v3_dot( dv
, ct
->t
[j
] );
211 float temp
= ct
->tangent_impulse
[j
];
212 ct
->tangent_impulse
[j
] = vg_clampf( temp
+vt
, -k_friction
, k_friction
);
213 vt
= ct
->tangent_impulse
[j
] - temp
;
217 v3_muls( ct
->t
[j
], vt
, impulse
);
218 v3_add( impulse
, rb
->v
, rb
->v
);
219 v3_cross( ct
->delta
, impulse
, impulse
);
220 v3_add( impulse
, rb
->I
, rb
->I
);
225 for( int i
=0; i
<rb
->manifold_count
; i
++ )
227 struct contact
*ct
= &rb
->manifold
[i
];
230 v3_cross( rb
->I
, ct
->delta
, dv
);
231 v3_add( rb
->v
, dv
, dv
);
233 float vn
= -v3_dot( dv
, ct
->n
);
236 float temp
= ct
->norm_impulse
;
237 ct
->norm_impulse
= vg_maxf( temp
+ vn
, 0.0f
);
238 vn
= ct
->norm_impulse
- temp
;
242 v3_muls( ct
->n
, vn
, impulse
);
243 v3_add( impulse
, rb
->v
, rb
->v
);
244 v3_cross( ct
->delta
, impulse
, impulse
);
245 v3_add( impulse
, rb
->I
, rb
->I
);
249 struct rb_angle_limit
251 rigidbody
*rba
, *rbb
;
256 static int rb_angle_limit_force( rigidbody
*rba
, v3f va
,
257 rigidbody
*rbb
, v3f vb
,
261 m3x3_mulv( rba
->to_world
, va
, wva
);
262 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
264 float dt
= v3_dot(wva
,wvb
)*0.999f
,
269 float correction
= max
-ang
;
272 v3_cross( wva
, wvb
, axis
);
275 q_axis_angle( rotation
, axis
, -correction
*0.25f
);
276 q_mul( rotation
, rba
->q
, rba
->q
);
278 q_axis_angle( rotation
, axis
, correction
*0.25f
);
279 q_mul( rotation
, rbb
->q
, rbb
->q
);
287 static void rb_constraint_angle_limit( struct rb_angle_limit
*limit
)
294 static void rb_constraint_angle( rigidbody
*rba
, v3f va
,
295 rigidbody
*rbb
, v3f vb
,
296 float max
, float spring
)
299 m3x3_mulv( rba
->to_world
, va
, wva
);
300 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
302 float dt
= v3_dot(wva
,wvb
)*0.999f
,
306 v3_cross( wva
, wvb
, axis
);
307 v3_muladds( rba
->I
, axis
, ang
*spring
*0.5f
, rba
->I
);
308 v3_muladds( rbb
->I
, axis
, -ang
*spring
*0.5f
, rbb
->I
);
312 /* TODO: convert max into the dot product value so we dont have to always
313 * evaluate acosf, only if its greater than the angle specified */
317 float correction
= max
-ang
;
320 q_axis_angle( rotation
, axis
, -correction
*0.125f
);
321 q_mul( rotation
, rba
->q
, rba
->q
);
323 q_axis_angle( rotation
, axis
, correction
*0.125f
);
324 q_mul( rotation
, rbb
->q
, rbb
->q
);
328 static void rb_relative_velocity( rigidbody
*ra
, v3f lca
,
329 rigidbody
*rb
, v3f lcb
, v3f rcv
)
332 m3x3_mulv( ra
->to_world
, lca
, wca
);
333 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
335 v3_sub( ra
->v
, rb
->v
, rcv
);
338 v3_cross( ra
->I
, wca
, rcv_Ra
);
339 v3_cross( rb
->I
, wcb
, rcv_Rb
);
340 v3_add( rcv_Ra
, rcv
, rcv
);
341 v3_sub( rcv
, rcv_Rb
, rcv
);
344 static void rb_constraint_position( rigidbody
*ra
, v3f lca
,
345 rigidbody
*rb
, v3f lcb
)
347 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
349 m3x3_mulv( ra
->to_world
, lca
, wca
);
350 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
353 v3_add( wcb
, rb
->co
, delta
);
354 v3_sub( delta
, wca
, delta
);
355 v3_sub( delta
, ra
->co
, delta
);
357 v3_muladds( ra
->co
, delta
, 0.5f
, ra
->co
);
358 v3_muladds( rb
->co
, delta
, -0.5f
, rb
->co
);
361 v3_sub( ra
->v
, rb
->v
, rcv
);
364 v3_cross( ra
->I
, wca
, rcv_Ra
);
365 v3_cross( rb
->I
, wcb
, rcv_Rb
);
366 v3_add( rcv_Ra
, rcv
, rcv
);
367 v3_sub( rcv
, rcv_Rb
, rcv
);
369 float nm
= 0.5f
/(rb
->inv_mass
+ ra
->inv_mass
);
371 float mass_a
= 1.0f
/ra
->inv_mass
,
372 mass_b
= 1.0f
/rb
->inv_mass
,
373 total_mass
= mass_a
+mass_b
;
376 v3_muls( rcv
, 1.0f
, impulse
);
377 v3_muladds( rb
->v
, impulse
, mass_b
/total_mass
, rb
->v
);
378 v3_cross( wcb
, impulse
, impulse
);
379 v3_add( impulse
, rb
->I
, rb
->I
);
381 v3_muls( rcv
, -1.0f
, impulse
);
382 v3_muladds( ra
->v
, impulse
, mass_a
/total_mass
, ra
->v
);
383 v3_cross( wca
, impulse
, impulse
);
384 v3_add( impulse
, ra
->I
, ra
->I
);
388 v3_muls( delta
, 0.5f
*spring
, impulse
);
390 v3_add( impulse
, ra
->v
, ra
->v
);
391 v3_cross( wca
, impulse
, impulse
);
392 v3_add( impulse
, ra
->I
, ra
->I
);
394 v3_muls( delta
, -0.5f
*spring
, impulse
);
396 v3_add( impulse
, rb
->v
, rb
->v
);
397 v3_cross( wcb
, impulse
, impulse
);
398 v3_add( impulse
, rb
->I
, rb
->I
);
402 static void rb_debug( rigidbody
*rb
, u32 colour
)
405 v3f p000
, p001
, p010
, p011
, p100
, p101
, p110
, p111
;
407 p000
[0]=box
[0][0];p000
[1]=box
[0][1];p000
[2]=box
[0][2];
408 p001
[0]=box
[0][0];p001
[1]=box
[0][1];p001
[2]=box
[1][2];
409 p010
[0]=box
[0][0];p010
[1]=box
[1][1];p010
[2]=box
[0][2];
410 p011
[0]=box
[0][0];p011
[1]=box
[1][1];p011
[2]=box
[1][2];
412 p100
[0]=box
[1][0];p100
[1]=box
[0][1];p100
[2]=box
[0][2];
413 p101
[0]=box
[1][0];p101
[1]=box
[0][1];p101
[2]=box
[1][2];
414 p110
[0]=box
[1][0];p110
[1]=box
[1][1];p110
[2]=box
[0][2];
415 p111
[0]=box
[1][0];p111
[1]=box
[1][1];p111
[2]=box
[1][2];
417 m4x3_mulv( rb
->to_world
, p000
, p000
);
418 m4x3_mulv( rb
->to_world
, p001
, p001
);
419 m4x3_mulv( rb
->to_world
, p010
, p010
);
420 m4x3_mulv( rb
->to_world
, p011
, p011
);
421 m4x3_mulv( rb
->to_world
, p100
, p100
);
422 m4x3_mulv( rb
->to_world
, p101
, p101
);
423 m4x3_mulv( rb
->to_world
, p110
, p110
);
424 m4x3_mulv( rb
->to_world
, p111
, p111
);
426 vg_line( p000
, p001
, colour
);
427 vg_line( p001
, p011
, colour
);
428 vg_line( p011
, p010
, colour
);
429 vg_line( p010
, p000
, colour
);
431 vg_line( p100
, p101
, colour
);
432 vg_line( p101
, p111
, colour
);
433 vg_line( p111
, p110
, colour
);
434 vg_line( p110
, p100
, colour
);
436 vg_line( p100
, p000
, colour
);
437 vg_line( p101
, p001
, colour
);
438 vg_line( p110
, p010
, colour
);
439 vg_line( p111
, p011
, colour
);
441 vg_line( p000
, p110
, colour
);
442 vg_line( p100
, p010
, colour
);
446 * out penetration distance, normal
448 static int rb_point_in_body( rigidbody
*rb
, v3f pos
, float *pen
, v3f normal
)
451 m4x3_mulv( rb
->to_local
, pos
, local
);
453 if( local
[0] > rb
->bbx
[0][0] && local
[0] < rb
->bbx
[1][0] &&
454 local
[1] > rb
->bbx
[0][1] && local
[1] < rb
->bbx
[1][1] &&
455 local
[2] > rb
->bbx
[0][2] && local
[2] < rb
->bbx
[1][2] )
457 v3f area
, com
, comrel
;
458 v3_add( rb
->bbx
[0], rb
->bbx
[1], com
);
459 v3_muls( com
, 0.5f
, com
);
461 v3_sub( rb
->bbx
[1], rb
->bbx
[0], area
);
462 v3_sub( local
, com
, comrel
);
463 v3_div( comrel
, area
, comrel
);
466 float max_mag
= fabsf(comrel
[0]);
468 if( fabsf(comrel
[1]) > max_mag
)
471 max_mag
= fabsf(comrel
[1]);
473 if( fabsf(comrel
[2]) > max_mag
)
476 max_mag
= fabsf(comrel
[2]);
480 normal
[axis
] = vg_signf(comrel
[axis
]);
482 if( normal
[axis
] < 0.0f
)
483 *pen
= local
[axis
] - rb
->bbx
[0][axis
];
485 *pen
= rb
->bbx
[1][axis
] - local
[axis
];
487 m3x3_mulv( rb
->to_world
, normal
, normal
);
494 static void rb_build_manifold_rb_static( rigidbody
*ra
, rigidbody
*rb_static
)
499 v3_copy( ra
->bbx
[0], a
);
500 v3_copy( ra
->bbx
[1], b
);
502 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], a
[1], a
[2] }, verts
[0] );
503 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], b
[1], a
[2] }, verts
[1] );
504 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], b
[1], a
[2] }, verts
[2] );
505 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], a
[1], a
[2] }, verts
[3] );
506 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], a
[1], b
[2] }, verts
[4] );
507 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], b
[1], b
[2] }, verts
[5] );
508 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], b
[1], b
[2] }, verts
[6] );
509 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], a
[1], b
[2] }, verts
[7] );
513 for( int i
=0; i
<8; i
++ )
515 if( ra
->manifold_count
== vg_list_size(ra
->manifold
) )
518 struct contact
*ct
= &ra
->manifold
[ ra
->manifold_count
];
523 if( rb_point_in_body( rb_static
, verts
[i
], &p
, normal
))
525 v3_copy( normal
, ct
->n
);
526 v3_muladds( verts
[i
], ct
->n
, p
*0.5f
, ct
->co
);
527 v3_sub( ct
->co
, ra
->co
, ct
->delta
);
529 vg_line_pt3( ct
->co
, 0.0125f
, 0xffff00ff );
531 ct
->bias
= -0.2f
* (1.0f
/k_rb_delta
) * vg_minf( 0.0f
, -p
+0.04f
);
532 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
534 ct
->norm_impulse
= 0.0f
;
535 ct
->tangent_impulse
[0] = 0.0f
;
536 ct
->tangent_impulse
[1] = 0.0f
;
538 ra
->manifold_count
++;
547 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
551 static void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
553 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
554 box_concat( bound
, rb
->bbx_world
);
557 static float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
559 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
560 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
563 static void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
565 rigidbody temp
, *rba
, *rbb
;
566 rba
= &((rigidbody
*)user
)[ ia
];
567 rbb
= &((rigidbody
*)user
)[ ib
];
574 static void rb_bh_debug( void *user
, u32 item_index
)
576 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
577 rb_debug( rb
, 0xff00ffff );
580 static bh_system bh_system_rigidbodies
=
582 .expand_bound
= rb_bh_expand_bound
,
583 .item_centroid
= rb_bh_centroid
,
584 .item_swap
= rb_bh_swap
,
585 .item_debug
= rb_bh_debug
,
588 .item_size
= sizeof(rigidbody
)
591 #endif /* RIGIDBODY_H */