-
- return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
-}
-
-/*
- * Generates up to two contacts; optimised for the most stable manifold
- */
-static int rb_capsule_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
-{
- float h = rba->inf.capsule.height,
- r = rba->inf.capsule.radius;
-
- /*
- * Solving this in symetric local space of the cube saves us some time and a
- * couple branches when it comes to the quad stage.
- */
- v3f centroid;
- v3_add( rbb->bbx[0], rbb->bbx[1], centroid );
- v3_muls( centroid, 0.5f, centroid );
-
- boxf bbx;
- v3_sub( rbb->bbx[0], centroid, bbx[0] );
- v3_sub( rbb->bbx[1], centroid, bbx[1] );
-
- v3f pc, p0w, p1w, p0, p1;
- v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
- v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
-
- m4x3_mulv( rbb->to_local, p0w, p0 );
- m4x3_mulv( rbb->to_local, p1w, p1 );
- v3_sub( p0, centroid, p0 );
- v3_sub( p1, centroid, p1 );
- v3_add( p0, p1, pc );
- v3_muls( pc, 0.5f, pc );
-
- /*
- * Finding an appropriate quad to collide lines with
- */
- v3f region;
- v3_div( pc, bbx[1], region );
-
- v3f quad[4];
- if( (fabsf(region[0]) > fabsf(region[1])) &&
- (fabsf(region[0]) > fabsf(region[2])) )
- {
- float px = vg_signf(region[0]) * bbx[1][0];
- v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
- v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
- v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
- v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
- }
- else if( fabsf(region[1]) > fabsf(region[2]) )
- {
- float py = vg_signf(region[1]) * bbx[1][1];
- v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
- v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
- v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
- v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
- }
- else
- {
- float pz = vg_signf(region[2]) * bbx[1][2];
- v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
- v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
- v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
- v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
- }
-
- capsule_manifold manifold;
- rb_capsule_manifold_init( &manifold );
-
- v3f c0, c1;
- closest_point_aabb( p0, bbx, c0 );
- closest_point_aabb( p1, bbx, c1 );
-
- v3f d0, d1, da;
- v3_sub( c0, p0, d0 );
- v3_sub( c1, p1, d1 );
- v3_sub( p1, p0, da );
-
- v3_normalize(d0);
- v3_normalize(d1);
- v3_normalize(da);
-
- if( v3_dot( da, d0 ) <= 0.01f )
- rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
-
- if( v3_dot( da, d1 ) >= -0.01f )
- rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
-
- for( int i=0; i<4; i++ )
- {
- int i0 = i,
- i1 = (i+1)%4;
-
- v3f ca, cb;
- float ta, tb;
- closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
- rb_capsule_manifold( ca, cb, ta, r, &manifold );
- }
-
- /*
- * Create final contacts based on line manifold
- */
- m3x3_mulv( rbb->to_world, manifold.d0, manifold.d0 );
- m3x3_mulv( rbb->to_world, manifold.d1, manifold.d1 );
-
- /*
- * Debugging
- */
-
-#if 0
- for( int i=0; i<4; i++ )
- {
- v3f q0, q1;
- int i0 = i,
- i1 = (i+1)%4;
-
- v3_add( quad[i0], centroid, q0 );
- v3_add( quad[i1], centroid, q1 );
-
- m4x3_mulv( rbb->to_world, q0, q0 );
- m4x3_mulv( rbb->to_world, q1, q1 );
-
- vg_line( q0, q1, 0xffffffff );
- }
-#endif
-
- return rb_capsule_manifold_done( rba, rbb, &manifold, buf );