v3f co, n;
v3f t[2];
float mass_total, p, bias, norm_impulse, tangent_impulse[2];
+ u32 element_id;
}
rb_contact_buffer[256];
static int rb_contact_count = 0;
}
else
{
- rb->inv_mass = 1.0f/(8.0f*volume);
+ rb->inv_mass = 1.0f/(8.0f*volume); /* TODO: Things get weird when mass
+ passes a certain point??? */
}
v3_zero( rb->v );
v3_muladds( dest, ac, w, dest );
}
+/* TODO */
+static void closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
+{
+ v3f ab, ac, ap;
+ float d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
+ {
+ v3_copy( tri[0], dest );
+ return;
+ }
+
+ /* Region outside B */
+ v3f bp;
+ float d3, d4;
+
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
+
+ if( d3 >= 0.0f && d4 <= d3 )
+ {
+ v3_copy( tri[1], dest );
+ return;
+ }
+
+ /* Edge region of AB */
+ float vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+ {
+ float v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ return;
+ }
+
+ /* Region outside C */
+ v3f cp;
+ float d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
+ {
+ v3_copy( tri[2], dest );
+ return;
+ }
+
+ /* Region of AC */
+ float vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ float w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ return;
+ }
+
+ /* Region of BC */
+ float va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ float w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ return;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ float d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, dest );
+}
+
+static int rb_intersect_planes( v4f p0, v4f p1, v4f p2, v3f p )
+{
+ v3f u;
+ v3_cross( p1, p2, u );
+ float d = v3_dot( p0, u );
+
+ if( fabsf(d) < 0.0001f )
+ return 0;
+
+ v3_muls( u, p0[3], p );
+
+ v3f v0, v1;
+ v3_muls( p1, p2[3], v0 );
+ v3_muladds( v0, p2, -p1[3], v0 );
+ v3_cross( p0, v0, v1 );
+ v3_add( v1, p, p );
+ v3_muls( p, 1.0f/d, p );
+
+ return 1;
+}
+
+int rb_intersect_planes_1( v4f a, v4f b, v4f c, v3f p )
+{
+ float const epsilon = 0.001;
+
+ v3f x, bc, ca, ab;
+ float d;
+
+ v3_cross( a, b, x );
+ d = v3_dot( x, c );
+
+ if( d < epsilon && d > -epsilon ) return 0;
+
+ v3_cross(b,c,bc);
+ v3_cross(c,a,ca);
+ v3_cross(a,b,ab);
+
+ v3_muls( bc, -a[3], p );
+ v3_muladds( p, ca, -b[3], p );
+ v3_muladds( p, ab, -c[3], p );
+
+ v3_negate( p, p );
+ v3_divs( p, d, p );
+
+ return 1;
+}
/*
* Contact generators
*
vg_line(tri[0],tri[1],0xff00ff00 );
vg_line(tri[1],tri[2],0xff00ff00 );
vg_line(tri[2],tri[0],0xff00ff00 );
-
+
+ buf[count].element_id = ptri[0];
count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
if( count == 12 )
return count;
}
+static float rb_box_plane_interval( rigidbody *rba, v4f p )
+{
+ /* TODO: Make boxes COG aligned as is every other shape.
+ * or create COG vector.
+ * TODO: Make forward actually point in the right fucking direction. */
+ v3f e,c;
+ v3_sub( rba->bbx[1], rba->bbx[0], e );
+ v3_muls( e, 0.5f, e );
+ v3_add( rba->bbx[0], e, c );
+ m4x3_mulv( rba->to_world, c, c );
+
+ float r =
+ e[0]*fabsf( v3_dot(p, rba->right)) +
+ e[1]*fabsf( v3_dot(p, rba->up)) +
+ e[2]*fabsf(-v3_dot(p, rba->forward)),
+ s = v3_dot( p, c ) - p[3];
+
+ return r-s;
+}
+
+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 i=0; i<len; i++ )
+ {
+ u32 *ptri = &sc->indices[ geo[i]*3 ];
+
+ 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 );
+
+ //count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
+ //
+
+ /* TODO: SAT test first */
+
+ /*
+ * each pair of faces on the box vs triangle normal
+ */
+
+ v3f v0, v1;
+ v4f tn;
+
+ v3_sub( tri[1],tri[0], v0 );
+ v3_sub( tri[2],tri[0], v1 );
+ v3_cross( v0, v1, tn );
+ v3_normalize( tn );
+
+ 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++ )
+ {
+ 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 ++;
+ }
+ }
+ }
+ }
+#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];
+
+ int count = 0;
+ for( int i=0; i<8; i++ )
+ {
+ m4x3_mulv( rba->to_world, pts[i], pts[i] );
+
+ vg_line_pt3( pts[i], 0.1f, 0xffff00ff );
+
+ if( pts[i][1] < 0.0f )
+ {
+ 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];
+ ct->rba = rba;
+ ct->rbb = rbb;
+ count ++;
+ }
+ }
+
+#endif
+
+ return count;
+}
+
static int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
{
vg_error( "Collision type is unimplemented between types %d and %d\n",
return rb_sphere_vs_box( rbb, rba, buf );
}
+static int rb_scene_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_box_vs_scene( rbb, rba, buf );
+}
+
static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
= {
- /* box */ /* Sphere */ /* Capsule */
-/*box */ { RB_MATRIX_ERROR, rb_box_vs_sphere, rb_box_vs_capsule, RB_MATRIX_ERROR },
+ /* box */ /* Sphere */ /* Capsule */ /* Mesh */
+/*box */ { RB_MATRIX_ERROR, rb_box_vs_sphere, rb_box_vs_capsule, rb_box_vs_scene },
/*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, rb_sphere_vs_scene },
/*capsule*/ { rb_capsule_vs_box,rb_capsule_vs_sphere,rb_capsule_vs_capsule,RB_MATRIX_ERROR },
-/*mesh */ { RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR }
+/*mesh */ { rb_scene_vs_box, RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR }
};
static int rb_collide( rigidbody *rba, rigidbody *rbb )
*/
static void rb_solve_contacts( rb_ct *buf, int len )
{
- float k_friction = 0.1f;
+ float k_friction = 0.5f;
/* Friction Impulse */
for( int i=0; i<len; i++ )
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff