static void rb_solver_reset(void);
static void rb_build_manifold_terrain( rigidbody *rb );
static void rb_build_manifold_terrain_sphere( rigidbody *rb );
-static void rb_solve_contacts(void);
+static void rb_solve_contacts( rb_ct *buf, int len );
+static void rb_presolve_contacts( rb_ct *buffer, int len );
/*
* These closest point tests were learned from Real-Time Collision Detection by
vg_line( ct->co, p1, 0xffffffff );
}
+static void rb_box_incident_dir( v3f p, boxf box, v3f dir )
+{
+
+}
+
+/*
+ * Generates up to two contacts; optimised for the most stable manifold
+ */
+static int rb_capsule_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ float h = rba->inf.capsule.height,
+ r = rba->inf.capsule.radius;
+
+ v3f p0, p1;
+ v3_muladds( rba->co, rba->up, h*0.5f-r*0.5f, p1 );
+ v3_muladds( rba->co, rba->up, -h*0.5f+r*0.5f, p0 );
+
+ m4x3_mulv( rbb->to_local, p0, p0 );
+ m4x3_mulv( rbb->to_local, p1, p1 );
+
+ /* Clip zone +y */
+
+ v3f c0, c1;
+ closest_point_aabb( p0, rbb->bbx, c0 );
+ closest_point_aabb( p1, rbb->bbx, c1 );
+
+ v3f vis0;
+ m4x3_mulv( rbb->to_world, c0, vis0 );
+ vg_line_pt3( vis0, 0.1f, 0xffff00ff );
+ m4x3_mulv( rbb->to_world, c1, vis0 );
+ vg_line_pt3( vis0, 0.1f, 0xffff00ff );
+
+ v3f d0, d1;
+ v3_sub( p0, c0, d0 );
+ v3_sub( p1, c1, d1 );
+
+ float d02 = v3_length2(d0),
+ d12 = v3_length2(d1);
+
+ int count = 0;
+
+ if( d02 <= r*r )
+ {
+ rb_ct *ct = buf+count;
+ float d = sqrtf(d02);
+
+ vg_info( "d: %.4f\n", d );
+
+ v3_muls( d0, -1.0f/d, ct->n );
+ ct->p = r-d;
+ v3_add( c0, p0, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+
+ m3x3_mulv( rbb->to_world, ct->n, ct->n );
+ m4x3_mulv( rbb->to_world, ct->co, ct->co );
+
+ ct->rba = rba;
+ ct->rbb = rbb;
+ count ++;
+ }
+
+ return count;
+}
+
static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
{
v3f co, delta;
if( d2 <= r*r )
{
float d;
+
+ rb_ct *ct = buf;
if( d2 <= 0.0001f )
{
- v3_sub( rbb->co, rba->co, delta );
- d2 = v3_length2(delta);
- }
+ v3_sub( rba->co, rbb->co, delta );
+
+ /*
+ * some extra testing is required to find the best axis to push the
+ * object back outside the box. Since there isnt a clear seperating
+ * vector already, especially on really high aspect boxes.
+ */
+ float lx = v3_dot( rbb->right, delta ),
+ ly = v3_dot( rbb->up, delta ),
+ lz = v3_dot( rbb->forward, delta ),
+ px = rbb->bbx[1][0] - fabsf(lx),
+ py = rbb->bbx[1][1] - fabsf(ly),
+ pz = rbb->bbx[1][2] - fabsf(lz);
+
+ if( px < py && px < pz )
+ v3_muls( rbb->right, vg_signf(lx), ct->n );
+ else if( py < pz )
+ v3_muls( rbb->up, vg_signf(ly), ct->n );
+ else
+ v3_muls( rbb->forward, vg_signf(lz), ct->n );
- d = sqrtf(d2);
+ v3_muladds( rba->co, ct->n, -r, ct->co );
+ ct->p = r;
+ }
+ else
+ {
+ d = sqrtf(d2);
+ v3_muls( delta, 1.0f/d, ct->n );
+ ct->p = r-d;
+ v3_add( co, rba->co, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+ }
- rb_ct *ct = buf;
- v3_muls( delta, 1.0f/d, ct->n );
- v3_copy( co, ct->co );
- ct->p = r-d;
ct->rba = rba;
ct->rbb = rbb;
return 1;
/*
* Initializing things like tangent vectors
*/
-static void rb_presolve_contacts(void)
+
+static void rb_presolve_contacts( rb_ct *buffer, int len )
{
- for( int i=0; i<rb_contact_count; i++ )
+ for( int i=0; i<len; i++ )
{
- rb_ct *ct = &rb_contact_buffer[i];
-
+ rb_ct *ct = &buffer[i];
ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f);
rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
}
/*
- * Creates relative contact velocity vector, and offsets between each body */
+ * Creates relative contact velocity vector, and offsets between each body
+ */
static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
{
rigidbody *rba = ct->rba,
v3_add( rva, rvb, rv );
}
+/*
+ * Apply regular and angular velocity impulses to objects involved in contact
+ */
static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
{
rigidbody *rba = ct->rba,
v3_muladds( rbb->w, wb, ct->mass_total * rbb->inv_mass, rbb->w );
}
-static void rb_solve_contacts(void)
+/*
+ * One iteration to solve the contact constraint
+ */
+static void rb_solve_contacts( rb_ct *buf, int len )
{
float k_friction = 0.1f;
- /* TODO: second object
- * Static objects route to static element */
-
/* Friction Impulse */
- for( int i=0; i<rb_contact_count; i++ )
+ for( int i=0; i<len; i++ )
{
- struct contact *ct = &rb_contact_buffer[i];
+ struct contact *ct = &buf[i];
rigidbody *rb = ct->rba;
v3f rv, da, db;
}
/* Normal Impulse */
- for( int i=0; i<rb_contact_count; i++ )
+ for( int i=0; i<len; i++ )
{
- struct contact *ct = &rb_contact_buffer[i];
+ struct contact *ct = &buf[i];
rigidbody *rba = ct->rba,
*rbb = ct->rbb;
}
}
+/*
+ * The following ventures into not really very sophisticated at all maths
+ */
+
struct rb_angle_limit
{
rigidbody *rba, *rbb;
{
debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
}
+ else if( rb->type == k_rb_shape_capsule )
+ {
+ m4x3f m0, m1;
+ float h = rb->inf.capsule.height,
+ r = rb->inf.capsule.radius;
+
+ m3x3_copy( rb->to_world, m0 );
+ m3x3_copy( rb->to_world, m1 );
+
+ v3_muladds( rb->co, rb->up, -h*0.5f, m0[3] );
+ v3_muladds( rb->co, rb->up, h*0.5f, m1[3] );
+
+ debug_sphere( m0, r, colour );
+ debug_sphere( m1, r, colour );
+ vg_line( m0[3], m1[3], colour );
+ }
}
/*
* Capsule phyics
*/
+RB_DEPR
static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
{
v3f last = { 0.0f, 0.0f, radius };
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff