#include "proj.h"
#include "MotionCapturer.h"
#include "Utils.h"
#include "vector.h"
#include "viewer.h"
#include "RotConverter.h"

#define twistbend 1

#define MAX_IKS 2
#define CLOSE_ENOUGH 3.0


bool b_closer_to_goal(bool &b_close_enough, double gb_goalPos[3], double old_dist[3],
						Node *selNode, RotType rotType, double *trans, double *rot );

void get_lc_pos_from_q(double v4_pos[4], Bone *bone, double q[4]);
void get_lc_pos_from_rot(double v4_pos[4], double v4_offset[4], double rot_axis[3], double aDeg);

bool check_rot_in_lim(Bone *bone, double q[4]);
bool check_rot_in_lim(Bone *bone, double rot_axis[3], double aDeg);

void avoid_crazy_twist(double rptr[4], Bone *bone);
bool avoid_crazy_twist(double rptr[4], Bone *bone, double virtual_bone[3], double rot_axis[3], double rot_amt ) ;

// fill in the rot angle lim spec
Status MotionCapturer::read_input_flex( FILE *fin, Node *node )
{

	Status status;
	int i;
	char hdr_lineBuf[LINESIZE];	// where to read the line into

	char *tok;

	do {	// if this bvh doesn't have the joint specified in flex, skip it
		if (!fgets(hdr_lineBuf, LINESIZE, fin)) {
			printf("bad, missing spec for joint %s\n", node->name);
			fl_alert("bad, missing spec for joint %s\n", node->name);
			return FAILURE;
		}
	  tok = strtok(hdr_lineBuf, " \t\n");
	} while ( tok==NULL ||strcmp(tok, node->name) != 0);


	status = SUCCESS;
	for (i=0; i<node->children_cnt; i++) {

		if (node->children[i]->type != END_SITE)
			status = read_input_flex(fin, node->children[i]);

		if (status != SUCCESS) return FAILURE;

	}

	return SUCCESS;
}


void Gl_Win::node_lc_to_gbCoord(Node *selNode, double gb_selPos[4])
{

  if (selNode->ancestors == NULL)
    selNode->ancestors = find_my_ancestors(selNode);

  lc_to_ac_nodePos( gb_selPos, NULL, selNode, QUAT, cur_tptr, cur_qptr);
    
}



void Gl_Win::do_ik(double gb_goal[4])
{
//  Node *node;
//  Bone *bone;
  bool b_sel_close_enough, b_pin_close_enough; 
  int num_tries;
//  double rot_sensitive;
  double root_t_orig[3];
  double gb_pinGoal[4] = {0, 0, 0, 1};
  double *cur_qiptr;
  int frame_size;
  bool b_move = false;

#ifdef moveroot
  // make back up of orig pos
  frame_size = rot_cnt * 4 *sizeof(double);
  cur_qiptr = cap->qinterp + cur_time * frame_size;	  
  memcpy(cur_qiptr, cur_qptr, frame_size );
  v3_assign(root_t_orig, cur_tptr);

  v3_assign(gb_pinGoal, cur_qptr+pinNode->q_off);

#endif

  b_move = false; // need_move_root(gb_goal);
/*  	
  printf("\nik on goal <%2.2lf, %2.2lf, %2.2lf>, sel %s=<%2.2lf, %2.2lf, %2.2lf>\n", 
	  gb_goal[X], gb_goal[Y], gb_goal[Z], selNode->name, 
	  selNode->gb_curPos[X], selNode->gb_curPos[Y], selNode->gb_curPos[Z]);
  */
  b_sel_close_enough = false;
  b_pin_close_enough = true;
  num_tries = 0;

  while (1) {

	  if (!b_sel_close_enough && (num_tries < MAX_IKS) )
		ik_for_node(b_sel_close_enough, selNode, gb_goal);
	  num_tries ++ ;
	  
	  if ( (b_sel_close_enough || num_tries > MAX_IKS) && b_pin_close_enough)
		  break;

#ifdef moveroot
	  if ( b_move )
		ik_for_node(b_pin_close_enough, pinNode, gb_pinGoal);
	  
	  if (!b_pin_close_enough && num_tries > MAX_IKS) {	// copy things back
		memcpy(cur_qptr, cur_qiptr, frame_size );
		v3_assign(cur_tptr, root_t_orig );
		break;
	  }		
#endif

  }

/*
  if (b_sel_close_enough)
	  printf("ik this target suceed!\n");
  else
	  printf("you slow sucker!\n");
	  */
}




void Gl_Win::ik_for_node(bool &b_close_enough, Node *targetNode, double gb_goal[4])
{
  Node *node;
  Bone *bone;
  bool b_closer_this_bone;
  double rot_sensitive;

  b_close_enough = false;
  b_closer_this_bone = false;
  rot_sensitive = ik_sens;
  node = targetNode;

  while (node->parent!= NULL && node->parent->parent != NULL) {
			
	if (rot_sensitive < NEAR_ZERO) break;	// motion doesn't propagate up

	bone = node->bone;	
#ifdef twistbend
	
	if ( !CLOSE_TO_ZERO(bone->ang_twist_lim) ) {
	    
		// first do twist			
		//printf("twist bone %s to %s\n", bone->pnode->name, bone->cnode->name);

		ik_this_bone(b_closer_this_bone, b_close_enough, 
				targetNode, rot_sensitive,bone, J_TWST, gb_goal);
	
		if (b_close_enough)  break;
	}
	
		// then bend
	ik_this_bone(b_closer_this_bone, b_close_enough, 
		   targetNode, rot_sensitive,bone, J_BEND, gb_goal);

	if (b_close_enough)  break;		// done!!	      		

#else

	    if (bone->joint_type == J_BEND) {
	      ik_this_bone(b_closer_this_bone, b_close_enough, 
			   targetNode, rot_sensitive,bone, J_BEND, gb_goal);
	    }
	    else if (bone->joint_type == J_SOCK) {
			
			// first do twist
			if (bone->ang_twist_lim > 0.0) {
				//printf("twist bone %s to %s\n", bone->pnode->name, bone->cnode->name);

				ik_this_bone(b_closer_this_bone, b_close_enough, 
					targetNode, rot_sensitive,bone, J_TWST, gb_goal);

				if (b_close_enough)  break;
			}	  

			// then sock
			ik_this_bone(b_closer_this_bone, b_close_enough, 
			   targetNode, rot_sensitive,bone, J_SOCK, gb_goal);

			if (b_close_enough)  break;			  
	    }

#endif


	rot_sensitive *= ik_damp;	//move upper joints less
	node = node->parent;
  }

}



// find which ensite the selnode follows, return idx of es in arr
int find_endsite_sel_on(Node **endsites, int es_cnt, Node *selNode)
{

	Node *es;
	Ancestry *a;
	for (int i=0; i<es_cnt; i++) {

		es = endsites[i];
		if (es->ancestors == NULL)
			es->ancestors = find_my_ancestors(es);

		for (a = es->ancestors; a!= NULL; a=a->child)
			if (a->node == selNode) break;

		if (a!=NULL) 
			return i;		
	}

	assert(0);

	return -1;

}



bool Gl_Win::need_move_root(double gb_goal[3])
{
	Node *endNode;
	double gb_selPos[4];
	double gb_pinPos[4];
	double gb_endPos[4];
	double vsum[3] = {0, 0, 0};
	double v[3] = {0, 0, 0};
	double hooks;
	double hook_fac = 0.001;
	int i, sel_i, pin_i;

	// update selnode's dist2root
	lc_to_ac_nodePos( gb_selPos, NULL, selNode, QUAT, cur_tptr, cur_qptr);
	v3_sub(gb_selPos, cap->root->offset, selNode->v4_dist_root2node);

	// update pinnode's dist2root
	lc_to_ac_nodePos( gb_pinPos, NULL, pinNode, QUAT, cur_tptr, cur_qptr);
	v3_sub(gb_pinPos, cap->root->offset, pinNode->v4_dist_root2node);

	if ((v3_magnitude(selNode->v4_dist_root2node) < selNode->dist_root2node_max) ||
		(v3_magnitude(pinNode->v4_dist_root2node) > pinNode->dist_root2node_max)	)
		return false;

	sel_i = find_endsite_sel_on(cap->endsites, cap->ends_cnt, selNode);
	assert(sel_i>=0 && sel_i<MAX_ENDSITES);
	pin_i = find_endsite_sel_on(cap->endsites, cap->ends_cnt, pinNode);
	assert(pin_i>=0 && pin_i<MAX_ENDSITES);
	
	if (sel_i == pin_i) return false;

	for (i=0; i<cap->ends_cnt; i++) {
		
		if (i==sel_i) {
			hooks = selNode->dist_root2node_max * hook_fac; 
			v3_scale(selNode->v4_dist_root2node, hooks, v);
		}
		else if (i==pin_i) {
			hooks = pinNode->dist_root2node_max * hook_fac; 
			v3_scale(pinNode->v4_dist_root2node, hooks, v);
		}		
		else {
			endNode = cap->endsites[i];

			lc_to_ac_nodePos( gb_endPos, NULL, endNode, QUAT, cur_tptr, cur_qptr);
			v3_sub(gb_endPos, cap->root->offset, endNode->v4_dist_root2node);

			hooks = endNode->dist_root2node_max * hook_fac;
			v3_scale(endNode->v4_dist_root2node, hooks, v);
		}

		v3_add(vsum, v, vsum);
	}
	
/*
	printf("MOVE root <%2.2lf, %2.2lf, %2.2lf> by <%2.2lf, %2.2lf, %2.2lf> \n",
		cur_tptr[X], cur_tptr[Y], cur_tptr[Z], 
		vsum[X], vsum[Y], vsum[Z]);
	v3_add(cur_tptr, vsum, cur_tptr);
*/
	return true;

}





/* in parent bone's rot quat and this bone's rot quat, see if in twist lim. */
void adjust_twist_amt(double &rot_amt, Bone *bone )
{
  	
	if (rot_amt + bone->ang_twist > bone->ang_twist_lim) {
	  rot_amt = bone->ang_twist_lim - bone->ang_twist;
	  bone->ang_twist = bone->ang_twist_lim;
	  //printf("%c, atw=%2.2lf, \n", SOUND_BELL, bone->ang_twist);
	  //printf("atw=%2.2lf, \n", bone->ang_twist);
	}
	else if (rot_amt + bone->ang_twist < - bone->ang_twist_lim) {
	  rot_amt = bone->ang_twist + bone->ang_twist_lim;
	  bone->ang_twist = - bone->ang_twist_lim;
	  //printf("%c, atw=%2.2lf, \n", SOUND_BELL, bone->ang_twist);
	  //printf("atw=%2.2lf, \n", bone->ang_twist);
	}
	else 
	  bone->ang_twist += rot_amt;

}



void add_rot(double rptr[4], Bone *bone, double rot_axis[3], double rot_amt ) 
{

	double q[4];
	double q_orig[4];
	
	v4_assign(q_orig, rptr);

	// add new rot to orig
	rotToQuat(q, rot_amt, rot_axis[X], rot_axis[Y], rot_axis[Z]);	
	qt_mult(q, q_orig, rptr);	// mult to get new rot
}


void get_lc_pos_from_q(double v4_pos[4], Bone *bone, double q[4])
{
	double aDeg;
	double rot_axis[3];

	quaternToRot(q, aDeg, rot_axis[X], rot_axis[Y], rot_axis[Z]);
	get_lc_pos_from_rot(v4_pos, bone->cnode->offset, rot_axis, aDeg);

}


void get_lc_pos_from_rot(double v4_pos[4], double v4_offset[4], double rot_axis[3], double aDeg)
{
	double model_mat[MAT44_SIZE];

	if (CLOSE_TO_ZERO(aDeg) || CLOSE_TO_ZERO(v3_magnitude(rot_axis)) )
		v4_assign(v4_pos, v4_offset);
	else {
		glPushMatrix();
		glLoadIdentity();
			glRotated(aDeg, rot_axis[X], rot_axis[Y], rot_axis[Z]);
			glGetDoublev(GL_MODELVIEW_MATRIX, model_mat);
			glmat44_mult_vec41(model_mat, v4_offset, v4_pos);
		glPopMatrix();
	}
}


// change the goal vec w/ added springy
void add_springy(double goal_vec[3], double spr_fac, Bone *bone, double pnode_q[4])
{
	
	double v3_twd_natural[3];
	double bone_endpos[4];
	double goal_mag;
	double a_bone2goal;
	
	goal_mag = v3_magnitude(goal_vec);

	get_lc_pos_from_q(bone_endpos, bone, pnode_q);
	
	a_bone2goal = acos( v3_cos_bw2v(bone_endpos, goal_vec) );
	if ( CLOSE_TO_ZERO(a_bone2goal) )
		return;	// the two are aligned, don't do anything to cause overshoot

	v3_normalize(bone_endpos, bone_endpos);	
	v3_sub(bone->v4_natural, bone_endpos, v3_twd_natural);	// get the unit vect twd natural
	v3_scale(v3_twd_natural, spr_fac * goal_mag, v3_twd_natural);
	v3_add(goal_vec, v3_twd_natural, goal_vec);

}


bool check_rot_in_lim(Bone *bone, double q[4])
{
	double aDeg;
	double rot_axis[3];

	quaternToRot(q, aDeg, rot_axis[X], rot_axis[Y], rot_axis[Z]);
	return check_rot_in_lim(bone, rot_axis, aDeg);
}


bool check_rot_in_lim(Bone *bone, double rot_axis[3], double aDeg)
{
	double a_bone2cen;
	double bone_endpos[4];

	// get the end pos after apply the rot
	get_lc_pos_from_rot(bone_endpos, bone->cnode->offset, rot_axis, aDeg);

	// check if new pos valid
	a_bone2cen = acos( v3_cos_bw2v(bone_endpos, bone->v4_central) ) * RadianToDegree;

/*	printf("Bone %s: a to lim =%2.2lf, a_rot is %2.2lf, axis=<%2.2lf,%2.2lf, %2.2lf>, \n",
		bone->pnode->name, a_bone2cen, aDeg, 
		rot_axis[X], rot_axis[Y], rot_axis[Z] );
		
	printf(" \tbone pos <%2.2lf,%2.2lf, %2.2lf>, central <%2.2lf,%2.2lf, %2.2lf> \n", 
		bone_endpos[X], bone_endpos[Y], bone_endpos[Z], 
		bone->v4_central[X], bone->v4_central[Y], bone->v4_central[Z]);
*/
	if ( (a_bone2cen < bone->j_ang_lim) && (a_bone2cen > - bone->j_ang_lim) ) {
//		printf("CHECK ok.\n");
		return true;
	}

//	printf("Bad rot out lim\n");
	return false;
}



void Gl_Win::ik_this_bone(bool &b_closer, bool &b_close_enough, 
			  Node *targetNode, double rot_sensitive, Bone *bone, 
			  JointType joint_type, double gb_goalPos[3])
{

  Point3D unit_x = {1, 0, 0};
  Point3D unit_y = {0, 1, 0};  // norm to bone is y-axis in local coord
  Point3D unit_z = {0, 0, 1};
  double gb_selPos[4]; // targetNode's pos in glob coord
  double goal_vec[3];
  double bone_vec[3];
  double gb_cn[4];
  double gb_pn[4];
  double gb_ppn[4];
  double pb_vec[4] = {0, 0, 0, 1};		// parent bone vec
  double up_vec[3];
  double jrot_vec[3];
  double *rptr;		// ptr to this bone's parent's spot in rot arr

  double rot_amt = 0;
  double magn_goal, sin_joint_goal, sin_bone_goal, cos_up_goal; //, cos_bones, a_bw_bones;

  double q_orig[4];

  Node *pnode = bone->pnode;
  Node *cnode = bone->cnode;

  b_closer = false;
  b_close_enough = false;

  //printf("ik_this bone: %s to %s \n", pnode->name, cnode->name );

   
  rptr = cur_qptr + pnode->q_off;	// the spot in arr to change
  
  v4_assign(q_orig, rptr);
	

  if (v3_magnitude(rptr) > 500 || v3_magnitude(rptr) < -500)
	  printf("WARNING! rptr=<%2.2lf, %2.2lf, %2.2lf>\n",
	  rptr[0], rptr[1], rptr[2]);

  // get targetNode's pos in cur bone's coord
  lc_to_ac_nodePos( gb_selPos, NULL, targetNode, QUAT, cur_tptr, cur_qptr);

  lc_to_ac_nodePos( gb_pn, NULL, pnode, QUAT, cur_tptr, cur_qptr);// get bone's pnode's gb pos
  lc_to_ac_nodePos( gb_cn, NULL, cnode, QUAT, cur_tptr, cur_qptr);// get bone's cnode's gb pos
  lc_to_ac_nodePos( gb_ppn, NULL, pnode->parent, QUAT, cur_tptr, cur_qptr);//get pnode's p's gb pos

  v3_sub(gb_goalPos, gb_selPos, goal_vec);  // goal vec = end eff -> goal

  if (ik_spring_fac > 0) {
	add_springy( goal_vec, ik_spring_fac, bone, rptr);
//	printf("After add spr: goal vec=<%2.2lf, %2.2lf, %2.2lf>\n",
//	  goal_vec[X], goal_vec[Y], goal_vec[Z]);
  }

  magn_goal = v3_magnitude(goal_vec);

  if (joint_type == J_BEND)
	  v3_sub(gb_cn, gb_pn, bone_vec);
  else	// twist	use virtual bone
	  v3_sub(gb_selPos, gb_pn, bone_vec);


  v3_sub(gb_pn, gb_ppn, pb_vec);   // parent bone = grandpa to parent  

  if (joint_type == J_BEND) {

#ifdef twistbend

	  if ( !(CLOSE_TO_ZERO(bone->ang_twist)) ) {	// take into acnt twist
		  double pb_rot_vec[4];		// rot pb a little to get the right jrot_vec
		  get_lc_pos_from_rot(pb_rot_vec, pb_vec, bone->v4_natural, bone->ang_twist);
/*		  printf("rotated pb = <%2.2lf, %2.2lf, %2.2lf>, pb=<%2.2lf, %2.2lf, %2.2lf>\n \t a=%2.2lf, b=<%2.2lf, %2.2lf, %2.2lf>,\n",
			  pb_rot_vec[X], pb_rot_vec[Y], pb_rot_vec[Z], pb_vec[X], pb_vec[Y], pb_vec[Z],
			  bone->ang_twist, bone_vec[X], bone_vec[Y], bone_vec[Z]); 
*/
		v3_assign(pb_vec, pb_rot_vec);
		  
	  }

#endif

	double a = acos( v3_cos_bw2v(pb_vec, bone_vec) );
	if ( CLOSE_TO_ZERO(a) ) {
		v3_crossp(pb_vec, bone->v4_central, jrot_vec); // hack	
/*		printf("BEND CLOSE 0, pbone=<%2.2lf, %2.2lf, %2.2lf>, b=<%2.2lf, %2.2lf, %2.2lf>\n \tnew BEND axis <%2.2lf, %2.2lf, %2.2lf>\n", 
			pb_vec[X], pb_vec[Y], pb_vec[Z], 
			bone_vec[X], bone_vec[Y], bone_vec[Z], 
			jrot_vec[X], jrot_vec[Y], jrot_vec[Z]);
*/	}
	else if ( CLOSE_TO(a, PI) ) {
		v3_crossp(bone->v4_central, pb_vec, jrot_vec); // hack	
/*		printf("BEND CLOSE 180, pbone=<%2.2lf, %2.2lf, %2.2lf>, b=<%2.2lf, %2.2lf, %2.2lf>\n \tnew BEND axis <%2.2lf, %2.2lf, %2.2lf>\n", 
			pb_vec[X], pb_vec[Y], pb_vec[Z], 
			bone_vec[X], bone_vec[Y], bone_vec[Z], 
			jrot_vec[X], jrot_vec[Y], jrot_vec[Z]);
*/	}
	else
		v3_crossp(pb_vec, bone_vec, jrot_vec); // joint axis perp to both parent bone and this bone
  }
  else if (joint_type == J_TWST)
    v3_assign(jrot_vec, pb_vec);  // in dir of actual bone

#ifndef twistbend
  else if (joint_type == J_SOCK)
    v3_crossp(bone_vec, goal_vec, jrot_vec); // get to goal as close as possib
#endif

  // for rot bone in joint's bend dir
  v3_crossp(jrot_vec, bone_vec, up_vec);

			  
  cos_up_goal = v3_cos_bw2v(up_vec, goal_vec);
  sin_bone_goal  = v3_sin_bw2v(bone_vec, goal_vec);
  sin_joint_goal = v3_sin_bw2v(jrot_vec, goal_vec);

  rot_amt = magn_goal * sin_joint_goal * sin_bone_goal * rot_sensitive * RadianToDegree;    


  if (rot_amt >= -0.01 && rot_amt <= 0.01) return;	// solver says no

  if (cos_up_goal < 0)
    rot_amt = 0 - rot_amt;  // negate direction  

  // check rot lims

  if (joint_type == J_TWST) {
	  adjust_twist_amt(rot_amt, bone );
//	  printf("twist=%2.2lf, tw lim =%2.2lf, after twist rot_amt = %2.2lf\n", 
//		  bone->ang_twist, bone->ang_twist_lim, rot_amt);
	  if (rot_amt >= -0.01 && rot_amt <= 0.01) return;	// solver says no
  }

   // regular add rot to prev rot
  add_rot(rptr, bone, jrot_vec, rot_amt);

  if ( check_rot_in_lim(bone, rptr) == false) {
	  v4_assign(rptr, q_orig);
	  return;
  }

/*
	printf("before q = <%2.2lf, %2.2lf, %2.2lf, %2.2lf>\n",
		rptr[0], rptr[1], rptr[2], rptr[3]);

	double new_bone_endpos[4];
	get_lc_pos_from_q(new_bone_endpos, bone, rptr);
	printf("new_bone_endpos = <%2.2lf, %2.2lf, %2.2lf>\n", 
		new_bone_endpos[X], new_bone_endpos[Y], new_bone_endpos[Z] );

	if ( !(avoid_crazy_twist(rptr, bone, new_bone_endpos, jrot_vec, rot_amt)) ) {
		v4_assign(rptr, q_orig);
		return;
	}
*/

	  b_closer = b_closer_to_goal(b_close_enough, gb_goalPos, goal_vec,
										targetNode, QUAT, cur_tptr, cur_qptr );
	
	  if (b_closer) {
		//printf("closer:bone %s to %s\n", pnode, cnode);

		//cap->calc_nodes_global_pos(QUAT, cur_tptr, cur_qptr );

		if (b_close_enough) {
			b_closer = true;
		return; // done
		}

	  }	else {
		//printf("\n possible to get further???\n");

		bone->ang_twist -= rot_amt;
		v4_assign(rptr, q_orig);	// revert back to orig	
	  }
		
  
}


