2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
6 * Resources: Box2D - Erin Catto
16 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
);
17 VG_STATIC bh_system bh_system_rigidbodies
;
23 * -----------------------------------------------------------------------------
25 * -----------------------------------------------------------------------------
29 k_rb_rate
= (1.0/VG_TIMESTEP_FIXED
),
30 k_rb_delta
= (1.0/k_rb_rate
),
32 k_damp_linear
= 0.1f
, /* scale velocity 1/(1+x) */
33 k_damp_angular
= 0.1f
, /* scale angular 1/(1+x) */
34 k_penetration_slop
= 0.01f
,
35 k_inertia_scale
= 8.0f
,
36 k_phys_baumgarte
= 0.2f
,
41 k_joint_correction
= 0.01f
,
42 k_joint_impulse
= 1.0f
,
43 k_joint_bias
= 0.08f
; /* positional joints */
45 VG_STATIC
void rb_register_cvar(void)
47 vg_var_push( (struct vg_var
){
48 .name
= "k_limit_bias", .data
= &k_limit_bias
,
49 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
52 vg_var_push( (struct vg_var
){
53 .name
= "k_joint_bias", .data
= &k_joint_bias
,
54 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
57 vg_var_push( (struct vg_var
){
58 .name
= "k_joint_correction", .data
= &k_joint_correction
,
59 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
62 vg_var_push( (struct vg_var
){
63 .name
= "k_joint_impulse", .data
= &k_joint_impulse
,
64 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
69 * -----------------------------------------------------------------------------
70 * structure definitions
71 * -----------------------------------------------------------------------------
74 typedef struct rigidbody rigidbody
;
75 typedef struct contact rb_ct
;
76 typedef struct rb_sphere rb_sphere
;
77 typedef struct rb_capsule rb_capsule
;
78 typedef struct rb_scene rb_scene
;
103 k_rb_shape_sphere
= 1,
104 k_rb_shape_capsule
= 2,
111 struct rb_sphere sphere
;
112 struct rb_capsule capsule
;
113 struct rb_scene scene
;
117 v3f right
, up
, forward
;
124 /* inertia model and inverse world tensor */
128 m4x3f to_world
, to_local
;
131 VG_STATIC
struct contact
133 rigidbody
*rba
, *rbb
;
136 float p
, bias
, norm_impulse
, tangent_impulse
[2],
137 normal_mass
, tangent_mass
[2];
141 enum contact_type type
;
143 rb_contact_buffer
[256];
144 VG_STATIC
int rb_contact_count
= 0;
146 typedef struct rb_constr_pos rb_constr_pos
;
147 typedef struct rb_constr_swingtwist rb_constr_swingtwist
;
151 rigidbody
*rba
, *rbb
;
155 struct rb_constr_swingtwist
157 rigidbody
*rba
, *rbb
;
159 v4f conevx
, conevy
; /* relative to rba */
160 v3f view_offset
, /* relative to rba */
161 coneva
, conevxb
;/* relative to rbb */
163 int tangent_violation
, axis_violation
;
164 v3f axis
, tangent_axis
, tangent_target
, axis_target
;
167 float tangent_mass
, axis_mass
;
170 struct rb_constr_spring
176 * -----------------------------------------------------------------------------
178 * -----------------------------------------------------------------------------
181 VG_STATIC
float sphere_volume( float radius
)
183 float r3
= radius
*radius
*radius
;
184 return (4.0f
/3.0f
) * VG_PIf
* r3
;
187 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
189 /* Compute tangent basis (box2d) */
190 if( fabsf( n
[0] ) >= 0.57735027f
)
204 v3_cross( n
, tx
, ty
);
208 * -----------------------------------------------------------------------------
210 * -----------------------------------------------------------------------------
213 VG_STATIC
void rb_debug_contact( rb_ct
*ct
)
216 v3_muladds( ct
->co
, ct
->n
, 0.05f
, p1
);
218 if( ct
->type
== k_contact_type_default
)
220 vg_line_pt3( ct
->co
, 0.0125f
, 0xff0000ff );
221 vg_line( ct
->co
, p1
, 0xffffffff );
223 else if( ct
->type
== k_contact_type_edge
)
225 vg_line_pt3( ct
->co
, 0.0125f
, 0xff00ffc0 );
226 vg_line( ct
->co
, p1
, 0xffffffff );
230 VG_STATIC
void debug_sphere( m4x3f m
, float radius
, u32 colour
)
232 v3f ly
= { 0.0f
, 0.0f
, radius
},
233 lx
= { 0.0f
, radius
, 0.0f
},
234 lz
= { 0.0f
, 0.0f
, radius
};
236 for( int i
=0; i
<16; i
++ )
238 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
242 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
243 px
= { s
*radius
, c
*radius
, 0.0f
},
244 pz
= { 0.0f
, s
*radius
, c
*radius
};
246 v3f p0
, p1
, p2
, p3
, p4
, p5
;
247 m4x3_mulv( m
, py
, p0
);
248 m4x3_mulv( m
, ly
, p1
);
249 m4x3_mulv( m
, px
, p2
);
250 m4x3_mulv( m
, lx
, p3
);
251 m4x3_mulv( m
, pz
, p4
);
252 m4x3_mulv( m
, lz
, p5
);
254 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
255 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
256 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
264 VG_STATIC
void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
266 v3f ly
= { 0.0f
, 0.0f
, radius
},
267 lx
= { 0.0f
, radius
, 0.0f
},
268 lz
= { 0.0f
, 0.0f
, radius
};
270 float s0
= sinf(0.0f
)*radius
,
271 c0
= cosf(0.0f
)*radius
;
273 v3f p0
, p1
, up
, right
, forward
;
274 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
275 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
276 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
277 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
278 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
281 v3_muladds( p0
, right
, radius
, a0
);
282 v3_muladds( p1
, right
, radius
, a1
);
283 v3_muladds( p0
, forward
, radius
, b0
);
284 v3_muladds( p1
, forward
, radius
, b1
);
285 vg_line( a0
, a1
, colour
);
286 vg_line( b0
, b1
, colour
);
288 v3_muladds( p0
, right
, -radius
, a0
);
289 v3_muladds( p1
, right
, -radius
, a1
);
290 v3_muladds( p0
, forward
, -radius
, b0
);
291 v3_muladds( p1
, forward
, -radius
, b1
);
292 vg_line( a0
, a1
, colour
);
293 vg_line( b0
, b1
, colour
);
295 for( int i
=0; i
<16; i
++ )
297 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
301 v3f e0
= { s0
, 0.0f
, c0
},
302 e1
= { s1
, 0.0f
, c1
},
303 e2
= { s0
, c0
, 0.0f
},
304 e3
= { s1
, c1
, 0.0f
},
305 e4
= { 0.0f
, c0
, s0
},
306 e5
= { 0.0f
, c1
, s1
};
308 m3x3_mulv( m
, e0
, e0
);
309 m3x3_mulv( m
, e1
, e1
);
310 m3x3_mulv( m
, e2
, e2
);
311 m3x3_mulv( m
, e3
, e3
);
312 m3x3_mulv( m
, e4
, e4
);
313 m3x3_mulv( m
, e5
, e5
);
315 v3_add( p0
, e0
, a0
);
316 v3_add( p0
, e1
, a1
);
317 v3_add( p1
, e0
, b0
);
318 v3_add( p1
, e1
, b1
);
320 vg_line( a0
, a1
, colour
);
321 vg_line( b0
, b1
, colour
);
325 v3_add( p0
, e2
, a0
);
326 v3_add( p0
, e3
, a1
);
327 v3_add( p0
, e4
, b0
);
328 v3_add( p0
, e5
, b1
);
332 v3_add( p1
, e2
, a0
);
333 v3_add( p1
, e3
, a1
);
334 v3_add( p1
, e4
, b0
);
335 v3_add( p1
, e5
, b1
);
338 vg_line( a0
, a1
, colour
);
339 vg_line( b0
, b1
, colour
);
346 VG_STATIC
void rb_debug( rigidbody
*rb
, u32 colour
)
348 if( rb
->type
== k_rb_shape_box
)
351 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
353 else if( rb
->type
== k_rb_shape_sphere
)
355 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
357 else if( rb
->type
== k_rb_shape_capsule
)
360 float h
= rb
->inf
.capsule
.height
,
361 r
= rb
->inf
.capsule
.radius
;
363 debug_capsule( rb
->to_world
, r
, h
, colour
);
365 else if( rb
->type
== k_rb_shape_scene
)
367 vg_line_boxf( rb
->bbx
, colour
);
372 * -----------------------------------------------------------------------------
374 * -----------------------------------------------------------------------------
378 * Update world space bounding box based on local one
380 VG_STATIC
void rb_update_bounds( rigidbody
*rb
)
382 box_copy( rb
->bbx
, rb
->bbx_world
);
383 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
387 * Commit transform to rigidbody. Updates matrices
389 VG_STATIC
void rb_update_transform( rigidbody
*rb
)
391 q_normalize( rb
->q
);
392 q_m3x3( rb
->q
, rb
->to_world
);
393 v3_copy( rb
->co
, rb
->to_world
[3] );
395 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
397 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
398 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
399 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
401 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
402 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
404 rb_update_bounds( rb
);
408 * Extrapolate rigidbody into a transform based on vg accumulator.
409 * Useful for rendering
412 __attribute__ ((deprecated
))
413 VG_STATIC
void rb_extrapolate_transform( rigidbody
*rb
, m4x3f transform
)
415 float substep
= vg_clampf( vg
.accumulator
/ k_rb_delta
, 0.0f
, 1.0f
);
420 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
422 if( v3_length2( rb
->w
) > 0.0f
)
426 v3_copy( rb
->w
, axis
);
428 float mag
= v3_length( axis
);
429 v3_divs( axis
, mag
, axis
);
430 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
431 q_mul( rotation
, rb
->q
, q
);
439 q_m3x3( q
, transform
);
440 v3_copy( co
, transform
[3] );
444 VG_STATIC
void rb_extrapolate( rigidbody
*rb
, v3f co
, v4f q
)
446 float substep
= vg_clampf( vg
.accumulator
/ k_rb_delta
, 0.0f
, 1.0f
);
448 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
450 if( v3_length2( rb
->w
) > 0.0f
)
454 v3_copy( rb
->w
, axis
);
456 float mag
= v3_length( axis
);
457 v3_divs( axis
, mag
, axis
);
458 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
459 q_mul( rotation
, rb
->q
, q
);
469 * Initialize rigidbody and calculate masses, inertia
471 VG_STATIC
void rb_init( rigidbody
*rb
)
475 if( rb
->type
== k_rb_shape_box
)
478 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
479 volume
= dims
[0]*dims
[1]*dims
[2];
481 else if( rb
->type
== k_rb_shape_sphere
)
483 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
484 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
485 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
487 else if( rb
->type
== k_rb_shape_capsule
)
489 float r
= rb
->inf
.capsule
.radius
,
490 h
= rb
->inf
.capsule
.height
;
491 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
493 v3_fill( rb
->bbx
[0], -r
);
494 v3_fill( rb
->bbx
[1], r
);
498 else if( rb
->type
== k_rb_shape_scene
)
501 box_copy( rb
->inf
.scene
.bh_scene
->nodes
[0].bbx
, rb
->bbx
);
512 float mass
= 2.0f
*volume
;
513 rb
->inv_mass
= 1.0f
/mass
;
516 v3_sub( rb
->bbx
[1], rb
->bbx
[0], extent
);
517 v3_muls( extent
, 0.5f
, extent
);
519 /* local intertia tensor */
520 float scale
= k_inertia_scale
;
521 float ex2
= scale
*extent
[0]*extent
[0],
522 ey2
= scale
*extent
[1]*extent
[1],
523 ez2
= scale
*extent
[2]*extent
[2];
525 rb
->I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
526 rb
->I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
527 rb
->I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
529 m3x3_identity( rb
->iI
);
530 rb
->iI
[0][0] = rb
->I
[0];
531 rb
->iI
[1][1] = rb
->I
[1];
532 rb
->iI
[2][2] = rb
->I
[2];
533 m3x3_inv( rb
->iI
, rb
->iI
);
539 rb_update_transform( rb
);
542 VG_STATIC
void rb_iter( rigidbody
*rb
)
544 if( !vg_validf( rb
->v
[0] ) ||
545 !vg_validf( rb
->v
[1] ) ||
546 !vg_validf( rb
->v
[2] ) )
548 vg_fatal_exit_loop( "NaN velocity" );
551 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
552 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
554 /* intergrate velocity */
555 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
556 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
558 /* inegrate inertia */
559 if( v3_length2( rb
->w
) > 0.0f
)
563 v3_copy( rb
->w
, axis
);
565 float mag
= v3_length( axis
);
566 v3_divs( axis
, mag
, axis
);
567 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
568 q_mul( rotation
, rb
->q
, rb
->q
);
572 v3_muls( rb
->v
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_linear
), rb
->v
);
573 v3_muls( rb
->w
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_angular
), rb
->w
);
578 * -----------------------------------------------------------------------------
579 * Boolean shape overlap functions
580 * -----------------------------------------------------------------------------
584 * Project AABB, and triangle interval onto axis to check if they overlap
586 VG_STATIC
int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] )
590 r
= extent
[0] * fabsf(axis
[0]) +
591 extent
[1] * fabsf(axis
[1]) +
592 extent
[2] * fabsf(axis
[2]),
594 p0
= v3_dot( axis
, tri
[0] ),
595 p1
= v3_dot( axis
, tri
[1] ),
596 p2
= v3_dot( axis
, tri
[2] ),
598 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
600 if( e
> r
) return 0;
605 * Seperating axis test box vs triangle
607 VG_STATIC
int rb_box_triangle_sat( v3f extent
, v3f center
,
608 m4x3f to_local
, v3f tri_src
[3] )
612 for( int i
=0; i
<3; i
++ ){
613 m4x3_mulv( to_local
, tri_src
[i
], tri
[i
] );
614 v3_sub( tri
[i
], center
, tri
[i
] );
618 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
619 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
620 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
622 v3f v0
,v1
,v2
,n
, e0
,e1
,e2
;
623 v3_sub( tri
[1], tri
[0], v0
);
624 v3_sub( tri
[2], tri
[0], v1
);
625 v3_sub( tri
[2], tri
[1], v2
);
629 v3_cross( v0
, v1
, n
);
630 v3_cross( v0
, n
, e0
);
631 v3_cross( n
, v1
, e1
);
632 v3_cross( v2
, n
, e2
);
635 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
638 v3_cross( e0
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[0] );
639 v3_cross( e0
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[1] );
640 v3_cross( e0
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[2] );
641 v3_cross( e1
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[3] );
642 v3_cross( e1
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[4] );
643 v3_cross( e1
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[5] );
644 v3_cross( e2
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[6] );
645 v3_cross( e2
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[7] );
646 v3_cross( e2
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[8] );
648 for( int i
=0; i
<9; i
++ )
649 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
655 * -----------------------------------------------------------------------------
657 * -----------------------------------------------------------------------------
660 VG_STATIC
int rb_manifold_apply_filtered( rb_ct
*man
, int len
)
664 for( int i
=0; i
<len
; i
++ )
668 if( ct
->type
== k_contact_type_disabled
)
678 * Merge two contacts if they are within radius(r) of eachother
680 VG_STATIC
void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
)
682 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
)
684 cj
->type
= k_contact_type_disabled
;
685 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
687 v3_add( ci
->co
, cj
->co
, ci
->co
);
688 v3_muls( ci
->co
, 0.5f
, ci
->co
);
691 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
693 float c0
= v3_dot( ci
->n
, delta
),
694 c1
= v3_dot( cj
->n
, delta
);
696 if( c0
< 0.0f
|| c1
< 0.0f
)
699 ci
->type
= k_contact_type_disabled
;
704 v3_muls( ci
->n
, c0
, n
);
705 v3_muladds( n
, cj
->n
, c1
, n
);
715 VG_STATIC
void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
)
717 for( int i
=0; i
<len
-1; i
++ )
720 if( ci
->type
!= k_contact_type_edge
)
723 for( int j
=i
+1; j
<len
; j
++ )
726 if( cj
->type
!= k_contact_type_edge
)
729 rb_manifold_contact_weld( ci
, cj
, r
);
735 * Resolve overlapping pairs
739 VG_STATIC
void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
)
741 for( int i
=0; i
<len
-1; i
++ )
746 if( ci
->type
== k_contact_type_disabled
) continue;
748 for( int j
=i
+1; j
<len
; j
++ )
752 if( cj
->type
== k_contact_type_disabled
) continue;
754 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
)
756 cj
->type
= k_contact_type_disabled
;
757 v3_add( cj
->n
, ci
->n
, ci
->n
);
765 float n
= 1.0f
/((float)similar
+1.0f
);
766 v3_muls( ci
->n
, n
, ci
->n
);
769 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
770 ci
->type
= k_contact_type_disabled
;
772 v3_normalize( ci
->n
);
778 * Remove contacts that are facing away from A
780 VG_STATIC
void rb_manifold_filter_backface( rb_ct
*man
, int len
)
782 for( int i
=0; i
<len
; i
++ )
785 if( ct
->type
== k_contact_type_disabled
)
789 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
791 if( v3_dot( delta
, ct
->n
) > -0.001f
)
792 ct
->type
= k_contact_type_disabled
;
797 * Filter out duplicate coplanar results. Good for spheres.
799 VG_STATIC
void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
)
801 for( int i
=0; i
<len
; i
++ )
804 if( ci
->type
== k_contact_type_disabled
||
805 ci
->type
== k_contact_type_edge
)
808 float d1
= v3_dot( ci
->co
, ci
->n
);
810 for( int j
=0; j
<len
; j
++ )
816 if( cj
->type
== k_contact_type_disabled
)
819 float d2
= v3_dot( cj
->co
, ci
->n
),
822 if( fabsf( d
) <= w
)
824 cj
->type
= k_contact_type_disabled
;
831 * -----------------------------------------------------------------------------
833 * -----------------------------------------------------------------------------
839 * These do not automatically allocate contacts, an appropriately sized
840 * buffer must be supplied. The function returns the size of the manifold
841 * which was generated.
843 * The values set on the contacts are: n, co, p, rba, rbb
847 * By collecting the minimum(time) and maximum(time) pairs of points, we
848 * build a reduced and stable exact manifold.
851 * rx: minimum distance of these points
852 * dx: the delta between the two points
854 * pairs will only ammend these if they are creating a collision
856 typedef struct capsule_manifold capsule_manifold
;
857 struct capsule_manifold
865 * Expand a line manifold with a new pair. t value is the time along segment
866 * on the oriented object which created this pair.
868 VG_STATIC
void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
869 capsule_manifold
*manifold
)
872 v3_sub( pa
, pb
, delta
);
874 if( v3_length2(delta
) < r
*r
)
876 if( t
< manifold
->t0
)
878 v3_copy( delta
, manifold
->d0
);
883 if( t
> manifold
->t1
)
885 v3_copy( delta
, manifold
->d1
);
892 VG_STATIC
void rb_capsule_manifold_init( capsule_manifold
*manifold
)
894 manifold
->t0
= INFINITY
;
895 manifold
->t1
= -INFINITY
;
899 __attribute__ ((deprecated
))
900 VG_STATIC
int rb_capsule_manifold_done( rigidbody
*rba
, rigidbody
*rbb
,
901 capsule_manifold
*manifold
, rb_ct
*buf
)
903 float h
= rba
->inf
.capsule
.height
,
904 ra
= rba
->inf
.capsule
.radius
;
907 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
908 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
911 if( manifold
->t0
<= 1.0f
)
916 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
917 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
919 float d
= v3_length( manifold
->d0
);
920 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
921 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
923 ct
->p
= manifold
->r0
- d
;
926 ct
->type
= k_contact_type_default
;
931 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
933 rb_ct
*ct
= buf
+count
;
936 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
937 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
939 float d
= v3_length( manifold
->d1
);
940 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
941 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
943 ct
->p
= manifold
->r1
- d
;
946 ct
->type
= k_contact_type_default
;
956 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
962 VG_STATIC
int rb_capsule__manifold_done( m4x3f mtx
, rb_capsule
*c
,
963 capsule_manifold
*manifold
,
967 v3_muladds( mtx
[3], mtx
[1], -c
->height
*0.5f
+c
->radius
, p0
);
968 v3_muladds( mtx
[3], mtx
[1], c
->height
*0.5f
-c
->radius
, p1
);
971 if( manifold
->t0
<= 1.0f
)
976 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
977 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
979 float d
= v3_length( manifold
->d0
);
980 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
981 v3_muladds( pa
, ct
->n
, -c
->radius
, ct
->co
);
983 ct
->p
= manifold
->r0
- d
;
984 ct
->type
= k_contact_type_default
;
988 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
990 rb_ct
*ct
= buf
+count
;
993 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
994 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
996 float d
= v3_length( manifold
->d1
);
997 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
998 v3_muladds( pa
, ct
->n
, -c
->radius
, ct
->co
);
1000 ct
->p
= manifold
->r1
- d
;
1001 ct
->type
= k_contact_type_default
;
1011 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
1016 VG_STATIC
int rb_capsule_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1018 float h
= rba
->inf
.capsule
.height
,
1019 ra
= rba
->inf
.capsule
.radius
,
1020 rb
= rbb
->inf
.sphere
.radius
;
1023 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
1024 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
1027 closest_point_segment( p0
, p1
, rbb
->co
, c
);
1028 v3_sub( c
, rbb
->co
, delta
);
1030 float d2
= v3_length2(delta
),
1035 float d
= sqrtf(d2
);
1038 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1042 v3_muladds( c
, ct
->n
, -ra
, p0
);
1043 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
1044 v3_add( p0
, p1
, ct
->co
);
1045 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1049 ct
->type
= k_contact_type_default
;
1057 VG_STATIC
int rb_capsule__capsule( m4x3f mtxA
, rb_capsule
*ca
,
1058 m4x3f mtxB
, rb_capsule
*cb
, rb_ct
*buf
)
1060 float ha
= ca
->height
,
1067 v3_muladds( mtxA
[3], mtxA
[1], -ha
*0.5f
+ra
, p0
);
1068 v3_muladds( mtxA
[3], mtxA
[1], ha
*0.5f
-ra
, p1
);
1069 v3_muladds( mtxB
[3], mtxB
[1], -hb
*0.5f
+rb
, p2
);
1070 v3_muladds( mtxB
[3], mtxB
[1], hb
*0.5f
-rb
, p3
);
1072 capsule_manifold manifold
;
1073 rb_capsule_manifold_init( &manifold
);
1077 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
1078 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
1080 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
1081 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
1082 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
1083 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
1085 closest_point_segment( p2
, p3
, p0
, pa
);
1086 closest_point_segment( p2
, p3
, p1
, pb
);
1087 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
1088 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
1090 return rb_capsule__manifold_done( mtxA
, ca
, &manifold
, buf
);
1095 * Generates up to two contacts; optimised for the most stable manifold
1097 VG_STATIC
int rb_capsule_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1099 float h
= rba
->inf
.capsule
.height
,
1100 r
= rba
->inf
.capsule
.radius
;
1103 * Solving this in symetric local space of the cube saves us some time and a
1104 * couple branches when it comes to the quad stage.
1107 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
1108 v3_muls( centroid
, 0.5f
, centroid
);
1111 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
1112 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
1114 v3f pc
, p0w
, p1w
, p0
, p1
;
1115 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
1116 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
1118 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
1119 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
1120 v3_sub( p0
, centroid
, p0
);
1121 v3_sub( p1
, centroid
, p1
);
1122 v3_add( p0
, p1
, pc
);
1123 v3_muls( pc
, 0.5f
, pc
);
1126 * Finding an appropriate quad to collide lines with
1129 v3_div( pc
, bbx
[1], region
);
1132 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
1133 (fabsf(region
[0]) > fabsf(region
[2])) )
1135 float px
= vg_signf(region
[0]) * bbx
[1][0];
1136 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
1137 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
1138 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
1139 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
1141 else if( fabsf(region
[1]) > fabsf(region
[2]) )
1143 float py
= vg_signf(region
[1]) * bbx
[1][1];
1144 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
1145 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
1146 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
1147 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
1151 float pz
= vg_signf(region
[2]) * bbx
[1][2];
1152 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
1153 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
1154 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
1155 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
1158 capsule_manifold manifold
;
1159 rb_capsule_manifold_init( &manifold
);
1162 closest_point_aabb( p0
, bbx
, c0
);
1163 closest_point_aabb( p1
, bbx
, c1
);
1166 v3_sub( c0
, p0
, d0
);
1167 v3_sub( c1
, p1
, d1
);
1168 v3_sub( p1
, p0
, da
);
1174 if( v3_dot( da
, d0
) <= 0.01f
)
1175 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
1177 if( v3_dot( da
, d1
) >= -0.01f
)
1178 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
1180 for( int i
=0; i
<4; i
++ )
1187 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
1188 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1192 * Create final contacts based on line manifold
1194 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
1195 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
1202 for( int i
=0; i
<4; i
++ )
1208 v3_add( quad
[i0
], centroid
, q0
);
1209 v3_add( quad
[i1
], centroid
, q1
);
1211 m4x3_mulv( rbb
->to_world
, q0
, q0
);
1212 m4x3_mulv( rbb
->to_world
, q1
, q1
);
1214 vg_line( q0
, q1
, 0xffffffff );
1218 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1222 VG_STATIC
int rb_sphere_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1226 closest_point_obb( rba
->co
, rbb
->bbx
, rbb
->to_world
, rbb
->to_local
, co
);
1227 v3_sub( rba
->co
, co
, delta
);
1229 float d2
= v3_length2(delta
),
1230 r
= rba
->inf
.sphere
.radius
;
1239 v3_sub( rba
->co
, rbb
->co
, delta
);
1242 * some extra testing is required to find the best axis to push the
1243 * object back outside the box. Since there isnt a clear seperating
1244 * vector already, especially on really high aspect boxes.
1246 float lx
= v3_dot( rbb
->right
, delta
),
1247 ly
= v3_dot( rbb
->up
, delta
),
1248 lz
= v3_dot( rbb
->forward
, delta
),
1249 px
= rbb
->bbx
[1][0] - fabsf(lx
),
1250 py
= rbb
->bbx
[1][1] - fabsf(ly
),
1251 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
1253 if( px
< py
&& px
< pz
)
1254 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
1256 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
1258 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
1260 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
1266 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1268 v3_copy( co
, ct
->co
);
1273 ct
->type
= k_contact_type_default
;
1280 VG_STATIC
int rb_sphere_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1283 v3_sub( rba
->co
, rbb
->co
, delta
);
1285 float d2
= v3_length2(delta
),
1286 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
1290 float d
= sqrtf(d2
);
1293 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1296 v3_muladds( rba
->co
, ct
->n
,-rba
->inf
.sphere
.radius
, p0
);
1297 v3_muladds( rbb
->co
, ct
->n
, rbb
->inf
.sphere
.radius
, p1
);
1298 v3_add( p0
, p1
, ct
->co
);
1299 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1300 ct
->type
= k_contact_type_default
;
1310 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1313 __attribute__ ((deprecated
))
1314 VG_STATIC
int rb_sphere_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1315 v3f tri
[3], rb_ct
*buf
)
1319 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1320 closest_on_triangle_1( rba
->co
, tri
, co
);
1322 enum contact_type type
= closest_on_triangle_1( rba
->co
, tri
, co
);
1325 v3_sub( rba
->co
, co
, delta
);
1327 float d2
= v3_length2( delta
),
1328 r
= rba
->inf
.sphere
.radius
;
1335 v3_sub( tri
[2], tri
[0], ab
);
1336 v3_sub( tri
[1], tri
[0], ac
);
1337 v3_cross( ac
, ab
, tn
);
1338 v3_copy( tn
, ct
->n
);
1340 if( v3_length2( ct
->n
) <= 0.00001f
)
1342 vg_error( "Zero area triangle!\n" );
1346 v3_normalize( ct
->n
);
1348 float d
= sqrtf(d2
);
1350 v3_copy( co
, ct
->co
);
1362 VG_STATIC
int rb_sphere__triangle( m4x3f mtxA
, rb_sphere
*b
,
1363 v3f tri
[3], rb_ct
*buf
)
1366 enum contact_type type
= closest_on_triangle_1( mtxA
[3], tri
, co
);
1368 v3_sub( mtxA
[3], co
, delta
);
1370 float d2
= v3_length2( delta
),
1378 v3_sub( tri
[2], tri
[0], ab
);
1379 v3_sub( tri
[1], tri
[0], ac
);
1380 v3_cross( ac
, ab
, tn
);
1381 v3_copy( tn
, ct
->n
);
1383 if( v3_length2( ct
->n
) <= 0.00001f
)
1385 vg_error( "Zero area triangle!\n" );
1389 v3_normalize( ct
->n
);
1391 float d
= sqrtf(d2
);
1393 v3_copy( co
, ct
->co
);
1402 VG_STATIC
void rb_debug_sharp_scene_edges( rigidbody
*rbb
, float sharp_ang
,
1403 boxf box
, u32 colour
)
1405 sharp_ang
= cosf( sharp_ang
);
1407 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1408 vg_line_boxf( box
, 0xff00ff00 );
1411 bh_iter_init( 0, &it
);
1414 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, box
, &idx
) )
1416 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1419 for( int j
=0; j
<3; j
++ )
1420 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1422 for( int j
=0; j
<3; j
++ )
1426 v3_sub( tri
[(j
+1)%3], tri
[j
], edir
);
1428 if( v3_dot( edir
, (v3f
){ 0.5184758473652127f
,
1429 0.2073903389460850f
,
1430 -0.8295613557843402f
} ) < 0.0f
)
1435 bh_iter_init( 0, &jt
);
1438 float const k_r
= 0.02f
;
1439 v3_add( (v3f
){ k_r
, k_r
, k_r
}, tri
[j
], region
[1] );
1440 v3_add( (v3f
){ -k_r
, -k_r
, -k_r
}, tri
[j
], region
[0] );
1443 while( bh_next( rbb
->inf
.scene
.bh_scene
, &jt
, region
, &jdx
) )
1448 u32
*ptrj
= &sc
->arrindices
[ jdx
*3 ];
1451 for( int k
=0; k
<3; k
++ )
1452 v3_copy( sc
->arrvertices
[ptrj
[k
]].co
, trj
[k
] );
1454 for( int k
=0; k
<3; k
++ )
1456 if( v3_dist2( tri
[j
], trj
[k
] ) <= k_r
*k_r
)
1463 if( v3_dist2( tri
[jp1
], trj
[km1
] ) <= k_r
*k_r
)
1466 v3_sub( tri
[jp1
], tri
[j
], b0
);
1467 v3_sub( tri
[jp2
], tri
[j
], b1
);
1468 v3_sub( trj
[km2
], tri
[j
], b2
);
1471 v3_cross( b0
, b1
, cx0
);
1472 v3_cross( b2
, b0
, cx1
);
1474 float polarity
= v3_dot( cx0
, b2
);
1476 if( polarity
< 0.0f
)
1479 vg_line( tri
[j
], tri
[jp1
], 0xff00ff00 );
1480 float ang
= v3_dot(cx0
,cx1
) /
1481 (v3_length(cx0
)*v3_length(cx1
));
1482 if( ang
< sharp_ang
)
1484 vg_line( tri
[j
], tri
[jp1
], 0xff00ff00 );
1496 VG_STATIC
int rb_sphere__scene( m4x3f mtxA
, rb_sphere
*b
,
1497 m4x3f mtxB
, rb_scene
*s
, rb_ct
*buf
)
1499 scene
*sc
= s
->bh_scene
->user
;
1502 bh_iter_init( 0, &it
);
1507 float r
= b
->radius
+ 0.1f
;
1509 v3_sub( mtxA
[3], (v3f
){ r
,r
,r
}, box
[0] );
1510 v3_add( mtxA
[3], (v3f
){ r
,r
,r
}, box
[1] );
1512 while( bh_next( s
->bh_scene
, &it
, box
, &idx
) )
1514 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1517 for( int j
=0; j
<3; j
++ )
1518 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1520 buf
[ count
].element_id
= ptri
[0];
1522 vg_line( tri
[0],tri
[1],0x70ff6000 );
1523 vg_line( tri
[1],tri
[2],0x70ff6000 );
1524 vg_line( tri
[2],tri
[0],0x70ff6000 );
1526 int contact
= rb_sphere__triangle( mtxA
, b
, tri
, &buf
[count
] );
1531 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1540 __attribute__ ((deprecated
))
1541 VG_STATIC
int rb_sphere_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1543 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1546 bh_iter_init( 0, &it
);
1551 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
1553 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1556 for( int j
=0; j
<3; j
++ )
1557 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1559 buf
[ count
].element_id
= ptri
[0];
1561 vg_line( tri
[0],tri
[1],0x70ff6000 );
1562 vg_line( tri
[1],tri
[2],0x70ff6000 );
1563 vg_line( tri
[2],tri
[0],0x70ff6000 );
1565 int contact
= rb_sphere_triangle( rba
, rbb
, tri
, buf
+count
);
1570 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1579 VG_STATIC
int rb_box__scene( m4x3f mtxA
, boxf bbx
,
1580 m4x3f mtxB
, rb_scene
*s
, rb_ct
*buf
)
1582 scene
*sc
= s
->bh_scene
->user
;
1586 v3_sub( bbx
[1], bbx
[0], extent
);
1587 v3_muls( extent
, 0.5f
, extent
);
1588 v3_add( bbx
[0], extent
, center
);
1590 float r
= v3_length(extent
);
1592 v3_fill( world_bbx
[0], -r
);
1593 v3_fill( world_bbx
[1], r
);
1594 for( int i
=0; i
<2; i
++ ){
1595 v3_add( center
, world_bbx
[i
], world_bbx
[i
] );
1596 v3_add( mtxA
[3], world_bbx
[i
], world_bbx
[i
] );
1600 m4x3_invert_affine( mtxA
, to_local
);
1603 bh_iter_init( 0, &it
);
1607 vg_line_boxf( world_bbx
, VG__RED
);
1609 while( bh_next( s
->bh_scene
, &it
, world_bbx
, &idx
) ){
1610 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1612 for( int j
=0; j
<3; j
++ )
1613 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1615 if( rb_box_triangle_sat( extent
, center
, to_local
, tri
) ){
1616 vg_line(tri
[0],tri
[1],0xff50ff00 );
1617 vg_line(tri
[1],tri
[2],0xff50ff00 );
1618 vg_line(tri
[2],tri
[0],0xff50ff00 );
1621 vg_line(tri
[0],tri
[1],0xff0000ff );
1622 vg_line(tri
[1],tri
[2],0xff0000ff );
1623 vg_line(tri
[2],tri
[0],0xff0000ff );
1628 v3_sub( tri
[1], tri
[0], v0
);
1629 v3_sub( tri
[2], tri
[0], v1
);
1630 v3_cross( v0
, v1
, n
);
1633 /* find best feature */
1634 float best
= v3_dot( mtxA
[0], n
);
1637 for( int i
=1; i
<3; i
++ ){
1638 float c
= v3_dot( mtxA
[i
], n
);
1640 if( fabsf(c
) > fabsf(best
) ){
1649 float px
= best
> 0.0f
? bbx
[0][0]: bbx
[1][0];
1650 manifold
[0][0] = px
;
1651 manifold
[0][1] = bbx
[0][1];
1652 manifold
[0][2] = bbx
[0][2];
1653 manifold
[1][0] = px
;
1654 manifold
[1][1] = bbx
[1][1];
1655 manifold
[1][2] = bbx
[0][2];
1656 manifold
[2][0] = px
;
1657 manifold
[2][1] = bbx
[1][1];
1658 manifold
[2][2] = bbx
[1][2];
1659 manifold
[3][0] = px
;
1660 manifold
[3][1] = bbx
[0][1];
1661 manifold
[3][2] = bbx
[1][2];
1663 else if( axis
== 1 ){
1664 float py
= best
> 0.0f
? bbx
[0][1]: bbx
[1][1];
1665 manifold
[0][0] = bbx
[0][0];
1666 manifold
[0][1] = py
;
1667 manifold
[0][2] = bbx
[0][2];
1668 manifold
[1][0] = bbx
[1][0];
1669 manifold
[1][1] = py
;
1670 manifold
[1][2] = bbx
[0][2];
1671 manifold
[2][0] = bbx
[1][0];
1672 manifold
[2][1] = py
;
1673 manifold
[2][2] = bbx
[1][2];
1674 manifold
[3][0] = bbx
[0][0];
1675 manifold
[3][1] = py
;
1676 manifold
[3][2] = bbx
[1][2];
1679 float pz
= best
> 0.0f
? bbx
[0][2]: bbx
[1][2];
1680 manifold
[0][0] = bbx
[0][0];
1681 manifold
[0][1] = bbx
[0][1];
1682 manifold
[0][2] = pz
;
1683 manifold
[1][0] = bbx
[1][0];
1684 manifold
[1][1] = bbx
[0][1];
1685 manifold
[1][2] = pz
;
1686 manifold
[2][0] = bbx
[1][0];
1687 manifold
[2][1] = bbx
[1][1];
1688 manifold
[2][2] = pz
;
1689 manifold
[3][0] = bbx
[0][0];
1690 manifold
[3][1] = bbx
[1][1];
1691 manifold
[3][2] = pz
;
1694 for( int j
=0; j
<4; j
++ )
1695 m4x3_mulv( mtxA
, manifold
[j
], manifold
[j
] );
1697 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1698 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1699 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1700 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1702 for( int j
=0; j
<4; j
++ ){
1703 rb_ct
*ct
= buf
+count
;
1705 v3_copy( manifold
[j
], ct
->co
);
1706 v3_copy( n
, ct
->n
);
1708 float l0
= v3_dot( tri
[0], n
),
1709 l1
= v3_dot( manifold
[j
], n
);
1711 ct
->p
= (l0
-l1
)*0.5f
;
1715 ct
->type
= k_contact_type_default
;
1726 __attribute__ ((deprecated
))
1727 VG_STATIC
int rb_box_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1729 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1734 bh_iter_init( 0, &it
);
1739 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
1741 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1743 for( int j
=0; j
<3; j
++ )
1744 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1746 if( rb_box_triangle_sat( rba
, tri
) )
1748 vg_line(tri
[0],tri
[1],0xff50ff00 );
1749 vg_line(tri
[1],tri
[2],0xff50ff00 );
1750 vg_line(tri
[2],tri
[0],0xff50ff00 );
1754 vg_line(tri
[0],tri
[1],0xff0000ff );
1755 vg_line(tri
[1],tri
[2],0xff0000ff );
1756 vg_line(tri
[2],tri
[0],0xff0000ff );
1762 v3_sub( tri
[1], tri
[0], v0
);
1763 v3_sub( tri
[2], tri
[0], v1
);
1764 v3_cross( v0
, v1
, n
);
1767 /* find best feature */
1768 float best
= v3_dot( rba
->right
, n
);
1771 float cy
= v3_dot( rba
->up
, n
);
1772 if( fabsf(cy
) > fabsf(best
) )
1778 float cz
= -v3_dot( rba
->forward
, n
);
1779 if( fabsf(cz
) > fabsf(best
) )
1789 float px
= best
> 0.0f
? rba
->bbx
[0][0]: rba
->bbx
[1][0];
1790 manifold
[0][0] = px
;
1791 manifold
[0][1] = rba
->bbx
[0][1];
1792 manifold
[0][2] = rba
->bbx
[0][2];
1793 manifold
[1][0] = px
;
1794 manifold
[1][1] = rba
->bbx
[1][1];
1795 manifold
[1][2] = rba
->bbx
[0][2];
1796 manifold
[2][0] = px
;
1797 manifold
[2][1] = rba
->bbx
[1][1];
1798 manifold
[2][2] = rba
->bbx
[1][2];
1799 manifold
[3][0] = px
;
1800 manifold
[3][1] = rba
->bbx
[0][1];
1801 manifold
[3][2] = rba
->bbx
[1][2];
1803 else if( axis
== 1 )
1805 float py
= best
> 0.0f
? rba
->bbx
[0][1]: rba
->bbx
[1][1];
1806 manifold
[0][0] = rba
->bbx
[0][0];
1807 manifold
[0][1] = py
;
1808 manifold
[0][2] = rba
->bbx
[0][2];
1809 manifold
[1][0] = rba
->bbx
[1][0];
1810 manifold
[1][1] = py
;
1811 manifold
[1][2] = rba
->bbx
[0][2];
1812 manifold
[2][0] = rba
->bbx
[1][0];
1813 manifold
[2][1] = py
;
1814 manifold
[2][2] = rba
->bbx
[1][2];
1815 manifold
[3][0] = rba
->bbx
[0][0];
1816 manifold
[3][1] = py
;
1817 manifold
[3][2] = rba
->bbx
[1][2];
1821 float pz
= best
> 0.0f
? rba
->bbx
[0][2]: rba
->bbx
[1][2];
1822 manifold
[0][0] = rba
->bbx
[0][0];
1823 manifold
[0][1] = rba
->bbx
[0][1];
1824 manifold
[0][2] = pz
;
1825 manifold
[1][0] = rba
->bbx
[1][0];
1826 manifold
[1][1] = rba
->bbx
[0][1];
1827 manifold
[1][2] = pz
;
1828 manifold
[2][0] = rba
->bbx
[1][0];
1829 manifold
[2][1] = rba
->bbx
[1][1];
1830 manifold
[2][2] = pz
;
1831 manifold
[3][0] = rba
->bbx
[0][0];
1832 manifold
[3][1] = rba
->bbx
[1][1];
1833 manifold
[3][2] = pz
;
1836 for( int j
=0; j
<4; j
++ )
1837 m4x3_mulv( rba
->to_world
, manifold
[j
], manifold
[j
] );
1839 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1840 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1841 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1842 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1844 for( int j
=0; j
<4; j
++ )
1846 rb_ct
*ct
= buf
+count
;
1848 v3_copy( manifold
[j
], ct
->co
);
1849 v3_copy( n
, ct
->n
);
1851 float l0
= v3_dot( tri
[0], n
),
1852 l1
= v3_dot( manifold
[j
], n
);
1854 ct
->p
= (l0
-l1
)*0.5f
;
1858 ct
->type
= k_contact_type_default
;
1871 VG_STATIC
int rb_capsule__triangle( m4x3f mtxA
, rb_capsule
*c
,
1872 v3f tri
[3], rb_ct
*buf
)
1875 v3_muladds( mtxA
[3], mtxA
[1], -c
->height
*0.5f
+c
->radius
, p0w
);
1876 v3_muladds( mtxA
[3], mtxA
[1], c
->height
*0.5f
-c
->radius
, p1w
);
1878 capsule_manifold manifold
;
1879 rb_capsule_manifold_init( &manifold
);
1882 closest_on_triangle_1( p0w
, tri
, c0
);
1883 closest_on_triangle_1( p1w
, tri
, c1
);
1886 v3_sub( c0
, p0w
, d0
);
1887 v3_sub( c1
, p1w
, d1
);
1888 v3_sub( p1w
, p0w
, da
);
1894 if( v3_dot( da
, d0
) <= 0.01f
)
1895 rb_capsule_manifold( p0w
, c0
, 0.0f
, c
->radius
, &manifold
);
1897 if( v3_dot( da
, d1
) >= -0.01f
)
1898 rb_capsule_manifold( p1w
, c1
, 1.0f
, c
->radius
, &manifold
);
1900 for( int i
=0; i
<3; i
++ )
1907 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
1908 rb_capsule_manifold( ca
, cb
, ta
, c
->radius
, &manifold
);
1912 v3_sub( tri
[1], tri
[0], v0
);
1913 v3_sub( tri
[2], tri
[0], v1
);
1914 v3_cross( v0
, v1
, n
);
1917 int count
= rb_capsule__manifold_done( mtxA
, c
, &manifold
, buf
);
1918 for( int i
=0; i
<count
; i
++ )
1919 v3_copy( n
, buf
[i
].n
);
1925 * Generates up to two contacts; optimised for the most stable manifold
1928 __attribute__ ((deprecated
))
1929 VG_STATIC
int rb_capsule_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1930 v3f tri
[3], rb_ct
*buf
)
1932 float h
= rba
->inf
.capsule
.height
,
1933 r
= rba
->inf
.capsule
.radius
;
1936 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
1937 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
1939 capsule_manifold manifold
;
1940 rb_capsule_manifold_init( &manifold
);
1943 closest_on_triangle_1( p0w
, tri
, c0
);
1944 closest_on_triangle_1( p1w
, tri
, c1
);
1947 v3_sub( c0
, p0w
, d0
);
1948 v3_sub( c1
, p1w
, d1
);
1949 v3_sub( p1w
, p0w
, da
);
1955 if( v3_dot( da
, d0
) <= 0.01f
)
1956 rb_capsule_manifold( p0w
, c0
, 0.0f
, r
, &manifold
);
1958 if( v3_dot( da
, d1
) >= -0.01f
)
1959 rb_capsule_manifold( p1w
, c1
, 1.0f
, r
, &manifold
);
1961 for( int i
=0; i
<3; i
++ )
1968 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
1969 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1973 v3_sub( tri
[1], tri
[0], v0
);
1974 v3_sub( tri
[2], tri
[0], v1
);
1975 v3_cross( v0
, v1
, n
);
1978 int count
= rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1979 for( int i
=0; i
<count
; i
++ )
1980 v3_copy( n
, buf
[i
].n
);
1986 /* mtxB is defined only for tradition; it is not used currently */
1987 VG_STATIC
int rb_capsule__scene( m4x3f mtxA
, rb_capsule
*c
,
1988 m4x3f mtxB
, rb_scene
*s
,
1992 bh_iter_init( 0, &it
);
1997 v3_sub( mtxA
[3], (v3f
){ c
->height
, c
->height
, c
->height
}, bbx
[0] );
1998 v3_add( mtxA
[3], (v3f
){ c
->height
, c
->height
, c
->height
}, bbx
[1] );
2000 scene
*sc
= s
->bh_scene
->user
;
2002 while( bh_next( s
->bh_scene
, &it
, bbx
, &idx
) )
2004 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
2007 for( int j
=0; j
<3; j
++ )
2008 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
2010 buf
[ count
].element_id
= ptri
[0];
2012 int contact
= rb_capsule__triangle( mtxA
, c
, tri
, &buf
[count
] );
2017 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
2026 __attribute__ ((deprecated
))
2027 VG_STATIC
int rb_capsule_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2029 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
2032 bh_iter_init( 0, &it
);
2037 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
2039 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
2042 for( int j
=0; j
<3; j
++ )
2043 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
2045 buf
[ count
].element_id
= ptri
[0];
2048 vg_line( tri
[0],tri
[1],0x70ff6000 );
2049 vg_line( tri
[1],tri
[2],0x70ff6000 );
2050 vg_line( tri
[2],tri
[0],0x70ff6000 );
2053 int contact
= rb_capsule_triangle( rba
, rbb
, tri
, buf
+count
);
2058 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
2066 VG_STATIC
int rb_scene_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2068 return rb_capsule_scene( rbb
, rba
, buf
);
2072 VG_STATIC
int RB_MATRIX_ERROR( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2075 vg_error( "Collision type is unimplemented between types %d and %d\n",
2076 rba
->type
, rbb
->type
);
2082 VG_STATIC
int rb_sphere_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2084 return rb_capsule_sphere( rbb
, rba
, buf
);
2088 VG_STATIC
int rb_box_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2090 return rb_capsule_box( rbb
, rba
, buf
);
2094 VG_STATIC
int rb_box_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2096 return rb_sphere_box( rbb
, rba
, buf
);
2100 VG_STATIC
int rb_scene_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2102 return rb_box_scene( rbb
, rba
, buf
);
2107 VG_STATIC
int (*rb_jump_table
[4][4])( rigidbody
*a
, rigidbody
*b
, rb_ct
*buf
) =
2109 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
2110 { RB_MATRIX_ERROR
, rb_box_sphere
, rb_box_capsule
, rb_box_scene
},
2111 { rb_sphere_box
, rb_sphere_sphere
, rb_sphere_capsule
, rb_sphere_scene
},
2112 { rb_capsule_box
, rb_capsule_sphere
, rb_capsule_capsule
, rb_capsule_scene
},
2113 { rb_scene_box
, RB_MATRIX_ERROR
, rb_scene_capsule
, RB_MATRIX_ERROR
}
2116 VG_STATIC
int rb_collide( rigidbody
*rba
, rigidbody
*rbb
)
2118 int (*collider_jump
)(rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
2119 = rb_jump_table
[rba
->type
][rbb
->type
];
2122 * 12 is the maximum manifold size we can generate, so we are forced to abort
2123 * potentially checking any more.
2125 if( rb_contact_count
+ 12 > vg_list_size(rb_contact_buffer
) )
2127 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
2128 rb_contact_count
, vg_list_size(rb_contact_buffer
) );
2133 * FUTURE: Replace this with a more dedicated broad phase pass
2135 if( box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
2137 int count
= collider_jump( rba
, rbb
, rb_contact_buffer
+rb_contact_count
);
2138 rb_contact_count
+= count
;
2146 VG_STATIC
int rb_global_has_space( void )
2148 if( rb_contact_count
+ 16 > vg_list_size(rb_contact_buffer
) )
2154 VG_STATIC rb_ct
*rb_global_buffer( void )
2156 return &rb_contact_buffer
[ rb_contact_count
];
2160 * -----------------------------------------------------------------------------
2162 * -----------------------------------------------------------------------------
2165 VG_STATIC
void rb_solver_reset(void)
2167 rb_contact_count
= 0;
2170 VG_STATIC rb_ct
*rb_global_ct(void)
2172 return rb_contact_buffer
+ rb_contact_count
;
2175 VG_STATIC
void rb_prepare_contact( rb_ct
*ct
, float timestep
)
2177 ct
->bias
= -0.2f
* (timestep
*3600.0f
)
2178 * vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
2180 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
2181 ct
->norm_impulse
= 0.0f
;
2182 ct
->tangent_impulse
[0] = 0.0f
;
2183 ct
->tangent_impulse
[1] = 0.0f
;
2186 /* calculate total move. manifold should belong to ONE object only */
2187 VG_STATIC
void rb_depenetrate( rb_ct
*manifold
, int len
, v3f dt
)
2191 for( int j
=0; j
<7; j
++ )
2193 for( int i
=0; i
<len
; i
++ )
2195 struct contact
*ct
= &manifold
[i
];
2197 float resolved_amt
= v3_dot( ct
->n
, dt
),
2198 remaining
= (ct
->p
-k_penetration_slop
) - resolved_amt
,
2199 apply
= vg_maxf( remaining
, 0.0f
) * 0.4f
;
2201 v3_muladds( dt
, ct
->n
, apply
, dt
);
2207 * Initializing things like tangent vectors
2209 VG_STATIC
void rb_presolve_contacts( rb_ct
*buffer
, int len
)
2211 for( int i
=0; i
<len
; i
++ ){
2212 rb_ct
*ct
= &buffer
[i
];
2213 rb_prepare_contact( ct
, k_rb_delta
);
2215 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
2216 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
2217 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
2218 v3_cross( ra
, ct
->n
, raCn
);
2219 v3_cross( rb
, ct
->n
, rbCn
);
2221 /* orient inverse inertia tensors */
2223 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
2224 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
2226 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
2227 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
2228 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
2229 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
2231 for( int j
=0; j
<2; j
++ ){
2233 v3_cross( ct
->t
[j
], ra
, raCt
);
2234 v3_cross( ct
->t
[j
], rb
, rbCt
);
2235 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
2236 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
2238 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
2239 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
2240 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
2241 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
2244 rb_debug_contact( ct
);
2249 * Creates relative contact velocity vector
2251 VG_STATIC
void rb_rcv( rigidbody
*rba
, rigidbody
*rbb
, v3f ra
, v3f rb
, v3f rv
)
2254 v3_cross( rba
->w
, ra
, rva
);
2255 v3_add( rba
->v
, rva
, rva
);
2256 v3_cross( rbb
->w
, rb
, rvb
);
2257 v3_add( rbb
->v
, rvb
, rvb
);
2259 v3_sub( rva
, rvb
, rv
);
2263 * Apply impulse to object
2265 VG_STATIC
void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
2268 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
2270 /* Angular velocity */
2272 v3_cross( delta
, impulse
, wa
);
2274 m3x3_mulv( rb
->iIw
, wa
, wa
);
2275 v3_add( rb
->w
, wa
, rb
->w
);
2279 * One iteration to solve the contact constraint
2281 VG_STATIC
void rb_solve_contacts( rb_ct
*buf
, int len
)
2283 for( int i
=0; i
<len
; i
++ ){
2284 struct contact
*ct
= &buf
[i
];
2287 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
2288 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
2289 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
2292 for( int j
=0; j
<2; j
++ ){
2293 float f
= k_friction
* ct
->norm_impulse
,
2294 vt
= v3_dot( rv
, ct
->t
[j
] ),
2295 lambda
= ct
->tangent_mass
[j
] * -vt
;
2297 float temp
= ct
->tangent_impulse
[j
];
2298 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
2299 lambda
= ct
->tangent_impulse
[j
] - temp
;
2302 v3_muls( ct
->t
[j
], lambda
, impulse
);
2303 rb_linear_impulse( ct
->rba
, ra
, impulse
);
2305 v3_muls( ct
->t
[j
], -lambda
, impulse
);
2306 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
2310 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
2311 float vn
= v3_dot( rv
, ct
->n
),
2312 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
2314 float temp
= ct
->norm_impulse
;
2315 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
2316 lambda
= ct
->norm_impulse
- temp
;
2319 v3_muls( ct
->n
, lambda
, impulse
);
2320 rb_linear_impulse( ct
->rba
, ra
, impulse
);
2322 v3_muls( ct
->n
, -lambda
, impulse
);
2323 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
2328 * -----------------------------------------------------------------------------
2330 * -----------------------------------------------------------------------------
2333 VG_STATIC
void rb_debug_position_constraints( rb_constr_pos
*buffer
, int len
)
2335 for( int i
=0; i
<len
; i
++ )
2337 rb_constr_pos
*constr
= &buffer
[i
];
2338 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
2341 m3x3_mulv( rba
->to_world
, constr
->lca
, wca
);
2342 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wcb
);
2345 v3_add( wca
, rba
->co
, p0
);
2346 v3_add( wcb
, rbb
->co
, p1
);
2347 vg_line_pt3( p0
, 0.0025f
, 0xff000000 );
2348 vg_line_pt3( p1
, 0.0025f
, 0xffffffff );
2349 vg_line2( p0
, p1
, 0xff000000, 0xffffffff );
2353 VG_STATIC
void rb_presolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2358 for( int i
=0; i
<len
; i
++ )
2360 rb_constr_swingtwist
*st
= &buf
[ i
];
2362 v3f vx
, vy
, va
, vxb
, axis
, center
;
2364 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
2365 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
2366 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
2367 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
2368 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
2369 v3_cross( vy
, vx
, axis
);
2371 /* Constraint violated ? */
2372 float fx
= v3_dot( vx
, va
), /* projection world */
2373 fy
= v3_dot( vy
, va
),
2374 fn
= v3_dot( va
, axis
),
2376 rx
= st
->conevx
[3], /* elipse radii */
2379 lx
= fx
/rx
, /* projection local (fn==lz) */
2382 st
->tangent_violation
= ((lx
*lx
+ ly
*ly
) > fn
*fn
) || (fn
<= 0.0f
);
2383 if( st
->tangent_violation
)
2385 /* Calculate a good position and the axis to solve on */
2386 v2f closest
, tangent
,
2387 p
= { fx
/fabsf(fn
), fy
/fabsf(fn
) };
2389 closest_point_elipse( p
, (v2f
){rx
,ry
}, closest
);
2390 tangent
[0] = -closest
[1] / (ry
*ry
);
2391 tangent
[1] = closest
[0] / (rx
*rx
);
2392 v2_normalize( tangent
);
2395 v3_muladds( axis
, vx
, closest
[0], v0
);
2396 v3_muladds( v0
, vy
, closest
[1], v0
);
2399 v3_muls( vx
, tangent
[0], v1
);
2400 v3_muladds( v1
, vy
, tangent
[1], v1
);
2402 v3_copy( v0
, st
->tangent_target
);
2403 v3_copy( v1
, st
->tangent_axis
);
2405 /* calculate mass */
2407 m3x3_mulv( st
->rba
->iIw
, st
->tangent_axis
, aIw
);
2408 m3x3_mulv( st
->rbb
->iIw
, st
->tangent_axis
, bIw
);
2409 st
->tangent_mass
= 1.0f
/ (v3_dot( st
->tangent_axis
, aIw
) +
2410 v3_dot( st
->tangent_axis
, bIw
));
2412 float angle
= v3_dot( va
, st
->tangent_target
);
2416 v3_cross( vy
, va
, refaxis
); /* our default rotation */
2417 v3_normalize( refaxis
);
2419 float angle
= v3_dot( refaxis
, vxb
);
2420 st
->axis_violation
= fabsf(angle
) < st
->conet
;
2422 if( st
->axis_violation
)
2425 v3_cross( refaxis
, vxb
, dir_test
);
2427 if( v3_dot(dir_test
, va
) < 0.0f
)
2428 st
->axis_violation
= -st
->axis_violation
;
2430 float newang
= (float)st
->axis_violation
* acosf(st
->conet
-0.0001f
);
2433 v3_cross( va
, refaxis
, refaxis_up
);
2434 v3_muls( refaxis_up
, sinf(newang
), st
->axis_target
);
2435 v3_muladds( st
->axis_target
, refaxis
, -cosf(newang
), st
->axis_target
);
2437 /* calculate mass */
2438 v3_copy( va
, st
->axis
);
2440 m3x3_mulv( st
->rba
->iIw
, st
->axis
, aIw
);
2441 m3x3_mulv( st
->rbb
->iIw
, st
->axis
, bIw
);
2442 st
->axis_mass
= 1.0f
/ (v3_dot( st
->axis
, aIw
) +
2443 v3_dot( st
->axis
, bIw
));
2448 VG_STATIC
void rb_debug_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2453 for( int i
=0; i
<len
; i
++ )
2455 rb_constr_swingtwist
*st
= &buf
[ i
];
2457 v3f vx
, vxb
, vy
, va
, axis
, center
;
2459 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
2460 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
2461 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
2462 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
2463 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
2464 v3_cross( vy
, vx
, axis
);
2466 float rx
= st
->conevx
[3], /* elipse radii */
2470 v3_muladds( center
, va
, size
, p1
);
2471 vg_line( center
, p1
, 0xffffffff );
2472 vg_line_pt3( p1
, 0.00025f
, 0xffffffff );
2474 if( st
->tangent_violation
)
2476 v3_muladds( center
, st
->tangent_target
, size
, p0
);
2478 vg_line( center
, p0
, 0xff00ff00 );
2479 vg_line_pt3( p0
, 0.00025f
, 0xff00ff00 );
2480 vg_line( p1
, p0
, 0xff000000 );
2483 for( int x
=0; x
<32; x
++ )
2485 float t0
= ((float)x
* (1.0f
/32.0f
)) * VG_TAUf
,
2486 t1
= (((float)x
+1.0f
) * (1.0f
/32.0f
)) * VG_TAUf
,
2493 v3_muladds( axis
, vx
, c0
*rx
, v0
);
2494 v3_muladds( v0
, vy
, s0
*ry
, v0
);
2495 v3_muladds( axis
, vx
, c1
*rx
, v1
);
2496 v3_muladds( v1
, vy
, s1
*ry
, v1
);
2501 v3_muladds( center
, v0
, size
, p0
);
2502 v3_muladds( center
, v1
, size
, p1
);
2504 u32 col0r
= fabsf(c0
) * 255.0f
,
2505 col0g
= fabsf(s0
) * 255.0f
,
2506 col1r
= fabsf(c1
) * 255.0f
,
2507 col1g
= fabsf(s1
) * 255.0f
,
2508 col
= st
->tangent_violation
? 0xff0000ff: 0xff000000,
2509 col0
= col
| (col0r
<<16) | (col0g
<< 8),
2510 col1
= col
| (col1r
<<16) | (col1g
<< 8);
2512 vg_line2( center
, p0
, VG__NONE
, col0
);
2513 vg_line2( p0
, p1
, col0
, col1
);
2517 v3_muladds( center
, va
, size
, p0
);
2518 v3_muladds( p0
, vxb
, size
, p1
);
2520 vg_line( p0
, p1
, 0xff0000ff );
2522 if( st
->axis_violation
)
2524 v3_muladds( p0
, st
->axis_target
, size
*1.25f
, p1
);
2525 vg_line( p0
, p1
, 0xffffff00 );
2526 vg_line_pt3( p1
, 0.0025f
, 0xffffff80 );
2530 v3_cross( vy
, va
, refaxis
); /* our default rotation */
2531 v3_normalize( refaxis
);
2533 v3_cross( va
, refaxis
, refaxis_up
);
2534 float newang
= acosf(st
->conet
-0.0001f
);
2536 v3_muladds( p0
, refaxis_up
, sinf(newang
)*size
, p1
);
2537 v3_muladds( p1
, refaxis
, -cosf(newang
)*size
, p1
);
2538 vg_line( p0
, p1
, 0xff000000 );
2540 v3_muladds( p0
, refaxis_up
, sinf(-newang
)*size
, p1
);
2541 v3_muladds( p1
, refaxis
, -cosf(-newang
)*size
, p1
);
2542 vg_line( p0
, p1
, 0xff404040 );
2547 * Solve a list of positional constraints
2549 VG_STATIC
void rb_solve_position_constraints( rb_constr_pos
*buf
, int len
)
2551 for( int i
=0; i
<len
; i
++ )
2553 rb_constr_pos
*constr
= &buf
[i
];
2554 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
2557 m3x3_mulv( rba
->to_world
, constr
->lca
, wa
);
2558 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wb
);
2560 m3x3f ssra
, ssrat
, ssrb
, ssrbt
;
2562 m3x3_skew_symetric( ssrat
, wa
);
2563 m3x3_skew_symetric( ssrbt
, wb
);
2564 m3x3_transpose( ssrat
, ssra
);
2565 m3x3_transpose( ssrbt
, ssrb
);
2567 v3f b
, b_wa
, b_wb
, b_a
, b_b
;
2568 m3x3_mulv( ssra
, rba
->w
, b_wa
);
2569 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
2570 v3_add( rba
->v
, b_wa
, b
);
2571 v3_sub( b
, rbb
->v
, b
);
2572 v3_sub( b
, b_wb
, b
);
2573 v3_muls( b
, -1.0f
, b
);
2576 m3x3_diagonal( invMa
, rba
->inv_mass
);
2577 m3x3_diagonal( invMb
, rbb
->inv_mass
);
2580 m3x3_mul( ssra
, rba
->iIw
, ia
);
2581 m3x3_mul( ia
, ssrat
, ia
);
2582 m3x3_mul( ssrb
, rbb
->iIw
, ib
);
2583 m3x3_mul( ib
, ssrbt
, ib
);
2586 m3x3_add( invMa
, ia
, cma
);
2587 m3x3_add( invMb
, ib
, cmb
);
2590 m3x3_add( cma
, cmb
, A
);
2592 /* Solve Ax = b ( A^-1*b = x ) */
2595 m3x3_inv( A
, invA
);
2596 m3x3_mulv( invA
, b
, impulse
);
2598 v3f delta_va
, delta_wa
, delta_vb
, delta_wb
;
2600 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
2601 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
2603 m3x3_mulv( invMa
, impulse
, delta_va
);
2604 m3x3_mulv( invMb
, impulse
, delta_vb
);
2605 m3x3_mulv( iwa
, impulse
, delta_wa
);
2606 m3x3_mulv( iwb
, impulse
, delta_wb
);
2608 v3_add( rba
->v
, delta_va
, rba
->v
);
2609 v3_add( rba
->w
, delta_wa
, rba
->w
);
2610 v3_sub( rbb
->v
, delta_vb
, rbb
->v
);
2611 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
2615 VG_STATIC
void rb_solve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2620 for( int i
=0; i
<len
; i
++ )
2622 rb_constr_swingtwist
*st
= &buf
[ i
];
2624 if( !st
->axis_violation
)
2627 float rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
2628 v3_dot( st
->axis
, st
->rba
->w
);
2630 if( rv
* (float)st
->axis_violation
> 0.0f
)
2633 v3f impulse
, wa
, wb
;
2634 v3_muls( st
->axis
, rv
*st
->axis_mass
, impulse
);
2635 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
2636 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
2638 v3_muls( impulse
, -1.0f
, impulse
);
2639 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
2640 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
2642 float rv2
= v3_dot( st
->axis
, st
->rbb
->w
) -
2643 v3_dot( st
->axis
, st
->rba
->w
);
2646 for( int i
=0; i
<len
; i
++ )
2648 rb_constr_swingtwist
*st
= &buf
[ i
];
2650 if( !st
->tangent_violation
)
2653 float rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
2654 v3_dot( st
->tangent_axis
, st
->rba
->w
);
2659 v3f impulse
, wa
, wb
;
2660 v3_muls( st
->tangent_axis
, rv
*st
->tangent_mass
, impulse
);
2661 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
2662 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
2664 v3_muls( impulse
, -1.0f
, impulse
);
2665 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
2666 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
2668 float rv2
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
2669 v3_dot( st
->tangent_axis
, st
->rba
->w
);
2673 VG_STATIC
void rb_solve_constr_angle( rigidbody
*rba
, rigidbody
*rbb
,
2676 m3x3f ssra
, ssrb
, ssrat
, ssrbt
;
2679 m3x3_skew_symetric( ssrat
, ra
);
2680 m3x3_skew_symetric( ssrbt
, rb
);
2681 m3x3_transpose( ssrat
, ssra
);
2682 m3x3_transpose( ssrbt
, ssrb
);
2684 m3x3_mul( ssra
, rba
->iIw
, cma
);
2685 m3x3_mul( cma
, ssrat
, cma
);
2686 m3x3_mul( ssrb
, rbb
->iIw
, cmb
);
2687 m3x3_mul( cmb
, ssrbt
, cmb
);
2690 m3x3_add( cma
, cmb
, A
);
2691 m3x3_inv( A
, invA
);
2694 m3x3_mulv( ssra
, rba
->w
, b_wa
);
2695 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
2696 v3_add( b_wa
, b_wb
, b
);
2700 m3x3_mulv( invA
, b
, impulse
);
2702 v3f delta_wa
, delta_wb
;
2704 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
2705 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
2706 m3x3_mulv( iwa
, impulse
, delta_wa
);
2707 m3x3_mulv( iwb
, impulse
, delta_wb
);
2708 v3_add( rba
->w
, delta_wa
, rba
->w
);
2709 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
2713 * Correct position constraint drift errors
2714 * [ 0.0 <= amt <= 1.0 ]: the correction amount
2716 VG_STATIC
void rb_correct_position_constraints( rb_constr_pos
*buf
, int len
,
2719 for( int i
=0; i
<len
; i
++ )
2721 rb_constr_pos
*constr
= &buf
[i
];
2722 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
2725 m3x3_mulv( rba
->to_world
, constr
->lca
, p0
);
2726 m3x3_mulv( rbb
->to_world
, constr
->lcb
, p1
);
2727 v3_add( rba
->co
, p0
, p0
);
2728 v3_add( rbb
->co
, p1
, p1
);
2729 v3_sub( p1
, p0
, d
);
2731 v3_muladds( rbb
->co
, d
, -1.0f
* amt
, rbb
->co
);
2732 rb_update_transform( rbb
);
2736 VG_STATIC
void rb_correct_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2737 int len
, float amt
)
2739 for( int i
=0; i
<len
; i
++ )
2741 rb_constr_swingtwist
*st
= &buf
[i
];
2743 if( !st
->tangent_violation
)
2747 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
2749 float angle
= v3_dot( va
, st
->tangent_target
);
2751 if( fabsf(angle
) < 0.9999f
)
2754 v3_cross( va
, st
->tangent_target
, axis
);
2757 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
2758 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
2759 rb_update_transform( st
->rbb
);
2763 for( int i
=0; i
<len
; i
++ )
2765 rb_constr_swingtwist
*st
= &buf
[i
];
2767 if( !st
->axis_violation
)
2771 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
2773 float angle
= v3_dot( vxb
, st
->axis_target
);
2775 if( fabsf(angle
) < 0.9999f
)
2778 v3_cross( vxb
, st
->axis_target
, axis
);
2781 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
2782 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
2783 rb_update_transform( st
->rbb
);
2788 VG_STATIC
void rb_correct_contact_constraints( rb_ct
*buf
, int len
, float amt
)
2790 for( int i
=0; i
<len
; i
++ )
2792 rb_ct
*ct
= &buf
[i
];
2793 rigidbody
*rba
= ct
->rba
,
2796 float mass_total
= 1.0f
/ (rba
->inv_mass
+ rbb
->inv_mass
);
2798 v3_muladds( rba
->co
, ct
->n
, -mass_total
* rba
->inv_mass
, rba
->co
);
2799 v3_muladds( rbb
->co
, ct
->n
, mass_total
* rbb
->inv_mass
, rbb
->co
);
2808 VG_STATIC
void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
2809 float amt
, float drag
)
2812 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
2813 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
2815 v3_muladds( ra
->v
, plane
, lambda
* k_rb_delta
, ra
->v
);
2818 v3_muls( ra
->v
, 1.0f
-(drag
*k_rb_delta
), ra
->v
);
2821 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
2824 VG_STATIC
void rb_effect_spring_target_vector( rigidbody
*rba
, v3f ra
, v3f rt
,
2825 float spring
, float dampening
,
2828 float d
= v3_dot( rt
, ra
);
2829 float a
= vg_signf( d
) * acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
2832 v3_cross( rt
, ra
, axis
);
2834 float Fs
= -a
* spring
,
2835 Fd
= -v3_dot( rba
->w
, axis
) * dampening
;
2837 v3_muladds( rba
->w
, axis
, (Fs
+Fd
) * timestep
, rba
->w
);
2841 * -----------------------------------------------------------------------------
2842 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
2844 * -----------------------------------------------------------------------------
2847 VG_STATIC
void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
2849 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2850 box_concat( bound
, rb
->bbx_world
);
2853 VG_STATIC
float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
2855 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2856 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
2859 VG_STATIC
void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
2861 rigidbody temp
, *rba
, *rbb
;
2862 rba
= &((rigidbody
*)user
)[ ia
];
2863 rbb
= &((rigidbody
*)user
)[ ib
];
2870 VG_STATIC
void rb_bh_debug( void *user
, u32 item_index
)
2872 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2873 rb_debug( rb
, 0xff00ffff );
2876 VG_STATIC bh_system bh_system_rigidbodies
=
2878 .expand_bound
= rb_bh_expand_bound
,
2879 .item_centroid
= rb_bh_centroid
,
2880 .item_swap
= rb_bh_swap
,
2881 .item_debug
= rb_bh_debug
,
2885 #endif /* RIGIDBODY_H */