-/* SHite fphysics */
+/*
+ * Resources: Box2D - Erin Catto
+ * qu3e - Randy Gaul
+ */
+
+#include "common.h"
+#include "bvh.h"
+
+static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
+static bh_system bh_system_rigidbodies;
#ifndef RIGIDBODY_H
#define RIGIDBODY_H
-#include "vg/vg.h"
-#include "scene.h"
-
+#define RB_DEPR
#define k_rb_delta (1.0f/60.0f)
typedef struct rigidbody rigidbody;
{
v3f co, v, I;
v4f q;
- boxf bbx;
+ boxf bbx, bbx_world;
+ float inv_mass;
struct contact
{
v3f t[2];
float bias, norm_impulse, tangent_impulse[2];
}
- manifold[4];
+ manifold[12];
int manifold_count;
+ v3f delta; /* where is the origin of this in relation to a parent body */
m4x3f to_world, to_local;
};
v3_copy( rb->co, rb->to_world[3] );
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 );
}
static void rb_init( rigidbody *rb )
v3_zero( rb->v );
v3_zero( rb->I );
+ v3f dims;
+ v3_sub( rb->bbx[1], rb->bbx[0], dims );
+
+ rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]);
+
rb_update_transform( rb );
}
v3_cross( n, tx, ty );
}
-static void rb_build_manifold( rigidbody *rb, scene *sc )
+#include "world.h"
+
+static void rb_manifold_reset( rigidbody *rb )
+{
+ rb->manifold_count = 0;
+}
+
+static void rb_build_manifold_terrain( rigidbody *rb )
{
v3f *box = rb->bbx;
v3f pts[8];
m4x3_mulv( rb->to_world, p110, p110 );
m4x3_mulv( rb->to_world, p111, p111 );
- rb->manifold_count = 0;
+ int count = 0;
for( int i=0; i<8; i++ )
{
struct contact *ct = &rb->manifold[rb->manifold_count];
v3f surface;
-
v3_copy( point, surface );
- bvh_scene_sample( sc, surface, ct->n );
+ surface[1] += 4.0f;
+
+ ray_hit hit;
+ hit.dist = INFINITY;
+ if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
+ continue;
+
+ v3_copy( hit.normal, ct->n );
+ v3_copy( hit.pos, surface );
float p = vg_minf( surface[1] - point[1], 1.0f );
ct->tangent_impulse[1] = 0.0f;
rb->manifold_count ++;
- if( rb->manifold_count == 4 )
+ count ++;
+ if( count == 4 )
break;
}
}
static void rb_constraint_manifold( rigidbody *rb )
{
- float k_friction = 0.07f;
+ float k_friction = 0.1f;
/* Friction Impulse */
for( int i=0; i<rb->manifold_count; i++ )
}
}
+struct rb_angle_limit
+{
+ rigidbody *rba, *rbb;
+ v3f axis;
+ float impulse, bias;
+};
+
+static int rb_angle_limit_force( rigidbody *rba, v3f va,
+ rigidbody *rbb, v3f vb,
+ float max )
+{
+ v3f wva, wvb;
+ m3x3_mulv( rba->to_world, va, wva );
+ m3x3_mulv( rbb->to_world, vb, wvb );
+
+ float dt = v3_dot(wva,wvb)*0.999f,
+ ang = fabsf(dt);
+ ang = acosf( dt );
+ if( ang > max )
+ {
+ float correction = max-ang;
+
+ v3f axis;
+ v3_cross( wva, wvb, axis );
+
+ v4f rotation;
+ q_axis_angle( rotation, axis, -correction*0.25f );
+ q_mul( rotation, rba->q, rba->q );
+
+ q_axis_angle( rotation, axis, correction*0.25f );
+ q_mul( rotation, rbb->q, rbb->q );
+
+ return 1;
+ }
+
+ return 0;
+}
+
+static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
+{
+
+}
+
+RB_DEPR
static void rb_constraint_angle( rigidbody *rba, v3f va,
rigidbody *rbb, v3f vb,
float max, float spring )
}
}
+static void rb_relative_velocity( rigidbody *ra, v3f lca,
+ rigidbody *rb, v3f lcb, v3f rcv )
+{
+ v3f wca, wcb;
+ m3x3_mulv( ra->to_world, lca, wca );
+ m3x3_mulv( rb->to_world, lcb, wcb );
+
+ v3_sub( ra->v, rb->v, rcv );
+
+ v3f rcv_Ra, rcv_Rb;
+ v3_cross( ra->I, wca, rcv_Ra );
+ v3_cross( rb->I, wcb, rcv_Rb );
+ v3_add( rcv_Ra, rcv, rcv );
+ v3_sub( rcv, rcv_Rb, rcv );
+}
+
static void rb_constraint_position( rigidbody *ra, v3f lca,
rigidbody *rb, v3f lcb )
{
/* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
-
v3f wca, wcb;
m3x3_mulv( ra->to_world, lca, wca );
m3x3_mulv( rb->to_world, lcb, wcb );
v3_cross( rb->I, wcb, rcv_Rb );
v3_add( rcv_Ra, rcv, rcv );
v3_sub( rcv, rcv_Rb, rcv );
+
+ float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
+
+ float mass_a = 1.0f/ra->inv_mass,
+ mass_b = 1.0f/rb->inv_mass,
+ total_mass = mass_a+mass_b;
v3f impulse;
- v3_muls( rcv, 0.5f, impulse );
- v3_add( impulse, rb->v, rb->v );
+ v3_muls( rcv, 1.0f, impulse );
+ v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
v3_cross( wcb, impulse, impulse );
v3_add( impulse, rb->I, rb->I );
- v3_muls( rcv, -0.5f, impulse );
- v3_add( impulse, ra->v, ra->v );
+ v3_muls( rcv, -1.0f, impulse );
+ v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
v3_cross( wca, impulse, impulse );
v3_add( impulse, ra->I, ra->I );
-
#if 0
+ /*
+ * this could be used for spring joints
+ * its not good for position constraint
+ */
v3f impulse;
v3_muls( delta, 0.5f*spring, impulse );
vg_line( p100, p010, colour );
}
+/*
+ * out penetration distance, normal
+ */
+static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
+{
+ v3f local;
+ m4x3_mulv( rb->to_local, pos, local );
+
+ if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
+ local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
+ local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
+ {
+ v3f area, com, comrel;
+ v3_add( rb->bbx[0], rb->bbx[1], com );
+ v3_muls( com, 0.5f, com );
+
+ v3_sub( rb->bbx[1], rb->bbx[0], area );
+ v3_sub( local, com, comrel );
+ v3_div( comrel, area, comrel );
+
+ int axis = 0;
+ float max_mag = fabsf(comrel[0]);
+
+ if( fabsf(comrel[1]) > max_mag )
+ {
+ axis = 1;
+ max_mag = fabsf(comrel[1]);
+ }
+ if( fabsf(comrel[2]) > max_mag )
+ {
+ axis = 2;
+ max_mag = fabsf(comrel[2]);
+ }
+
+ v3_zero( normal );
+ normal[axis] = vg_signf(comrel[axis]);
+
+ if( normal[axis] < 0.0f )
+ *pen = local[axis] - rb->bbx[0][axis];
+ else
+ *pen = rb->bbx[1][axis] - local[axis];
+
+ m3x3_mulv( rb->to_world, normal, normal );
+ return 1;
+ }
+
+ return 0;
+}
+
+static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
+{
+ v3f verts[8];
+
+ v3f a, b;
+ v3_copy( ra->bbx[0], a );
+ v3_copy( ra->bbx[1], b );
+
+ m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
+ m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
+ m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
+ m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
+
+ int count = 0;
+
+ for( int i=0; i<8; i++ )
+ {
+ if( ra->manifold_count == vg_list_size(ra->manifold) )
+ return;
+
+ struct contact *ct = &ra->manifold[ ra->manifold_count ];
+
+ float p;
+ v3f normal;
+
+ if( rb_point_in_body( rb_static, verts[i], &p, normal ))
+ {
+ v3_copy( normal, ct->n );
+ v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
+ v3_sub( ct->co, ra->co, ct->delta );
+
+ vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
+
+ ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -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;
+
+ ra->manifold_count ++;
+ count ++;
+ if( count == 4 )
+ return;
+ }
+ }
+}
+
+/*
+ * BVH implementation, this is ONLY for static rigidbodies, its to slow for
+ * realtime use.
+ */
+
+static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
+{
+ rigidbody *rb = &((rigidbody *)user)[ item_index ];
+ box_concat( bound, rb->bbx_world );
+}
+
+static float rb_bh_centroid( void *user, u32 item_index, int axis )
+{
+ rigidbody *rb = &((rigidbody *)user)[ item_index ];
+ return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
+}
+
+static void rb_bh_swap( void *user, u32 ia, u32 ib )
+{
+ rigidbody temp, *rba, *rbb;
+ rba = &((rigidbody *)user)[ ia ];
+ rbb = &((rigidbody *)user)[ ib ];
+
+ temp = *rba;
+ *rba = *rbb;
+ *rbb = temp;
+}
+
+static void rb_bh_debug( void *user, u32 item_index )
+{
+ rigidbody *rb = &((rigidbody *)user)[ item_index ];
+ rb_debug( rb, 0xff00ffff );
+}
+
+static bh_system bh_system_rigidbodies =
+{
+ .expand_bound = rb_bh_expand_bound,
+ .item_centroid = rb_bh_centroid,
+ .item_swap = rb_bh_swap,
+ .item_debug = rb_bh_debug,
+ .cast_ray = NULL
+};
+
#endif /* RIGIDBODY_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff