+#ifndef IK_H
+#define IK_H
+
#include "vg/vg.h"
/*
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 );
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 );
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