#include "common.h"
#include "bvh.h"
+#include "scene.h"
static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
static bh_system bh_system_rigidbodies;
#ifndef RIGIDBODY_H
#define RIGIDBODY_H
-#define RB_DEPR
+//#define RB_DEPR
#define k_rb_rate 60.0f
#define k_rb_delta (1.0f/k_rb_rate)
enum rb_shape
{
- k_rb_shape_box,
- k_rb_shape_sphere,
- k_rb_shape_capsule
+ k_rb_shape_box = 0,
+ k_rb_shape_sphere = 1,
+ k_rb_shape_capsule = 2,
+ k_rb_shape_scene = 3
}
type;
float height, radius;
}
capsule;
+
+ struct rb_scene
+ {
+ scene *pscene;
+ }
+ scene;
}
inf;
boxf bbx, bbx_world;
float inv_mass;
- v3f delta; /* where is the origin of this in relation to a parent body */
+ 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
+ */
m4x3f to_world, to_local;
};
+#ifdef RB_DEPR
+/*
+ * Impulses on static objects get re-routed here
+ */
static rigidbody rb_static_null =
{
.co={0.0f,0.0f,0.0f},
.is_world = 1,
.inv_mass = 0.0f
};
+#endif
static void rb_debug( rigidbody *rb, u32 colour );
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;
+static void rb_update_bounds( rigidbody *rb )
+{
+ box_copy( rb->bbx, rb->bbx_world );
+ m4x3_transform_aabb( rb->to_world, rb->bbx_world );
+}
+
static void rb_update_transform( rigidbody *rb )
{
q_normalize( rb->q );
m4x3_invert_affine( rb->to_world, rb->to_local );
- box_copy( rb->bbx, rb->bbx_world );
- m4x3_transform_aabb( rb->to_world, rb->bbx_world );
-
m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f, 0.0f}, rb->right );
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 );
+
+ rb_update_bounds( rb );
}
static float sphere_volume( float radius )
rb->bbx[0][1] = -h;
rb->bbx[1][1] = h;
}
+ else if( rb->type == k_rb_shape_scene )
+ {
+ rb->is_world = 1;
+ box_copy( rb->inf.scene.pscene->bbx, rb->bbx );
+ }
if( rb->is_world )
{
}
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 );
}
static void rb_solver_reset(void);
+#ifdef RB_DEPR
static void rb_build_manifold_terrain( rigidbody *rb );
static void rb_build_manifold_terrain_sphere( rigidbody *rb );
+#endif
static void rb_solve_contacts( rb_ct *buf, int len );
static void rb_presolve_contacts( rb_ct *buffer, int len );
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
*
return 0;
}
+/* TODO: these guys */
+
+static int rb_capsule_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ u32 geo[128];
+ v3f tri[3];
+ int len = bh_select( &rbb->inf.scene.pscene->bhtris,
+ rba->bbx_world, geo, 128 );
+
+ return 0;
+}
+
+static int rb_sphere_vs_triangle( rigidbody *rba, rigidbody *rbb,
+ v3f tri[3], rb_ct *buf )
+{
+ v3f delta, co;
+
+ closest_on_triangle( rba->co, tri, co );
+ v3_sub( rba->co, co, delta );
+
+ vg_line( rba->co, co, 0xffff0000 );
+ vg_line_pt3( rba->co, 0.1f, 0xff00ffff );
+
+ float d2 = v3_length2( delta ),
+ r = rba->inf.sphere.radius;
+
+ if( d2 < r*r )
+ {
+ rb_ct *ct = buf;
+
+ v3f ab, ac, tn;
+ v3_sub( tri[2], tri[0], ab );
+ v3_sub( tri[1], tri[0], ac );
+ v3_cross( ac, ab, tn );
+ v3_copy( tn, ct->n );
+ v3_normalize( ct->n );
+
+ float d = sqrtf(d2);
+
+ v3_copy( co, ct->co );
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+static int rb_sphere_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ scene *sc = rbb->inf.scene.pscene;
+
+ u32 geo[128];
+ v3f tri[3];
+ int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
+
+ 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 );
+
+ buf[count].element_id = ptri[0];
+ count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
+
+ if( count == 12 )
+ {
+ vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+ return count;
+ }
+ }
+
+ 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 },
-/*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_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 )
+{
+ int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+ = rb_jump_table[rba->type][rbb->type];
+
+ /*
+ * 12 is the maximum manifold size we can generate, so we are forced to abort
+ * potentially checking any more.
+ */
+ if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
+ {
+ vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
+ rb_contact_count, vg_list_size(rb_contact_buffer) );
+ return 0;
+ }
+
+ /*
+ * TODO: Replace this with a more dedicated broad phase pass
+ */
+ if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
+ {
+ int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count);
+ rb_contact_count += count;
+ return count;
+ }
+ else
+ return 0;
+}
/*
* Generic functions
*/
+#ifdef RB_DEPR
/*
* This function does not accept triangle as a dynamic object, it is assumed
* to always be static.
return 0;
}
-
-RB_DEPR
static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
v3f co, v3f norm, float *p )
{
}
#include "world.h"
+#endif
static void rb_solver_reset(void)
{
return rb_contact_buffer + rb_contact_count;
}
+#ifdef RB_DEPR
static struct contact *rb_start_contact(void)
{
if( rb_contact_count == vg_list_size(rb_contact_buffer) )
}
-RB_DEPR
static void rb_build_manifold_terrain( rigidbody *rb )
{
v3f *box = rb->bbx;
}
}
}
+#endif
/*
* Initializing things like tangent vectors
*/
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++ )
}
-RB_DEPR
static void rb_constraint_angle( rigidbody *rba, v3f va,
rigidbody *rbb, v3f vb,
float max, float spring )
debug_capsule( rb->to_world, r, h, colour );
}
+ else if( rb->type == k_rb_shape_scene )
+ {
+ vg_line_boxf( rb->bbx, colour );
+ }
}
+#ifdef RB_DEPR
/*
* out penetration distance, normal
*/
.cast_ray = NULL
};
+#endif
+
#endif /* RIGIDBODY_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff