From: hgn <hgodden00@gmail.com>
Date: Wed, 14 Sep 2022 15:18:06 +0000 (+0100)
Subject: angle limit constraint
X-Git-Url: https://harrygodden.com/git/?a=commitdiff_plain;h=b4c9550f206c476bb38b0bb2855d35e6b31bee83;p=carveJwlIkooP6JGAAIwe30JlM.git

angle limit constraint
---

diff --git a/blender_export.py b/blender_export.py
index edb758f..04fd6c3 100644
--- a/blender_export.py
+++ b/blender_export.py
@@ -154,6 +154,8 @@ class classtype_bone(Structure):
                ("ik_target",c_uint32),
                ("ik_pole",c_uint32),
                ("collider",c_uint32),
+               ("use_limits",c_uint32),
+               ("angle_limits",(c_float*3)*2),
                ("hitbox",(c_float*3)*2)]
 
 # Exporter
@@ -725,6 +727,23 @@ def write_model(collection_name):
             bone.hitbox[1][1] = 0.0
             bone.hitbox[1][2] = 0.0
 
+         if obj.cv_data.con0:
+            bone.use_limits = 1 
+            bone.angle_limits[0][0] =  obj.cv_data.mins[0]
+            bone.angle_limits[0][1] =  obj.cv_data.mins[2]
+            bone.angle_limits[0][2] = -obj.cv_data.maxs[1]
+            bone.angle_limits[1][0] =  obj.cv_data.maxs[0]
+            bone.angle_limits[1][1] =  obj.cv_data.maxs[2]
+            bone.angle_limits[1][2] = -obj.cv_data.mins[1]
+         else:
+            bone.use_limits = 0
+            bone.angle_limits[0][0] = 0.0
+            bone.angle_limits[0][1] = 0.0
+            bone.angle_limits[0][2] = 0.0
+            bone.angle_limits[1][0] = 0.0
+            bone.angle_limits[1][1] = 0.0
+            bone.angle_limits[1][2] = 0.0
+
          bone.deform = node_def['deform']
          entdata_buffer += [bone]
 
diff --git a/model.h b/model.h
index fe238e1..78e67c0 100644
--- a/model.h
+++ b/model.h
@@ -150,12 +150,12 @@ struct classtype_route
 struct classtype_bone
 {
    u32 deform,
-
        ik_target,
        ik_pole,
+       collider,
+       use_limits;
 
-       collider;
-
+   v3f angle_limits[2];
    boxf hitbox;
 };
 
diff --git a/models_src/ch_new.mdl b/models_src/ch_new.mdl
index 7b81848..0b041a3 100644
Binary files a/models_src/ch_new.mdl and b/models_src/ch_new.mdl differ
diff --git a/player_model.h b/player_model.h
index 542c7b2..a067b03 100644
--- a/player_model.h
+++ b/player_model.h
@@ -46,7 +46,12 @@ struct character
    {
       u32 bone_id;
       v3f offset;
-      rigidbody rb;
+
+      u32 use_limits;
+      v3f limits[2];
+
+      rigidbody  rb;
+      u32 parent;
    }
    *ragdoll;
    u32 ragdoll_count;
@@ -112,6 +117,27 @@ static int character_load( struct character *ch, const char *name )
             v3_add( bone->co, rp->offset, rp->rb.co );
             rp->rb.type = k_rb_shape_box;
             rp->rb.is_world = 0;
+            rp->parent = 0xffffffff;
+
+            if( bone->parent )
+            {
+               for( u32 j=0; j<ch->ragdoll_count; j++ )
+               {
+                  if( ch->ragdoll[ j ].bone_id == bone->parent )
+                  {
+                     rp->parent = j;
+                     break;
+                  }
+               }
+            }
+            
+            /* TODO: refactor to use this style elswhere */
+            struct mdl_node *pnode = mdl_node_from_id( src, bone->orig_node );
+            struct classtype_bone *bone_inf = mdl_get_entdata( src, pnode );
+
+            rp->use_limits = bone_inf->use_limits;
+            v3_copy( bone_inf->angle_limits[0], rp->limits[0] );
+            v3_copy( bone_inf->angle_limits[1], rp->limits[1] );
 
             rb_init( &rp->rb );
          }
@@ -169,10 +195,58 @@ static void character_ragdoll_iter( struct character *ch )
       if( ch->shoes[i] )
          shoe_vel[i] = v3_length( ch->ragdoll[i].rb.v );
    