bool b_closer_to_goal(bool &b_close_enough, double gb_goalPos[3], double old_dist[3],
						Node *targetNode, RotType rotType, double *trans, double *rot )
{
	
  double new_dist;
  double dist_vec[3];	
  double gb_new_selPos[4];  // targetNode in gb coord    

  lc_to_ac_nodePos( gb_new_selPos, NULL, targetNode, rotType, trans, rot);	// get targetNode's gb pos
  
  v3_sub(gb_goalPos, gb_new_selPos, dist_vec );
  new_dist = v3_magnitude ( dist_vec );
  
  if (new_dist < CLOSE_ENOUGH) {
    b_close_enough = true;
    return true;
  }

  b_close_enough = false;
  if ( new_dist < v3_magnitude(old_dist))    
    return true;

  return false;
}



// not used
// this ver does no limit checking
void avoid_crazy_twist(double rptr[4], Bone *bone)
{
	double bone_endpos[4];

	// get the end pos after apply the rot
	get_lc_pos_from_q(bone_endpos, bone, rptr);

	// get q rot from orig pos to new pos, store in q
	calc_q_v1tov2(rptr, bone->v4_central, bone_endpos);

}




// not used 
// a vec in loc coord to a child coord
void lc_to_cc_vec(double return_val[4], Node *node, double lc_vec[4],
		   RotType rotType, double *trans, double *rot )
{
  int		r_off;	// rotation offset stored at node, = root->q_off or e_off depending on rotType
  double	rotatX, rotatY, rotatZ;
  double	aDeg, x, y, z;
  double	model_mat[MAT44_SIZE];		// modelview mat

  glPushMatrix();
  glLoadIdentity();

   // do rotate 
  if (node->type != END_SITE) {
    	// do rotate 
    if (rotType == EULER) {
      // rot = (Z,X,Y)`
      r_off = node->e_off;
      rotatZ = rot[r_off];
      rotatX = rot[r_off + 1];
      rotatY = rot[r_off + 2];

      glRotated(-rotatY, 0, 1, 0);      
      glRotated(-rotatX, 1, 0, 0);
      glRotated(-rotatZ, 0, 0, 1);

    }
    else {
      if (rotType == QUAT) {
	r_off = node->q_off;
	quaternToRot(rot + r_off, aDeg, x, y, z);
      }
      else {// rotType = EXPMAP
	r_off = node->e_off;
	emapToRot(rot + r_off, aDeg, x, y, z);
      }
      
      if (aDeg != 0)
	glRotated(-aDeg, x, y, z);
    }
  }

  glTranslated(-trans[X], -trans[Y], -trans[Z]);

  // get the node's cur global pos
  glGetDoublev(GL_MODELVIEW_MATRIX, model_mat);  
  glmat44_mult_vec41(model_mat, lc_vec, return_val);

  glPopMatrix();
  

}




// to jumpy.  too much springy???
bool added_highpotency_rot(Bone *bone, double rot_axis[3], double rot_amt, double rptr[4]) 
{

	double q[4];
	double q_adj[4];	// adjusted w/ springyness
	double q_orig[4];
	double model_mat[MAT44_SIZE];
	double ang, aDeg, x, y, z;
	double new_ang_bone2natural;
	double bone_endpos[4];
	double vrot_natural[3];
	bool valid_before_adj = false;
	double spring_fac;
	double aDeg_natural;

	v4_assign(q_orig, rptr);


	// get new ang apply cur pos
	rotToQuat(q, rot_amt, rot_axis[X], rot_axis[Y], rot_axis[Z]);	
	qt_mult(q, q_orig, rptr);	// mult to get new rot
	quaternToRot(rptr, aDeg, x, y, z);
	
	glPushMatrix();
	glLoadIdentity();
		glRotated(aDeg, x, y, z);
		glGetDoublev(GL_MODELVIEW_MATRIX, model_mat);  
		glmat44_mult_vec41(model_mat, bone->cnode->offset, bone_endpos);
	glPopMatrix();

	// check if new pos valid
	ang = acos( v3_cos_bw2v(bone_endpos, bone->v4_central) ) * RadianToDegree;
	if ( (ang < bone->j_ang_lim) && (ang > - bone->j_ang_lim) )
		valid_before_adj = true;
	
	// adj according to how much further/closer new pos bring to natural
	new_ang_bone2natural = acos( v3_cos_bw2v(bone_endpos, bone->v4_natural) );

	//printf("amt was %2.2lf, valid=%d, new dist %2.2lf\n",
	//	rot_amt, valid_before_adj, new_ang_bone2natural);

	if (new_ang_bone2natural < 0) 
		new_ang_bone2natural = -new_ang_bone2natural; 

	if (new_ang_bone2natural < 0.1 &&  valid_before_adj)	// close enough to natural, no adj
		return valid_before_adj;


	ang = ang < 0 ? -ang : ang;		// get abs of ang
	spring_fac =  ang / bone->j_ang_lim;

	v3_crossp(bone->v4_natural, bone_endpos, vrot_natural);

	aDeg_natural = new_ang_bone2natural * spring_fac * RadianToDegree;

	rotToQuat(q_adj, aDeg_natural, vrot_natural[X], vrot_natural[Y], vrot_natural[Z] );
	qt_mult(q_adj, rptr, rptr);

	//printf("ang to nat=%2.2lf, spr=%2.2lf\n", aDeg_natural, spring_fac);
	
	quaternToRot(rptr, aDeg, x, y, z);	
	
	// check lim on adjusted ang	
	glPushMatrix();
	glLoadIdentity();
		glRotated(aDeg, x, y, z);
		glGetDoublev(GL_MODELVIEW_MATRIX, model_mat);
		glmat44_mult_vec41(model_mat, bone->cnode->offset, bone_endpos);
	glPopMatrix();

	ang = acos( v3_cos_bw2v(bone_endpos, bone->v4_central) ) * RadianToDegree;

	//printf("amt is %2.2lf, a=%2.2lf, lim = (%2.2lf, %2.2lf, %2.2lf)\n",
	//	rot_amt, ang, bone->v4_central[X], bone->v4_central[Y], bone->v4_central[Z]);
		
	if ( (ang < bone->j_ang_lim) && (ang > - bone->j_ang_lim) ) {
		//printf("using adj rot\n");
		return true;
	}
	else if (valid_before_adj) {
		v4_assign(rptr, q);
		return true;
	}

	//printf("Bad rot not added \n");
	v4_assign(rptr, q_orig);	// revert back to orig
	return false;

}





/* updates quat stored at rptr w/ appropriate rot amt */
bool avoid_crazy_twist(double rptr[4], Bone *bone, double new_bone_endpos[4], double rot_axis[3], double rot_amt ) 
{

	double q[4];
	double aDeg_cen2new;	// natural ang from bone->cnetral to new bone_endpos.
	bool   b_was_outlim = false;

	aDeg_cen2new = acos( v3_cos_bw2v(bone->v4_central, new_bone_endpos) ) * RadianToDegree;

	// first get rot from offset to central
	if ( CLOSE_TO_ZERO(aDeg_cen2new) ) {
		v4_assign(q, 1, 0, 0, 0);
		printf("central to new bone 0.\n");
	}
	else if (CLOSE_TO(aDeg_cen2new, 180)) {
		v4_assign(q, -1, 0, 0, 0);
		printf("central to new bone 180.\n");
	}
	else {
		// to keep it in lim
		if (aDeg_cen2new > bone->j_ang_lim || aDeg_cen2new < -bone->j_ang_lim ) {
					
			printf("NOTE: was out lim\n");
			return false;
		}
			
		printf("bone %s: a_cen_to_new =%2.2lf\n,\t cen = (%2.2lf, %2.2lf, %2.2lf), newb = (%2.2lf, %2.2lf, %2.2lf)\n",
			bone->pnode->name, aDeg_cen2new, 
			bone->v4_central[X], bone->v4_central[Y], bone->v4_central[Z],
			new_bone_endpos[X], new_bone_endpos[Y], new_bone_endpos[Z]);

	}


	calc_q_v1tov2(rptr, bone->cnode->offset, new_bone_endpos);
	return true;
}