void Move_Apply_Constraints(float dt) {
	  int i;
	  float scale=0.5f;

	  // adjust body orientation
	  body.forward.set( target.forward );
	  body.forward.normalize();
	  body.up.set( target.up );
	  body.up.normalize();
	  body.left.cross( body.up, body.forward );
	  body.forward.cross( body.left, body.up );

	  for(i=0;i<6;++i) {
	    Leg leg=legs[i];
	    // keep shoulders locked in relative position
	    leg.pan_joint.pos.set( body.pos );
	    Vector3f F=new Vector3f(body.forward);
	    Vector3f L=new Vector3f(body.left);
	    Vector3f U=new Vector3f(body.up);
	    F.scale(leg.pan_joint.relative.y);
	    L.scale(leg.pan_joint.relative.x);
	    U.scale(leg.pan_joint.relative.z);
	    leg.pan_joint.pos.add(F);
	    leg.pan_joint.pos.sub(L);
	    leg.pan_joint.pos.add(U);
	    
	    // make sure feet can not come under the body or get too far from the shoulder
	    Vector3f ds = new Vector3f( leg.pan_joint.pos );
	    ds.sub( body.pos );
	    Vector3f df = new Vector3f( leg.ankle_joint.pos );
	    df.sub( body.pos );
	    float dfl=( df.length() );
	    float dsl=( ds.length() );

	    ds.z = 0;
	    ds.normalize();
	    if( dfl < dsl ) {
	      ds.scale(dsl-dfl);
	      leg.ankle_joint.pos.add(ds);
	    } else if( dfl - dsl > max_leg_length ) {
	      // @TODO: should this test should be ankle - pan > max_leg_length ?
	      ds.scale( dfl - dsl - max_leg_length );
	      leg.ankle_joint.pos.sub(ds);
	    }

	    // calculate the pan joint matrix
	    leg.pan_joint.up.set( body.up );
	    leg.pan_joint.forward.set( leg.ankle_joint.pos );
	    leg.pan_joint.forward.sub( leg.pan_joint.pos );
	    
	    df.set(body.up);
	    df.scale( leg.pan_joint.forward.dot( body.up ) );
	    leg.pan_joint.forward.sub(df);
	    
	    if( leg.pan_joint.forward.length() < 0.01f ) {
	      leg.pan_joint.forward.set( leg.tilt_joint.pos );
	      leg.pan_joint.forward.sub( leg.pan_joint.pos );
	    }
	    leg.pan_joint.forward.normalize();
	    leg.pan_joint.left.cross( leg.pan_joint.up, leg.pan_joint.forward );

	    // zero the distance between the pan joint and the tilt joint
	    df.set(leg.pan_joint.forward);
	    df.scale(leg.tilt_joint.relative.length());
	    leg.tilt_joint.pos.set( leg.pan_joint.pos );
	    leg.tilt_joint.pos.add(df); 

	    // zero the knee/foot distance
	    Vector3f a = new Vector3f(leg.knee_joint.pos);
	    a.sub(leg.ankle_joint.pos);
	    float kf = a.length() - leg.ankle_joint.relative.length();
	    if(Math.abs(kf)>0.001) {
	      a.normalize();
	      a.scale(kf*scale);
	      leg.knee_joint.pos.sub(a);
	    }

	    // validate the tilt/knee plane
	    a.set(leg.knee_joint.pos );
	    a.sub( leg.pan_joint.pos );
	    df.set(leg.pan_joint.left);
	    df.scale( a.dot( leg.pan_joint.left ) );
	    leg.knee_joint.pos.sub( df );
	    leg.knee_joint.left.set( leg.pan_joint.left );

	    // zero the tilt/knee distance
	    a.set( leg.knee_joint.pos );
	    a.sub( leg.tilt_joint.pos );
	    float kt = a.length() - leg.knee_joint.relative.length();
	    if(Math.abs(kt)>0.001) {
	      a.normalize();
	      a.scale(kt);
	      // don't push back on the tilt joint, it makes the simulation too unstable.
	      leg.knee_joint.pos.sub(a);
	    }


	    // calculate the tilt joint matrix
	    leg.tilt_joint.left.set( leg.pan_joint.left );
	    a.set( leg.knee_joint.pos );
	    a.sub( leg.tilt_joint.pos );
	    Vector3f b = new Vector3f();
	    b.cross( a, leg.tilt_joint.left );
	    leg.tilt_joint.forward.set(a );
	    leg.tilt_joint.forward.sub( b );
	    leg.tilt_joint.forward.normalize();
	    leg.tilt_joint.up.cross( leg.tilt_joint.forward, leg.tilt_joint.left );

	    // calculate the knee matrix
	    leg.knee_joint.forward.set( leg.ankle_joint.pos );
	    leg.knee_joint.forward.sub( leg.knee_joint.pos );
	    leg.knee_joint.forward.normalize();
	    leg.knee_joint.up.cross( leg.knee_joint.forward, leg.knee_joint.left );
	    //leg.knee_joint.forward = leg.knee_joint.left ^ leg.knee_joint.up;

	    // calculate the ankle matrix
	    leg.ankle_joint.forward.set( leg.knee_joint.forward );
	    leg.ankle_joint.left.set( leg.knee_joint.left );
	    leg.ankle_joint.up.set( leg.knee_joint.up );
	  }
	}