+   for( int j=0; j<ch->ragdoll_count; j++ )
+   {
+      struct ragdoll_part *pj = &ch->ragdoll[j];
+      struct skeleton_bone *bj = &ch->sk.bones[pj->bone_id];
+
+      if( pj->parent != 0xffffffff )
+      {
+         struct ragdoll_part *pp = &ch->ragdoll[pj->parent];
+         struct skeleton_bone *bp = &ch->sk.bones[pp->bone_id];
+
+         v3f lca, lcb;
+         v3_negate( pj->offset, lca );
+         v3_add( bp->co, pp->offset, lcb );
+         v3_sub( bj->co, lcb, lcb );
+
+         rb_debug_constraint_position( &pj->rb, lca, &pp->rb, lcb );
+
+         if( pj->use_limits )
+         {
+            rb_debug_constraint_limits( &pj->rb, &pp->rb, lca, pj->limits );
+         }
+      }
+   }
+
    /* CONSTRAINTS */
-   for( int i=0; i<5; i++ )
+   for( int i=0; i<10; i++ )
    {
       rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+
+      for( int j=0; j<ch->ragdoll_count; j++ )
+      {
+         struct ragdoll_part *pj = &ch->ragdoll[j];
+         struct skeleton_bone *bj = &ch->sk.bones[pj->bone_id];
+
+         if( pj->parent != 0xffffffff )
+         {
+            struct ragdoll_part *pp = &ch->ragdoll[pj->parent];
+            struct skeleton_bone *bp = &ch->sk.bones[pp->bone_id];
+
+            v3f lca, lcb;
+            v3_negate( pj->offset, lca );
+            v3_add( bp->co, pp->offset, lcb );
+            v3_sub( bj->co, lcb, lcb );
+
+            rb_constraint_position( &pj->rb, lca, &pp->rb, lcb );
+
+            if( pj->use_limits )
+            {
+               rb_constraint_limits( &pj->rb, lca, &pp->rb, lcb, pj->limits );
+            }
+         }
+      }
    }
 
    /* INTEGRATION */
diff --git a/rigidbody.h b/rigidbody.h
index 32753d1..7f7187d 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -184,9 +184,10 @@ static void rb_init( rigidbody *rb )
       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];
+      float scale = 4.0f;
+      float ex2 = scale*extent[0]*extent[0],
+            ey2 = scale*extent[1]*extent[1],
+            ez2 = scale*extent[2]*extent[2];
 
       rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
       rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
@@ -1347,9 +1348,8 @@ static int rb_scene_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    return rb_box_vs_scene( rbb, rba, buf );
 }
 
