boxf bbx, bbx_world;
float inv_mass;
- v3f delta; /* where is the origin of this in relation to a parent body
- TODO: Move this somewhere other than rigidbody struct
- it is only used by character.h's ragdoll
- */
+ /* inertia model and inverse world tensor */
+ v3f I;
+ m3x3f iI, iIw;
+
m4x3f to_world, to_local;
};
rigidbody *rba, *rbb;
v3f co, n;
v3f t[2];
- float mass_total, p, bias, norm_impulse, tangent_impulse[2];
+ float p, bias, norm_impulse, tangent_impulse[2],
+ normal_mass, tangent_mass[2];
+
u32 element_id;
}
rb_contact_buffer[256];
m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f, 0.0f}, rb->up );
m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,-1.0f}, rb->forward );
+ m3x3_mul( rb->iI, rb->to_local, rb->iIw );
+ m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
+
rb_update_bounds( rb );
}
v3f dims;
v3_sub( rb->bbx[1], rb->bbx[0], dims );
volume = dims[0]*dims[1]*dims[2];
+
+ if( !rb->is_world )
+ vg_info( "Box volume: %f\n", volume );
}
else if( rb->type == k_rb_shape_sphere )
{
volume = sphere_volume( rb->inf.sphere.radius );
v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
v3_fill( rb->bbx[1], rb->inf.sphere.radius );
+
+ vg_info( "Sphere volume: %f\n", volume );
}
else if( rb->type == k_rb_shape_capsule )
{
if( rb->is_world )
{
rb->inv_mass = 0.0f;
+ v3_zero( rb->I );
+ m3x3_zero(rb->iI);
}
else
{
- rb->inv_mass = 1.0f/(8.0f*volume); /* TODO: Things get weird when mass
- passes a certain point??? */
+ float mass = 2.0f*volume;
+ rb->inv_mass = 1.0f/mass;
+
+ v3f extent;
+ v3_sub( rb->bbx[1], rb->bbx[0], extent );
+ v3_muls( extent, 0.5f, extent );
+
+ /* local intertia tensor */
+ float ex2 = 4.0f*extent[0]*extent[0],
+ ey2 = 4.0f*extent[1]*extent[1],
+ ez2 = 4.0f*extent[2]*extent[2];
+
+ rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
+ rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
+ rb->I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
+
+ m3x3_identity( rb->iI );
+ rb->iI[0][0] = rb->I[0];
+ rb->iI[1][1] = rb->I[1];
+ rb->iI[2][2] = rb->I[2];
+ m3x3_inv( rb->iI, rb->iI );
}
v3_zero( rb->v );
static void rb_iter( rigidbody *rb )
{
- v3f gravity = { 0.0f, -9.6f, 0.0f };
+ v3f gravity = { 0.0f, -9.8f, 0.0f };
v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
/* intergrate velocity */
static void rb_debug_contact( rb_ct *ct )
{
v3f p1;
- v3_muladds( ct->co, ct->n, 0.2f, p1 );
- vg_line_pt3( ct->co, 0.1f, 0xff0000ff );
+ v3_muladds( ct->co, ct->n, 0.1f, p1 );
+ vg_line_pt3( ct->co, 0.025f, 0xff0000ff );
vg_line( ct->co, p1, 0xffffffff );
}
return r-s;
}
+static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] )
+{
+ float
+
+ r = extent[0] * fabsf(axis[0]) +
+ extent[1] * fabsf(axis[1]) +
+ extent[2] * fabsf(axis[2]),
+
+ p0 = v3_dot( axis, tri[0] ),
+ p1 = v3_dot( axis, tri[1] ),
+ p2 = v3_dot( axis, tri[2] ),
+
+ e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2)));
+
+ if( e > r ) return 0;
+ else return 1;
+}
+
+static int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] )
+{
+ v3f tri[3];
+
+ v3f extent, c;
+ v3_sub( rba->bbx[1], rba->bbx[0], extent );
+ v3_muls( extent, 0.5f, extent );
+ v3_add( rba->bbx[0], extent, c );
+
+ for( int i=0; i<3; i++ )
+ {
+ m4x3_mulv( rba->to_local, tri_src[i], tri[i] );
+ v3_sub( tri[i], c, tri[i] );
+ }
+
+ /* u0, u1, u2 */
+ if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0;
+ if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0;
+ if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0;
+
+ v3f v0,v1,v2,n, e0,e1,e2;
+ v3_sub( tri[1], tri[0], v0 );
+ v3_sub( tri[2], tri[0], v1 );
+ v3_sub( tri[2], tri[1], v2 );
+ v3_normalize( v0 );
+ v3_normalize( v1 );
+ v3_normalize( v2 );
+ v3_cross( v0, v1, n );
+ v3_cross( v0, n, e0 );
+ v3_cross( n, v1, e1 );
+ v3_cross( v2, n, e2 );
+
+ /* normal */
+ if(!rb_box_triangle_interval( extent, n, tri )) return 0;
+
+ v3f axis[9];
+ v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
+ v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
+ v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
+ v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
+ v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
+ v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
+ v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
+ v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
+ v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
+
+ for( int i=0; i<9; i++ )
+ if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
+
+ return 1;
+}
+
static int rb_box_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
{
-#if 1
scene *sc = rbb->inf.scene.pscene;
u32 geo[128];
v3f tri[3];
int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
-
- vg_info( "%d\n", len );
int count = 0;
for( int j=0; j<3; j++ )
v3_copy( sc->verts[ptri[j]].co, tri[j] );
- vg_line(tri[0],tri[1],0xff00ff00 );
- vg_line(tri[1],tri[2],0xff00ff00 );
- vg_line(tri[2],tri[0],0xff00ff00 );
+ 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 );
- //count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
- //
-
- /* TODO: SAT test first */
-
- /*
- * each pair of faces on the box vs triangle normal
- */
+ continue;
+ }
- v3f v0, v1;
- v4f tn;
+ 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 );
- v3_sub( tri[1],tri[0], v0 );
- v3_sub( tri[2],tri[0], v1 );
- v3_cross( v0, v1, tn );
- v3_normalize( tn );
+ /* find best feature */
+ float best = v3_dot( rba->right, n );
+ int axis = 0;
- tn[3] = v3_dot( tn, tri[0] );
-
- v4f pa, pb, pc, pd, pe, pf;
- v3_copy( rba->right, pa );
- v3_muls( rba->right, -1.0f, pb );
- v3_copy( rba->up, pc );
- v3_muls( rba->up, -1.0f, pd );
- v3_copy( rba->forward, pf );
- v3_muls( rba->forward, -1.0f, pe );
-
- float dx = v3_dot( rba->co, rba->right ),
- dy = v3_dot( rba->co, rba->up ),
- dz = -v3_dot( rba->co, rba->forward );
-
- pa[3] = dx + rba->bbx[1][0];
- pb[3] = -dx - rba->bbx[0][0];
- pc[3] = dy + rba->bbx[1][1];
- pd[3] = -dy - rba->bbx[0][1];
- pe[3] = dz + rba->bbx[1][2];
- pf[3] = -dz - rba->bbx[0][2];
-
- float *pairs[][2] = { {pc, pa}, {pc,pe}, {pc,pb}, {pc,pf},
- {pd, pa}, {pd,pe}, {pd,pb}, {pd,pf},
- {pf, pa}, {pa,pe}, {pe,pb}, {pb,pf}};
- for( int j=0; j<vg_list_size(pairs); j++ )
+ float cy = v3_dot( rba->up, n );
+ if( fabsf(cy) > fabsf(best) )
{
- v3f p;
-
- if( rb_intersect_planes_1( pairs[j][0], pairs[j][1], tn, p ))
- {
- v3f p_tri, p_box;
- closest_point_obb( p, rba, p_box );
- closest_on_triangle_1( p, tri, p_tri );
-
- //vg_line_pt3( p, 0.1f, 0xffeeaaff );
-
- if( v3_dist( p_tri, p ) < 0.001f && v3_dist( p_box, p ) < 0.001f )
- {
- if( count == 12 )
- {
- vg_warn( "Exceeding box_vs_scene capacity."
- "Geometry too dense!\n" );
- return count;
- }
-
- rb_ct *ct = buf+count;
-
- v3_copy( tn, ct->n );
- v3_copy( p_box, ct->co );
-
- ct->p = rb_box_plane_interval( rba, tn );
- ct->rba = rba;
- ct->rbb = rbb;
- count ++;
- }
- }
+ best = cy;
+ axis = 1;
+ }
+
+ /* TODO: THIS IS WRONG DIRECTION */
+ float cz = -v3_dot( rba->forward, n );
+ if( fabsf(cz) > fabsf(best) )
+ {
+ best = cz;
+ axis = 2;
}
- }
-#else
-
- v3f pts[8];
- float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
- *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
- p000[0]=rba->bbx[0][0];p000[1]=rba->bbx[0][1];p000[2]=rba->bbx[0][2];
- p001[0]=rba->bbx[0][0];p001[1]=rba->bbx[0][1];p001[2]=rba->bbx[1][2];
- p010[0]=rba->bbx[0][0];p010[1]=rba->bbx[1][1];p010[2]=rba->bbx[0][2];
- p011[0]=rba->bbx[0][0];p011[1]=rba->bbx[1][1];p011[2]=rba->bbx[1][2];
- p100[0]=rba->bbx[1][0];p100[1]=rba->bbx[0][1];p100[2]=rba->bbx[0][2];
- p101[0]=rba->bbx[1][0];p101[1]=rba->bbx[0][1];p101[2]=rba->bbx[1][2];
- p110[0]=rba->bbx[1][0];p110[1]=rba->bbx[1][1];p110[2]=rba->bbx[0][2];
- p111[0]=rba->bbx[1][0];p111[1]=rba->bbx[1][1];p111[2]=rba->bbx[1][2];
+ v3f manifold[4];
- int count = 0;
- for( int i=0; i<8; i++ )
- {
- m4x3_mulv( rba->to_world, pts[i], pts[i] );
+ 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_pt3( pts[i], 0.1f, 0xffff00ff );
+ 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 );
- if( pts[i][1] < 0.0f )
+ for( int j=0; j<4; j++ )
{
rb_ct *ct = buf+count;
- v3_copy( (v3f){0.0f,1.0f,0.0f}, ct->n );
- v3_copy( pts[i], ct->co );
-
- ct->p = 0.0f-pts[i][1];
+ 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->rba = rba;
ct->rbb = rbb;
count ++;
+
+ if( count >= 12 )
+ return count;
}
}
-
-#endif
-
return count;
}
for( int i=0; i<len; i++ )
{
rb_ct *ct = &buffer[i];
- ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f);
+ ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.01f);
rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
ct->norm_impulse = 0.0f;
ct->tangent_impulse[0] = 0.0f;
ct->tangent_impulse[1] = 0.0f;
- ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass);
+
+ v3f ra, rb, raCn, rbCn, raCt, rbCt;
+ v3_sub( ct->co, ct->rba->co, ra );
+ v3_sub( ct->co, ct->rbb->co, rb );
+ v3_cross( ra, ct->n, raCn );
+ v3_cross( rb, ct->n, rbCn );
+
+ /* orient inverse inertia tensors */
+ v3f raCnI, rbCnI;
+ m3x3_mulv( ct->rba->iIw, raCn, raCnI );
+ m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI );
+
+ ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass;
+ ct->normal_mass += v3_dot( raCn, raCnI );
+ ct->normal_mass += v3_dot( rbCn, rbCnI );
+ ct->normal_mass = 1.0f/ct->normal_mass;
+
+ for( int j=0; j<2; j++ )
+ {
+ v3f raCtI, rbCtI;
+ v3_cross( ct->t[j], ra, raCt );
+ v3_cross( ct->t[j], rb, rbCt );
+ m3x3_mulv( ct->rba->iIw, raCt, raCtI );
+ m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI );
+
+ ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass;
+ ct->tangent_mass[j] += v3_dot( raCt, raCtI );
+ ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
+ ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j];
+ }
rb_debug_contact( ct );
}
rigidbody *rba = ct->rba,
*rbb = ct->rbb;
- v3f ia, ib;
- v3_muls( impulse, ct->mass_total*rba->inv_mass, ia );
- v3_muls( impulse, -ct->mass_total*rbb->inv_mass, ib );
-
- /* response */
- v3_add( rba->v, ia, rba->v );
- v3_add( rbb->v, ib, rbb->v );
+ v3_muladds( rba->v, impulse, rba->inv_mass, rba->v );
+ v3_muladds( rbb->v, impulse, -rbb->inv_mass, rbb->v );
/* Angular velocity */
- v3f wa, wb;
- v3_cross( da, ia, wa );
- v3_cross( db, ib, wb );
+ v3f wa, wb, invim;
+ v3_cross( da, impulse, wa );
+ v3_negate( impulse, invim );
+ v3_cross( db, invim, wb );
+
+ m3x3_mulv( ct->rba->iIw, wa, wa );
+ m3x3_mulv( ct->rbb->iIw, wb, wb );
v3_add( rba->w, wa, rba->w );
v3_add( rbb->w, wb, rbb->w );
}
*/
static void rb_solve_contacts( rb_ct *buf, int len )
{
- float k_friction = 0.5f;
+ float k_friction = 0.2f;
- /* Friction Impulse */
for( int i=0; i<len; i++ )
{
struct contact *ct = &buf[i];
v3f rv, da, db;
rb_rcv( ct, rv, da, db );
+ /* Friction */
for( int j=0; j<2; j++ )
{
- float f = k_friction * ct->norm_impulse,
- vt = -v3_dot( rv, ct->t[j] );
+ float f = k_friction * ct->norm_impulse,
+ vt = v3_dot( rv, ct->t[j] ),
+ lambda = ct->tangent_mass[j] * -vt;
float temp = ct->tangent_impulse[j];
- ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f );
- vt = ct->tangent_impulse[j] - temp;
+ ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
+ lambda = ct->tangent_impulse[j] - temp;
v3f impulse;
- v3_muls( ct->t[j], vt, impulse );
+ v3_muls( ct->t[j], lambda, impulse );
rb_standard_impulse( ct, da, db, impulse );
}
- }
-
- /* Normal Impulse */
- for( int i=0; i<len; i++ )
- {
- struct contact *ct = &buf[i];
- rigidbody *rba = ct->rba,
- *rbb = ct->rbb;
- v3f rv, da, db;
+ /* Normal */
rb_rcv( ct, rv, da, db );
-
- float vn = -v3_dot( rv, ct->n ) + ct->bias;
+ float vn = v3_dot( rv, ct->n ),
+ lambda = ct->normal_mass * (-vn + ct->bias);
float temp = ct->norm_impulse;
- ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
- vn = ct->norm_impulse - temp;
+ ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
+ lambda = ct->norm_impulse - temp;
v3f impulse;
- v3_muls( ct->n, vn, impulse );
+ v3_muls( ct->n, lambda, impulse );
rb_standard_impulse( ct, da, db, impulse );
}
}