From: hgn Date: Wed, 14 Sep 2022 00:44:28 +0000 (+0100) Subject: inertia tensors X-Git-Url: https://harrygodden.com/git/?a=commitdiff_plain;h=b0a4fb814d794157c55212191df200915ab99515;hp=46643f969b12c2144a5f15ac5509610f18b467e4;p=carveJwlIkooP6JGAAIwe30JlM.git inertia tensors --- diff --git a/blender_export.py b/blender_export.py index 3f2dd79..edb758f 100644 --- a/blender_export.py +++ b/blender_export.py @@ -144,18 +144,17 @@ class classtype_skeleton(Structure): _pack_ = 1 _fields_ = [("channels",c_uint32), ("ik_count",c_uint32), + ("collider_count",c_uint32), ("anim_start",c_uint32), ("anim_count",c_uint32)] class classtype_bone(Structure): - _pack_ = 1 - _fields_ = [("deform",c_uint32)] - -class classtype_ik_bone(Structure): _pack_ = 1 _fields_ = [("deform",c_uint32), - ("target",c_uint32), - ("pole",c_uint32)] + ("ik_target",c_uint32), + ("ik_pole",c_uint32), + ("collider",c_uint32), + ("hitbox",(c_float*3)*2)] # Exporter # ============================================================================== @@ -281,6 +280,7 @@ def write_model(collection_name): if n.type == 'ARMATURE': tree["bones"] = [None] # None is the root transform tree["ik_count"] = 0 + tree["collider_count"] = 0 def _extendb( p, n, d ): nonlocal tree @@ -302,6 +302,9 @@ def write_model(collection_name): btree["pole"] = c.pole_subtarget tree["ik_count"] += 1 + if n.cv_data.collider: + tree['collider_count'] += 1 + btree['deform'] = n.use_deform p['children'] += [btree] @@ -371,10 +374,7 @@ def write_model(collection_name): node.parent = node_def["parent"]["uid"] if objt == 'BONE': - if 'target' in node_def: - classtype = 'k_classtype_ik_bone' - else: - classtype = 'k_classtype_bone' + classtype = 'k_classtype_bone' elif objt == 'ARMATURE': classtype = 'k_classtype_skeleton' else: @@ -399,6 +399,8 @@ def write_model(collection_name): if mod.type == 'ARMATURE': classtype = 'k_classtype_skin' armature_def = graph_lookup[mod.object] + POSE_OR_REST_CACHE = armature_def['obj'].data.pose_position + armature_def['obj'].data.pose_position = 'REST' if can_use_cache and obj.data.name in mesh_cache: @@ -580,7 +582,7 @@ def write_model(collection_name): s005 = "" if classtype == 'k_classtype_skin': - armature_def['obj'].data.pose_position = 'POSE' + armature_def['obj'].data.pose_position = POSE_OR_REST_CACHE s005 = F" [armature -> {armature_def['obj'].cv_data.uid}]" scmp = F"{s002:<32} {s003:<22} {s004} {s005}" @@ -614,6 +616,7 @@ def write_model(collection_name): bones = armature_def['bones'] skeleton.channels = len(bones) skeleton.ik_count = armature_def["ik_count"] + skeleton.collider_count = armature_def["collider_count"] if armature.animation_data: previous_frame = bpy.context.scene.frame_current @@ -698,18 +701,32 @@ def write_model(collection_name): bone = classtype_bone() bone.deform = node_def['deform'] - entdata_buffer += [bone] - - elif classtype == 'k_classtype_ik_bone': - node.classtype = 13 - entdata_length += sizeof( classtype_ik_bone ) - ikbone = classtype_ik_bone() - ikbone.target = armature_def['bones'].index( node_def['target'] ) - ikbone.pole = armature_def['bones'].index( node_def['pole'] ) - ikbone.deform = node_def['deform'] + if 'target' in node_def: + bone.ik_target = armature_def['bones'].index( node_def['target'] ) + bone.ik_pole = armature_def['bones'].index( node_def['pole'] ) + else: + bone.ik_target = 0 + bone.ik_pole = 0 + + bone.collider = 1 if obj.cv_data.collider else 0 + if obj.cv_data.collider: + bone.hitbox[0][0] = obj.cv_data.v0[0] + bone.hitbox[0][1] = obj.cv_data.v0[2] + bone.hitbox[0][2] = -obj.cv_data.v1[1] + bone.hitbox[1][0] = obj.cv_data.v1[0] + bone.hitbox[1][1] = obj.cv_data.v1[2] + bone.hitbox[1][2] = -obj.cv_data.v0[1] + else: + bone.hitbox[0][0] = 0.0 + bone.hitbox[0][1] = 0.0 + bone.hitbox[0][2] = 0.0 + bone.hitbox[1][0] = 0.0 + bone.hitbox[1][1] = 0.0 + bone.hitbox[1][2] = 0.0 - entdata_buffer += [ikbone] + bone.deform = node_def['deform'] + entdata_buffer += [bone] elif classtype == 'k_classtype_gate': node.classtype = 1 @@ -973,6 +990,76 @@ def cv_draw(): colours += [c0,c1] for obj in bpy.context.collection.objects: + if obj.type == 'ARMATURE': + for bone in obj.data.bones: + if bone.cv_data.collider: + c = bone.head_local + a = bone.cv_data.v0 + b = bone.cv_data.v1 + + vs = [None]*8 + vs[0]=obj.matrix_world@Vector((c[0]+a[0],c[1]+a[1],c[2]+a[2])) + vs[1]=obj.matrix_world@Vector((c[0]+a[0],c[1]+b[1],c[2]+a[2])) + vs[2]=obj.matrix_world@Vector((c[0]+b[0],c[1]+b[1],c[2]+a[2])) + vs[3]=obj.matrix_world@Vector((c[0]+b[0],c[1]+a[1],c[2]+a[2])) + vs[4]=obj.matrix_world@Vector((c[0]+a[0],c[1]+a[1],c[2]+b[2])) + vs[5]=obj.matrix_world@Vector((c[0]+a[0],c[1]+b[1],c[2]+b[2])) + vs[6]=obj.matrix_world@Vector((c[0]+b[0],c[1]+b[1],c[2]+b[2])) + vs[7]=obj.matrix_world@Vector((c[0]+b[0],c[1]+a[1],c[2]+b[2])) + + indices = [(0,1),(1,2),(2,3),(3,0),(4,5),(5,6),(6,7),(7,4),\ + (0,4),(1,5),(2,6),(3,7)] + + for l in indices: + v0 = vs[l[0]] + v1 = vs[l[1]] + verts += [(v0[0],v0[1],v0[2])] + verts += [(v1[0],v1[1],v1[2])] + colours += [(0.5,0.5,0.5,0.5),(0.5,0.5,0.5,0.5)] + + center=obj.matrix_world@c + + def _angle_lim( major, minor, amin, amax, colour ): + nonlocal verts, colours + f = 0.05 + ay = major*f + ax = minor*f + + for x in range(16): + t0 = x/16 + t1 = (x+1)/16 + a0 = amin*(1.0-t0)+amax*t0 + a1 = amin*(1.0-t1)+amax*t1 + + p0 = c + major*f*math.cos(a0) + minor*f*math.sin(a0) + p1 = c + major*f*math.cos(a1) + minor*f*math.sin(a1) + + p0=obj.matrix_world @ p0 + p1=obj.matrix_world @ p1 + verts += [p0,p1] + colours += [colour,colour] + + if x == 0: + verts += [p0,c] + colours += [colour,colour] + if x == 15: + verts += [p1,c] + colours += [colour,colour] + + verts += [c+major*1.2*f,c+major*f*0.8] + colours += [colour,colour] + + if bone.cv_data.con0: + _angle_lim( Vector((0,1,0)),Vector((0,0,1)), \ + bone.cv_data.mins[0], bone.cv_data.maxs[0], \ + (1,0,0,1)) + _angle_lim( Vector((0,0,1)),Vector((1,0,0)), \ + bone.cv_data.mins[1], bone.cv_data.maxs[1], \ + (0,1,0,1)) + _angle_lim( Vector((1,0,0)),Vector((0,1,0)), \ + bone.cv_data.mins[2], bone.cv_data.maxs[2], \ + (0,0,1,1)) + if obj.cv_data.classtype == 'k_classtype_gate': if obj.type == 'MESH': @@ -1225,6 +1312,52 @@ class CV_OBJ_SETTINGS(bpy.types.PropertyGroup): ('k_classtype_SKIN',"","",12) ]) +class CV_BONE_SETTINGS(bpy.types.PropertyGroup): + collider: bpy.props.BoolProperty(name="Collider",default=False) + v0: bpy.props.FloatVectorProperty(name="v0",size=3) + v1: bpy.props.FloatVectorProperty(name="v1",size=3) + + mins: bpy.props.FloatVectorProperty(name="mins",size=3) + maxs: bpy.props.FloatVectorProperty(name="maxs",size=3) + + con0: bpy.props.BoolProperty(name="Constriant 0",default=False) + c0: bpy.props.FloatVectorProperty(name="dir",size=3) + s0: bpy.props.FloatVectorProperty(name="limits",size=3) + + con1: bpy.props.BoolProperty(name="Constriant 1",default=False) + c1: bpy.props.FloatVectorProperty(name="dir",size=3) + s1: bpy.props.FloatVectorProperty(name="limits",size=3) + +class CV_BONE_PANEL(bpy.types.Panel): + bl_label="Bone Config" + bl_idname="SCENE_PT_cv_bone" + bl_space_type='PROPERTIES' + bl_region_type='WINDOW' + bl_context='bone' + + def draw(_,context): + active_object = context.active_object + if active_object == None: return + + bone = active_object.data.bones.active + if bone == None: return + + _.layout.prop( bone.cv_data, "collider" ) + _.layout.prop( bone.cv_data, "v0" ) + _.layout.prop( bone.cv_data, "v1" ) + + _.layout.label( text="Angle Limits" ) + _.layout.prop( bone.cv_data, "mins" ) + _.layout.prop( bone.cv_data, "maxs" ) + + _.layout.prop( bone.cv_data, "con0" ) + _.layout.prop( bone.cv_data, "c0" ) + _.layout.prop( bone.cv_data, "s0" ) + + _.layout.prop( bone.cv_data, "con1" ) + _.layout.prop( bone.cv_data, "c1" ) + _.layout.prop( bone.cv_data, "s1" ) + class CV_SCENE_SETTINGS(bpy.types.PropertyGroup): use_hidden: bpy.props.BoolProperty( name="use hidden", default=False ) @@ -1300,7 +1433,8 @@ class CV_COMPILE(bpy.types.Operator): return {'FINISHED'} classes = [CV_OBJ_SETTINGS,CV_OBJ_PANEL,CV_COMPILE,CV_INTERFACE,\ - CV_MESH_SETTINGS, CV_SCENE_SETTINGS] + CV_MESH_SETTINGS, CV_SCENE_SETTINGS, CV_BONE_SETTINGS,\ + CV_BONE_PANEL] def register(): global cv_view_draw_handler @@ -1311,6 +1445,7 @@ def register(): bpy.types.Object.cv_data = bpy.props.PointerProperty(type=CV_OBJ_SETTINGS) bpy.types.Mesh.cv_data = bpy.props.PointerProperty(type=CV_MESH_SETTINGS) bpy.types.Scene.cv_data = bpy.props.PointerProperty(type=CV_SCENE_SETTINGS) + bpy.types.Bone.cv_data = bpy.props.PointerProperty(type=CV_BONE_SETTINGS) cv_view_draw_handler = bpy.types.SpaceView3D.draw_handler_add(\ cv_draw,(),'WINDOW','POST_VIEW') diff --git a/common.h b/common.h index efe9a88..587c31b 100644 --- a/common.h +++ b/common.h @@ -23,8 +23,7 @@ enum classtype k_classtype_route = 9, k_classtype_bone = 10, k_classtype_skeleton = 11, - k_classtype_skin = 12, - k_classtype_ik_bone = 13 + k_classtype_skin = 12 }; /* TODO: he needs a home somewhere */ diff --git a/model.h b/model.h index 54874bd..fe238e1 100644 --- a/model.h +++ b/model.h @@ -148,21 +148,22 @@ struct classtype_route }; struct classtype_bone -{ - u32 deform; -}; - -struct classtype_ik_bone { u32 deform, - target, - pole; + + ik_target, + ik_pole, + + collider; + + boxf hitbox; }; struct classtype_skeleton { u32 channels, ik_count, + collider_count, anim_start, anim_count; }; diff --git a/models_src/ch_new.mdl b/models_src/ch_new.mdl index c9febad..7b81848 100644 Binary files a/models_src/ch_new.mdl and b/models_src/ch_new.mdl differ diff --git a/models_src/mp_dev.mdl b/models_src/mp_dev.mdl index e5166ff..f2f09cc 100644 Binary files a/models_src/mp_dev.mdl and b/models_src/mp_dev.mdl differ diff --git a/physics_test.h b/physics_test.h index 33a6b6b..acf4ed3 100644 --- a/physics_test.h +++ b/physics_test.h @@ -22,7 +22,7 @@ rigidbody blocky = rigidbody marko = { .type = k_rb_shape_box, - .bbx = {{-0.5f,-0.5f,-0.5f},{0.5f,0.5f,0.5f}}, + .bbx = {{-2.0f,-2.0f,-2.0f},{2.0f,2.0f,2.0f}}, .co = {-36.0f,8.0f,-36.0f}, .q = {0.0f,0.0f,0.0f,1.0f}, .is_world = 0 @@ -73,7 +73,7 @@ rigidbody jeff1 = { .type = k_rb_shape_capsule, }; rigidbody ball = { .type = k_rb_shape_sphere, - .inf.sphere = { .radius = 2.0f }, + .inf.sphere = { .radius = 4.0f }, .co = {0.0f,20.0f,2.0f}, .q = {0.0f,0.0f,0.0f,1.0f}}, @@ -88,6 +88,8 @@ static void reorg_jeffs(void) { for( int i=0; i world.sm_geo_std_oob.vertex_count; -} - -/* - * Get a sample at this pole location, will return 1 if the sample is valid, - * and pos will be updated to be the intersection location. - */ -static void player_walkgrid_samplepole( struct grid_sample *s ) -{ - boxf region = {{ s->pos[0] -0.01f, s->pos[1] - 4.0f, s->pos[2] -0.01f}, - { s->pos[0] +0.01f, s->pos[1] + 4.0f, s->pos[2] +0.01f}}; - - u32 geo[256]; - v3f tri[3]; - int len = bh_select( &world.geo.bhtris, region, geo, 256 ); - - const float k_minworld_y = -2000.0f; - - float walk_height = k_minworld_y, - block_height = k_minworld_y; - - s->type = k_sample_type_air; - - for( int i=0; ipos, sample_from ); - sample_from[1] = region[1][1]; - - float dist; - if( ray_tri( tri, sample_from, vdown, &dist )) - { - v3f p0; - v3_muladds( sample_from, vdown, dist, p0 ); - - if( player_walkgrid_tri_walkable(ptri) ) - { - if( p0[1] > walk_height ) - { - walk_height = p0[1]; - } - } - else - { - if( p0[1] > block_height ) - block_height = p0[1]; - } - } - } - - s->pos[1] = walk_height; - - if( walk_height > k_minworld_y ) - if( block_height > walk_height ) - s->type = k_sample_type_invalid; - else - s->type = k_sample_type_valid; - else - s->type = k_sample_type_air; -} - -float const k_gridscale = 0.5f; - -enum eclipdir -{ - k_eclipdir_h = 0, - k_eclipdir_v = 1 -}; - -static void player_walkgrid_clip_blocker( struct grid_sample *sa, - struct grid_sample *sb, - struct grid_sample *st, - enum eclipdir dir ) -{ - v3f clipdir, pos; - int valid_a = sa->type == k_sample_type_valid, - valid_b = sb->type == k_sample_type_valid; - struct grid_sample *target = valid_a? sa: sb, - *other = valid_a? sb: sa; - v3_copy( target->pos, pos ); - v3_sub( other->pos, target->pos, clipdir ); - - boxf cell_region; - v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, -k_gridscale*2.1f, cell_region[0]); - v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, k_gridscale*2.1f, cell_region[1]); - - u32 geo[256]; - v3f tri[3]; - int len = bh_select( &world.geo.bhtris, cell_region, geo, 256 ); - - float start_time = v3_length( clipdir ), - min_time = start_time; - v3_normalize( clipdir ); - v3_muls( clipdir, 0.0001f, st->clip[dir] ); - - for( int i=0; i 0.0f && dist < min_time ) - { - min_time = dist; - sb->type = k_sample_type_air; - } - } - } - - if( !(min_time < start_time) ) - min_time = 0.5f * k_gridscale; - - min_time = vg_clampf( min_time/k_gridscale, 0.01f, 0.99f ); - - v3_muls( clipdir, min_time, st->clip[dir] ); - - v3f p0; - v3_muladds( target->pos, st->clip[dir], k_gridscale, p0 ); -} - -static void player_walkgrid_clip_edge( struct grid_sample *sa, - struct grid_sample *sb, - struct grid_sample *st, /* data store */ - enum eclipdir dir ) -{ - v3f clipdir = { 0.0f, 0.0f, 0.0f }, pos; - int valid_a = sa->type == k_sample_type_valid, - valid_b = sb->type == k_sample_type_valid; - - struct grid_sample *target = valid_a? sa: sb, - *other = valid_a? sb: sa; - - v3_sub( other->pos, target->pos, clipdir ); - clipdir[1] = 0.0f; - - v3_copy( target->pos, pos ); - - boxf cell_region; - v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, -k_gridscale*1.1f, cell_region[0]); - v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, k_gridscale*1.1f, cell_region[1]); - - u32 geo[256]; - int len = bh_select( &world.geo.bhtris, cell_region, geo, 256 ); - - float max_dist = 0.0f; - v3f tri[3]; - v3f perp; - v3_cross( clipdir,(v3f){0.0f,1.0f,0.0f},perp ); - v3_muls( clipdir, 0.001f, st->clip[dir] ); - - for( int i=0; i= max_dist && h <= 1.0f ) - { - max_dist = h; - float l = 1.0f/v3_length(clipdir); - v3_muls( p0, l, st->clip[dir] ); - } - } - } - } -} - -static const struct conf -{ - struct confedge - { - /* i: sample index - * d: data index - * a: axis index - * o: the 'other' point to do a A/B test with - * if its -1, all AB is done. - */ - int i0, i1, - d0, d1, - a0, a1, - o0, o1; - } - edges[2]; - int edge_count; -} -k_walkgrid_configs[16] = { - {{},0}, - {{{ 3,3, 3,0, 1,0, -1,-1 }}, 1}, - {{{ 2,2, 1,3, 0,1, -1,-1 }}, 1}, - {{{ 2,3, 1,0, 0,0, 3,-1 }}, 1}, - - {{{ 1,1, 0,1, 1,0, -1,-1 }}, 1}, - {{{ 3,3, 3,0, 1,0, -1,-1 }, - { 1,1, 0,1, 1,0, -1,-1 }}, 2}, - {{{ 1,2, 0,3, 1,1, 2,-1 }}, 1}, - {{{ 1,3, 0,0, 1,0, 2, 2 }}, 1}, - - {{{ 0,0, 0,0, 0,1, -1,-1 }}, 1}, - {{{ 3,0, 3,0, 1,1, 0,-1 }}, 1}, - {{{ 2,2, 1,3, 0,1, -1,-1 }, - { 0,0, 0,0, 0,1, -1,-1 }}, 2}, - {{{ 2,0, 1,0, 0,1, 3, 3 }}, 1}, - - {{{ 0,1, 0,1, 0,0, 1,-1 }}, 1}, - {{{ 3,1, 3,1, 1,0, 0, 0 }}, 1}, - {{{ 0,2, 0,3, 0,1, 1, 1 }}, 1}, - {{},0}, -}; - -/* - * Get a buffer of edges from cell location - */ -static const struct conf *player_walkgrid_conf( struct walkgrid *wg, - v2i cell, - struct grid_sample *corners[4] ) -{ - corners[0] = &wg->samples[cell[1] ][cell[0] ]; - corners[1] = &wg->samples[cell[1]+1][cell[0] ]; - corners[2] = &wg->samples[cell[1]+1][cell[0]+1]; - corners[3] = &wg->samples[cell[1] ][cell[0]+1]; - - u32 vd0 = corners[0]->type == k_sample_type_valid, - vd1 = corners[1]->type == k_sample_type_valid, - vd2 = corners[2]->type == k_sample_type_valid, - vd3 = corners[3]->type == k_sample_type_valid, - config = (vd0<<3) | (vd1<<2) | (vd2<<1) | vd3; - - return &k_walkgrid_configs[ config ]; -} - -static void player_walkgrid_floor(v3f pos) -{ - v3_muls( pos, 1.0f/k_gridscale, pos ); - v3_floor( pos, pos ); - v3_muls( pos, k_gridscale, pos ); -} - -/* - * Computes the barycentric coordinate of location on a triangle (vertical), - * then sets the Y position to the interpolation of the three points - */ -static void player_walkgrid_stand_tri( v3f a, v3f b, v3f c, v3f pos ) -{ - v3f v0,v1,v2; - v3_sub( b, a, v0 ); - v3_sub( c, a, v1 ); - v3_sub( pos, a, v2 ); - - float d = v0[0]*v1[2] - v1[0]*v0[2], - v = (v2[0]*v1[2] - v1[0]*v2[2]) / d, - w = (v0[0]*v2[2] - v2[0]*v0[2]) / d, - u = 1.0f - v - w; - - vg_line( pos, a, 0xffff0000 ); - vg_line( pos, b, 0xff00ff00 ); - vg_line( pos, c, 0xff0000ff ); - pos[1] = u*a[1] + v*b[1] + w*c[1]; -} - -/* - * Get the minimum time value of pos+dir until a cell edge - * - * t[0] -> t[3] are the individual time values - * t[5] & t[6] are the maximum axis values - * t[6] is the minimum value - * - */ -static void player_walkgrid_min_cell( float t[7], v2f pos, v2f dir ) -{ - v2f frac = { 1.0f/dir[0], 1.0f/dir[1] }; - - t[0] = 999.9f; - t[1] = 999.9f; - t[2] = 999.9f; - t[3] = 999.9f; - - if( fabsf(dir[0]) > 0.0001f ) - { - t[0] = (0.0f-pos[0]) * frac[0]; - t[1] = (1.0f-pos[0]) * frac[0]; - } - if( fabsf(dir[1]) > 0.0001f ) - { - t[2] = (0.0f-pos[1]) * frac[1]; - t[3] = (1.0f-pos[1]) * frac[1]; - } - - t[4] = vg_maxf(t[0],t[1]); - t[5] = vg_maxf(t[2],t[3]); - t[6] = vg_minf(t[4],t[5]); -} - -static void player_walkgrid_iter(struct walkgrid *wg, int iter) -{ - - /* - * For each walkgrid iteration we are stepping through cells and determining - * the intersections with the grid, and any edges that are present - */ - - u32 icolours[] = { 0xffff00ff, 0xff00ffff, 0xffffff00 }; - - v3f pa, pb, pc, pd, pl0, pl1; - pa[0] = wg->region[0][0] + (float)wg->cell_id[0] *k_gridscale; - pa[1] = (wg->region[0][1] + wg->region[1][1]) * 0.5f + k_gridscale; - pa[2] = wg->region[0][2] + (float)wg->cell_id[1] *k_gridscale; -#if 0 - pb[0] = pa[0]; - pb[1] = pa[1]; - pb[2] = pa[2] + k_gridscale; - pc[0] = pa[0] + k_gridscale; - pc[1] = pa[1]; - pc[2] = pa[2] + k_gridscale; - pd[0] = pa[0] + k_gridscale; - pd[1] = pa[1]; - pd[2] = pa[2]; - /* if you want to draw the current cell */ - vg_line( pa, pb, 0xff00ffff ); - vg_line( pb, pc, 0xff00ffff ); - vg_line( pc, pd, 0xff00ffff ); - vg_line( pd, pa, 0xff00ffff ); -#endif - pl0[0] = pa[0] + wg->pos[0]*k_gridscale; - pl0[1] = pa[1]; - pl0[2] = pa[2] + wg->pos[1]*k_gridscale; - - /* - * If there are edges present, we need to create a 'substep' event, where - * we find the intersection point, find the fully resolved position, - * then the new pos dir is the intersection->resolution - * - * the resolution is applied in non-discretized space in order to create a - * suitable vector for finding outflow, we want it to leave the cell so it - * can be used by the quad - */ - - v2f pos, dir; - v2_copy( wg->pos, pos ); - v2_muls( wg->dir, wg->move, dir ); - - struct grid_sample *corners[4]; - v2f corners2d[4] = {{0.0f,0.0f},{0.0f,1.0f},{1.0f,1.0f},{1.0f,0.0f}}; - const struct conf *conf = player_walkgrid_conf( wg, wg->cell_id, corners ); - - float t[7]; - player_walkgrid_min_cell( t, pos, dir ); - - for( int i=0; iedge_count; i++ ) - { - const struct confedge *edge = &conf->edges[i]; - - v2f e0, e1, n, r, target, res, tangent; - e0[0] = corners2d[edge->i0][0] + corners[edge->d0]->clip[edge->a0][0]; - e0[1] = corners2d[edge->i0][1] + corners[edge->d0]->clip[edge->a0][2]; - e1[0] = corners2d[edge->i1][0] + corners[edge->d1]->clip[edge->a1][0]; - e1[1] = corners2d[edge->i1][1] + corners[edge->d1]->clip[edge->a1][2]; - - v3f pe0 = { pa[0] + e0[0]*k_gridscale, - pa[1], - pa[2] + e0[1]*k_gridscale }; - v3f pe1 = { pa[0] + e1[0]*k_gridscale, - pa[1], - pa[2] + e1[1]*k_gridscale }; - - v2_sub( e1, e0, tangent ); - n[0] = -tangent[1]; - n[1] = tangent[0]; - v2_normalize( n ); - - /* - * If we find ourselfs already penetrating the edge, move back out a - * little - */ - v2_sub( e0, pos, r ); - float p1 = v2_dot(r,n); - - if( -p1 < 0.0001f ) - { - v2_muladds( pos, n, p1+0.0001f, pos ); - v2_copy( pos, wg->pos ); - v3f p_new = { pa[0] + pos[0]*k_gridscale, - pa[1], - pa[2] + pos[1]*k_gridscale }; - v3_copy( p_new, pl0 ); - } - - v2_add( pos, dir, target ); - - v2f v1, v2, v3; - v2_sub( e0, pos, v1 ); - v2_sub( target, pos, v2 ); - - v2_copy( n, v3 ); - - v2_sub( e0, target, r ); - float p = v2_dot(r,n), - t1 = v2_dot(v1,v3)/v2_dot(v2,v3); - - if( t1 < t[6] && t1 > 0.0f && -p < 0.001f ) - { - v2_muladds( target, n, p+0.0001f, res ); - - v2f intersect; - v2_muladds( pos, dir, t1, intersect ); - v2_copy( intersect, pos ); - v2_sub( res, intersect, dir ); - - v3f p_res = { pa[0] + res[0]*k_gridscale, - pa[1], - pa[2] + res[1]*k_gridscale }; - v3f p_int = { pa[0] + intersect[0]*k_gridscale, - pa[1], - pa[2] + intersect[1]*k_gridscale }; - - vg_line( pl0, p_int, icolours[iter%3] ); - v3_copy( p_int, pl0 ); - v2_copy( pos, wg->pos ); - - player_walkgrid_min_cell( t, pos, dir ); - } - } - - /* - * Compute intersection with grid cell moving outwards - */ - t[6] = vg_minf( t[6], 1.0f ); - - pl1[0] = pl0[0] + dir[0]*k_gridscale*t[6]; - pl1[1] = pl0[1]; - pl1[2] = pl0[2] + dir[1]*k_gridscale*t[6]; - vg_line( pl0, pl1, icolours[iter%3] ); - - if( t[6] < 1.0f ) - { - /* - * To figure out what t value created the clip so we know which edge - * to wrap around - */ - - if( t[4] < t[5] ) - { - wg->pos[1] = pos[1] + dir[1]*t[6]; - - if( t[0] > t[1] ) /* left edge */ - { - wg->pos[0] = 0.9999f; - wg->cell_id[0] --; - - if( wg->cell_id[0] == 0 ) - wg->move = -1.0f; - } - else /* Right edge */ - { - wg->pos[0] = 0.0001f; - wg->cell_id[0] ++; - - if( wg->cell_id[0] == WALKGRID_SIZE-2 ) - wg->move = -1.0f; - } - } - else - { - wg->pos[0] = pos[0] + dir[0]*t[6]; - - if( t[2] > t[3] ) /* bottom edge */ - { - wg->pos[1] = 0.9999f; - wg->cell_id[1] --; - - if( wg->cell_id[1] == 0 ) - wg->move = -1.0f; - } - else /* top edge */ - { - wg->pos[1] = 0.0001f; - wg->cell_id[1] ++; - - if( wg->cell_id[1] == WALKGRID_SIZE-2 ) - wg->move = -1.0f; - } - } - - wg->move -= t[6]; - } - else - { - v2_muladds( wg->pos, dir, wg->move, wg->pos ); - wg->move = 0.0f; - } -} - -static void player_walkgrid_stand_cell(struct walkgrid *wg) -{ - /* - * NOTE: as opposed to the other function which is done in discretized space - * this use a combination of both. - */ - - v3f world; - world[0] = wg->region[0][0]+((float)wg->cell_id[0]+wg->pos[0])*k_gridscale; - world[1] = player.rb.co[1]; - world[2] = wg->region[0][2]+((float)wg->cell_id[1]+wg->pos[1])*k_gridscale; - - struct grid_sample *corners[4]; - const struct conf *conf = player_walkgrid_conf( wg, wg->cell_id, corners ); - - if( conf != k_walkgrid_configs ) - { - if( conf->edge_count == 0 ) - { - v3f v0; - - /* Split the basic quad along the shortest diagonal */ - if( fabsf(corners[2]->pos[1] - corners[0]->pos[1]) < - fabsf(corners[3]->pos[1] - corners[1]->pos[1]) ) - { - vg_line( corners[2]->pos, corners[0]->pos, 0xffaaaaaa ); - - if( wg->pos[0] > wg->pos[1] ) - player_walkgrid_stand_tri( corners[0]->pos, - corners[3]->pos, - corners[2]->pos, world ); - else - player_walkgrid_stand_tri( corners[0]->pos, - corners[2]->pos, - corners[1]->pos, world ); - } - else - { - vg_line( corners[3]->pos, corners[1]->pos, 0xffaaaaaa ); - - if( wg->pos[0] < 1.0f-wg->pos[1] ) - player_walkgrid_stand_tri( corners[0]->pos, - corners[3]->pos, - corners[1]->pos, world ); - else - player_walkgrid_stand_tri( corners[3]->pos, - corners[2]->pos, - corners[1]->pos, world ); - } - } - else - { - for( int i=0; iedge_count; i++ ) - { - const struct confedge *edge = &conf->edges[i]; - - v3f p0, p1; - v3_muladds( corners[edge->i0]->pos, - corners[edge->d0]->clip[edge->a0], k_gridscale, p0 ); - v3_muladds( corners[edge->i1]->pos, - corners[edge->d1]->clip[edge->a1], k_gridscale, p1 ); - - /* - * Find penetration distance between player position and the edge - */ - - v2f normal = { -(p1[2]-p0[2]), p1[0]-p0[0] }, - rel = { world[0]-p0[0], world[2]-p0[2] }; - - if( edge->o0 == -1 ) - { - /* No subregions (default case), just use triangle created by - * i0, e0, e1 */ - player_walkgrid_stand_tri( corners[edge->i0]->pos, - p0, - p1, world ); - } - else - { - /* - * Test if we are in the first region, which is - * edge.i0, edge.e0, edge.o0, - */ - v3f v0, ref; - v3_sub( p0, corners[edge->o0]->pos, ref ); - v3_sub( world, corners[edge->o0]->pos, v0 ); - - vg_line( corners[edge->o0]->pos, p0, 0xffffff00 ); - vg_line( corners[edge->o0]->pos, world, 0xff000000 ); - - if( ref[0]*v0[2] - ref[2]*v0[0] < 0.0f ) - { - player_walkgrid_stand_tri( corners[edge->i0]->pos, - p0, - corners[edge->o0]->pos, world ); - } - else - { - if( edge->o1 == -1 ) - { - /* - * No other edges mean we just need to use the opposite - * - * e0, e1, o0 (in our case, also i1) - */ - player_walkgrid_stand_tri( p0, - p1, - corners[edge->o0]->pos, world ); - } - else - { - /* - * Note: this v0 calculation can be ommited with the - * current tileset. - * - * the last two triangles we have are: - * e0, e1, o1 - * and - * e1, i1, o1 - */ - v3_sub( p1, corners[edge->o1]->pos, ref ); - v3_sub( world, corners[edge->o1]->pos, v0 ); - vg_line( corners[edge->o1]->pos, p1, 0xff00ffff ); - - if( ref[0]*v0[2] - ref[2]*v0[0] < 0.0f ) - { - player_walkgrid_stand_tri( p0, - p1, - corners[edge->o1]->pos, - world ); - } - else - { - player_walkgrid_stand_tri( p1, - corners[edge->i1]->pos, - corners[edge->o1]->pos, - world ); - } - } - } - } - } - } - } - - v3_copy( world, player.rb.co ); -} - -static void player_walkgrid_getsurface(void) -{ - float const k_stepheight = 0.5f; - float const k_miny = 0.6f; - float const k_height = 1.78f; - float const k_region_size = (float)WALKGRID_SIZE/2.0f * k_gridscale; - - static struct walkgrid wg; - - v3f cell; - v3_copy( player.rb.co, cell ); - player_walkgrid_floor( cell ); - - v3_muladds( cell, (v3f){-1.0f,-1.0f,-1.0f}, k_region_size, wg.region[0] ); - v3_muladds( cell, (v3f){ 1.0f, 1.0f, 1.0f}, k_region_size, wg.region[1] ); - - - /* - * Create player input vector - */ - v3f delta = {0.0f,0.0f,0.0f}; - v3f fwd = { -sinf(-player.angles[0]), 0.0f, -cosf(-player.angles[0]) }, - side = { -fwd[2], 0.0f, fwd[0] }; - - /* Temp */ - if( !vg_console_enabled() ) - { - if( glfwGetKey( vg_window, GLFW_KEY_W ) ) - v3_muladds( delta, fwd, ktimestep*k_walkspeed, delta ); - if( glfwGetKey( vg_window, GLFW_KEY_S ) ) - v3_muladds( delta, fwd, -ktimestep*k_walkspeed, delta ); - - if( glfwGetKey( vg_window, GLFW_KEY_A ) ) - v3_muladds( delta, side, -ktimestep*k_walkspeed, delta ); - if( glfwGetKey( vg_window, GLFW_KEY_D ) ) - v3_muladds( delta, side, ktimestep*k_walkspeed, delta ); - - v3_muladds( delta, fwd, - vg_get_axis("vertical")*-ktimestep*k_walkspeed, delta ); - v3_muladds( delta, side, - vg_get_axis("horizontal")*ktimestep*k_walkspeed, delta ); - } - - /* - * Create our move in grid space - */ - wg.dir[0] = delta[0] * (1.0f/k_gridscale); - wg.dir[1] = delta[2] * (1.0f/k_gridscale); - wg.move = 1.0f; - - v2f region_pos = - { - (player.rb.co[0] - wg.region[0][0]) * (1.0f/k_gridscale), - (player.rb.co[2] - wg.region[0][2]) * (1.0f/k_gridscale) - }; - v2f region_cell_pos; - v2_floor( region_pos, region_cell_pos ); - v2_sub( region_pos, region_cell_pos, wg.pos ); - - wg.cell_id[0] = region_cell_pos[0]; - wg.cell_id[1] = region_cell_pos[1]; - - for(int y=0; ypos ); - s->state = k_traverse_none; - s->type = k_sample_type_air; - v3_zero( s->clip[0] ); - v3_zero( s->clip[1] ); - } - } - - v2i border[WALKGRID_SIZE*WALKGRID_SIZE]; - v2i *cborder = border; - u32 border_length = 1; - - struct grid_sample *base = NULL; - - v2i starters[] = {{0,0},{1,1},{0,1},{1,0}}; - - for( int i=0;i<4;i++ ) - { - v2i test; - v2i_add( wg.cell_id, starters[i], test ); - v2i_copy( test, border[0] ); - base = &wg.samples[test[1]][test[0]]; - - base->pos[1] = cell[1]; - player_walkgrid_samplepole( base ); - - if( base->type == k_sample_type_valid ) - break; - else - base->type = k_sample_type_air; - } - - vg_line_pt3( base->pos, 0.1f, 0xffffffff ); - - int iter = 0; - - while( border_length ) - { - v2i directions[] = {{1,0},{0,1},{-1,0},{0,-1}}; - - v2i *old_border = cborder; - int len = border_length; - - border_length = 0; - cborder = old_border+len; - - for( int i=0; istate & thismove) == 0x00 || - sb->type == k_sample_type_air ) - { - sb->pos[1] = sa->pos[1]; - - player_walkgrid_samplepole( sb ); - - if( sb->type != k_sample_type_air ) - { - /* - * Need to do a blocker pass - */ - - struct grid_sample *store = (j>>1 == 0)? sa: sb; - player_walkgrid_clip_blocker( sa, sb, store, j%2 ); - - - if( sb->type != k_sample_type_air ) - { - vg_line( sa->pos, sb->pos, 0xffffffff ); - - if( sb->state == k_traverse_none ) - v2i_copy( newp, cborder[ border_length ++ ] ); - } - else - { - v3f p1; - v3_muladds( sa->pos, store->clip[j%2], k_gridscale, p1 ); - vg_line( sa->pos, p1, 0xffffffff ); - } - } - else - { - /* - * A clipping pass is now done on the edge of the walkable - * surface - */ - - struct grid_sample *store = (j>>1 == 0)? sa: sb; - player_walkgrid_clip_edge( sa, sb, store, j%2 ); - - v3f p1; - v3_muladds( sa->pos, store->clip[j%2], k_gridscale, p1 ); - vg_line( sa->pos, p1, 0xffffffff ); - } - - sb->state |= thismove; - } - } - - sa->state = k_traverse_h|k_traverse_v; - } - - iter ++; - if( iter == walk_grid_iterations ) - break; - } - - /* Draw connections */ - struct grid_sample *corners[4]; - for( int x=0; xedge_count; i++ ) - { - const struct confedge *edge = &conf->edges[i]; - - v3f p0, p1; - v3_muladds( corners[edge->i0]->pos, - corners[edge->d0]->clip[edge->a0], k_gridscale, p0 ); - v3_muladds( corners[edge->i1]->pos, - corners[edge->d1]->clip[edge->a1], k_gridscale, p1 ); - - vg_line( p0, p1, 0xff0000ff ); - } - } - } - - /* - * Commit player movement into the grid - */ - - if( v3_length2(delta) <= 0.00001f ) - return; - - int i=0; - for(; i<8 && wg.move > 0.001f; i++ ) - player_walkgrid_iter( &wg, i ); - - player_walkgrid_stand_cell( &wg ); -} - -static void player_walkgrid(void) -{ - player_walkgrid_getsurface(); - - m4x3_mulv( player.rb.to_world, (v3f){0.0f,1.8f,0.0f}, player.camera_pos ); - player_mouseview(); - rb_update_transform( &player.rb ); -} - /* * Animation */ @@ -2002,6 +1046,12 @@ static void player_update(void) player.on_board ^= 0x1; } + if( glfwGetKey( vg_window, GLFW_KEY_O ) ) + { + character_ragdoll_copypose( &player.mdl, player.rb.v ); + player.is_dead = 1; + } + if( player.is_dead ) { character_ragdoll_iter( &player.mdl ); @@ -2020,10 +1070,6 @@ static void player_update(void) if( !freecam ) player_animate_camera(); } - else - { - player_walkgrid(); - } } if( freecam ) diff --git a/player_model.h b/player_model.h index 95b5a76..542c7b2 100644 --- a/player_model.h +++ b/player_model.h @@ -6,6 +6,7 @@ #include "rigidbody.h" #include "render.h" #include "skeleton.h" +#include "world.h" #include "skeleton_animator.h" #include "shaders/viewchar.h" @@ -41,6 +42,15 @@ struct character v3f cam_pos; + struct ragdoll_part + { + u32 bone_id; + v3f offset; + rigidbody rb; + } + *ragdoll; + u32 ragdoll_count; + int shoes[2]; }; @@ -54,11 +64,7 @@ static int character_load( struct character *ch, const char *name ) if( !src ) return 0; - int error_count = 0; mdl_unpack_glmesh( src, &ch->mesh ); - - if( !error_count ) - vg_success( "Loaded character file '%s' with no errors\n", name ); skeleton_setup( &ch->sk, src ); ch->anim_stand = skeleton_get_anim( &ch->sk, "pose_stand" ); @@ -76,6 +82,42 @@ static int character_load( struct character *ch, const char *name ) ch->id_ik_elbow_r = skeleton_bone_id( &ch->sk, "elbow.R" ); ch->id_head = skeleton_bone_id( &ch->sk, "head" ); + /* setup ragdoll */ + + if( ch->sk.collider_count ) + { + vg_info( "Alloc: %d\n", ch->sk.collider_count ); + ch->ragdoll = malloc(sizeof(struct ragdoll_part) * ch->sk.collider_count); + ch->ragdoll_count = 0; + + for( u32 i=0; isk.bone_count; i ++ ) + { + struct skeleton_bone *bone = &ch->sk.bones[i]; + + if( bone->collider ) + { + struct ragdoll_part *rp = &ch->ragdoll[ ch->ragdoll_count ++ ]; + rp->bone_id = i; + + v3f delta; + v3_sub( bone->hitbox[1], bone->hitbox[0], delta ); + v3_muls( delta, 0.5f, delta ); + + v3_add( bone->hitbox[0], delta, rp->offset ); + + v3_copy( delta, rp->rb.bbx[1] ); + v3_muls( delta, -1.0f, rp->rb.bbx[0] ); + + q_identity( rp->rb.q ); + v3_add( bone->co, rp->offset, rp->rb.co ); + rp->rb.type = k_rb_shape_box; + rp->rb.is_world = 0; + + rb_init( &rp->rb ); + } + } + } + free( src ); return 1; } @@ -85,8 +127,62 @@ static void character_draw( struct character *ch, float temp, m4x3f camera ){} static void character_init_ragdoll_joints( struct character *ch ){} static void character_init_ragdoll( struct character *ch ){} static void character_ragdoll_go( struct character *ch, v3f pos ){} -static void character_ragdoll_copypose( struct character *ch, v3f v ){} -static void character_debug_ragdoll( struct character *ch ){} -static void character_ragdoll_iter( struct character *ch ){} +static void character_ragdoll_copypose( struct character *ch, v3f v ) +{ + for( int i=0; iragdoll_count; i++ ) + { + struct ragdoll_part *part = &ch->ragdoll[i]; + + v3f pos, offset; + u32 bone = part->bone_id; + + m4x3_mulv( ch->sk.final_mtx[bone], ch->sk.bones[bone].co, pos ); + m3x3_mulv( ch->sk.final_mtx[bone], part->offset, offset ); + v3_add( pos, offset, part->rb.co ); + m3x3_q( ch->sk.final_mtx[bone], part->rb.q ); + v3_copy( v, part->rb.v ); + v3_zero( part->rb.w ); + + rb_update_transform( &part->rb ); + } +} + +static void character_debug_ragdoll( struct character *ch ) +{ + for( u32 i=0; iragdoll_count; i ++ ) + rb_debug( &ch->ragdoll[i].rb, 0xff00ff00 ); +} + +static void character_ragdoll_iter( struct character *ch ) +{ + rb_solver_reset(); + + for( int i=0; iragdoll_count; i ++ ) + rb_collide( &ch->ragdoll[i].rb, &world.rb_geo ); + + rb_presolve_contacts( rb_contact_buffer, rb_contact_count ); + + v3f rv; + + float shoe_vel[2] = {0.0f,0.0f}; + for( int i=0; i<2; i++ ) + if( ch->shoes[i] ) + shoe_vel[i] = v3_length( ch->ragdoll[i].rb.v ); + + /* CONSTRAINTS */ + for( int i=0; i<5; i++ ) + { + rb_solve_contacts( rb_contact_buffer, rb_contact_count ); + } + + /* INTEGRATION */ + for( int i=0; iragdoll_count; i++ ) + rb_iter( &ch->ragdoll[i].rb ); + + /* SHOES */ + + for( int i=0; iragdoll_count; i++ ) + rb_update_transform( &ch->ragdoll[i].rb ); +} #endif diff --git a/player_walkgrid.h b/player_walkgrid.h new file mode 100644 index 0000000..937c305 --- /dev/null +++ b/player_walkgrid.h @@ -0,0 +1,963 @@ +#ifndef PLAYER_WALKGRID_H +#define PLAYER_WALKGRID_H + +#include "common.h" +#include "player.h" + +/* + * Walkgrid implementation, + * loosely based of cmuratoris youtube video 'Killing the Walkmonster' + */ + +#define WALKGRID_SIZE 16 +struct walkgrid +{ + struct grid_sample + { + enum sample_type + { + k_sample_type_air, /* Nothing was hit. */ + k_sample_type_invalid, /* The point is invalid, but there is a sample + underneath that can be used */ + k_sample_type_valid, /* This point is good */ + } + type; + + v3f clip[2]; + v3f pos; + + enum traverse_state + { + k_traverse_none = 0x00, + k_traverse_h = 0x01, + k_traverse_v = 0x02 + } + state; + } + samples[WALKGRID_SIZE][WALKGRID_SIZE]; + + boxf region; + + float move; /* Current amount of movement we have left to apply */ + v2f dir; /* The movement delta */ + v2i cell_id;/* Current cell */ + v2f pos; /* Local position (in cell) */ + float h; +}; + +static int player_walkgrid_tri_walkable( u32 tri[3] ) +{ + return tri[0] > world.sm_geo_std_oob.vertex_count; +} + +/* + * Get a sample at this pole location, will return 1 if the sample is valid, + * and pos will be updated to be the intersection location. + */ +static void player_walkgrid_samplepole( struct grid_sample *s ) +{ + boxf region = {{ s->pos[0] -0.01f, s->pos[1] - 4.0f, s->pos[2] -0.01f}, + { s->pos[0] +0.01f, s->pos[1] + 4.0f, s->pos[2] +0.01f}}; + + u32 geo[256]; + v3f tri[3]; + int len = bh_select( &world.geo.bhtris, region, geo, 256 ); + + const float k_minworld_y = -2000.0f; + + float walk_height = k_minworld_y, + block_height = k_minworld_y; + + s->type = k_sample_type_air; + + for( int i=0; ipos, sample_from ); + sample_from[1] = region[1][1]; + + float dist; + if( ray_tri( tri, sample_from, vdown, &dist )) + { + v3f p0; + v3_muladds( sample_from, vdown, dist, p0 ); + + if( player_walkgrid_tri_walkable(ptri) ) + { + if( p0[1] > walk_height ) + { + walk_height = p0[1]; + } + } + else + { + if( p0[1] > block_height ) + block_height = p0[1]; + } + } + } + + s->pos[1] = walk_height; + + if( walk_height > k_minworld_y ) + if( block_height > walk_height ) + s->type = k_sample_type_invalid; + else + s->type = k_sample_type_valid; + else + s->type = k_sample_type_air; +} + +float const k_gridscale = 0.5f; + +enum eclipdir +{ + k_eclipdir_h = 0, + k_eclipdir_v = 1 +}; + +static void player_walkgrid_clip_blocker( struct grid_sample *sa, + struct grid_sample *sb, + struct grid_sample *st, + enum eclipdir dir ) +{ + v3f clipdir, pos; + int valid_a = sa->type == k_sample_type_valid, + valid_b = sb->type == k_sample_type_valid; + struct grid_sample *target = valid_a? sa: sb, + *other = valid_a? sb: sa; + v3_copy( target->pos, pos ); + v3_sub( other->pos, target->pos, clipdir ); + + boxf cell_region; + v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, -k_gridscale*2.1f, cell_region[0]); + v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, k_gridscale*2.1f, cell_region[1]); + + u32 geo[256]; + v3f tri[3]; + int len = bh_select( &world.geo.bhtris, cell_region, geo, 256 ); + + float start_time = v3_length( clipdir ), + min_time = start_time; + v3_normalize( clipdir ); + v3_muls( clipdir, 0.0001f, st->clip[dir] ); + + for( int i=0; i 0.0f && dist < min_time ) + { + min_time = dist; + sb->type = k_sample_type_air; + } + } + } + + if( !(min_time < start_time) ) + min_time = 0.5f * k_gridscale; + + min_time = vg_clampf( min_time/k_gridscale, 0.01f, 0.99f ); + + v3_muls( clipdir, min_time, st->clip[dir] ); + + v3f p0; + v3_muladds( target->pos, st->clip[dir], k_gridscale, p0 ); +} + +static void player_walkgrid_clip_edge( struct grid_sample *sa, + struct grid_sample *sb, + struct grid_sample *st, /* data store */ + enum eclipdir dir ) +{ + v3f clipdir = { 0.0f, 0.0f, 0.0f }, pos; + int valid_a = sa->type == k_sample_type_valid, + valid_b = sb->type == k_sample_type_valid; + + struct grid_sample *target = valid_a? sa: sb, + *other = valid_a? sb: sa; + + v3_sub( other->pos, target->pos, clipdir ); + clipdir[1] = 0.0f; + + v3_copy( target->pos, pos ); + + boxf cell_region; + v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, -k_gridscale*1.1f, cell_region[0]); + v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, k_gridscale*1.1f, cell_region[1]); + + u32 geo[256]; + int len = bh_select( &world.geo.bhtris, cell_region, geo, 256 ); + + float max_dist = 0.0f; + v3f tri[3]; + v3f perp; + v3_cross( clipdir,(v3f){0.0f,1.0f,0.0f},perp ); + v3_muls( clipdir, 0.001f, st->clip[dir] ); + + for( int i=0; i= max_dist && h <= 1.0f ) + { + max_dist = h; + float l = 1.0f/v3_length(clipdir); + v3_muls( p0, l, st->clip[dir] ); + } + } + } + } +} + +static const struct conf +{ + struct confedge + { + /* i: sample index + * d: data index + * a: axis index + * o: the 'other' point to do a A/B test with + * if its -1, all AB is done. + */ + int i0, i1, + d0, d1, + a0, a1, + o0, o1; + } + edges[2]; + int edge_count; +} +k_walkgrid_configs[16] = { + {{},0}, + {{{ 3,3, 3,0, 1,0, -1,-1 }}, 1}, + {{{ 2,2, 1,3, 0,1, -1,-1 }}, 1}, + {{{ 2,3, 1,0, 0,0, 3,-1 }}, 1}, + + {{{ 1,1, 0,1, 1,0, -1,-1 }}, 1}, + {{{ 3,3, 3,0, 1,0, -1,-1 }, + { 1,1, 0,1, 1,0, -1,-1 }}, 2}, + {{{ 1,2, 0,3, 1,1, 2,-1 }}, 1}, + {{{ 1,3, 0,0, 1,0, 2, 2 }}, 1}, + + {{{ 0,0, 0,0, 0,1, -1,-1 }}, 1}, + {{{ 3,0, 3,0, 1,1, 0,-1 }}, 1}, + {{{ 2,2, 1,3, 0,1, -1,-1 }, + { 0,0, 0,0, 0,1, -1,-1 }}, 2}, + {{{ 2,0, 1,0, 0,1, 3, 3 }}, 1}, + + {{{ 0,1, 0,1, 0,0, 1,-1 }}, 1}, + {{{ 3,1, 3,1, 1,0, 0, 0 }}, 1}, + {{{ 0,2, 0,3, 0,1, 1, 1 }}, 1}, + {{},0}, +}; + +/* + * Get a buffer of edges from cell location + */ +static const struct conf *player_walkgrid_conf( struct walkgrid *wg, + v2i cell, + struct grid_sample *corners[4] ) +{ + corners[0] = &wg->samples[cell[1] ][cell[0] ]; + corners[1] = &wg->samples[cell[1]+1][cell[0] ]; + corners[2] = &wg->samples[cell[1]+1][cell[0]+1]; + corners[3] = &wg->samples[cell[1] ][cell[0]+1]; + + u32 vd0 = corners[0]->type == k_sample_type_valid, + vd1 = corners[1]->type == k_sample_type_valid, + vd2 = corners[2]->type == k_sample_type_valid, + vd3 = corners[3]->type == k_sample_type_valid, + config = (vd0<<3) | (vd1<<2) | (vd2<<1) | vd3; + + return &k_walkgrid_configs[ config ]; +} + +static void player_walkgrid_floor(v3f pos) +{ + v3_muls( pos, 1.0f/k_gridscale, pos ); + v3_floor( pos, pos ); + v3_muls( pos, k_gridscale, pos ); +} + +/* + * Computes the barycentric coordinate of location on a triangle (vertical), + * then sets the Y position to the interpolation of the three points + */ +static void player_walkgrid_stand_tri( v3f a, v3f b, v3f c, v3f pos ) +{ + v3f v0,v1,v2; + v3_sub( b, a, v0 ); + v3_sub( c, a, v1 ); + v3_sub( pos, a, v2 ); + + float d = v0[0]*v1[2] - v1[0]*v0[2], + v = (v2[0]*v1[2] - v1[0]*v2[2]) / d, + w = (v0[0]*v2[2] - v2[0]*v0[2]) / d, + u = 1.0f - v - w; + + vg_line( pos, a, 0xffff0000 ); + vg_line( pos, b, 0xff00ff00 ); + vg_line( pos, c, 0xff0000ff ); + pos[1] = u*a[1] + v*b[1] + w*c[1]; +} + +/* + * Get the minimum time value of pos+dir until a cell edge + * + * t[0] -> t[3] are the individual time values + * t[5] & t[6] are the maximum axis values + * t[6] is the minimum value + * + */ +static void player_walkgrid_min_cell( float t[7], v2f pos, v2f dir ) +{ + v2f frac = { 1.0f/dir[0], 1.0f/dir[1] }; + + t[0] = 999.9f; + t[1] = 999.9f; + t[2] = 999.9f; + t[3] = 999.9f; + + if( fabsf(dir[0]) > 0.0001f ) + { + t[0] = (0.0f-pos[0]) * frac[0]; + t[1] = (1.0f-pos[0]) * frac[0]; + } + if( fabsf(dir[1]) > 0.0001f ) + { + t[2] = (0.0f-pos[1]) * frac[1]; + t[3] = (1.0f-pos[1]) * frac[1]; + } + + t[4] = vg_maxf(t[0],t[1]); + t[5] = vg_maxf(t[2],t[3]); + t[6] = vg_minf(t[4],t[5]); +} + +static void player_walkgrid_iter(struct walkgrid *wg, int iter) +{ + + /* + * For each walkgrid iteration we are stepping through cells and determining + * the intersections with the grid, and any edges that are present + */ + + u32 icolours[] = { 0xffff00ff, 0xff00ffff, 0xffffff00 }; + + v3f pa, pb, pc, pd, pl0, pl1; + pa[0] = wg->region[0][0] + (float)wg->cell_id[0] *k_gridscale; + pa[1] = (wg->region[0][1] + wg->region[1][1]) * 0.5f + k_gridscale; + pa[2] = wg->region[0][2] + (float)wg->cell_id[1] *k_gridscale; +#if 0 + pb[0] = pa[0]; + pb[1] = pa[1]; + pb[2] = pa[2] + k_gridscale; + pc[0] = pa[0] + k_gridscale; + pc[1] = pa[1]; + pc[2] = pa[2] + k_gridscale; + pd[0] = pa[0] + k_gridscale; + pd[1] = pa[1]; + pd[2] = pa[2]; + /* if you want to draw the current cell */ + vg_line( pa, pb, 0xff00ffff ); + vg_line( pb, pc, 0xff00ffff ); + vg_line( pc, pd, 0xff00ffff ); + vg_line( pd, pa, 0xff00ffff ); +#endif + pl0[0] = pa[0] + wg->pos[0]*k_gridscale; + pl0[1] = pa[1]; + pl0[2] = pa[2] + wg->pos[1]*k_gridscale; + + /* + * If there are edges present, we need to create a 'substep' event, where + * we find the intersection point, find the fully resolved position, + * then the new pos dir is the intersection->resolution + * + * the resolution is applied in non-discretized space in order to create a + * suitable vector for finding outflow, we want it to leave the cell so it + * can be used by the quad + */ + + v2f pos, dir; + v2_copy( wg->pos, pos ); + v2_muls( wg->dir, wg->move, dir ); + + struct grid_sample *corners[4]; + v2f corners2d[4] = {{0.0f,0.0f},{0.0f,1.0f},{1.0f,1.0f},{1.0f,0.0f}}; + const struct conf *conf = player_walkgrid_conf( wg, wg->cell_id, corners ); + + float t[7]; + player_walkgrid_min_cell( t, pos, dir ); + + for( int i=0; iedge_count; i++ ) + { + const struct confedge *edge = &conf->edges[i]; + + v2f e0, e1, n, r, target, res, tangent; + e0[0] = corners2d[edge->i0][0] + corners[edge->d0]->clip[edge->a0][0]; + e0[1] = corners2d[edge->i0][1] + corners[edge->d0]->clip[edge->a0][2]; + e1[0] = corners2d[edge->i1][0] + corners[edge->d1]->clip[edge->a1][0]; + e1[1] = corners2d[edge->i1][1] + corners[edge->d1]->clip[edge->a1][2]; + + v3f pe0 = { pa[0] + e0[0]*k_gridscale, + pa[1], + pa[2] + e0[1]*k_gridscale }; + v3f pe1 = { pa[0] + e1[0]*k_gridscale, + pa[1], + pa[2] + e1[1]*k_gridscale }; + + v2_sub( e1, e0, tangent ); + n[0] = -tangent[1]; + n[1] = tangent[0]; + v2_normalize( n ); + + /* + * If we find ourselfs already penetrating the edge, move back out a + * little + */ + v2_sub( e0, pos, r ); + float p1 = v2_dot(r,n); + + if( -p1 < 0.0001f ) + { + v2_muladds( pos, n, p1+0.0001f, pos ); + v2_copy( pos, wg->pos ); + v3f p_new = { pa[0] + pos[0]*k_gridscale, + pa[1], + pa[2] + pos[1]*k_gridscale }; + v3_copy( p_new, pl0 ); + } + + v2_add( pos, dir, target ); + + v2f v1, v2, v3; + v2_sub( e0, pos, v1 ); + v2_sub( target, pos, v2 ); + + v2_copy( n, v3 ); + + v2_sub( e0, target, r ); + float p = v2_dot(r,n), + t1 = v2_dot(v1,v3)/v2_dot(v2,v3); + + if( t1 < t[6] && t1 > 0.0f && -p < 0.001f ) + { + v2_muladds( target, n, p+0.0001f, res ); + + v2f intersect; + v2_muladds( pos, dir, t1, intersect ); + v2_copy( intersect, pos ); + v2_sub( res, intersect, dir ); + + v3f p_res = { pa[0] + res[0]*k_gridscale, + pa[1], + pa[2] + res[1]*k_gridscale }; + v3f p_int = { pa[0] + intersect[0]*k_gridscale, + pa[1], + pa[2] + intersect[1]*k_gridscale }; + + vg_line( pl0, p_int, icolours[iter%3] ); + v3_copy( p_int, pl0 ); + v2_copy( pos, wg->pos ); + + player_walkgrid_min_cell( t, pos, dir ); + } + } + + /* + * Compute intersection with grid cell moving outwards + */ + t[6] = vg_minf( t[6], 1.0f ); + + pl1[0] = pl0[0] + dir[0]*k_gridscale*t[6]; + pl1[1] = pl0[1]; + pl1[2] = pl0[2] + dir[1]*k_gridscale*t[6]; + vg_line( pl0, pl1, icolours[iter%3] ); + + if( t[6] < 1.0f ) + { + /* + * To figure out what t value created the clip so we know which edge + * to wrap around + */ + + if( t[4] < t[5] ) + { + wg->pos[1] = pos[1] + dir[1]*t[6]; + + if( t[0] > t[1] ) /* left edge */ + { + wg->pos[0] = 0.9999f; + wg->cell_id[0] --; + + if( wg->cell_id[0] == 0 ) + wg->move = -1.0f; + } + else /* Right edge */ + { + wg->pos[0] = 0.0001f; + wg->cell_id[0] ++; + + if( wg->cell_id[0] == WALKGRID_SIZE-2 ) + wg->move = -1.0f; + } + } + else + { + wg->pos[0] = pos[0] + dir[0]*t[6]; + + if( t[2] > t[3] ) /* bottom edge */ + { + wg->pos[1] = 0.9999f; + wg->cell_id[1] --; + + if( wg->cell_id[1] == 0 ) + wg->move = -1.0f; + } + else /* top edge */ + { + wg->pos[1] = 0.0001f; + wg->cell_id[1] ++; + + if( wg->cell_id[1] == WALKGRID_SIZE-2 ) + wg->move = -1.0f; + } + } + + wg->move -= t[6]; + } + else + { + v2_muladds( wg->pos, dir, wg->move, wg->pos ); + wg->move = 0.0f; + } +} + +static void player_walkgrid_stand_cell(struct walkgrid *wg) +{ + /* + * NOTE: as opposed to the other function which is done in discretized space + * this use a combination of both. + */ + + v3f world; + world[0] = wg->region[0][0]+((float)wg->cell_id[0]+wg->pos[0])*k_gridscale; + world[1] = player.rb.co[1]; + world[2] = wg->region[0][2]+((float)wg->cell_id[1]+wg->pos[1])*k_gridscale; + + struct grid_sample *corners[4]; + const struct conf *conf = player_walkgrid_conf( wg, wg->cell_id, corners ); + + if( conf != k_walkgrid_configs ) + { + if( conf->edge_count == 0 ) + { + v3f v0; + + /* Split the basic quad along the shortest diagonal */ + if( fabsf(corners[2]->pos[1] - corners[0]->pos[1]) < + fabsf(corners[3]->pos[1] - corners[1]->pos[1]) ) + { + vg_line( corners[2]->pos, corners[0]->pos, 0xffaaaaaa ); + + if( wg->pos[0] > wg->pos[1] ) + player_walkgrid_stand_tri( corners[0]->pos, + corners[3]->pos, + corners[2]->pos, world ); + else + player_walkgrid_stand_tri( corners[0]->pos, + corners[2]->pos, + corners[1]->pos, world ); + } + else + { + vg_line( corners[3]->pos, corners[1]->pos, 0xffaaaaaa ); + + if( wg->pos[0] < 1.0f-wg->pos[1] ) + player_walkgrid_stand_tri( corners[0]->pos, + corners[3]->pos, + corners[1]->pos, world ); + else + player_walkgrid_stand_tri( corners[3]->pos, + corners[2]->pos, + corners[1]->pos, world ); + } + } + else + { + for( int i=0; iedge_count; i++ ) + { + const struct confedge *edge = &conf->edges[i]; + + v3f p0, p1; + v3_muladds( corners[edge->i0]->pos, + corners[edge->d0]->clip[edge->a0], k_gridscale, p0 ); + v3_muladds( corners[edge->i1]->pos, + corners[edge->d1]->clip[edge->a1], k_gridscale, p1 ); + + /* + * Find penetration distance between player position and the edge + */ + + v2f normal = { -(p1[2]-p0[2]), p1[0]-p0[0] }, + rel = { world[0]-p0[0], world[2]-p0[2] }; + + if( edge->o0 == -1 ) + { + /* No subregions (default case), just use triangle created by + * i0, e0, e1 */ + player_walkgrid_stand_tri( corners[edge->i0]->pos, + p0, + p1, world ); + } + else + { + /* + * Test if we are in the first region, which is + * edge.i0, edge.e0, edge.o0, + */ + v3f v0, ref; + v3_sub( p0, corners[edge->o0]->pos, ref ); + v3_sub( world, corners[edge->o0]->pos, v0 ); + + vg_line( corners[edge->o0]->pos, p0, 0xffffff00 ); + vg_line( corners[edge->o0]->pos, world, 0xff000000 ); + + if( ref[0]*v0[2] - ref[2]*v0[0] < 0.0f ) + { + player_walkgrid_stand_tri( corners[edge->i0]->pos, + p0, + corners[edge->o0]->pos, world ); + } + else + { + if( edge->o1 == -1 ) + { + /* + * No other edges mean we just need to use the opposite + * + * e0, e1, o0 (in our case, also i1) + */ + player_walkgrid_stand_tri( p0, + p1, + corners[edge->o0]->pos, world ); + } + else + { + /* + * Note: this v0 calculation can be ommited with the + * current tileset. + * + * the last two triangles we have are: + * e0, e1, o1 + * and + * e1, i1, o1 + */ + v3_sub( p1, corners[edge->o1]->pos, ref ); + v3_sub( world, corners[edge->o1]->pos, v0 ); + vg_line( corners[edge->o1]->pos, p1, 0xff00ffff ); + + if( ref[0]*v0[2] - ref[2]*v0[0] < 0.0f ) + { + player_walkgrid_stand_tri( p0, + p1, + corners[edge->o1]->pos, + world ); + } + else + { + player_walkgrid_stand_tri( p1, + corners[edge->i1]->pos, + corners[edge->o1]->pos, + world ); + } + } + } + } + } + } + } + + v3_copy( world, player.rb.co ); +} + +static void player_walkgrid_getsurface(void) +{ + float const k_stepheight = 0.5f; + float const k_miny = 0.6f; + float const k_height = 1.78f; + float const k_region_size = (float)WALKGRID_SIZE/2.0f * k_gridscale; + + static struct walkgrid wg; + + v3f cell; + v3_copy( player.rb.co, cell ); + player_walkgrid_floor( cell ); + + v3_muladds( cell, (v3f){-1.0f,-1.0f,-1.0f}, k_region_size, wg.region[0] ); + v3_muladds( cell, (v3f){ 1.0f, 1.0f, 1.0f}, k_region_size, wg.region[1] ); + + + /* + * Create player input vector + */ + v3f delta = {0.0f,0.0f,0.0f}; + v3f fwd = { -sinf(-player.angles[0]), 0.0f, -cosf(-player.angles[0]) }, + side = { -fwd[2], 0.0f, fwd[0] }; + + /* Temp */ + if( !vg_console_enabled() ) + { + if( glfwGetKey( vg_window, GLFW_KEY_W ) ) + v3_muladds( delta, fwd, ktimestep*k_walkspeed, delta ); + if( glfwGetKey( vg_window, GLFW_KEY_S ) ) + v3_muladds( delta, fwd, -ktimestep*k_walkspeed, delta ); + + if( glfwGetKey( vg_window, GLFW_KEY_A ) ) + v3_muladds( delta, side, -ktimestep*k_walkspeed, delta ); + if( glfwGetKey( vg_window, GLFW_KEY_D ) ) + v3_muladds( delta, side, ktimestep*k_walkspeed, delta ); + + v3_muladds( delta, fwd, + vg_get_axis("vertical")*-ktimestep*k_walkspeed, delta ); + v3_muladds( delta, side, + vg_get_axis("horizontal")*ktimestep*k_walkspeed, delta ); + } + + /* + * Create our move in grid space + */ + wg.dir[0] = delta[0] * (1.0f/k_gridscale); + wg.dir[1] = delta[2] * (1.0f/k_gridscale); + wg.move = 1.0f; + + v2f region_pos = + { + (player.rb.co[0] - wg.region[0][0]) * (1.0f/k_gridscale), + (player.rb.co[2] - wg.region[0][2]) * (1.0f/k_gridscale) + }; + v2f region_cell_pos; + v2_floor( region_pos, region_cell_pos ); + v2_sub( region_pos, region_cell_pos, wg.pos ); + + wg.cell_id[0] = region_cell_pos[0]; + wg.cell_id[1] = region_cell_pos[1]; + + for(int y=0; ypos ); + s->state = k_traverse_none; + s->type = k_sample_type_air; + v3_zero( s->clip[0] ); + v3_zero( s->clip[1] ); + } + } + + v2i border[WALKGRID_SIZE*WALKGRID_SIZE]; + v2i *cborder = border; + u32 border_length = 1; + + struct grid_sample *base = NULL; + + v2i starters[] = {{0,0},{1,1},{0,1},{1,0}}; + + for( int i=0;i<4;i++ ) + { + v2i test; + v2i_add( wg.cell_id, starters[i], test ); + v2i_copy( test, border[0] ); + base = &wg.samples[test[1]][test[0]]; + + base->pos[1] = cell[1]; + player_walkgrid_samplepole( base ); + + if( base->type == k_sample_type_valid ) + break; + else + base->type = k_sample_type_air; + } + + vg_line_pt3( base->pos, 0.1f, 0xffffffff ); + + int iter = 0; + + while( border_length ) + { + v2i directions[] = {{1,0},{0,1},{-1,0},{0,-1}}; + + v2i *old_border = cborder; + int len = border_length; + + border_length = 0; + cborder = old_border+len; + + for( int i=0; istate & thismove) == 0x00 || + sb->type == k_sample_type_air ) + { + sb->pos[1] = sa->pos[1]; + + player_walkgrid_samplepole( sb ); + + if( sb->type != k_sample_type_air ) + { + /* + * Need to do a blocker pass + */ + + struct grid_sample *store = (j>>1 == 0)? sa: sb; + player_walkgrid_clip_blocker( sa, sb, store, j%2 ); + + + if( sb->type != k_sample_type_air ) + { + vg_line( sa->pos, sb->pos, 0xffffffff ); + + if( sb->state == k_traverse_none ) + v2i_copy( newp, cborder[ border_length ++ ] ); + } + else + { + v3f p1; + v3_muladds( sa->pos, store->clip[j%2], k_gridscale, p1 ); + vg_line( sa->pos, p1, 0xffffffff ); + } + } + else + { + /* + * A clipping pass is now done on the edge of the walkable + * surface + */ + + struct grid_sample *store = (j>>1 == 0)? sa: sb; + player_walkgrid_clip_edge( sa, sb, store, j%2 ); + + v3f p1; + v3_muladds( sa->pos, store->clip[j%2], k_gridscale, p1 ); + vg_line( sa->pos, p1, 0xffffffff ); + } + + sb->state |= thismove; + } + } + + sa->state = k_traverse_h|k_traverse_v; + } + + iter ++; + if( iter == walk_grid_iterations ) + break; + } + + /* Draw connections */ + struct grid_sample *corners[4]; + for( int x=0; xedge_count; i++ ) + { + const struct confedge *edge = &conf->edges[i]; + + v3f p0, p1; + v3_muladds( corners[edge->i0]->pos, + corners[edge->d0]->clip[edge->a0], k_gridscale, p0 ); + v3_muladds( corners[edge->i1]->pos, + corners[edge->d1]->clip[edge->a1], k_gridscale, p1 ); + + vg_line( p0, p1, 0xff0000ff ); + } + } + } + + /* + * Commit player movement into the grid + */ + + if( v3_length2(delta) <= 0.00001f ) + return; + + int i=0; + for(; i<8 && wg.move > 0.001f; i++ ) + player_walkgrid_iter( &wg, i ); + + player_walkgrid_stand_cell( &wg ); +} + +static void player_walkgrid(void) +{ + player_walkgrid_getsurface(); + + m4x3_mulv( player.rb.to_world, (v3f){0.0f,1.8f,0.0f}, player.camera_pos ); + player_mouseview(); + rb_update_transform( &player.rb ); +} + +#endif /* PLAYER_WALKGRID_H */ diff --git a/rigidbody.h b/rigidbody.h index 2714c43..32753d1 100644 --- a/rigidbody.h +++ b/rigidbody.h @@ -63,10 +63,10 @@ struct rigidbody boxf bbx, bbx_world; float inv_mass; - 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 - */ + /* inertia model and inverse world tensor */ + v3f I; + m3x3f iI, iIw; + m4x3f to_world, to_local; }; @@ -92,7 +92,9 @@ static struct contact rigidbody *rba, *rbb; v3f co, n; v3f t[2]; - float mass_total, p, bias, norm_impulse, tangent_impulse[2]; + float p, bias, norm_impulse, tangent_impulse[2], + normal_mass, tangent_mass[2]; + u32 element_id; } rb_contact_buffer[256]; @@ -116,6 +118,9 @@ static void rb_update_transform( rigidbody *rb ) 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 ); + m3x3_mul( rb->iI, rb->to_local, rb->iIw ); + m3x3_mul( rb->to_world, rb->iIw, rb->iIw ); + rb_update_bounds( rb ); } @@ -134,12 +139,17 @@ static void rb_init( rigidbody *rb ) v3f dims; v3_sub( rb->bbx[1], rb->bbx[0], dims ); volume = dims[0]*dims[1]*dims[2]; + + if( !rb->is_world ) + vg_info( "Box volume: %f\n", volume ); } else if( rb->type == k_rb_shape_sphere ) { volume = sphere_volume( rb->inf.sphere.radius ); v3_fill( rb->bbx[0], -rb->inf.sphere.radius ); v3_fill( rb->bbx[1], rb->inf.sphere.radius ); + + vg_info( "Sphere volume: %f\n", volume ); } else if( rb->type == k_rb_shape_capsule ) { @@ -161,11 +171,32 @@ static void rb_init( rigidbody *rb ) if( rb->is_world ) { rb->inv_mass = 0.0f; + v3_zero( rb->I ); + m3x3_zero(rb->iI); } else { - rb->inv_mass = 1.0f/(8.0f*volume); /* TODO: Things get weird when mass - passes a certain point??? */ + float mass = 2.0f*volume; + rb->inv_mass = 1.0f/mass; + + v3f extent; + v3_sub( rb->bbx[1], rb->bbx[0], extent ); + v3_muls( extent, 0.5f, extent ); + + /* local intertia tensor */ + float ex2 = 4.0f*extent[0]*extent[0], + ey2 = 4.0f*extent[1]*extent[1], + ez2 = 4.0f*extent[2]*extent[2]; + + rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2)); + rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2)); + rb->I[2] = ((1.0f/12.0f) * mass * (ex2+ey2)); + + m3x3_identity( rb->iI ); + rb->iI[0][0] = rb->I[0]; + rb->iI[1][1] = rb->I[1]; + rb->iI[2][2] = rb->I[2]; + m3x3_inv( rb->iI, rb->iI ); } v3_zero( rb->v ); @@ -176,7 +207,7 @@ static void rb_init( rigidbody *rb ) static void rb_iter( rigidbody *rb ) { - v3f gravity = { 0.0f, -9.6f, 0.0f }; + v3f gravity = { 0.0f, -9.8f, 0.0f }; v3_muladds( rb->v, gravity, k_rb_delta, rb->v ); /* intergrate velocity */ @@ -569,8 +600,8 @@ int rb_intersect_planes_1( v4f a, v4f b, v4f c, v3f p ) 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 ); + v3_muladds( ct->co, ct->n, 0.1f, p1 ); + vg_line_pt3( ct->co, 0.025f, 0xff0000ff ); vg_line( ct->co, p1, 0xffffffff ); } @@ -1077,16 +1108,83 @@ static float rb_box_plane_interval( rigidbody *rba, v4f p ) return r-s; } +static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ) +{ + float + + r = extent[0] * fabsf(axis[0]) + + extent[1] * fabsf(axis[1]) + + extent[2] * fabsf(axis[2]), + + p0 = v3_dot( axis, tri[0] ), + p1 = v3_dot( axis, tri[1] ), + p2 = v3_dot( axis, tri[2] ), + + e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2))); + + if( e > r ) return 0; + else return 1; +} + +static int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] ) +{ + v3f tri[3]; + + v3f extent, c; + v3_sub( rba->bbx[1], rba->bbx[0], extent ); + v3_muls( extent, 0.5f, extent ); + v3_add( rba->bbx[0], extent, c ); + + for( int i=0; i<3; i++ ) + { + m4x3_mulv( rba->to_local, tri_src[i], tri[i] ); + v3_sub( tri[i], c, tri[i] ); + } + + /* u0, u1, u2 */ + if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0; + if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0; + if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0; + + v3f v0,v1,v2,n, e0,e1,e2; + v3_sub( tri[1], tri[0], v0 ); + v3_sub( tri[2], tri[0], v1 ); + v3_sub( tri[2], tri[1], v2 ); + v3_normalize( v0 ); + v3_normalize( v1 ); + v3_normalize( v2 ); + v3_cross( v0, v1, n ); + v3_cross( v0, n, e0 ); + v3_cross( n, v1, e1 ); + v3_cross( v2, n, e2 ); + + /* normal */ + if(!rb_box_triangle_interval( extent, n, tri )) return 0; + + v3f axis[9]; + v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] ); + v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] ); + v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] ); + v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] ); + v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] ); + v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] ); + v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] ); + v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] ); + v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] ); + + for( int i=0; i<9; i++ ) + if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0; + + return 1; +} + 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; @@ -1097,123 +1195,127 @@ static int rb_box_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) 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 ); + if( rb_box_triangle_sat( rba, tri ) ) + { + vg_line(tri[0],tri[1],0xff50ff00 ); + vg_line(tri[1],tri[2],0xff50ff00 ); + vg_line(tri[2],tri[0],0xff50ff00 ); + } + else + { + vg_line(tri[0],tri[1],0xff0000ff ); + vg_line(tri[1],tri[2],0xff0000ff ); + vg_line(tri[2],tri[0],0xff0000ff ); - //count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count ); - // - - /* TODO: SAT test first */ - - /* - * each pair of faces on the box vs triangle normal - */ + continue; + } - v3f v0, v1; - v4f tn; + v3f v0,v1,n; + v3_sub( tri[1], tri[0], v0 ); + v3_sub( tri[2], tri[0], v1 ); + v3_cross( v0, v1, n ); + v3_normalize( n ); - v3_sub( tri[1],tri[0], v0 ); - v3_sub( tri[2],tri[0], v1 ); - v3_cross( v0, v1, tn ); - v3_normalize( tn ); + /* find best feature */ + float best = v3_dot( rba->right, n ); + int axis = 0; - 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; jup, n ); + if( fabsf(cy) > fabsf(best) ) { - 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 ++; - } - } + best = cy; + axis = 1; + } + + /* TODO: THIS IS WRONG DIRECTION */ + float cz = -v3_dot( rba->forward, n ); + if( fabsf(cz) > fabsf(best) ) + { + best = cz; + axis = 2; } - } -#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]; + v3f manifold[4]; - int count = 0; - for( int i=0; i<8; i++ ) - { - m4x3_mulv( rba->to_world, pts[i], pts[i] ); + if( axis == 0 ) + { + float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0]; + manifold[0][0] = px; + manifold[0][1] = rba->bbx[0][1]; + manifold[0][2] = rba->bbx[0][2]; + manifold[1][0] = px; + manifold[1][1] = rba->bbx[1][1]; + manifold[1][2] = rba->bbx[0][2]; + manifold[2][0] = px; + manifold[2][1] = rba->bbx[1][1]; + manifold[2][2] = rba->bbx[1][2]; + manifold[3][0] = px; + manifold[3][1] = rba->bbx[0][1]; + manifold[3][2] = rba->bbx[1][2]; + } + else if( axis == 1 ) + { + float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1]; + manifold[0][0] = rba->bbx[0][0]; + manifold[0][1] = py; + manifold[0][2] = rba->bbx[0][2]; + manifold[1][0] = rba->bbx[1][0]; + manifold[1][1] = py; + manifold[1][2] = rba->bbx[0][2]; + manifold[2][0] = rba->bbx[1][0]; + manifold[2][1] = py; + manifold[2][2] = rba->bbx[1][2]; + manifold[3][0] = rba->bbx[0][0]; + manifold[3][1] = py; + manifold[3][2] = rba->bbx[1][2]; + } + else + { + float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2]; + manifold[0][0] = rba->bbx[0][0]; + manifold[0][1] = rba->bbx[0][1]; + manifold[0][2] = pz; + manifold[1][0] = rba->bbx[1][0]; + manifold[1][1] = rba->bbx[0][1]; + manifold[1][2] = pz; + manifold[2][0] = rba->bbx[1][0]; + manifold[2][1] = rba->bbx[1][1]; + manifold[2][2] = pz; + manifold[3][0] = rba->bbx[0][0]; + manifold[3][1] = rba->bbx[1][1]; + manifold[3][2] = pz; + } + + for( int j=0; j<4; j++ ) + m4x3_mulv( rba->to_world, manifold[j], manifold[j] ); - vg_line_pt3( pts[i], 0.1f, 0xffff00ff ); + vg_line( manifold[0], manifold[1], 0xffffffff ); + vg_line( manifold[1], manifold[2], 0xffffffff ); + vg_line( manifold[2], manifold[3], 0xffffffff ); + vg_line( manifold[3], manifold[0], 0xffffffff ); - if( pts[i][1] < 0.0f ) + for( int j=0; j<4; j++ ) { 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]; + v3_copy( manifold[j], ct->co ); + v3_copy( n, ct->n ); + + float l0 = v3_dot( tri[0], n ), + l1 = v3_dot( manifold[j], n ); + + ct->p = (l0-l1)*0.5f; + if( ct->p < 0.0f ) + continue; + ct->rba = rba; ct->rbb = rbb; count ++; + + if( count >= 12 ) + return count; } } - -#endif - return count; } @@ -1516,13 +1618,42 @@ static void rb_presolve_contacts( rb_ct *buffer, int len ) for( int i=0; ibias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f); + ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.01f); 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); + + v3f ra, rb, raCn, rbCn, raCt, rbCt; + v3_sub( ct->co, ct->rba->co, ra ); + v3_sub( ct->co, ct->rbb->co, rb ); + v3_cross( ra, ct->n, raCn ); + v3_cross( rb, ct->n, rbCn ); + + /* orient inverse inertia tensors */ + v3f raCnI, rbCnI; + m3x3_mulv( ct->rba->iIw, raCn, raCnI ); + m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI ); + + ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass; + ct->normal_mass += v3_dot( raCn, raCnI ); + ct->normal_mass += v3_dot( rbCn, rbCnI ); + ct->normal_mass = 1.0f/ct->normal_mass; + + for( int j=0; j<2; j++ ) + { + v3f raCtI, rbCtI; + v3_cross( ct->t[j], ra, raCt ); + v3_cross( ct->t[j], rb, rbCt ); + m3x3_mulv( ct->rba->iIw, raCt, raCtI ); + m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI ); + + ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass; + ct->tangent_mass[j] += v3_dot( raCt, raCtI ); + ct->tangent_mass[j] += v3_dot( rbCt, rbCtI ); + ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j]; + } rb_debug_contact( ct ); } @@ -1556,18 +1687,17 @@ static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse ) rigidbody *rba = ct->rba, *rbb = ct->rbb; - v3f ia, ib; - v3_muls( impulse, ct->mass_total*rba->inv_mass, ia ); - v3_muls( impulse, -ct->mass_total*rbb->inv_mass, ib ); - - /* response */ - v3_add( rba->v, ia, rba->v ); - v3_add( rbb->v, ib, rbb->v ); + v3_muladds( rba->v, impulse, rba->inv_mass, rba->v ); + v3_muladds( rbb->v, impulse, -rbb->inv_mass, rbb->v ); /* Angular velocity */ - v3f wa, wb; - v3_cross( da, ia, wa ); - v3_cross( db, ib, wb ); + v3f wa, wb, invim; + v3_cross( da, impulse, wa ); + v3_negate( impulse, invim ); + v3_cross( db, invim, wb ); + + m3x3_mulv( ct->rba->iIw, wa, wa ); + m3x3_mulv( ct->rbb->iIw, wb, wb ); v3_add( rba->w, wa, rba->w ); v3_add( rbb->w, wb, rbb->w ); } @@ -1577,9 +1707,8 @@ static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse ) */ static void rb_solve_contacts( rb_ct *buf, int len ) { - float k_friction = 0.5f; + float k_friction = 0.2f; - /* Friction Impulse */ for( int i=0; inorm_impulse, - vt = -v3_dot( rv, ct->t[j] ); + float f = k_friction * ct->norm_impulse, + vt = v3_dot( rv, ct->t[j] ), + lambda = ct->tangent_mass[j] * -vt; float temp = ct->tangent_impulse[j]; - ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f ); - vt = ct->tangent_impulse[j] - temp; + ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f ); + lambda = ct->tangent_impulse[j] - temp; v3f impulse; - v3_muls( ct->t[j], vt, impulse ); + v3_muls( ct->t[j], lambda, impulse ); rb_standard_impulse( ct, da, db, impulse ); } - } - - /* Normal Impulse */ - for( int i=0; irba, - *rbb = ct->rbb; - v3f rv, da, db; + /* Normal */ rb_rcv( ct, rv, da, db ); - - float vn = -v3_dot( rv, ct->n ) + ct->bias; + float vn = v3_dot( rv, ct->n ), + lambda = ct->normal_mass * (-vn + ct->bias); float temp = ct->norm_impulse; - ct->norm_impulse = vg_maxf( temp + vn, 0.0f ); - vn = ct->norm_impulse - temp; + ct->norm_impulse = vg_maxf( temp + lambda, 0.0f ); + lambda = ct->norm_impulse - temp; v3f impulse; - v3_muls( ct->n, vn, impulse ); + v3_muls( ct->n, lambda, impulse ); rb_standard_impulse( ct, da, db, impulse ); } } diff --git a/skeleton.h b/skeleton.h index ce71283..dc50fe9 100644 --- a/skeleton.h +++ b/skeleton.h @@ -19,6 +19,9 @@ struct skeleton mdl_keyframe kf; + int collider; + boxf hitbox; + char name[16]; } *bones; @@ -42,6 +45,7 @@ struct skeleton u32 bone_count, ik_count, + collider_count, anim_count, bindable_count; /* TODO: try to place IK last in the rig from export so that we dont always upload transforms for @@ -385,7 +389,7 @@ static struct skeleton_anim *skeleton_get_anim( struct skeleton *skele, /* Setup an animated skeleton from model */ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl ) { - u32 bone_count = 1, skeleton_root = 0, ik_count = 0; + u32 bone_count = 1, skeleton_root = 0, ik_count = 0, collider_count = 0; skele->bone_count = 0; skele->bones = NULL; skele->final_mtx = NULL; @@ -408,14 +412,14 @@ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl ) skele->bone_count = inf->channels; skele->ik_count = inf->ik_count; + skele->collider_count = inf->collider_count; skele->bones = malloc(sizeof(struct skeleton_bone)*skele->bone_count); skele->ik = malloc(sizeof(struct skeleton_ik)*skele->ik_count); skeleton_root = i; } else if( skele->bone_count ) { - int is_ik = pnode->classtype == k_classtype_ik_bone, - is_bone = (pnode->classtype == k_classtype_bone) || is_ik; + int is_bone = pnode->classtype == k_classtype_bone; if( is_bone ) { @@ -429,16 +433,17 @@ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl ) } struct skeleton_bone *sb = &skele->bones[bone_count]; + struct classtype_bone *bone_inf = mdl_get_entdata( mdl, pnode ); + int is_ik = bone_inf->ik_target; v3_copy( pnode->co, sb->co ); v3_copy( pnode->s, sb->end ); sb->parent = pnode->parent-skeleton_root; strncpy( sb->name, mdl_pstr(mdl,pnode->pstr_name), 15 ); + sb->deform = bone_inf->deform; if( is_ik ) { - struct classtype_ik_bone *ik_inf = mdl_get_entdata( mdl, pnode ); - sb->deform = ik_inf->deform; sb->ik = 1; /* TODO: place into new IK array */ skele->bones[ sb->parent ].ik = 1; @@ -451,16 +456,28 @@ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl ) struct skeleton_ik *ik = &skele->ik[ ik_count ++ ]; ik->upper = bone_count; ik->lower = sb->parent; - ik->target = ik_inf->target; - ik->pole = ik_inf->pole; + ik->target = bone_inf->ik_target; + ik->pole = bone_inf->ik_pole; } else { - struct classtype_bone *bone_inf = mdl_get_entdata( mdl, pnode ); - sb->deform = bone_inf->deform; sb->ik = 0; } + sb->collider = bone_inf->collider; + box_copy( bone_inf->hitbox, sb->hitbox ); + + if( bone_inf->collider ) + { + if( collider_count == skele->collider_count ) + { + vg_error( "Too many collider bones\n" ); + goto error_dealloc; + } + + collider_count ++; + } + bone_count ++; } else @@ -476,6 +493,13 @@ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl ) return 0; } + if( collider_count != skele->collider_count ) + { + vg_error( "Loaded %u colliders out of %u\n", collider_count, + skele->collider_count ); + goto error_dealloc; + } + if( bone_count != skele->bone_count ) { vg_error( "Loaded %u bones out of %u\n", bone_count, skele->bone_count ); @@ -492,6 +516,7 @@ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl ) v3_zero( skele->bones[0].co ); v3_copy( (v3f){0.0f,1.0f,0.0f}, skele->bones[0].end ); skele->bones[0].parent = 0xffffffff; + skele->bones[0].collider = 0; skele->final_mtx = malloc( sizeof(m4x3f) * skele->bone_count ); skele->anim_count = inf->anim_count; @@ -516,6 +541,7 @@ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl ) skeleton_create_inverses( skele ); vg_success( "Loaded skeleton with %u bones\n", skele->bone_count ); + vg_success( " %u colliders\n", skele->collider_count ); return 1; error_dealloc: