- for( int j=0; j<3; j++ )
- v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
-
- if( rb_box_triangle_sat( rba, tri ) )
- {
- vg_line(tri[0],tri[1],0xff50ff00 );
- vg_line(tri[1],tri[2],0xff50ff00 );
- vg_line(tri[2],tri[0],0xff50ff00 );
- }
- else
- {
- vg_line(tri[0],tri[1],0xff0000ff );
- vg_line(tri[1],tri[2],0xff0000ff );
- vg_line(tri[2],tri[0],0xff0000ff );
-
- continue;
- }
-
- v3f v0,v1,n;
- v3_sub( tri[1], tri[0], v0 );
- v3_sub( tri[2], tri[0], v1 );
- v3_cross( v0, v1, n );
- v3_normalize( n );
-
- /* find best feature */
- float best = v3_dot( rba->right, n );
- int axis = 0;
-
- float cy = v3_dot( rba->up, n );
- if( fabsf(cy) > fabsf(best) )
- {
- best = cy;
- axis = 1;
- }
-
- float cz = -v3_dot( rba->forward, n );
- if( fabsf(cz) > fabsf(best) )
- {
- best = cz;
- axis = 2;
- }
-
- v3f manifold[4];
-
- if( axis == 0 )
- {
- float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
- manifold[0][0] = px;
- manifold[0][1] = rba->bbx[0][1];
- manifold[0][2] = rba->bbx[0][2];
- manifold[1][0] = px;
- manifold[1][1] = rba->bbx[1][1];
- manifold[1][2] = rba->bbx[0][2];
- manifold[2][0] = px;
- manifold[2][1] = rba->bbx[1][1];
- manifold[2][2] = rba->bbx[1][2];
- manifold[3][0] = px;
- manifold[3][1] = rba->bbx[0][1];
- manifold[3][2] = rba->bbx[1][2];
- }
- else if( axis == 1 )
- {
- float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
- manifold[0][0] = rba->bbx[0][0];
- manifold[0][1] = py;
- manifold[0][2] = rba->bbx[0][2];
- manifold[1][0] = rba->bbx[1][0];
- manifold[1][1] = py;
- manifold[1][2] = rba->bbx[0][2];
- manifold[2][0] = rba->bbx[1][0];
- manifold[2][1] = py;
- manifold[2][2] = rba->bbx[1][2];
- manifold[3][0] = rba->bbx[0][0];
- manifold[3][1] = py;
- manifold[3][2] = rba->bbx[1][2];
- }
- else
- {
- float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
- manifold[0][0] = rba->bbx[0][0];
- manifold[0][1] = rba->bbx[0][1];
- manifold[0][2] = pz;
- manifold[1][0] = rba->bbx[1][0];
- manifold[1][1] = rba->bbx[0][1];
- manifold[1][2] = pz;
- manifold[2][0] = rba->bbx[1][0];
- manifold[2][1] = rba->bbx[1][1];
- manifold[2][2] = pz;
- manifold[3][0] = rba->bbx[0][0];
- manifold[3][1] = rba->bbx[1][1];
- manifold[3][2] = pz;
- }
-
- for( int j=0; j<4; j++ )
- m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
-
- vg_line( manifold[0], manifold[1], 0xffffffff );
- vg_line( manifold[1], manifold[2], 0xffffffff );
- vg_line( manifold[2], manifold[3], 0xffffffff );
- vg_line( manifold[3], manifold[0], 0xffffffff );
-
- for( int j=0; j<4; j++ )
- {
- rb_ct *ct = buf+count;
-
- v3_copy( manifold[j], ct->co );
- v3_copy( n, ct->n );
-
- float l0 = v3_dot( tri[0], n ),
- l1 = v3_dot( manifold[j], n );
-
- ct->p = (l0-l1)*0.5f;
- if( ct->p < 0.0f )
- continue;
-
- ct->type = k_contact_type_default;
- ct->rba = rba;
- ct->rbb = rbb;
- count ++;
-
- if( count >= 12 )
- return count;
- }
- }
- return count;
-}
-#endif
-
-VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
- v3f tri[3], rb_ct *buf )
-{
- v3f pc, p0w, p1w;
- v3_muladds( mtxA[3], mtxA[1], -c->height*0.5f+c->radius, p0w );
- v3_muladds( mtxA[3], mtxA[1], c->height*0.5f-c->radius, p1w );
-
- capsule_manifold manifold;
- rb_capsule_manifold_init( &manifold );
-
- v3f c0, c1;
- closest_on_triangle_1( p0w, tri, c0 );
- closest_on_triangle_1( p1w, tri, c1 );
-
- v3f d0, d1, da;
- v3_sub( c0, p0w, d0 );
- v3_sub( c1, p1w, d1 );
- v3_sub( p1w, p0w, da );
-
- v3_normalize(d0);
- v3_normalize(d1);
- v3_normalize(da);
-
- if( v3_dot( da, d0 ) <= 0.01f )
- rb_capsule_manifold( p0w, c0, 0.0f, c->radius, &manifold );