2 * Resources: Box2D - Erin Catto
9 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
);
10 static bh_system bh_system_rigidbodies
;
16 #define k_rb_rate 60.0f
17 #define k_rb_delta (1.0f/k_rb_rate)
19 typedef struct rigidbody rigidbody
;
20 typedef struct contact rb_ct
;
51 v3f right
, up
, forward
;
58 v3f delta
; /* where is the origin of this in relation to a parent body */
59 m4x3f to_world
, to_local
;
62 static rigidbody rb_static_null
=
65 .q
={0.0f
,0.0f
,0.0f
,1.0f
},
72 static void rb_debug( rigidbody
*rb
, u32 colour
);
79 float mass_total
, p
, bias
, norm_impulse
, tangent_impulse
[2];
81 rb_contact_buffer
[256];
82 static int rb_contact_count
= 0;
84 static void rb_update_transform( rigidbody
*rb
)
87 q_m3x3( rb
->q
, rb
->to_world
);
88 v3_copy( rb
->co
, rb
->to_world
[3] );
90 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
92 box_copy( rb
->bbx
, rb
->bbx_world
);
93 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
95 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
96 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
97 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
100 static float sphere_volume( float radius
)
102 float r3
= radius
*radius
*radius
;
103 return (4.0f
/3.0f
) * VG_PIf
* r3
;
106 static void rb_init( rigidbody
*rb
)
110 if( rb
->type
== k_rb_shape_box
)
113 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
114 volume
= dims
[0]*dims
[1]*dims
[2];
116 else if( rb
->type
== k_rb_shape_sphere
)
118 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
119 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
120 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
122 else if( rb
->type
== k_rb_shape_capsule
)
124 float r
= rb
->inf
.capsule
.radius
,
125 h
= rb
->inf
.capsule
.height
;
126 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
128 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
129 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
140 rb
->inv_mass
= 1.0f
/(8.0f
*volume
);
146 rb_update_transform( rb
);
149 static void rb_iter( rigidbody
*rb
)
151 v3f gravity
= { 0.0f
, -9.6f
, 0.0f
};
152 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
154 /* intergrate velocity */
155 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
156 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
158 /* inegrate inertia */
159 if( v3_length2( rb
->w
) > 0.0f
)
163 v3_copy( rb
->w
, axis
);
165 float mag
= v3_length( axis
);
166 v3_divs( axis
, mag
, axis
);
167 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
168 q_mul( rotation
, rb
->q
, rb
->q
);
172 static void rb_torque( rigidbody
*rb
, v3f axis
, float mag
)
174 v3_muladds( rb
->w
, axis
, mag
*k_rb_delta
, rb
->w
);
177 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
179 /* Compute tangent basis (box2d) */
180 if( fabsf( n
[0] ) >= 0.57735027f
)
194 v3_cross( n
, tx
, ty
);
197 static void rb_solver_reset(void);
198 static void rb_build_manifold_terrain( rigidbody
*rb
);
199 static void rb_build_manifold_terrain_sphere( rigidbody
*rb
);
200 static void rb_solve_contacts( rb_ct
*buf
, int len
);
201 static void rb_presolve_contacts( rb_ct
*buffer
, int len
);
204 * These closest point tests were learned from Real-Time Collision Detection by
207 static float closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
208 float *s
, float *t
, v3f c1
, v3f c2
)
211 v3_sub( q1
, p1
, d1
);
212 v3_sub( q2
, p2
, d2
);
215 float a
= v3_length2( d1
),
216 e
= v3_length2( d2
),
219 const float kEpsilon
= 0.0001f
;
221 if( a
<= kEpsilon
&& e
<= kEpsilon
)
229 v3_sub( c1
, c2
, v0
);
231 return v3_length2( v0
);
237 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
241 float c
= v3_dot( d1
, r
);
245 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
249 float b
= v3_dot(d1
,d2
),
254 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
266 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
271 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
276 v3_muladds( p1
, d1
, *s
, c1
);
277 v3_muladds( p2
, d2
, *t
, c2
);
280 v3_sub( c1
, c2
, v0
);
281 return v3_length2( v0
);
284 static void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
286 v3_maxv( p
, box
[0], dest
);
287 v3_minv( dest
, box
[1], dest
);
290 static void closest_point_obb( v3f p
, rigidbody
*rb
, v3f dest
)
293 m4x3_mulv( rb
->to_local
, p
, local
);
294 closest_point_aabb( local
, rb
->bbx
, local
);
295 m4x3_mulv( rb
->to_world
, local
, dest
);
298 static void closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
302 v3_sub( point
, a
, v1
);
304 float t
= v3_dot( v1
, v0
) / v3_length2(v0
);
305 v3_muladds( a
, v0
, vg_clampf(t
,0.0f
,1.0f
), dest
);
308 static void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
313 /* Region outside A */
314 v3_sub( tri
[1], tri
[0], ab
);
315 v3_sub( tri
[2], tri
[0], ac
);
316 v3_sub( p
, tri
[0], ap
);
320 if( d1
<= 0.0f
&& d2
<= 0.0f
)
322 v3_copy( tri
[0], dest
);
323 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
327 /* Region outside B */
331 v3_sub( p
, tri
[1], bp
);
332 d3
= v3_dot( ab
, bp
);
333 d4
= v3_dot( ac
, bp
);
335 if( d3
>= 0.0f
&& d4
<= d3
)
337 v3_copy( tri
[1], dest
);
338 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
342 /* Edge region of AB */
343 float vc
= d1
*d4
- d3
*d2
;
344 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
346 float v
= d1
/ (d1
-d3
);
347 v3_muladds( tri
[0], ab
, v
, dest
);
348 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
352 /* Region outside C */
355 v3_sub( p
, tri
[2], cp
);
359 if( d6
>= 0.0f
&& d5
<= d6
)
361 v3_copy( tri
[2], dest
);
362 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
367 float vb
= d5
*d2
- d1
*d6
;
368 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
370 float w
= d2
/ (d2
-d6
);
371 v3_muladds( tri
[0], ac
, w
, dest
);
372 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
377 float va
= d3
*d6
- d5
*d4
;
378 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
380 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
382 v3_sub( tri
[2], tri
[1], bc
);
383 v3_muladds( tri
[1], bc
, w
, dest
);
384 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
388 /* P inside region, Q via barycentric coordinates uvw */
389 float d
= 1.0f
/(va
+vb
+vc
),
393 v3_muladds( tri
[0], ab
, v
, dest
);
394 v3_muladds( dest
, ac
, w
, dest
);
400 * These do not automatically allocate contacts, an appropriately sized
401 * buffer must be supplied. The function returns the size of the manifold
402 * which was generated.
404 * The values set on the contacts are: n, co, p, rba, rbb
407 static void rb_debug_contact( rb_ct
*ct
)
410 v3_muladds( ct
->co
, ct
->n
, 0.2f
, p1
);
411 vg_line_pt3( ct
->co
, 0.1f
, 0xff0000ff );
412 vg_line( ct
->co
, p1
, 0xffffffff );
415 static void rb_box_incident_dir( v3f p
, boxf box
, v3f dir
)
421 * Generates up to two contacts; optimised for the most stable manifold
423 static int rb_capsule_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
425 float h
= rba
->inf
.capsule
.height
,
426 r
= rba
->inf
.capsule
.radius
;
429 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
*0.5f
, p1
);
430 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
*0.5f
, p0
);
432 m4x3_mulv( rbb
->to_local
, p0
, p0
);
433 m4x3_mulv( rbb
->to_local
, p1
, p1
);
438 closest_point_aabb( p0
, rbb
->bbx
, c0
);
439 closest_point_aabb( p1
, rbb
->bbx
, c1
);
442 m4x3_mulv( rbb
->to_world
, c0
, vis0
);
443 vg_line_pt3( vis0
, 0.1f
, 0xffff00ff );
444 m4x3_mulv( rbb
->to_world
, c1
, vis0
);
445 vg_line_pt3( vis0
, 0.1f
, 0xffff00ff );
448 v3_sub( p0
, c0
, d0
);
449 v3_sub( p1
, c1
, d1
);
451 float d02
= v3_length2(d0
),
452 d12
= v3_length2(d1
);
458 rb_ct
*ct
= buf
+count
;
459 float d
= sqrtf(d02
);
461 vg_info( "d: %.4f\n", d
);
463 v3_muls( d0
, -1.0f
/d
, ct
->n
);
465 v3_add( c0
, p0
, ct
->co
);
466 v3_muls( ct
->co
, 0.5f
, ct
->co
);
468 m3x3_mulv( rbb
->to_world
, ct
->n
, ct
->n
);
469 m4x3_mulv( rbb
->to_world
, ct
->co
, ct
->co
);
479 static int rb_sphere_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
483 closest_point_obb( rba
->co
, rbb
, co
);
484 v3_sub( rba
->co
, co
, delta
);
486 float d2
= v3_length2(delta
),
487 r
= rba
->inf
.sphere
.radius
;
496 v3_sub( rba
->co
, rbb
->co
, delta
);
499 * some extra testing is required to find the best axis to push the
500 * object back outside the box. Since there isnt a clear seperating
501 * vector already, especially on really high aspect boxes.
503 float lx
= v3_dot( rbb
->right
, delta
),
504 ly
= v3_dot( rbb
->up
, delta
),
505 lz
= v3_dot( rbb
->forward
, delta
),
506 px
= rbb
->bbx
[1][0] - fabsf(lx
),
507 py
= rbb
->bbx
[1][1] - fabsf(ly
),
508 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
510 if( px
< py
&& px
< pz
)
511 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
513 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
515 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
517 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
523 v3_muls( delta
, 1.0f
/d
, ct
->n
);
525 v3_add( co
, rba
->co
, ct
->co
);
526 v3_muls( ct
->co
, 0.5f
, ct
->co
);
537 static int rb_sphere_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
540 v3_sub( rba
->co
, rbb
->co
, delta
);
542 float d2
= v3_length2(delta
),
543 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
550 v3_muls( delta
, -1.0f
/d
, ct
->n
);
553 v3_muladds( rba
->co
, ct
->n
, rba
->inf
.sphere
.radius
, p0
);
554 v3_muladds( rbb
->co
, ct
->n
,-rbb
->inf
.sphere
.radius
, p1
);
555 v3_add( p0
, p1
, ct
->co
);
556 v3_muls( ct
->co
, 0.5f
, ct
->co
);
566 static int rb_box_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
568 return rb_sphere_vs_box( rbb
, rba
, buf
);
571 static int rb_box_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
573 /* TODO: Generating a stable quad manifold, lots of clipping */
578 * This function does not accept triangle as a dynamic object, it is assumed
579 * to always be static.
581 * The triangle is also assumed to be one sided for better detection
583 static int rb_sphere_vs_triangle( rigidbody
*rba
, v3f tri
[3], rb_ct
*buf
)
587 closest_on_triangle( rba
->co
, tri
, co
);
588 v3_sub( rba
->co
, co
, delta
);
590 float d2
= v3_length2( delta
),
591 r
= rba
->inf
.sphere
.radius
;
596 v3_sub( tri
[1], tri
[0], ab
);
597 v3_sub( tri
[2], tri
[0], ac
);
598 v3_cross( ac
, ab
, tn
);
600 if( v3_dot( delta
, tn
) > 0.0f
)
601 v3_muls( delta
, -1.0f
, delta
);
606 v3_muls( delta
, 1.0f
/d
, ct
->n
);
607 v3_copy( co
, ct
->co
);
610 ct
->rbb
= &rb_static_null
;
623 static int sphere_vs_triangle( v3f c
, float r
, v3f tri
[3],
624 v3f co
, v3f norm
, float *p
)
627 closest_on_triangle( c
, tri
, co
);
629 v3_sub( c
, co
, delta
);
632 float d
= v3_length2( delta
);
636 v3_sub( tri
[1], tri
[0], ab
);
637 v3_sub( tri
[2], tri
[0], ac
);
638 v3_cross( ac
, ab
, tn
);
640 if( v3_dot( delta
, tn
) > 0.0f
)
641 v3_muls( delta
, -1.0f
, delta
);
643 vg_line_pt3( co
, 0.05f
, 0xff00ff00 );
646 v3_muls( delta
, 1.0f
/d
, norm
);
657 static void rb_solver_reset(void)
659 rb_contact_count
= 0;
662 static rb_ct
*rb_global_ct(void)
664 return rb_contact_buffer
+ rb_contact_count
;
667 static struct contact
*rb_start_contact(void)
669 if( rb_contact_count
== vg_list_size(rb_contact_buffer
) )
671 vg_error( "rigidbody: too many contacts generated (%u)\n",
676 return &rb_contact_buffer
[ rb_contact_count
];
679 static void rb_commit_contact( struct contact
*ct
, float p
)
681 ct
->bias
= -0.2f
*k_rb_rate
*vg_minf(0.0f
,-p
+0.04f
);
682 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
684 ct
->norm_impulse
= 0.0f
;
685 ct
->tangent_impulse
[0] = 0.0f
;
686 ct
->tangent_impulse
[1] = 0.0f
;
691 static void rb_build_manifold_terrain_sphere( rigidbody
*rb
)
695 int len
= bh_select( &world
.geo
.bhtris
, rb
->bbx_world
, geo
, 256 );
697 for( int i
=0; i
<len
; i
++ )
699 u32
*ptri
= &world
.geo
.indices
[ geo
[i
]*3 ];
701 for( int j
=0; j
<3; j
++ )
702 v3_copy( world
.geo
.verts
[ptri
[j
]].co
, tri
[j
] );
704 vg_line(tri
[0],tri
[1],0xff00ff00 );
705 vg_line(tri
[1],tri
[2],0xff00ff00 );
706 vg_line(tri
[2],tri
[0],0xff00ff00 );
711 for( int j
=0; j
<2; j
++ )
713 if( sphere_vs_triangle( rb
->co
, rb
->inf
.sphere
.radius
, tri
,co
,norm
,&p
))
715 struct contact
*ct
= rb_start_contact();
721 v3_muladds( rb
->co
, norm
, p
, p1
);
722 vg_line( rb
->co
, p1
, 0xffffffff );
725 v3_copy( co
, ct
->co
);
726 v3_copy( norm
, ct
->n
);
727 rb_commit_contact( ct
, p
);
735 static void rb_build_manifold_terrain( rigidbody
*rb
)
739 float *p000
= pts
[0], *p001
= pts
[1], *p010
= pts
[2], *p011
= pts
[3],
740 *p100
= pts
[4], *p101
= pts
[5], *p110
= pts
[6], *p111
= pts
[7];
742 p000
[0]=box
[0][0];p000
[1]=box
[0][1];p000
[2]=box
[0][2];
743 p001
[0]=box
[0][0];p001
[1]=box
[0][1];p001
[2]=box
[1][2];
744 p010
[0]=box
[0][0];p010
[1]=box
[1][1];p010
[2]=box
[0][2];
745 p011
[0]=box
[0][0];p011
[1]=box
[1][1];p011
[2]=box
[1][2];
747 p100
[0]=box
[1][0];p100
[1]=box
[0][1];p100
[2]=box
[0][2];
748 p101
[0]=box
[1][0];p101
[1]=box
[0][1];p101
[2]=box
[1][2];
749 p110
[0]=box
[1][0];p110
[1]=box
[1][1];p110
[2]=box
[0][2];
750 p111
[0]=box
[1][0];p111
[1]=box
[1][1];p111
[2]=box
[1][2];
752 m4x3_mulv( rb
->to_world
, p000
, p000
);
753 m4x3_mulv( rb
->to_world
, p001
, p001
);
754 m4x3_mulv( rb
->to_world
, p010
, p010
);
755 m4x3_mulv( rb
->to_world
, p011
, p011
);
756 m4x3_mulv( rb
->to_world
, p100
, p100
);
757 m4x3_mulv( rb
->to_world
, p101
, p101
);
758 m4x3_mulv( rb
->to_world
, p110
, p110
);
759 m4x3_mulv( rb
->to_world
, p111
, p111
);
763 for( int i
=0; i
<8; i
++ )
765 float *point
= pts
[i
];
766 struct contact
*ct
= rb_start_contact();
774 v3_copy( point
, surface
);
779 if( !ray_world( surface
, (v3f
){0.0f
,-1.0f
,0.0f
}, &hit
))
782 v3_copy( hit
.pos
, surface
);
784 float p
= vg_minf( surface
[1] - point
[1], 1.0f
);
788 v3_copy( hit
.normal
, ct
->n
);
789 v3_add( point
, surface
, ct
->co
);
790 v3_muls( ct
->co
, 0.5f
, ct
->co
);
792 rb_commit_contact( ct
, p
);
801 * Initializing things like tangent vectors
804 static void rb_presolve_contacts( rb_ct
*buffer
, int len
)
806 for( int i
=0; i
<len
; i
++ )
808 rb_ct
*ct
= &buffer
[i
];
809 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf(0.0f
,-ct
->p
+0.04f
);
810 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
812 ct
->norm_impulse
= 0.0f
;
813 ct
->tangent_impulse
[0] = 0.0f
;
814 ct
->tangent_impulse
[1] = 0.0f
;
815 ct
->mass_total
= 1.0f
/(ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
);
817 rb_debug_contact( ct
);
822 * Creates relative contact velocity vector, and offsets between each body
824 static void rb_rcv( rb_ct
*ct
, v3f rv
, v3f da
, v3f db
)
826 rigidbody
*rba
= ct
->rba
,
829 v3_sub( rba
->co
, ct
->co
, da
);
830 v3_sub( rbb
->co
, ct
->co
, db
);
833 v3_cross( rba
->w
, da
, rva
);
834 v3_add( rba
->v
, rva
, rva
);
836 v3_cross( rbb
->w
, db
, rvb
);
837 v3_add( rbb
->v
, rvb
, rvb
);
838 v3_add( rva
, rvb
, rv
);
842 * Apply regular and angular velocity impulses to objects involved in contact
844 static void rb_standard_impulse( rb_ct
*ct
, v3f da
, v3f db
, v3f impulse
)
846 rigidbody
*rba
= ct
->rba
,
850 v3_muladds( rba
->v
, impulse
, ct
->mass_total
* rba
->inv_mass
, rba
->v
);
851 v3_muladds( rbb
->v
, impulse
, ct
->mass_total
* rbb
->inv_mass
, rbb
->v
);
853 /* Angular velocity */
855 v3_cross( da
, impulse
, wa
);
856 v3_cross( db
, impulse
, wb
);
857 v3_muladds( rba
->w
, wa
, ct
->mass_total
* rba
->inv_mass
, rba
->w
);
858 v3_muladds( rbb
->w
, wb
, ct
->mass_total
* rbb
->inv_mass
, rbb
->w
);
862 * One iteration to solve the contact constraint
864 static void rb_solve_contacts( rb_ct
*buf
, int len
)
866 float k_friction
= 0.1f
;
868 /* Friction Impulse */
869 for( int i
=0; i
<len
; i
++ )
871 struct contact
*ct
= &buf
[i
];
872 rigidbody
*rb
= ct
->rba
;
875 rb_rcv( ct
, rv
, da
, db
);
877 for( int j
=0; j
<2; j
++ )
879 float f
= k_friction
* ct
->norm_impulse
,
880 vt
= -v3_dot( rv
, ct
->t
[j
] );
882 float temp
= ct
->tangent_impulse
[j
];
883 ct
->tangent_impulse
[j
] = vg_clampf( temp
+vt
, -f
, f
);
884 vt
= ct
->tangent_impulse
[j
] - temp
;
887 v3_muls( ct
->t
[j
], vt
, impulse
);
888 rb_standard_impulse( ct
, da
, db
, impulse
);
893 for( int i
=0; i
<len
; i
++ )
895 struct contact
*ct
= &buf
[i
];
896 rigidbody
*rba
= ct
->rba
,
900 rb_rcv( ct
, rv
, da
, db
);
902 float vn
= -v3_dot( rv
, ct
->n
);
905 float temp
= ct
->norm_impulse
;
906 ct
->norm_impulse
= vg_maxf( temp
+ vn
, 0.0f
);
907 vn
= ct
->norm_impulse
- temp
;
910 v3_muls( ct
->n
, vn
, impulse
);
911 rb_standard_impulse( ct
, da
, db
, impulse
);
916 * The following ventures into not really very sophisticated at all maths
919 struct rb_angle_limit
921 rigidbody
*rba
, *rbb
;
926 static int rb_angle_limit_force( rigidbody
*rba
, v3f va
,
927 rigidbody
*rbb
, v3f vb
,
931 m3x3_mulv( rba
->to_world
, va
, wva
);
932 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
934 float dt
= v3_dot(wva
,wvb
)*0.999f
,
939 float correction
= max
-ang
;
942 v3_cross( wva
, wvb
, axis
);
945 q_axis_angle( rotation
, axis
, -correction
*0.25f
);
946 q_mul( rotation
, rba
->q
, rba
->q
);
948 q_axis_angle( rotation
, axis
, correction
*0.25f
);
949 q_mul( rotation
, rbb
->q
, rbb
->q
);
957 static void rb_constraint_angle_limit( struct rb_angle_limit
*limit
)
963 static void rb_constraint_angle( rigidbody
*rba
, v3f va
,
964 rigidbody
*rbb
, v3f vb
,
965 float max
, float spring
)
968 m3x3_mulv( rba
->to_world
, va
, wva
);
969 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
971 float dt
= v3_dot(wva
,wvb
)*0.999f
,
975 v3_cross( wva
, wvb
, axis
);
976 v3_muladds( rba
->w
, axis
, ang
*spring
*0.5f
, rba
->w
);
977 v3_muladds( rbb
->w
, axis
, -ang
*spring
*0.5f
, rbb
->w
);
981 /* TODO: convert max into the dot product value so we dont have to always
982 * evaluate acosf, only if its greater than the angle specified */
986 float correction
= max
-ang
;
989 q_axis_angle( rotation
, axis
, -correction
*0.125f
);
990 q_mul( rotation
, rba
->q
, rba
->q
);
992 q_axis_angle( rotation
, axis
, correction
*0.125f
);
993 q_mul( rotation
, rbb
->q
, rbb
->q
);
997 static void rb_relative_velocity( rigidbody
*ra
, v3f lca
,
998 rigidbody
*rb
, v3f lcb
, v3f rcv
)
1001 m3x3_mulv( ra
->to_world
, lca
, wca
);
1002 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1004 v3_sub( ra
->v
, rb
->v
, rcv
);
1007 v3_cross( ra
->w
, wca
, rcv_Ra
);
1008 v3_cross( rb
->w
, wcb
, rcv_Rb
);
1009 v3_add( rcv_Ra
, rcv
, rcv
);
1010 v3_sub( rcv
, rcv_Rb
, rcv
);
1013 static void rb_constraint_position( rigidbody
*ra
, v3f lca
,
1014 rigidbody
*rb
, v3f lcb
)
1016 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
1018 m3x3_mulv( ra
->to_world
, lca
, wca
);
1019 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1022 v3_add( wcb
, rb
->co
, delta
);
1023 v3_sub( delta
, wca
, delta
);
1024 v3_sub( delta
, ra
->co
, delta
);
1026 v3_muladds( ra
->co
, delta
, 0.5f
, ra
->co
);
1027 v3_muladds( rb
->co
, delta
, -0.5f
, rb
->co
);
1030 v3_sub( ra
->v
, rb
->v
, rcv
);
1033 v3_cross( ra
->w
, wca
, rcv_Ra
);
1034 v3_cross( rb
->w
, wcb
, rcv_Rb
);
1035 v3_add( rcv_Ra
, rcv
, rcv
);
1036 v3_sub( rcv
, rcv_Rb
, rcv
);
1038 float nm
= 0.5f
/(rb
->inv_mass
+ ra
->inv_mass
);
1040 float mass_a
= 1.0f
/ra
->inv_mass
,
1041 mass_b
= 1.0f
/rb
->inv_mass
,
1042 total_mass
= mass_a
+mass_b
;
1045 v3_muls( rcv
, 1.0f
, impulse
);
1046 v3_muladds( rb
->v
, impulse
, mass_b
/total_mass
, rb
->v
);
1047 v3_cross( wcb
, impulse
, impulse
);
1048 v3_add( impulse
, rb
->w
, rb
->w
);
1050 v3_muls( rcv
, -1.0f
, impulse
);
1051 v3_muladds( ra
->v
, impulse
, mass_a
/total_mass
, ra
->v
);
1052 v3_cross( wca
, impulse
, impulse
);
1053 v3_add( impulse
, ra
->w
, ra
->w
);
1057 * this could be used for spring joints
1058 * its not good for position constraint
1061 v3_muls( delta
, 0.5f
*spring
, impulse
);
1063 v3_add( impulse
, ra
->v
, ra
->v
);
1064 v3_cross( wca
, impulse
, impulse
);
1065 v3_add( impulse
, ra
->w
, ra
->w
);
1067 v3_muls( delta
, -0.5f
*spring
, impulse
);
1069 v3_add( impulse
, rb
->v
, rb
->v
);
1070 v3_cross( wcb
, impulse
, impulse
);
1071 v3_add( impulse
, rb
->w
, rb
->w
);
1075 static void debug_sphere( m4x3f m
, float radius
, u32 colour
)
1077 v3f ly
= { 0.0f
, 0.0f
, radius
},
1078 lx
= { 0.0f
, radius
, 0.0f
},
1079 lz
= { 0.0f
, 0.0f
, radius
};
1081 for( int i
=0; i
<16; i
++ )
1083 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
1087 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
1088 px
= { s
*radius
, c
*radius
, 0.0f
},
1089 pz
= { 0.0f
, s
*radius
, c
*radius
};
1091 v3f p0
, p1
, p2
, p3
, p4
, p5
;
1092 m4x3_mulv( m
, py
, p0
);
1093 m4x3_mulv( m
, ly
, p1
);
1094 m4x3_mulv( m
, px
, p2
);
1095 m4x3_mulv( m
, lx
, p3
);
1096 m4x3_mulv( m
, pz
, p4
);
1097 m4x3_mulv( m
, lz
, p5
);
1099 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
1100 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
1101 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
1109 static void rb_debug( rigidbody
*rb
, u32 colour
)
1111 if( rb
->type
== k_rb_shape_box
)
1114 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
1116 else if( rb
->type
== k_rb_shape_sphere
)
1118 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
1120 else if( rb
->type
== k_rb_shape_capsule
)
1123 float h
= rb
->inf
.capsule
.height
,
1124 r
= rb
->inf
.capsule
.radius
;
1126 m3x3_copy( rb
->to_world
, m0
);
1127 m3x3_copy( rb
->to_world
, m1
);
1129 v3_muladds( rb
->co
, rb
->up
, -h
*0.5f
, m0
[3] );
1130 v3_muladds( rb
->co
, rb
->up
, h
*0.5f
, m1
[3] );
1132 debug_sphere( m0
, r
, colour
);
1133 debug_sphere( m1
, r
, colour
);
1134 vg_line( m0
[3], m1
[3], colour
);
1139 * out penetration distance, normal
1141 static int rb_point_in_body( rigidbody
*rb
, v3f pos
, float *pen
, v3f normal
)
1144 m4x3_mulv( rb
->to_local
, pos
, local
);
1146 if( local
[0] > rb
->bbx
[0][0] && local
[0] < rb
->bbx
[1][0] &&
1147 local
[1] > rb
->bbx
[0][1] && local
[1] < rb
->bbx
[1][1] &&
1148 local
[2] > rb
->bbx
[0][2] && local
[2] < rb
->bbx
[1][2] )
1150 v3f area
, com
, comrel
;
1151 v3_add( rb
->bbx
[0], rb
->bbx
[1], com
);
1152 v3_muls( com
, 0.5f
, com
);
1154 v3_sub( rb
->bbx
[1], rb
->bbx
[0], area
);
1155 v3_sub( local
, com
, comrel
);
1156 v3_div( comrel
, area
, comrel
);
1159 float max_mag
= fabsf(comrel
[0]);
1161 if( fabsf(comrel
[1]) > max_mag
)
1164 max_mag
= fabsf(comrel
[1]);
1166 if( fabsf(comrel
[2]) > max_mag
)
1169 max_mag
= fabsf(comrel
[2]);
1173 normal
[axis
] = vg_signf(comrel
[axis
]);
1175 if( normal
[axis
] < 0.0f
)
1176 *pen
= local
[axis
] - rb
->bbx
[0][axis
];
1178 *pen
= rb
->bbx
[1][axis
] - local
[axis
];
1180 m3x3_mulv( rb
->to_world
, normal
, normal
);
1188 static void rb_build_manifold_rb_static( rigidbody
*ra
, rigidbody
*rb_static
)
1193 v3_copy( ra
->bbx
[0], a
);
1194 v3_copy( ra
->bbx
[1], b
);
1196 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], a
[1], a
[2] }, verts
[0] );
1197 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], b
[1], a
[2] }, verts
[1] );
1198 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], b
[1], a
[2] }, verts
[2] );
1199 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], a
[1], a
[2] }, verts
[3] );
1200 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], a
[1], b
[2] }, verts
[4] );
1201 m4x3_mulv( ra
->to_world
, (v3f
){ a
[0], b
[1], b
[2] }, verts
[5] );
1202 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], b
[1], b
[2] }, verts
[6] );
1203 m4x3_mulv( ra
->to_world
, (v3f
){ b
[0], a
[1], b
[2] }, verts
[7] );
1205 vg_line_boxf_transformed( rb_static
->to_world
, rb_static
->bbx
, 0xff0000ff );
1209 for( int i
=0; i
<8; i
++ )
1211 if( ra
->manifold_count
== vg_list_size(ra
->manifold
) )
1214 struct contact
*ct
= &ra
->manifold
[ ra
->manifold_count
];
1219 if( rb_point_in_body( rb_static
, verts
[i
], &p
, normal
))
1221 v3_copy( normal
, ct
->n
);
1222 v3_muladds( verts
[i
], ct
->n
, p
*0.5f
, ct
->co
);
1223 v3_sub( ct
->co
, ra
->co
, ct
->delta
);
1225 vg_line_pt3( ct
->co
, 0.0125f
, 0xffff00ff );
1227 ct
->bias
= -0.2f
* (1.0f
/k_rb_delta
) * vg_minf( 0.0f
, -p
+0.04f
);
1228 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1230 ct
->norm_impulse
= 0.0f
;
1231 ct
->tangent_impulse
[0] = 0.0f
;
1232 ct
->tangent_impulse
[1] = 0.0f
;
1234 ra
->manifold_count
++;
1248 static void debug_capsule( m4x3f m
, float height
, float radius
, u32 colour
)
1250 v3f last
= { 0.0f
, 0.0f
, radius
};
1252 m3x3_copy( m
, lower
);
1253 m3x3_copy( m
, upper
);
1254 m4x3_mulv( m
, (v3f
){0.0f
,-height
*0.5f
+radius
,0.0f
}, lower
[3] );
1255 m4x3_mulv( m
, (v3f
){0.0f
, height
*0.5f
-radius
,0.0f
}, upper
[3] );
1257 for( int i
=0; i
<16; i
++ )
1259 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
1263 v3f p
= { s
*radius
, 0.0f
, c
*radius
};
1266 m4x3_mulv( lower
, p
, p0
);
1267 m4x3_mulv( lower
, last
, p1
);
1268 vg_line( p0
, p1
, colour
);
1270 m4x3_mulv( upper
, p
, p0
);
1271 m4x3_mulv( upper
, last
, p1
);
1272 vg_line( p0
, p1
, colour
);
1277 for( int i
=0; i
<4; i
++ )
1279 float t
= ((float)(i
) * (1.0f
/4.0f
)) * VG_PIf
* 2.0f
,
1283 v3f p
= { s
*radius
, 0.0f
, c
*radius
};
1286 m4x3_mulv( lower
, p
, p0
);
1287 m4x3_mulv( upper
, p
, p1
);
1288 vg_line( p0
, p1
, colour
);
1290 m4x3_mulv( lower
, (v3f
){0.0f
,-radius
,0.0f
}, p0
);
1291 m4x3_mulv( upper
, (v3f
){0.0f
, radius
,0.0f
}, p1
);
1292 vg_line( p0
, p1
, colour
);
1297 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
1301 static void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
1303 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1304 box_concat( bound
, rb
->bbx_world
);
1307 static float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
1309 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1310 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
1313 static void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
1315 rigidbody temp
, *rba
, *rbb
;
1316 rba
= &((rigidbody
*)user
)[ ia
];
1317 rbb
= &((rigidbody
*)user
)[ ib
];
1324 static void rb_bh_debug( void *user
, u32 item_index
)
1326 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1327 rb_debug( rb
, 0xff00ffff );
1330 static bh_system bh_system_rigidbodies
=
1332 .expand_bound
= rb_bh_expand_bound
,
1333 .item_centroid
= rb_bh_centroid
,
1334 .item_swap
= rb_bh_swap
,
1335 .item_debug
= rb_bh_debug
,
1339 #endif /* RIGIDBODY_H */