X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=ik.h;h=e02fd30af91dbd76a1ca865003714a4bb75bcd95;hb=b4c9550f206c476bb38b0bb2855d35e6b31bee83;hp=0bbe3b2aed581cbc8cfb3f7136c3f080472b2372;hpb=54ca09093b8d5f752b4a0897bd9703f3b2ad6ed1;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/ik.h b/ik.h
index 0bbe3b2..e02fd30 100644
--- a/ik.h
+++ b/ik.h
@@ -1,3 +1,6 @@
+#ifndef IK_H
+#define IK_H
+
 #include "vg/vg.h"
 
 /*
@@ -22,13 +25,74 @@ struct ik_basic
    float l1, l2;
 };
 
-static void ik_basic( struct ik_basic *ik, m4x3f m1, m4x3f m2 )
+typedef enum ik_dir ik_dir;
+enum ik_dir
+{
+   k_ikX,
+   k_iknX,
+   k_ikY,
+   k_iknY,
+   k_ikZ,
+   k_iknZ
+};
+
+static void ik_track_to( m4x3f m, v3f pos, v3f target, v3f axis, 
+      ik_dir fwd, ik_dir up )
+{
+   v3f dir, other;
+   v3_sub( target, pos, dir );
+   v3_normalize( dir );
+   v3_cross( dir, axis, other );
+
+   if( fwd == k_ikX && up == k_ikY )
+   {
+      v3_copy( axis,  m[0] );
+      v3_copy( dir,   m[1] );
+      v3_copy( other, m[2] );
+   }
+
+   if( fwd == k_ikY && up == k_ikX )
+   {
+      v3_negate( axis, m[2] );
+      v3_copy( dir,    m[1] );
+      v3_negate( other,m[0] );
+   }
+
+   if( fwd == k_ikY && up == k_iknX )
+   {
+      v3_negate( axis, m[2] );
+      v3_negate( dir,    m[1] );
+      v3_copy( other,m[0] );
+   }
+
+   if( fwd == k_ikZ && up == k_ikY )
+   {
+      v3_copy( axis,  m[1] );
+      v3_copy( dir,   m[2] );
+      v3_negate( other, m[0] );
+   }
+
+   if( fwd == k_iknZ && up == k_ikY )
+   {
+      v3_negate( axis,  m[1] );
+      v3_negate( dir,   m[2] );
+      v3_negate( other, m[0] );
+   }
+
+   v3_copy( pos, m[3] );
+}
+
+static void ik_basic( struct ik_basic *ik, m4x3f m1, m4x3f m2, 
+      enum ik_dir fwd, enum ik_dir up )
 {
    /* Localize the system into 2d */
-   v3f v0, v1, axis;
+   v3f local[3];
+   float *v0 = local[0],
+         *v1 = local[1],
+         *axis = local[2];
 
-   vg_line( ik->base, ik->pole, 0xffffff00 );
-   vg_line( ik->end, ik->pole, 0xffffff00 );
+   vg_line( ik->base, ik->pole, 0x6fffff00 );
+   vg_line( ik->end, ik->pole, 0x6fffff00 );
 
    v3_sub( ik->base, ik->pole, v0 );
    v3_sub( ik->end, ik->pole, v1 );
@@ -39,9 +103,9 @@ static void ik_basic( struct ik_basic *ik, m4x3f m1, m4x3f m2 )
    v3_cross( axis, v0, v1 );
 
    v3f p0, p1, p2;
-   v3_muladds( ik->base, axis, 0.2f, p0 );
-   v3_muladds( ik->base, v0, 0.2f, p1 );
-   v3_muladds( ik->base, v1, 0.2f, p2 );
+   v3_muladds( ik->base, axis, 0.05f, p0 );
+   v3_muladds( ik->base, v0, 0.05f, p1 );
+   v3_muladds( ik->base, v1, 0.05f, p2 );
    vg_line( ik->base, p0, 0xffff0000 );
    vg_line( ik->base, p1, 0xff00ff00 );
    vg_line( ik->base, p2, 0xff0000ff );
@@ -69,9 +133,12 @@ static void ik_basic( struct ik_basic *ik, m4x3f m1, m4x3f m2 )
    v3_muladds( world_knee, v1, knee[1], world_knee );
 
    /* Create matrices */
-   m4x3_lookat( m1, ik->base, world_knee, axis );
-   m4x3_lookat( m2, world_knee, ik->end, axis );
+   
+   ik_track_to( m1, ik->base, world_knee, axis, fwd, up );
+   ik_track_to( m2, world_knee, ik->end, axis, fwd, up );
 
-   vg_line( ik->base, world_knee, 0xff0000ff );
-   vg_line( world_knee, ik->end, 0xff00ff40 );
+   vg_line( ik->base, world_knee, 0xa00000ff );
+   vg_line( world_knee, ik->end, 0xa000ff40 );
 }
+
+#endif