#include "vg/vg.h"
#include "model.h"
+#include "scene.h"
#include "ik.h"
+#include "rigidbody.h"
SHADER_DEFINE( shader_player,
FOREACH_PART( MAKE_STRING )
};
-
struct character
{
glmesh mesh;
/* Auxillary information */
v3f offsets[ PART_COUNT ];
+ rigidbody ragdoll[ PART_COUNT ];
/*
* Controls
SHADER_INIT(shader_player);
}
+
+/*
+ * Ragdoll Stuff
+ */
+
+static void character_rd_box( struct character *ch, enum character_part id,
+ v3f dims )
+{
+ v3_muls( dims, -0.5f, ch->ragdoll[id].bbx[0] );
+ v3_muls( dims, 0.5f, ch->ragdoll[id].bbx[1] );
+}
+
+struct rd_joint
+{
+ enum character_part ia, ib;
+ v3f lca, lcb;
+
+ struct rd_joint_axis
+ {
+ v3f va, vb;
+ float spring, ang;
+ }
+ min, maj;
+};
+
+static const float k_human_major = 0.9f,
+ k_human_minor = 0.9f,
+ k_human_major_max = 1.4f,
+ k_human_minor_max = 0.4f;
+
+#define HUMAN_VERTICAL_DEFAULT \
+.min = { \
+ .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f}, \
+ .spring = k_human_minor, .ang = k_human_minor_max \
+}, \
+.maj = { \
+ .va = {0.0f,1.0f,0.0f}, .vb = {0.0f,1.0f,0.0f}, \
+ .spring = k_human_major, .ang = k_human_major_max \
+}
+
+#define HUMAN_ARM_LEFT \
+.min = { \
+ .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f}, \
+ .spring = k_human_minor, .ang = k_human_minor_max \
+}, \
+.maj = { \
+ .va = {0.0f,0.0f,1.0f}, .vb = {0.0f,0.0f,1.0f}, \
+ .spring = k_human_major, .ang = k_human_major_max \
+}
+
+#define HUMAN_ARM_RIGHT \
+.min = { \
+ .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f}, \
+ .spring = k_human_minor, .ang = k_human_minor_max \
+}, \
+.maj = { \
+ .va = {0.0f,0.0f,-1.0f}, .vb = {0.0f,0.0f,-1.0f}, \
+ .spring = k_human_major, .ang = k_human_major_max \
+}
+
+static struct rd_joint rd_joints[] =
+{
+ { .ia = k_chpart_body0, .ib = k_chpart_body1, HUMAN_VERTICAL_DEFAULT },
+ { .ia = k_chpart_body1, .ib = k_chpart_neck, HUMAN_VERTICAL_DEFAULT },
+ { .ia = k_chpart_neck, .ib = k_chpart_head, HUMAN_VERTICAL_DEFAULT },
+ { .ia = k_chpart_body0, .ib = k_chpart_leg_l0, HUMAN_VERTICAL_DEFAULT },
+ { .ia = k_chpart_leg_l0, .ib = k_chpart_leg_l1, HUMAN_VERTICAL_DEFAULT },
+ { .ia = k_chpart_body0, .ib = k_chpart_leg_r0, HUMAN_VERTICAL_DEFAULT },
+ { .ia = k_chpart_leg_r0, .ib = k_chpart_leg_r1, HUMAN_VERTICAL_DEFAULT },
+
+ { .ia = k_chpart_body1, .ib = k_chpart_arm_l0, HUMAN_ARM_LEFT },
+ { .ia = k_chpart_arm_l0, .ib = k_chpart_arm_l1, HUMAN_ARM_LEFT },
+ { .ia = k_chpart_arm_l1, .ib = k_chpart_hand_l, HUMAN_ARM_LEFT },
+
+ { .ia = k_chpart_body1, .ib = k_chpart_arm_r0, HUMAN_ARM_RIGHT },
+ { .ia = k_chpart_arm_r0, .ib = k_chpart_arm_r1, HUMAN_ARM_RIGHT },
+ { .ia = k_chpart_arm_r1, .ib = k_chpart_hand_r, HUMAN_ARM_RIGHT },
+};
+
+static void character_init_ragdoll_joints( struct character *ch )
+{
+ for( int i=0; i<vg_list_size(rd_joints); i++ )
+ {
+ struct rd_joint *joint = &rd_joints[i];
+
+ float *hinge = ch->parts[joint->ib].pivot;
+ v3_sub( hinge, ch->ragdoll[joint->ia].co, joint->lca );
+ v3_sub( hinge, ch->ragdoll[joint->ib].co, joint->lcb );
+ }
+}
+
+static void character_init_ragdoll( struct character *ch )
+{
+ v3f *offs = ch->offsets;
+ rigidbody *rbs = ch->ragdoll;
+
+ /* CHest */
+ float chest_width = fabsf(offs[k_chpart_arm_r0][2])*2.0f,
+ chest_depth = chest_width * 0.571f,
+ chest_height = offs[k_chpart_neck][1];
+ v3f chest_dims = { chest_depth, chest_height, chest_width };
+ character_rd_box( ch, k_chpart_body1, chest_dims );
+
+ v3_copy( ch->parts[k_chpart_body1].pivot, rbs[k_chpart_body1].co );
+ rbs[k_chpart_body1].co[1] += chest_height*0.5f;
+
+ /* Torso */
+ v3f torso_dims = { chest_depth,
+ offs[k_chpart_body1][1]-offs[k_chpart_leg_l0][1],
+ chest_width*0.85f };
+ v3_copy( ch->parts[k_chpart_body0].pivot, rbs[k_chpart_body0].co );
+ character_rd_box( ch, k_chpart_body0, torso_dims );
+
+ /* Neck */
+ v3f neck_dims = { chest_depth*0.5f,
+ offs[k_chpart_head][1],
+ chest_depth*0.5f };
+ v3_copy( ch->parts[k_chpart_neck].pivot, rbs[k_chpart_neck].co );
+ rbs[k_chpart_neck].co[1] += neck_dims[1]*0.5f;
+ character_rd_box( ch, k_chpart_neck, neck_dims );
+
+ /* Head */
+ v3f head_dims = { chest_width*0.5f, chest_width*0.5f, chest_width*0.5f };
+ v3_copy( ch->parts[k_chpart_head].pivot, rbs[k_chpart_head].co );
+ rbs[k_chpart_head].co[1] += head_dims[1]*0.5f;
+ character_rd_box( ch, k_chpart_head, head_dims );
+
+ /* ARms */
+ v3f ua_dims = { 0.0f, 0.0f, fabsf(offs[k_chpart_arm_l1][2]) };
+ ua_dims[1] = 0.38f*ua_dims[2];
+ ua_dims[0] = 0.38f*ua_dims[2];
+ v3f la_dims = { ua_dims[0], ua_dims[1], fabsf(offs[k_chpart_hand_l][2]) };
+ v3f hand_dims = { ua_dims[1], ua_dims[1]*0.5f, ua_dims[1] };
+
+ character_rd_box( ch, k_chpart_arm_l0, ua_dims );
+ character_rd_box( ch, k_chpart_arm_r0, ua_dims );
+ character_rd_box( ch, k_chpart_arm_l1, la_dims );
+ character_rd_box( ch, k_chpart_arm_r1, la_dims );
+ character_rd_box( ch, k_chpart_hand_l, hand_dims );
+ character_rd_box( ch, k_chpart_hand_r, hand_dims );
+
+ v3_copy( ch->parts[k_chpart_arm_l0].pivot, rbs[k_chpart_arm_l0].co );
+ rbs[k_chpart_arm_l0].co[2] += ua_dims[2] * 0.5f;
+ v3_copy( ch->parts[k_chpart_arm_l1].pivot, rbs[k_chpart_arm_l1].co );
+ rbs[k_chpart_arm_l1].co[2] += la_dims[2] * 0.5f;
+ v3_copy( ch->parts[k_chpart_hand_l].pivot, rbs[k_chpart_hand_l].co );
+ rbs[k_chpart_hand_l].co[2] += hand_dims[2] * 0.5f;
+
+ v3_copy( ch->parts[k_chpart_arm_r0].pivot, rbs[k_chpart_arm_r0].co );
+ rbs[k_chpart_arm_r0].co[2] -= ua_dims[2] * 0.5f;
+ v3_copy( ch->parts[k_chpart_arm_r1].pivot, rbs[k_chpart_arm_r1].co );
+ rbs[k_chpart_arm_r1].co[2] -= la_dims[2] * 0.5f;
+ v3_copy( ch->parts[k_chpart_hand_r].pivot, rbs[k_chpart_hand_r].co );
+ rbs[k_chpart_hand_r].co[2] -= hand_dims[2] * 0.5f;
+
+ /* LEgs */
+ v3f ul_dims = { 0.0f, fabsf(offs[k_chpart_leg_l1][1]), 0.0f };
+ ul_dims[0] = 0.38f*ul_dims[1];
+ ul_dims[2] = 0.38f*ul_dims[1];
+ v3f ll_dims = { ul_dims[0], fabsf(offs[k_chpart_foot_l][1]), ul_dims[2] };
+ v3f foot_dims = { 2.0f*ul_dims[0], ul_dims[0], ul_dims[0] };
+
+ character_rd_box( ch, k_chpart_leg_l0, ul_dims );
+ character_rd_box( ch, k_chpart_leg_r0, ul_dims );
+ character_rd_box( ch, k_chpart_leg_l1, ll_dims );
+ character_rd_box( ch, k_chpart_leg_r1, ll_dims );
+ character_rd_box( ch, k_chpart_foot_l, foot_dims );
+ character_rd_box( ch, k_chpart_foot_r, foot_dims );
+
+ v3_copy( ch->parts[k_chpart_leg_l0].pivot, rbs[k_chpart_leg_l0].co );
+ rbs[k_chpart_leg_l0].co[1] -= ul_dims[1] * 0.5f;
+ v3_copy( ch->parts[k_chpart_leg_l1].pivot, rbs[k_chpart_leg_l1].co );
+ rbs[k_chpart_leg_l1].co[1] -= ll_dims[1] * 0.5f;
+ v3_copy( ch->parts[k_chpart_foot_l].pivot, rbs[k_chpart_foot_l].co );
+ rbs[k_chpart_foot_l].co[1] -= foot_dims[1] * 0.5f;
+ rbs[k_chpart_foot_l].co[0] -= foot_dims[0] * 0.5f;
+
+ v3_copy( ch->parts[k_chpart_leg_r0].pivot, rbs[k_chpart_leg_r0].co );
+ rbs[k_chpart_leg_r0].co[1] -= ul_dims[1] * 0.5f;
+ v3_copy( ch->parts[k_chpart_leg_r1].pivot, rbs[k_chpart_leg_r1].co );
+ rbs[k_chpart_leg_r1].co[1] -= ll_dims[1] * 0.5f;
+ v3_copy( ch->parts[k_chpart_foot_r].pivot, rbs[k_chpart_foot_r].co );
+ rbs[k_chpart_foot_r].co[1] -= foot_dims[1] * 0.5f;
+ rbs[k_chpart_foot_r].co[0] -= foot_dims[0] * 0.5f;
+
+ character_init_ragdoll_joints( ch );
+
+ for( int i=0; i<PART_COUNT; i++ )
+ rb_init( &ch->ragdoll[i] );
+}
+
+static void character_ragdoll_go( struct character *ch, v3f pos )
+{
+ character_init_ragdoll( ch );
+ for( int i=0; i<PART_COUNT; i++ )
+ v3_add( pos, ch->ragdoll[i].co, ch->ragdoll[i].co );
+}
+
+static void character_ragdoll_copypose( struct character *ch, v3f v )
+{
+ character_init_ragdoll(ch);
+
+ for( int i=0; i<PART_COUNT; i++ )
+ {
+ v3f offset;
+ rigidbody *rb = &ch->ragdoll[i];
+
+ v3_sub( rb->co, ch->parts[i].pivot, offset );
+
+ m4x3_mulv( ch->matrices[i], offset, rb->co );
+ m3x3_q( ch->matrices[i], rb->q );
+ v3_copy( v, rb->v );
+ v3_zero( rb->I );
+ rb->manifold_count = 0; /* ? */
+
+ rb_update_transform( rb );
+ }
+}
+
+static void character_debug_ragdoll( struct character *ch )
+{
+ rb_debug( &ch->ragdoll[k_chpart_body0], 0xffffffff );
+ rb_debug( &ch->ragdoll[k_chpart_body1], 0xffffffff );
+ rb_debug( &ch->ragdoll[k_chpart_neck], 0xff00ff00 );
+ rb_debug( &ch->ragdoll[k_chpart_head], 0xff00ff00 );
+
+ rb_debug( &ch->ragdoll[k_chpart_arm_l0], 0xffffa500 );
+ rb_debug( &ch->ragdoll[k_chpart_arm_l1], 0xffffa500 );
+ rb_debug( &ch->ragdoll[k_chpart_hand_l], 0xffffa500 );
+
+ rb_debug( &ch->ragdoll[k_chpart_arm_r0], 0xff00a5ff );
+ rb_debug( &ch->ragdoll[k_chpart_arm_r1], 0xff00a5ff );
+ rb_debug( &ch->ragdoll[k_chpart_hand_r], 0xff00a5ff );
+
+ rb_debug( &ch->ragdoll[k_chpart_leg_l0], 0xffffa500 );
+ rb_debug( &ch->ragdoll[k_chpart_leg_l1], 0xffffa500 );
+ rb_debug( &ch->ragdoll[k_chpart_foot_l], 0xffffa500 );
+ rb_debug( &ch->ragdoll[k_chpart_leg_r0], 0xff00a5ff );
+ rb_debug( &ch->ragdoll[k_chpart_leg_r1], 0xff00a5ff );
+ rb_debug( &ch->ragdoll[k_chpart_foot_r], 0xff00a5ff );
+}
+
+static void character_ragdoll_iter( struct character *ch, scene *sc )
+{
+ for( int i=0; i<PART_COUNT; i++ )
+ {
+ rb_build_manifold( &ch->ragdoll[i], sc );
+ }
+
+ for( int i=0; i<20; i++ )
+ {
+ float const k_springfactor = 1.0f/20.0f;
+
+ for( int j=0; j<PART_COUNT; j++ )
+ rb_constraint_manifold( &ch->ragdoll[j] );
+
+ for( int j=0; j<vg_list_size(rd_joints); j++ )
+ {
+ struct rd_joint *joint = &rd_joints[j];
+ rigidbody *rba = &ch->ragdoll[joint->ia],
+ *rbb = &ch->ragdoll[joint->ib];
+
+ rb_constraint_position( rba, joint->lca, rbb, joint->lcb );
+ rb_constraint_angle( rba, joint->maj.va, rbb, joint->maj.vb,
+ joint->maj.ang,
+ joint->maj.spring * k_springfactor );
+
+ rb_constraint_angle( rba, joint->min.va, rbb, joint->min.vb,
+ joint->min.ang,
+ joint->min.spring * k_springfactor );
+ }
+ }
+
+ for( int i=0; i<PART_COUNT; i++ )
+ rb_iter( &ch->ragdoll[i] );
+
+ for( int i=0; i<PART_COUNT; i++ )
+ rb_update_transform( &ch->ragdoll[i] );
+}
+
#endif
#include "ik.h"
#include "character.h"
#include "terrain.h"
+#include "ragdoll.h"
+#include "rigidbody.h"
int main( int argc, char *argv[] )
{
float pitch;
v3f land_target;
- v3f land_target_log[12];
+ v3f land_target_log[22];
int land_log_count;
m3x3f vr;
}
render;
+rigidbody mr_box = {
+ .bbx = {{ -1.0f, -0.25f, -0.25f }, { 1.0f, 0.25f, 0.25f }}
+};
+
+rigidbody mrs_box = {
+ .bbx = {{ -0.5f, -0.25f, -0.25f }, { 0.5f, 0.25f, 0.25f }}
+};
+
static void player_transform_update(void)
{
+ q_normalize( player.rot );
q_m3x3( player.rot, player.to_world );
v3_copy( player.co, player.to_world[3] );
replay_buffer = malloc( sizeof(m4x3f) * REPLAY_LENGTH * (PART_COUNT) );
vg_tex2d_init( texture_list, vg_list_size( texture_list ) );
-
+
+ rb_init( &mr_box );
+ rb_init( &mrs_box );
+ mrs_box.co[2] += 2.0f;
+
vg_convar_push( (struct vg_convar){
.name = "frame",
.data = &replay_buffer_frame,
v3_normalize( lightDir );
character_load( &player.mdl, "ch_default" );
+ character_init_ragdoll( &player.mdl );
/* Setup scene */
scene_init( &world.geo );
scene_add_model( &world.geo, mworld, submodel_get( mworld, "mp_dev" ),
(v3f){0.0f,0.0f,0.0f}, 0.0f, 1.0f );
+ scene_add_model( &world.geo, mworld, submodel_get( mworld, "terrain" ),
+ (v3f){0.0f,0.0f,0.0f}, 0.0f, 1.0f );
+
free( mworld );
scene_upload( &world.geo );
bvh_create( &world.geo );
m3x3_identity( player.vr );
- for( int m=0;m<=5; m++ )
+ for( int m=-3;m<=12; m++ )
{
- float vmod = ((float)m / 5.0f)*0.09f;
+ float vmod = ((float)m / 15.0f)*0.09f;
v3f pco, pco1, pv;
v3_copy( player.co, pco );
* Getting surface collision points,
* the contact manifold is a triangle for simplicity.
*/
- v3f contact_front, contact_back, fwd, fwd1, contact_norm, vup, vside,
+ v3f contact_front, contact_back, contact_norm, vup, vside,
axis;
float klength = 0.65f;
- m3x3_mulv( player.to_world, (v3f){ 0.0f, 0.0f,-1.0f}, fwd );
m4x3_mulv( player.to_world, (v3f){ 0.15f,0.0f,-klength}, contact_norm );
m4x3_mulv( player.to_world, (v3f){-0.15f,0.0f,-klength}, contact_front );
m4x3_mulv( player.to_world, (v3f){ 0.00f,0.0f, klength}, contact_back );
}
float resistance = v3_dot( norm, player.v );
-
if( resistance >= 0.0f )
{
player_start_air();
{
static int clock = 0;
- clock ++;
- if( clock >= clock_divider )
- clock = 0;
- else
- return;
-
/* temp */
if( freecam )
{
player_freecam();
return;
}
+
+ clock ++;
+ if( clock >= clock_divider )
+ clock = 0;
+ else
+ return;
if( vg_get_axis("grabl")>0.0f)
reset_player(0,NULL);
void vg_update(void)
{
player_update();
- bvh_debug( &world.geo, player.co );
+
+ //rb_torque( &mr_box, (v3f){0.0f,0.0f,1.0f}, 0.01f );
+
+ if( glfwGetKey( vg_window, GLFW_KEY_F ) )
+ character_ragdoll_go( &player.mdl, player.view );
+
+ if( glfwGetKey( vg_window, GLFW_KEY_G ) )
+ character_ragdoll_copypose( &player.mdl, player.v );
+
+ static int clock = 0;
+
+ clock ++;
+ if( clock >= clock_divider )
+ {
+ character_debug_ragdoll( &player.mdl );
+ character_ragdoll_iter( &player.mdl, &world.geo );
+
+
+ rb_build_manifold( &mr_box, &world.geo );
+ rb_build_manifold( &mrs_box, &world.geo );
+ rb_constraint_manifold( &mr_box );
+ rb_constraint_manifold( &mrs_box );
+
+ rb_iter( &mr_box );
+ rb_iter( &mrs_box );
+
+ rb_debug( &mr_box, 0xffffffff );
+ rb_debug( &mrs_box, 0xff00ff00 );
+
+ rb_update_transform( &mr_box );
+ rb_update_transform( &mrs_box );
+
+ clock = 0;
+ }
+
}
static void player_animate(void)
glBindFramebuffer( GL_FRAMEBUFFER, 0 );
glClearColor( 0.0f, 0.0f, 0.0f, 0.0f );
-#if 1
+#if 0
glClear( GL_COLOR_BUFFER_BIT );
#else
glClear( GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT );
#endif
- if( !freecam )
draw_player();
/* Draw back in the background */
--- /dev/null
+#ifndef RAGDOLL_H
+#define RAGDOLL_H
+
+#include "vg/vg.h"
+#include "scene.h"
+
+
+
+#endif
--- /dev/null
+/* SHite fphysics */
+
+#ifndef RIGIDBODY_H
+#define RIGIDBODY_H
+
+#include "vg/vg.h"
+#include "scene.h"
+
+#define k_rb_delta (1.0f/60.0f)
+
+typedef struct rigidbody rigidbody;
+struct rigidbody
+{
+ v3f co, v, I;
+ v4f q;
+ boxf bbx;
+
+ struct contact
+ {
+ v3f co, n, delta;
+ v3f t[2];
+ float bias, norm_impulse, tangent_impulse[2];
+ }
+ manifold[4];
+ int manifold_count;
+
+ m4x3f to_world, to_local;
+};
+
+static void rb_update_transform( rigidbody *rb )
+{
+ q_normalize( rb->q );
+ q_m3x3( rb->q, rb->to_world );
+ v3_copy( rb->co, rb->to_world[3] );
+
+ m4x3_invert_affine( rb->to_world, rb->to_local );
+}
+
+static void rb_init( rigidbody *rb )
+{
+ q_identity( rb->q );
+ v3_zero( rb->v );
+ v3_zero( rb->I );
+
+ rb_update_transform( rb );
+}
+
+static void rb_iter( rigidbody *rb )
+{
+ v3f gravity = { 0.0f, -9.6f, 0.0f };
+ v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
+
+ /* intergrate velocity */
+ v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
+
+ v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
+
+ /* inegrate inertia */
+ if( v3_length2( rb->I ) > 0.0f )
+ {
+ v4f rotation;
+ v3f axis;
+ v3_copy( rb->I, axis );
+
+ float mag = v3_length( axis );
+ v3_divs( axis, mag, axis );
+ q_axis_angle( rotation, axis, mag*k_rb_delta );
+ q_mul( rotation, rb->q, rb->q );
+ }
+}
+
+static void rb_torque( rigidbody *rb, v3f axis, float mag )
+{
+ v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I );
+}
+
+static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
+{
+ /* Compute tangent basis (box2d) */
+ if( fabsf( n[0] ) >= 0.57735027f )
+ {
+ tx[0] = n[1];
+ tx[1] = -n[0];
+ tx[2] = 0.0f;
+ }
+ else
+ {
+ tx[0] = 0.0f;
+ tx[1] = n[2];
+ tx[2] = -n[1];
+ }
+
+ v3_normalize( tx );
+ v3_cross( n, tx, ty );
+}
+
+static void rb_build_manifold( rigidbody *rb, scene *sc )
+{
+ v3f *box = rb->bbx;
+ 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]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
+ p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
+ p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
+ p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
+
+ p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
+ p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
+ p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
+ p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
+
+ m4x3_mulv( rb->to_world, p000, p000 );
+ m4x3_mulv( rb->to_world, p001, p001 );
+ m4x3_mulv( rb->to_world, p010, p010 );
+ m4x3_mulv( rb->to_world, p011, p011 );
+ m4x3_mulv( rb->to_world, p100, p100 );
+ m4x3_mulv( rb->to_world, p101, p101 );
+ m4x3_mulv( rb->to_world, p110, p110 );
+ m4x3_mulv( rb->to_world, p111, p111 );
+
+ rb->manifold_count = 0;
+
+ for( int i=0; i<8; i++ )
+ {
+ float *point = pts[i];
+ struct contact *ct = &rb->manifold[rb->manifold_count];
+
+ v3f surface;
+
+ v3_copy( point, surface );
+ bvh_scene_sample( sc, surface, ct->n );
+
+ float p = vg_minf( surface[1] - point[1], 1.0f );
+
+ if( p > 0.0f )
+ {
+ v3_add( point, surface, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+
+ //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
+
+ v3_sub( ct->co, rb->co, ct->delta );
+ 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;
+
+ rb->manifold_count ++;
+ if( rb->manifold_count == 4 )
+ break;
+ }
+ }
+}
+
+static void rb_constraint_manifold( rigidbody *rb )
+{
+ float k_friction = 0.07f;
+
+ /* Friction Impulse */
+ for( int i=0; i<rb->manifold_count; i++ )
+ {
+ struct contact *ct = &rb->manifold[i];
+
+ v3f dv;
+ v3_cross( rb->I, ct->delta, dv );
+ v3_add( rb->v, dv, dv );
+
+ for( int j=0; j<2; j++ )
+ {
+ float vt = vg_clampf( -v3_dot( dv, ct->t[j] ),
+ -k_friction, k_friction );
+
+ vt = -v3_dot( dv, ct->t[j] );
+
+ float temp = ct->tangent_impulse[j];
+ ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
+ vt = ct->tangent_impulse[j] - temp;
+
+ v3f impulse;
+
+ v3_muls( ct->t[j], vt, impulse );
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( ct->delta, impulse, impulse );
+ v3_add( impulse, rb->I, rb->I );
+ }
+ }
+
+ /* Normal Impulse */
+ for( int i=0; i<rb->manifold_count; i++ )
+ {
+ struct contact *ct = &rb->manifold[i];
+
+ v3f dv;
+ v3_cross( rb->I, ct->delta, dv );
+ v3_add( rb->v, dv, dv );
+
+ float vn = -v3_dot( dv, ct->n );
+ vn += ct->bias;
+
+ float temp = ct->norm_impulse;
+ ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
+ vn = ct->norm_impulse - temp;
+
+ v3f impulse;
+
+ v3_muls( ct->n, vn, impulse );
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( ct->delta, impulse, impulse );
+ v3_add( impulse, rb->I, rb->I );
+ }
+}
+
+static void rb_constraint_angle( rigidbody *rba, v3f va,
+ rigidbody *rbb, v3f vb,
+ float max, float spring )
+{
+ 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);
+
+ v3f axis;
+ v3_cross( wva, wvb, axis );
+ v3_muladds( rba->I, axis, ang*spring*0.5f, rba->I );
+ v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I );
+
+ return;
+
+ /* TODO: convert max into the dot product value so we dont have to always
+ * evaluate acosf, only if its greater than the angle specified */
+ ang = acosf( dt );
+ if( ang > max )
+ {
+ float correction = max-ang;
+
+ v4f rotation;
+ q_axis_angle( rotation, axis, -correction*0.125f );
+ q_mul( rotation, rba->q, rba->q );
+
+ q_axis_angle( rotation, axis, correction*0.125f );
+ q_mul( rotation, rbb->q, rbb->q );
+ }
+}
+
+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 );
+
+ v3f delta;
+ v3_add( wcb, rb->co, delta );
+ v3_sub( delta, wca, delta );
+ v3_sub( delta, ra->co, delta );
+
+ v3_muladds( ra->co, delta, 0.5f, ra->co );
+ v3_muladds( rb->co, delta, -0.5f, rb->co );
+
+ v3f rcv;
+ 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 );
+
+ v3f impulse;
+ v3_muls( rcv, 0.5f, impulse );
+ v3_add( impulse, rb->v, 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_cross( wca, impulse, impulse );
+ v3_add( impulse, ra->I, ra->I );
+
+
+#if 0
+ v3f impulse;
+ v3_muls( delta, 0.5f*spring, impulse );
+
+ v3_add( impulse, ra->v, ra->v );
+ v3_cross( wca, impulse, impulse );
+ v3_add( impulse, ra->I, ra->I );
+
+ v3_muls( delta, -0.5f*spring, impulse );
+
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( wcb, impulse, impulse );
+ v3_add( impulse, rb->I, rb->I );
+#endif
+}
+
+static void rb_debug( rigidbody *rb, u32 colour )
+{
+ v3f *box = rb->bbx;
+ v3f p000, p001, p010, p011, p100, p101, p110, p111;
+
+ p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
+ p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
+ p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
+ p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
+
+ p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
+ p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
+ p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
+ p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
+
+ m4x3_mulv( rb->to_world, p000, p000 );
+ m4x3_mulv( rb->to_world, p001, p001 );
+ m4x3_mulv( rb->to_world, p010, p010 );
+ m4x3_mulv( rb->to_world, p011, p011 );
+ m4x3_mulv( rb->to_world, p100, p100 );
+ m4x3_mulv( rb->to_world, p101, p101 );
+ m4x3_mulv( rb->to_world, p110, p110 );
+ m4x3_mulv( rb->to_world, p111, p111 );
+
+ vg_line( p000, p001, colour );
+ vg_line( p001, p011, colour );
+ vg_line( p011, p010, colour );
+ vg_line( p010, p000, colour );
+
+ vg_line( p100, p101, colour );
+ vg_line( p101, p111, colour );
+ vg_line( p111, p110, colour );
+ vg_line( p110, p100, colour );
+
+ vg_line( p100, p000, colour );
+ vg_line( p101, p001, colour );
+ vg_line( p110, p010, colour );
+ vg_line( p111, p011, colour );
+
+ vg_line( p000, p110, colour );
+ vg_line( p100, p010, colour );
+}
+
+#endif /* RIGIDBODY_H */
float split = node->bbx[0][axis] + extent[axis]*0.5f;
+ /* To beat: 121,687 / 136,579
+ * 136,375
+ */
+
+ float avg = 0.0;
+ for( u32 t=0; t<node->count; t++ )
+ {
+ u32 *ti = &s->indices[(node->start+t)*3];
+ float a = s->verts[ti[0]].co[axis],
+ b = s->verts[ti[1]].co[axis],
+ c = s->verts[ti[2]].co[axis];
+ avg += (a+b+c)/3.0;
+ }
+ avg /= (float)node->count;
+
+ split = avg;
+
i32 i = node->start,
j = i + node->count-1;
bvh_debug_node( s, 0, pos, 0x4000ffa8 );
}
-static int bvh_scene_sample_node( scene *s, u32 inode, v3f pos, v3f norm )
-{
- bvh_node *node = &s->bvh.nodes[ inode ];
-
- if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
- (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
- {
- if( !node->count )
- {
- if( bvh_scene_sample_node( s, node->il, pos, norm )) return 1;
- if( bvh_scene_sample_node( s, node->ir, pos, norm )) return 1;
- }
- else
- {
- for( u32 i=0; i<node->count; i++ )
- {
- u32 idx = (node->start+i)*3;
- model_vert *pa = &s->verts[ s->indices[ idx+0 ] ],
- *pb = &s->verts[ s->indices[ idx+1 ] ],
- *pc = &s->verts[ s->indices[ idx+2 ] ];
-
- float height;
- if( triangle_raycast2d( pa->co, pb->co, pc->co, pos, &height ))
- {
- pos[1] = height;
-
- if( norm )
- {
- v3f v0, v1;
- v3_sub( pa->co, pb->co, v0 );
- v3_sub( pc->co, pb->co, v1 );
- v3_cross( v1, v0, norm );
- v3_normalize( norm );
- }
-
- return 1;
- }
- }
- }
- }
-
- return 0;
-}
-
-static int bvh_scene_sample( scene *s, v3f pos, v3f norm)
-{
- return bvh_scene_sample_node( s, 0, pos, norm );
-}
-
typedef struct ray_hit ray_hit;
struct ray_hit
{
if( count )
{
- vg_line( co, pb, 0xff00ffff );
+ //vg_line( co, pb, 0xff00ffff );
v3f v0, v1;
v3_muladds( co, dir, hit->dist, hit->pos );
}
else
- vg_line( co, pb, 0xff0000ff );
+ {
+ //vg_line( co, pb, 0xff0000ff );
+ }
return count;
}
+static int bvh_scene_sample_node_h( scene *s, u32 inode, v3f pos, v3f norm )
+{
+ bvh_node *node = &s->bvh.nodes[ inode ];
+
+ if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
+ (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
+ {
+ if( !node->count )
+ {
+ if( bvh_scene_sample_node_h( s, node->il, pos, norm )) return 1;
+ if( bvh_scene_sample_node_h( s, node->ir, pos, norm )) return 1;
+ }
+ else
+ {
+ for( u32 i=0; i<node->count; i++ )
+ {
+ u32 idx = (node->start+i)*3;
+ model_vert *pa = &s->verts[ s->indices[ idx+0 ] ],
+ *pb = &s->verts[ s->indices[ idx+1 ] ],
+ *pc = &s->verts[ s->indices[ idx+2 ] ];
+
+ float height;
+ if( triangle_raycast2d( pa->co, pb->co, pc->co, pos, &height ))
+ {
+ pos[1] = height;
+
+ if( norm )
+ {
+ v3f v0, v1;
+ v3_sub( pa->co, pb->co, v0 );
+ v3_sub( pc->co, pb->co, v1 );
+ v3_cross( v1, v0, norm );
+ v3_normalize( norm );
+ }
+
+ return 1;
+ }
+ }
+ }
+ }
+
+ return 0;
+}
+
+static int bvh_scene_sample_h( scene *s, v3f pos, v3f norm)
+{
+ return bvh_scene_sample_node_h( s, 0, pos, norm );
+}
+
+static int bvh_scene_sample( scene *s, v3f pos, v3f norm )
+{
+ ray_hit hit;
+ hit.dist = INFINITY;
+
+ v3f ray_pos;
+ v3_add( pos, (v3f){0.0f,16000.0f,0.0f}, ray_pos );
+
+ if( bvh_raycast( s, ray_pos, (v3f){0.0f,-1.0f,0.0f}, &hit ))
+ {
+ pos[1] = hit.pos[1];
+
+ if( norm )
+ v3_copy( hit.normal, norm );
+
+ return 1;
+ }
+
+ return 0;
+}
+
#endif
projects / carveJwlIkooP6JGAAIwe30JlM.git / commitdiff