-static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
-= {
-              /* box */         /* Sphere */         /* Capsule */         /* Mesh */
+static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )= 
+{             /* box */         /* Sphere */         /* Capsule */         /* Mesh */
 /*box    */ { RB_MATRIX_ERROR,  rb_box_vs_sphere,    rb_box_vs_capsule,    rb_box_vs_scene },
 /*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, rb_sphere_vs_scene },
 /*capsule*/ { rb_capsule_vs_box,rb_capsule_vs_sphere,rb_capsule_vs_capsule,RB_MATRIX_ERROR },
@@ -1682,6 +1682,8 @@ static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
 /*
  * Apply regular and angular velocity impulses to objects involved in contact
  */
+
+/* TODO REMOVEEE................... */
 static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
 {
    rigidbody *rba = ct->rba,
@@ -1702,6 +1704,20 @@ static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
    v3_add( rbb->w, wb, rbb->w );
 }
 
+/* ......... USE THIS */
+static void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
+{
+   /* linear */
+   v3_muladds( rb->v, impulse,  rb->inv_mass, rb->v );
+   
+   /* Angular velocity */
+   v3f wa;
+   v3_cross( delta, impulse, wa );
+
+   m3x3_mulv( rb->iIw, wa, wa );
+   v3_add( rb->w, wa, rb->w );
+}
+
 /*
  * One iteration to solve the contact constraint
  */
@@ -1792,7 +1808,7 @@ static int rb_angle_limit_force(  rigidbody *rba, v3f va,
 
 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 {
-
+   
 }
 
 static void rb_constraint_angle( rigidbody *rba, v3f va,
@@ -1829,20 +1845,153 @@ static void rb_constraint_angle( rigidbody *rba, v3f va,
    }
 }
 
-static void rb_relative_velocity( rigidbody *ra, v3f lca,
-                                  rigidbody *rb, v3f lcb, v3f rcv )
+static void draw_angle_limit( v3f c, v3f major, v3f minor, 
+                              float amin, float amax, float measured,
+                              u32 colour )
+{
+   float f = 0.05f;
+   v3f ay, ax;
+   v3_muls( major, f, ay );
+   v3_muls( minor, f, ax );
+
+   for( int x=0; x<16; x++ )
+   {
+      float t0 = (float)x / 16.0f,
+            t1 = (float)(x+1) / 16.0f,
+            a0 = vg_lerpf( amin, amax, t0 ),
+            a1 = vg_lerpf( amin, amax, t1 );
+
+      v3f p0, p1;
+      v3_muladds(  c, ay, cosf(a0), p0 );
+      v3_muladds( p0, ax, sinf(a0), p0 );
+      v3_muladds(  c, ay, cosf(a1), p1 );
+      v3_muladds( p1, ax, sinf(a1), p1 );
+      
+      vg_line( p0, p1, colour );
+
+      if( x == 0 )
+         vg_line( c, p0, colour );
+      if( x == 15 )
+         vg_line( c, p1, colour );
+   }
+
+   v3f p2;
+   v3_muladds(  c, ay, cosf(measured)*1.2f, p2 );
+   v3_muladds( p2, ax, sinf(measured)*1.2f, p2 );
+   vg_line( c, p2, colour );
+}
+
+static void rb_debug_constraint_limits( rigidbody *ra, rigidbody *rb, v3f lca,
+                                        v3f limits[2] )
+{
+   v3f ax, ay, az, bx, by, bz;
+   m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+   m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+   v2f px, py, pz;
+   px[0] = v3_dot( ay, by );
+   px[1] = v3_dot( az, by );
+
+   py[0] = v3_dot( az, bz );
+   py[1] = v3_dot( ax, bz );
+
+   pz[0] = v3_dot( ax, bx );
+   pz[1] = v3_dot( ay, bx );
+
+   float r0 = atan2f( px[1], px[0] ),
+         r1 = atan2f( py[1], py[0] ),
+         r2 = atan2f( pz[1], pz[0] );
+
+   v3f c;
+   m4x3_mulv( ra->to_world, lca, c );
+   draw_angle_limit( c, ay, az, limits[0][0], limits[1][0], r0, 0xff0000ff );
+   draw_angle_limit( c, az, ax, limits[0][1], limits[1][1], r1, 0xff00ff00 );
+   draw_angle_limit( c, ax, ay, limits[0][2], limits[1][2], r2, 0xffff0000 );
+}
+
+static void rb_limit_cure( rigidbody *ra, rigidbody *rb, v3f axis, float d )
+{
+   if( d != 0.0f )
+   {
+      float avx = v3_dot( ra->w, axis ) - v3_dot( rb->w, axis );
+      float joint_mass = rb->inv_mass + ra->inv_mass;
+      joint_mass = 1.0f/joint_mass;
+
+      float bias = (0.04f * k_rb_rate) * d,
+            lambda = -(avx + bias) * joint_mass;
+
+      /* Angular velocity */
+      v3f wa, wb;
+      v3_muls( axis,  lambda * ra->inv_mass, wa );
+      v3_muls( axis, -lambda * rb->inv_mass, wb );
+
+      v3_add( ra->w, wa, ra->w );
+      v3_add( rb->w, wb, rb->w );
+   }
+}
+
+static void rb_constraint_limits( rigidbody *ra, v3f lca, 
+                                  rigidbody *rb, v3f lcb, v3f limits[2] )
 {
+   /* TODO: Code dupe remover */
+   v3f ax, ay, az, bx, by, bz;
+   m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+   m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+   v2f px, py, pz;
+   px[0] = v3_dot( ay, by );
+   px[1] = v3_dot( az, by );
+
+   py[0] = v3_dot( az, bz );
+   py[1] = v3_dot( ax, bz );
+
+   pz[0] = v3_dot( ax, bx );
+   pz[1] = v3_dot( ay, bx );
+
+   float r0 = atan2f( px[1], px[0] ),
+         r1 = atan2f( py[1], py[0] ),
+         r2 = atan2f( pz[1], pz[0] );
+
+   /* calculate angle deltas */
+   float dx = 0.0f, dy = 0.0f, dz = 0.0f;
+
+   if( r0 < limits[0][0] ) dx = limits[0][0] - r0;
+   if( r0 > limits[1][0] ) dx = limits[1][0] - r0;
+   if( r1 < limits[0][1] ) dy = limits[0][1] - r1;
+   if( r1 > limits[1][1] ) dy = limits[1][1] - r1;
+   if( r2 < limits[0][2] ) dz = limits[0][2] - r2;
+   if( r2 > limits[1][2] ) dz = limits[1][2] - r2;
+
    v3f wca, wcb;
    m3x3_mulv( ra->to_world, lca, wca );
    m3x3_mulv( rb->to_world, lcb, wcb );
 
-   v3_sub( ra->v, rb->v, rcv );
+   rb_limit_cure( ra, rb, ax, dx );
+   rb_limit_cure( ra, rb, ay, dy );
+   rb_limit_cure( ra, rb, az, dz );
+}
 
-   v3f rcv_Ra, rcv_Rb;
-   v3_cross( ra->w, wca, rcv_Ra );
-   v3_cross( rb->w, wcb, rcv_Rb );
-   v3_add( rcv_Ra, rcv, rcv );
-   v3_sub( rcv, rcv_Rb, rcv );
+static void rb_debug_constraint_position( rigidbody *ra, v3f lca,
+                                          rigidbody *rb, v3f lcb )
+{
+   v3f wca, wcb;
+   m3x3_mulv( ra->to_world, lca, wca );
+   m3x3_mulv( rb->to_world, lcb, wcb );
+
+   v3f p0, p1;
+   v3_add( wca, ra->co, p0 );
+   v3_add( wcb, rb->co, p1 );
+   vg_line_pt3( p0, 0.005f, 0xffffff00 );
+   vg_line_pt3( p1, 0.005f, 0xffffff00 );
+   vg_line( p0, p1, 0xffffff00 );
 }
 
 static void rb_constraint_position( rigidbody *ra, v3f lca,
@@ -1853,14 +2002,6 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    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 );
 
@@ -1870,41 +2011,47 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    v3_add( rcv_Ra, rcv, rcv );
    v3_sub( rcv, rcv_Rb, rcv );
 
-   float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
+   v3f delta;
+   v3f p0, p1;
+   v3_add( wca, ra->co, p0 );
+   v3_add( wcb, rb->co, p1 );
+   v3_sub( p1, p0, delta );
 
-   float mass_a = 1.0f/ra->inv_mass,
-         mass_b = 1.0f/rb->inv_mass,
-         total_mass = mass_a+mass_b;
-   
-   v3f impulse;
-   v3_muls( rcv, 1.0f, impulse );
-   v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
-   v3_cross( wcb, impulse, impulse );
-   v3_add( impulse, rb->w, rb->w );
+   float dist2 = v3_length2( delta );
 
-   v3_muls( rcv, -1.0f, impulse );
-   v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
-   v3_cross( wca, impulse, impulse );
-   v3_add( impulse, ra->w, ra->w );
+   if( dist2 > 0.00001f )
+   {
+      float dist = sqrtf(dist2);
+      v3_muls( delta, 1.0f/dist, delta );
 
-#if 0
-   /*
-    * this could be used for spring joints
-    * its not good for position constraint
-    */
-   v3f impulse;
-   v3_muls( delta, 0.5f*spring, impulse );
+      float joint_mass = rb->inv_mass + ra->inv_mass;
 
-   v3_add( impulse, ra->v, ra->v );
-   v3_cross( wca, impulse, impulse );
-   v3_add( impulse, ra->w, ra->w );
+      v3f raCn, rbCn, raCt, rbCt;
+      v3_cross( wca, delta, raCn );
+      v3_cross( wcb, delta, rbCn );
+      
+      /* orient inverse inertia tensors */
+      v3f raCnI, rbCnI;
+      m3x3_mulv( ra->iIw, raCn, raCnI );
+      m3x3_mulv( rb->iIw, rbCn, rbCnI );
+      joint_mass += v3_dot( raCn, raCnI );
+      joint_mass += v3_dot( rbCn, rbCnI );
+      joint_mass = 1.0f/joint_mass;
 
-   v3_muls( delta, -0.5f*spring, impulse );
+      float vd = v3_dot( rcv, delta ),
+            bias = -(0.08f * k_rb_rate) * dist,
+            lambda = -(vd + bias) * joint_mass;
 
-   v3_add( impulse, rb->v, rb->v );
-   v3_cross( wcb, impulse, impulse );
-   v3_add( impulse, rb->w, rb->w );
-#endif
+      v3f impulse;
+      v3_muls( delta,  lambda, impulse );
+      rb_linear_impulse( ra, wca, impulse );
+      v3_muls( delta, -lambda, impulse );
+      rb_linear_impulse( rb, wcb, impulse );
+
+      /* 'fake' snap */
+      v3_muladds( ra->co, delta,  dist * 0.01f, ra->co );
+      v3_muladds( rb->co, delta, -dist * 0.01f, rb->co );
+   }
 }
 
 static void debug_sphere( m4x3f m, float radius, u32 colour )
diff --git a/skeleton.h b/skeleton.h
index dc50fe9..dc8342b 100644
--- a/skeleton.h
+++ b/skeleton.h
@@ -19,6 +19,8 @@ struct skeleton
 
       mdl_keyframe kf;
 
+      u32 orig_node;
+
       int collider;
       boxf hitbox;
 
@@ -465,6 +467,7 @@ static int skeleton_setup( struct skeleton *skele, mdl_header *mdl )
             }
 
             sb->collider = bone_inf->collider;
+            sb->orig_node = i;
             box_copy( bone_inf->hitbox, sb->hitbox );
 
             if( bone_inf->collider )