#define k_rb_delta (1.0f/k_rb_rate)
typedef struct rigidbody rigidbody;
+typedef struct contact rb_ct;
+
struct rigidbody
{
v3f co, v, w;
v3f right, up, forward;
+ int is_world;
+
boxf bbx, bbx_world;
float inv_mass;
m4x3f to_world, to_local;
};
+static rigidbody rb_static_null =
+{
+ .co={0.0f,0.0f,0.0f},
+ .q={0.0f,0.0f,0.0f,1.0f},
+ .v={0.0f,0.0f,0.0f},
+ .w={0.0f,0.0f,0.0f},
+ .is_world = 1,
+ .inv_mass = 0.0f
+};
+
static void rb_debug( rigidbody *rb, u32 colour );
static struct contact
{
- rigidbody *rba;
- v3f co, n, delta;
+ rigidbody *rba, *rbb;
+ v3f co, n;
v3f t[2];
- float bias, norm_impulse, tangent_impulse[2];
+ float mass_total, p, bias, norm_impulse, tangent_impulse[2];
}
rb_contact_buffer[256];
static int rb_contact_count = 0;
rb->bbx[1][1] = h;
}
- rb->inv_mass = 1.0f/(8.0f*volume);
+ if( rb->is_world )
+ {
+ rb->inv_mass = 0.0f;
+ }
+ else
+ {
+ rb->inv_mass = 1.0f/(8.0f*volume);
+ }
v3_zero( rb->v );
v3_zero( rb->w );
static void rb_build_manifold_terrain_sphere( rigidbody *rb );
static void rb_solve_contacts(void);
+/*
+ * These closest point tests were learned from Real-Time Collision Detection by
+ * Christer Ericson
+ */
static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
float *s, float *t, v3f c1, v3f c2)
{
return v3_length2( v0 );
}
+static void closest_point_aabb( v3f p, boxf box, v3f dest )
+{
+ v3_maxv( p, box[0], dest );
+ v3_minv( dest, box[1], dest );
+}
+
+static void closest_point_obb( v3f p, rigidbody *rb, v3f dest )
+{
+ v3f local;
+ m4x3_mulv( rb->to_local, p, local );
+ closest_point_aabb( local, rb->bbx, local );
+ m4x3_mulv( rb->to_world, local, dest );
+}
+
static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
{
v3f v0, v1;
v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
}
-/* Real-Time Collision Detection */
static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
{
v3f ab, ac, ap;
v3_muladds( dest, ac, w, dest );
}
+/*
+ * Contact generators
+ *
+ * These do not automatically allocate contacts, an appropriately sized
+ * buffer must be supplied. The function returns the size of the manifold
+ * which was generated.
+ *
+ * The values set on the contacts are: n, co, p, rba, rbb
+ */
+
+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 );
+ vg_line( ct->co, p1, 0xffffffff );
+}
+
+static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ v3f co, delta;
+
+ closest_point_obb( rba->co, rbb, co );
+ v3_sub( rba->co, co, delta );
+
+ float d2 = v3_length2(delta),
+ r = rba->inf.sphere.radius;
+
+ if( d2 <= r*r )
+ {
+ float d;
+ if( d2 <= 0.0001f )
+ {
+ v3_sub( rbb->co, rba->co, delta );
+ d2 = v3_length2(delta);
+ }
+
+ d = sqrtf(d2);
+
+ 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;
+ }
+
+ return 0;
+}
+
+static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ v3f delta;
+ v3_sub( rba->co, rbb->co, delta );
+
+ float d2 = v3_length2(delta),
+ r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
+
+ if( d2 < r*r )
+ {
+ float d = sqrtf(d2);
+
+ rb_ct *ct = buf;
+ v3_muls( delta, -1.0f/d, ct->n );
+
+ v3f p0, p1;
+ v3_muladds( rba->co, ct->n, rba->inf.sphere.radius, p0 );
+ v3_muladds( rbb->co, ct->n,-rbb->inf.sphere.radius, p1 );
+ v3_add( p0, p1, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_sphere_vs_box( rbb, rba, buf );
+}
+
+static int rb_box_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ /* TODO: Generating a stable quad manifold, lots of clipping */
+ return 0;
+}
+
+/*
+ * This function does not accept triangle as a dynamic object, it is assumed
+ * to always be static.
+ *
+ * The triangle is also assumed to be one sided for better detection
+ */
+static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
+{
+ v3f delta, co;
+
+ closest_on_triangle( rba->co, tri, co );
+ v3_sub( rba->co, co, delta );
+
+ float d2 = v3_length2( delta ),
+ r = rba->inf.sphere.radius;
+
+ if( d2 < r*r )
+ {
+ v3f ab, ac, tn;
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_cross( ac, ab, tn );
+
+ if( v3_dot( delta, tn ) > 0.0f )
+ v3_muls( delta, -1.0f, delta );
+
+ float d = sqrtf(d2);
+
+ 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 = &rb_static_null;
+ return 1;
+ }
+
+ return 0;
+}
+
+
+/*
+ * Generic functions
+ */
+
+RB_DEPR
static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
v3f co, v3f norm, float *p )
{
rb_contact_count = 0;
}
+static rb_ct *rb_global_ct(void)
+{
+ return rb_contact_buffer + rb_contact_count;
+}
+
static struct contact *rb_start_contact(void)
{
if( rb_contact_count == vg_list_size(rb_contact_buffer) )
ct->rba = rb;
v3_copy( co, ct->co );
v3_copy( norm, ct->n );
-
- v3_sub( co, rb->co, ct->delta );
rb_commit_contact( ct, p );
}
}
v3_copy( hit.normal, ct->n );
v3_add( point, surface, ct->co );
v3_muls( ct->co, 0.5f, ct->co );
- v3_sub( ct->co, rb->co, ct->delta );
rb_commit_contact( ct, p );
count ++;
}
}
+/*
+ * Initializing things like tangent vectors
+ */
+static void rb_presolve_contacts(void)
+{
+ for( int i=0; i<rb_contact_count; i++ )
+ {
+ rb_ct *ct = &rb_contact_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] );
+
+ 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);
+
+ rb_debug_contact( ct );
+ }
+}
+
+/*
+ * 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,
+ *rbb = ct->rbb;
+
+ v3_sub( rba->co, ct->co, da );
+ v3_sub( rbb->co, ct->co, db );
+
+ v3f rva, rvb;
+ v3_cross( rba->w, da, rva );
+ v3_add( rba->v, rva, rva );
+
+ v3_cross( rbb->w, db, rvb );
+ v3_add( rbb->v, rvb, rvb );
+ v3_add( rva, rvb, rv );
+}
+
+static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
+{
+ rigidbody *rba = ct->rba,
+ *rbb = ct->rbb;
+
+ /* response */
+ v3_muladds( rba->v, impulse, ct->mass_total * rba->inv_mass, rba->v );
+ v3_muladds( rbb->v, impulse, ct->mass_total * rbb->inv_mass, rbb->v );
+
+ /* Angular velocity */
+ v3f wa, wb;
+ v3_cross( da, impulse, wa );
+ v3_cross( db, impulse, wb );
+ v3_muladds( rba->w, wa, ct->mass_total * rba->inv_mass, rba->w );
+ v3_muladds( rbb->w, wb, ct->mass_total * rbb->inv_mass, rbb->w );
+}
+
static void rb_solve_contacts(void)
{
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++ )
{
struct contact *ct = &rb_contact_buffer[i];
rigidbody *rb = ct->rba;
- v3f dv;
- v3_cross( rb->w, ct->delta, dv );
- v3_add( rb->v, dv, dv );
+ v3f rv, da, db;
+ rb_rcv( ct, rv, da, db );
for( int j=0; j<2; j++ )
{
- float vt = vg_clampf( -v3_dot( dv, ct->t[j] ),
- -k_friction, k_friction );
-
- vt = -v3_dot( dv, ct->t[j] );
+ float f = k_friction * ct->norm_impulse,
+ vt = -v3_dot( rv, ct->t[j] );
float temp = ct->tangent_impulse[j];
- ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
+ ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f );
vt = ct->tangent_impulse[j] - temp;
v3f impulse;
-
v3_muls( ct->t[j], vt, impulse );
- v3_add( impulse, rb->v, rb->v );
- v3_cross( ct->delta, impulse, impulse );
- v3_add( impulse, rb->w, rb->w );
+ rb_standard_impulse( ct, da, db, impulse );
}
}
for( int i=0; i<rb_contact_count; i++ )
{
struct contact *ct = &rb_contact_buffer[i];
- rigidbody *rb = ct->rba;
+ rigidbody *rba = ct->rba,
+ *rbb = ct->rbb;
- v3f dv;
- v3_cross( rb->w, ct->delta, dv );
- v3_add( rb->v, dv, dv );
+ v3f rv, da, db;
+ rb_rcv( ct, rv, da, db );
- float vn = -v3_dot( dv, ct->n );
+ float vn = -v3_dot( rv, ct->n );
vn += ct->bias;
float temp = ct->norm_impulse;
vn = ct->norm_impulse - temp;
v3f impulse;
-
v3_muls( ct->n, vn, impulse );
- v3_add( impulse, rb->v, rb->v );
- v3_cross( ct->delta, impulse, impulse );
- v3_add( impulse, rb->w, rb->w );
+ rb_standard_impulse( ct, da, db, impulse );
}
}
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff