blender-archive/source/blender/blenkernel/intern/armature.c

/**
 * $Id$
 *
 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version. 
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
 * All rights reserved.
 *
 * Contributor(s): Full recode, Ton Roosendaal, Crete 2005
 *
 * ***** END GPL/BL DUAL LICENSE BLOCK *****
 */

#include <ctype.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <stdio.h>
#include "MEM_guardedalloc.h"

#include "nla.h"

#include "BLI_arithb.h"
#include "BLI_blenlib.h"

#include "DNA_armature_types.h"
#include "DNA_action_types.h"
#include "DNA_constraint_types.h"
#include "DNA_mesh_types.h"
#include "DNA_lattice_types.h"
#include "DNA_meshdata_types.h"
#include "DNA_nla_types.h"
#include "DNA_object_types.h"
#include "DNA_scene_types.h"
#include "DNA_view3d_types.h"

#include "BKE_armature.h"
#include "BKE_action.h"
#include "BKE_blender.h"
#include "BKE_constraint.h"
#include "BKE_curve.h"
#include "BKE_deform.h"
#include "BKE_depsgraph.h"
#include "BKE_DerivedMesh.h"
#include "BKE_displist.h"
#include "BKE_global.h"
#include "BKE_library.h"
#include "BKE_lattice.h"
#include "BKE_main.h"
#include "BKE_object.h"
#include "BKE_object.h"
#include "BKE_utildefines.h"

#include "BIF_editdeform.h"

#include "IK_solver.h"

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

/*	**************** Generic Functions, data level *************** */

bArmature *get_armature(Object *ob)
{
	if(ob==NULL) return NULL;
	if(ob->type==OB_ARMATURE) return ob->data;
	else return NULL;
}

bArmature *add_armature(char *name)
{
	bArmature *arm;
	
	arm= alloc_libblock (&G.main->armature, ID_AR, name);
	arm->deformflag = ARM_DEF_VGROUP|ARM_DEF_ENVELOPE;
	arm->layer= 1;
	return arm;
}


void free_boneChildren(Bone *bone)
{ 
	Bone *child;
	
	if (bone) {
		
		child=bone->childbase.first;
		if (child){
			while (child){
				free_boneChildren (child);
				child=child->next;
			}
			BLI_freelistN (&bone->childbase);
		}
	}
}

void free_bones (bArmature *arm)
{
	Bone *bone;
	/*	Free children (if any)	*/
	bone= arm->bonebase.first;
	if (bone) {
		while (bone){
			free_boneChildren (bone);
			bone=bone->next;
		}
	}
	
	
	BLI_freelistN(&arm->bonebase);
}

void free_armature(bArmature *arm)
{
	if (arm) {
		/*		unlink_armature(arm);*/
		free_bones(arm);
	}
}

void make_local_armature(bArmature *arm)
{
	int local=0, lib=0;
	Object *ob;
	bArmature *newArm;
	
	if (arm->id.lib==0)
		return;
	if (arm->id.us==1) {
		arm->id.lib= 0;
		arm->id.flag= LIB_LOCAL;
		new_id(0, (ID*)arm, 0);
		return;
	}
	
	if(local && lib==0) {
		arm->id.lib= 0;
		arm->id.flag= LIB_LOCAL;
		new_id(0, (ID *)arm, 0);
	}
	else if(local && lib) {
		newArm= copy_armature(arm);
		newArm->id.us= 0;
		
		ob= G.main->object.first;
		while(ob) {
			if(ob->data==arm) {
				
				if(ob->id.lib==0) {
					ob->data= newArm;
					newArm->id.us++;
					arm->id.us--;
				}
			}
			ob= ob->id.next;
		}
	}
}

static void	copy_bonechildren (Bone* newBone, Bone* oldBone)
{
	Bone	*curBone, *newChildBone;
	
	/*	Copy this bone's list*/
	duplicatelist (&newBone->childbase, &oldBone->childbase);
	
	/*	For each child in the list, update it's children*/
	newChildBone=newBone->childbase.first;
	for (curBone=oldBone->childbase.first;curBone;curBone=curBone->next){
		newChildBone->parent=newBone;
		copy_bonechildren(newChildBone,curBone);
		newChildBone=newChildBone->next;
	}
}

bArmature *copy_armature(bArmature *arm)
{
	bArmature *newArm;
	Bone		*oldBone, *newBone;
	
	newArm= copy_libblock (arm);
	duplicatelist(&newArm->bonebase, &arm->bonebase);
	
	/*	Duplicate the childrens' lists*/
	newBone=newArm->bonebase.first;
	for (oldBone=arm->bonebase.first;oldBone;oldBone=oldBone->next){
		newBone->parent=NULL;
		copy_bonechildren (newBone, oldBone);
		newBone=newBone->next;
	};
	
	return newArm;
}

static Bone *get_named_bone_bonechildren (Bone *bone, const char *name)
{
	Bone *curBone, *rbone;
	
	if (!strcmp (bone->name, name))
		return bone;
	
	for (curBone=bone->childbase.first; curBone; curBone=curBone->next){
		rbone=get_named_bone_bonechildren (curBone, name);
		if (rbone)
			return rbone;
	}
	
	return NULL;
}


Bone *get_named_bone (bArmature *arm, const char *name)
/*
	Walk the list until the bone is found
 */
{
	Bone *bone=NULL, *curBone;
	
	if (!arm) return NULL;
	
	for (curBone=arm->bonebase.first; curBone; curBone=curBone->next){
		bone = get_named_bone_bonechildren (curBone, name);
		if (bone)
			return bone;
	}
	
	return bone;
}


#define IS_SEPARATOR(a)	(a=='.' || a==' ' || a=='-' || a=='_')

/* finds the best possible flipped name. For renaming; check for unique names afterwards */
/* if strip_number: removes number extensions */
void bone_flip_name (char *name, int strip_number)
{
	int		len;
	char	prefix[128]={""};	/* The part before the facing */
	char	suffix[128]={""};	/* The part after the facing */
	char	replace[128]={""};	/* The replacement string */
	char	number[128]={""};	/* The number extension string */
	char	*index=NULL;

	len= strlen(name);
	if(len<3) return;	// we don't do names like .R or .L

	/* We first check the case with a .### extension, let's find the last period */
	if(isdigit(name[len-1])) {
		index= strrchr(name, '.');	// last occurrance
		if (index && isdigit(index[1]) ) {		// doesnt handle case bone.1abc2 correct..., whatever!
			if(strip_number==0) 
				strcpy(number, index);
			*index= 0;
			len= strlen(name);
		}
	}

	strcpy (prefix, name);

	/* first case; separator . - _ with extensions r R l L  */
	if( IS_SEPARATOR(name[len-2]) ) {
		switch(name[len-1]) {
			case 'l':
				prefix[len-1]= 0;
				strcpy(replace, "r");
				break;
			case 'r':
				prefix[len-1]= 0;
				strcpy(replace, "l");
				break;
			case 'L':
				prefix[len-1]= 0;
				strcpy(replace, "R");
				break;
			case 'R':
				prefix[len-1]= 0;
				strcpy(replace, "L");
				break;
		}
	}
	/* case; beginning with r R l L , with separator after it */
	else if( IS_SEPARATOR(name[1]) ) {
		switch(name[0]) {
			case 'l':
				strcpy(replace, "r");
				strcpy(suffix, name+1);
				prefix[0]= 0;
				break;
			case 'r':
				strcpy(replace, "l");
				strcpy(suffix, name+1);
				prefix[0]= 0;
				break;
			case 'L':
				strcpy(replace, "R");
				strcpy(suffix, name+1);
				prefix[0]= 0;
				break;
			case 'R':
				strcpy(replace, "L");
				strcpy(suffix, name+1);
				prefix[0]= 0;
				break;
		}
	}
	else if(len > 5) {
		/* hrms, why test for a separator? lets do the rule 'ultimate left or right' */
		index = BLI_strcasestr(prefix, "right");
		if (index==prefix || index==prefix+len-5) {
			if(index[0]=='r') 
				strcpy (replace, "left");
			else {
				if(index[1]=='I') 
					strcpy (replace, "LEFT");
				else
					strcpy (replace, "Left");
			}
			*index= 0;
			strcpy (suffix, index+5);
		}
		else {
			index = BLI_strcasestr(prefix, "left");
			if (index==prefix || index==prefix+len-4) {
				if(index[0]=='l') 
					strcpy (replace, "right");
				else {
					if(index[1]=='E') 
						strcpy (replace, "RIGHT");
					else
						strcpy (replace, "Right");
				}
				*index= 0;
				strcpy (suffix, index+4);
			}
		}		
	}

	sprintf (name, "%s%s%s%s", prefix, replace, suffix, number);
}


/* ************* B-Bone support ******************* */

#define MAX_BBONE_SUBDIV	32

/* data has MAX_BBONE_SUBDIV+1 interpolated points, will become desired amount with equal distances */
static void equalize_bezier(float *data, int desired)
{
	float *fp, totdist, ddist, dist, fac1, fac2;
	float pdist[MAX_BBONE_SUBDIV+1];
	float temp[MAX_BBONE_SUBDIV+1][4];
	int a, nr;
	
	pdist[0]= 0.0f;
	for(a=0, fp= data; a<MAX_BBONE_SUBDIV; a++, fp+=4) {
		QUATCOPY(temp[a], fp);
		pdist[a+1]= pdist[a]+VecLenf(fp, fp+4);
	}
	/* do last point */
	QUATCOPY(temp[a], fp);
	totdist= pdist[a];
	
	/* go over distances and calculate new points */
	ddist= totdist/((float)desired);
	nr= 1;
	for(a=1, fp= data+4; a<desired; a++, fp+=4) {
		
		dist= ((float)a)*ddist;
		
		/* we're looking for location (distance) 'dist' in the array */
		while((dist>= pdist[nr]) && nr<MAX_BBONE_SUBDIV) {
			nr++;
		}
		
		fac1= pdist[nr]- pdist[nr-1];
		fac2= pdist[nr]-dist;
		fac1= fac2/fac1;
		fac2= 1.0f-fac1;
		
		fp[0]= fac1*temp[nr-1][0]+ fac2*temp[nr][0];
		fp[1]= fac1*temp[nr-1][1]+ fac2*temp[nr][1];
		fp[2]= fac1*temp[nr-1][2]+ fac2*temp[nr][2];
		fp[3]= fac1*temp[nr-1][3]+ fac2*temp[nr][3];
	}
	/* set last point, needed for orientation calculus */
	QUATCOPY(fp, temp[MAX_BBONE_SUBDIV]);
}

/* returns pointer to static array, filled with desired amount of bone->segments elements */
/* this calculation is done  within unit bone space */
Mat4 *b_bone_spline_setup(bPoseChannel *pchan, int rest)
{
	static Mat4 bbone_array[MAX_BBONE_SUBDIV];
	static Mat4 bbone_rest_array[MAX_BBONE_SUBDIV];
	Mat4 *result_array= (rest)? bbone_rest_array: bbone_array;
	bPoseChannel *next, *prev;
	Bone *bone= pchan->bone;
	float h1[3], h2[3], scale[3], length, hlength1, hlength2, roll1=0.0f, roll2;
	float mat3[3][3], imat[4][4], posemat[4][4], scalemat[4][4], iscalemat[4][4];
	float data[MAX_BBONE_SUBDIV+1][4], *fp;
	int a, doscale= 0;

	length= bone->length;

	if(!rest) {
		/* check if we need to take non-uniform bone scaling into account */
		scale[0]= VecLength(pchan->pose_mat[0]);
		scale[1]= VecLength(pchan->pose_mat[1]);
		scale[2]= VecLength(pchan->pose_mat[2]);

		if(fabs(scale[0] - scale[1]) > 1e-6f || fabs(scale[1] - scale[2]) > 1e-6f) {
			Mat4One(scalemat);
			scalemat[0][0]= scale[0];
			scalemat[1][1]= scale[1];
			scalemat[2][2]= scale[2];
			Mat4Invert(iscalemat, scalemat);

			length *= scale[1];
			doscale = 1;
		}
	}
	
	hlength1= bone->ease1*length*0.390464f;		// 0.5*sqrt(2)*kappa, the handle length for near-perfect circles
	hlength2= bone->ease2*length*0.390464f;
	
	/* evaluate next and prev bones */
	if(bone->flag & BONE_CONNECTED)
		prev= pchan->parent;
	else
		prev= NULL;
	
	next= pchan->child;
	
	/* find the handle points, since this is inside bone space, the 
		first point = (0,0,0)
		last point =  (0, length, 0) */
	
	if(rest) {
		Mat4Invert(imat, pchan->bone->arm_mat);
	}
	else if(doscale) {
		Mat4CpyMat4(posemat, pchan->pose_mat);
		Mat4Ortho(posemat);
		Mat4Invert(imat, posemat);
	}
	else
		Mat4Invert(imat, pchan->pose_mat);
	
	if(prev) {
		float difmat[4][4], result[3][3], imat3[3][3];

		/* transform previous point inside this bone space */
		if(rest)
			VECCOPY(h1, prev->bone->arm_head)
		else
			VECCOPY(h1, prev->pose_head)
		Mat4MulVecfl(imat, h1);

		if(prev->bone->segments>1) {
			/* if previous bone is B-bone too, use average handle direction */
			h1[1]-= length;
			roll1= 0.0f;
		}

		Normalize(h1);
		VecMulf(h1, -hlength1);

		if(prev->bone->segments==1) {
			/* find the previous roll to interpolate */
			if(rest)
				Mat4MulMat4(difmat, prev->bone->arm_mat, imat);
			else
				Mat4MulMat4(difmat, prev->pose_mat, imat);
			Mat3CpyMat4(result, difmat);				// the desired rotation at beginning of next bone
			
			vec_roll_to_mat3(h1, 0.0f, mat3);			// the result of vec_roll without roll
			
			Mat3Inv(imat3, mat3);
			Mat3MulMat3(mat3, result, imat3);			// the matrix transforming vec_roll to desired roll
			
			roll1= atan2(mat3[2][0], mat3[2][2]);
		}
	}
	else {
		h1[0]= 0.0f; h1[1]= hlength1; h1[2]= 0.0f;
		roll1= 0.0f;
	}
	if(next) {
		float difmat[4][4], result[3][3], imat3[3][3];
		
		/* transform next point inside this bone space */
		if(rest)
			VECCOPY(h2, next->bone->arm_tail)
		else
			VECCOPY(h2, next->pose_tail)
		Mat4MulVecfl(imat, h2);
		/* if next bone is B-bone too, use average handle direction */
		if(next->bone->segments>1);
		else h2[1]-= length;
		Normalize(h2);
		
		/* find the next roll to interpolate as well */
		if(rest)
			Mat4MulMat4(difmat, next->bone->arm_mat, imat);
		else
			Mat4MulMat4(difmat, next->pose_mat, imat);
		Mat3CpyMat4(result, difmat);				// the desired rotation at beginning of next bone
		
		vec_roll_to_mat3(h2, 0.0f, mat3);			// the result of vec_roll without roll
		
		Mat3Inv(imat3, mat3);
		Mat3MulMat3(mat3, imat3, result);			// the matrix transforming vec_roll to desired roll
		
		roll2= atan2(mat3[2][0], mat3[2][2]);
		
		/* and only now negate handle */
		VecMulf(h2, -hlength2);
	}
	else {
		h2[0]= 0.0f; h2[1]= -hlength2; h2[2]= 0.0f;
		roll2= 0.0;
	}

	/* make curve */
	if(bone->segments > MAX_BBONE_SUBDIV)
		bone->segments= MAX_BBONE_SUBDIV;
	
	forward_diff_bezier(0.0, h1[0],		h2[0],			0.0,		data[0],	MAX_BBONE_SUBDIV, 4);
	forward_diff_bezier(0.0, h1[1],		length + h2[1],	length,		data[0]+1,	MAX_BBONE_SUBDIV, 4);
	forward_diff_bezier(0.0, h1[2],		h2[2],			0.0,		data[0]+2,	MAX_BBONE_SUBDIV, 4);
	forward_diff_bezier(roll1, roll1 + 0.390464f*(roll2-roll1), roll2 - 0.390464f*(roll2-roll1),	roll2,	data[0]+3,	MAX_BBONE_SUBDIV, 4);
	
	equalize_bezier(data[0], bone->segments);	// note: does stride 4!
	
	/* make transformation matrices for the segments for drawing */
	for(a=0, fp= data[0]; a<bone->segments; a++, fp+=4) {
		VecSubf(h1, fp+4, fp);
		vec_roll_to_mat3(h1, fp[3], mat3);		// fp[3] is roll

		Mat4CpyMat3(result_array[a].mat, mat3);
		VECCOPY(result_array[a].mat[3], fp);

		if(doscale) {
			/* correct for scaling when this matrix is used in scaled space */
			Mat4MulSerie(result_array[a].mat, iscalemat, result_array[a].mat,
				scalemat, NULL, NULL, NULL, NULL, NULL);
		}
	}
	
	return result_array;
}

/* ************ Armature Deform ******************* */

static void pchan_b_bone_defmats(bPoseChannel *pchan, int use_quaternion, int rest_def)
{
	Bone *bone= pchan->bone;
	Mat4 *b_bone= b_bone_spline_setup(pchan, 0);
	Mat4 *b_bone_rest= (rest_def)? NULL: b_bone_spline_setup(pchan, 1);
	Mat4 *b_bone_mats;
	DualQuat *b_bone_dual_quats= NULL;
	float tmat[4][4];
	int a;
	
	/* allocate b_bone matrices and dual quats */
	b_bone_mats= MEM_mallocN((1+bone->segments)*sizeof(Mat4), "BBone defmats");
	pchan->b_bone_mats= b_bone_mats;

	if(use_quaternion) {
		b_bone_dual_quats= MEM_mallocN((bone->segments)*sizeof(DualQuat), "BBone dqs");
		pchan->b_bone_dual_quats= b_bone_dual_quats;
	}
	
	/* first matrix is the inverse arm_mat, to bring points in local bone space
	   for finding out which segment it belongs to */
	Mat4Invert(b_bone_mats[0].mat, bone->arm_mat);

	/* then we make the b_bone_mats:
	    - first transform to local bone space
		- translate over the curve to the bbone mat space
		- transform with b_bone matrix
		- transform back into global space */
	Mat4One(tmat);

	for(a=0; a<bone->segments; a++) {
		if(b_bone_rest)
			Mat4Invert(tmat, b_bone_rest[a].mat);
		else
			tmat[3][1] = -a*(bone->length/(float)bone->segments);

		Mat4MulSerie(b_bone_mats[a+1].mat, pchan->chan_mat, bone->arm_mat,
			b_bone[a].mat, tmat, b_bone_mats[0].mat, NULL, NULL, NULL);

		if(use_quaternion)
			Mat4ToDQuat(bone->arm_mat, b_bone_mats[a+1].mat, &b_bone_dual_quats[a]);
	}
}

static void b_bone_deform(bPoseChannel *pchan, Bone *bone, float *co, DualQuat *dq, float defmat[][3])
{
	Mat4 *b_bone= pchan->b_bone_mats;
	float (*mat)[4]= b_bone[0].mat;
	float segment, y;
	int a;
	
	/* need to transform co back to bonespace, only need y */
	y= mat[0][1]*co[0] + mat[1][1]*co[1] + mat[2][1]*co[2] + mat[3][1];
	
	/* now calculate which of the b_bones are deforming this */
	segment= bone->length/((float)bone->segments);
	a= (int)(y/segment);
	
	/* note; by clamping it extends deform at endpoints, goes best with
	   straight joints in restpos. */
	CLAMP(a, 0, bone->segments-1);

	if(dq) {
		DQuatCpyDQuat(dq, &((DualQuat*)pchan->b_bone_dual_quats)[a]);
	}
	else {
		Mat4MulVecfl(b_bone[a+1].mat, co);

		if(defmat)
			Mat3CpyMat4(defmat, b_bone[a+1].mat);
	}
}

/* using vec with dist to bone b1 - b2 */
float distfactor_to_bone (float vec[3], float b1[3], float b2[3], float rad1, float rad2, float rdist)
{
	float dist=0.0f; 
	float bdelta[3];
	float pdelta[3];
	float hsqr, a, l, rad;
	
	VecSubf (bdelta, b2, b1);
	l = Normalize (bdelta);
	
	VecSubf (pdelta, vec, b1);
	
	a = bdelta[0]*pdelta[0] + bdelta[1]*pdelta[1] + bdelta[2]*pdelta[2];
	hsqr = ((pdelta[0]*pdelta[0]) + (pdelta[1]*pdelta[1]) + (pdelta[2]*pdelta[2]));
	
	if (a < 0.0F){
		/* If we're past the end of the bone, do a spherical field attenuation thing */
		dist= ((b1[0]-vec[0])*(b1[0]-vec[0]) +(b1[1]-vec[1])*(b1[1]-vec[1]) +(b1[2]-vec[2])*(b1[2]-vec[2])) ;
		rad= rad1;
	}
	else if (a > l){
		/* If we're past the end of the bone, do a spherical field attenuation thing */
		dist= ((b2[0]-vec[0])*(b2[0]-vec[0]) +(b2[1]-vec[1])*(b2[1]-vec[1]) +(b2[2]-vec[2])*(b2[2]-vec[2])) ;
		rad= rad2;
	}
	else {
		dist= (hsqr - (a*a));
		
		if(l!=0.0f) {
			rad= a/l;
			rad= rad*rad2 + (1.0-rad)*rad1;
		}
		else rad= rad1;
	}
	
	a= rad*rad;
	if(dist < a) 
		return 1.0f;
	else {
		l= rad+rdist;
		l*= l;
		if(rdist==0.0f || dist >= l) 
			return 0.0f;
		else {
			a= sqrt(dist)-rad;
			return 1.0-( a*a )/( rdist*rdist );
		}
	}
}

static void pchan_deform_mat_add(bPoseChannel *pchan, float weight, float bbonemat[][3], float mat[][3])
{
	float wmat[3][3];

	if(pchan->bone->segments>1)
		Mat3CpyMat3(wmat, bbonemat);
	else
		Mat3CpyMat4(wmat, pchan->chan_mat);

	Mat3MulFloat((float*)wmat, weight);
	Mat3AddMat3(mat, mat, wmat);
}

static float dist_bone_deform(bPoseChannel *pchan, float *vec, DualQuat *dq, float mat[][3], float *co)
{
	Bone *bone= pchan->bone;
	float fac, contrib=0.0;
	float cop[3], bbonemat[3][3];
	DualQuat bbonedq;

	if(bone==NULL) return 0.0f;
	
	VECCOPY (cop, co);

	fac= distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
	
	if (fac>0.0) {
		
		fac*=bone->weight;
		contrib= fac;
		if(contrib>0.0) {
			if(vec) {
				if(bone->segments>1)
					// applies on cop and bbonemat
					b_bone_deform(pchan, bone, cop, NULL, (mat)?bbonemat:NULL);
				else
					Mat4MulVecfl(pchan->chan_mat, cop);

				//	Make this a delta from the base position
				VecSubf (cop, cop, co);
				cop[0]*=fac; cop[1]*=fac; cop[2]*=fac;
				VecAddf (vec, vec, cop);

				if(mat)
					pchan_deform_mat_add(pchan, fac, bbonemat, mat);
			}
			else {
				if(bone->segments>1) {
					b_bone_deform(pchan, bone, cop, &bbonedq, NULL);
					DQuatAddWeighted(dq, &bbonedq, fac);
				}
				else
					DQuatAddWeighted(dq, pchan->dual_quat, fac);
			}
		}
	}
	
	return contrib;
}

static void pchan_bone_deform(bPoseChannel *pchan, float weight, float *vec, DualQuat *dq, float mat[][3], float *co, float *contrib)
{
	float cop[3], bbonemat[3][3];
	DualQuat bbonedq;

	if (!weight)
		return;

	VECCOPY(cop, co);

	if(vec) {
		if(pchan->bone->segments>1)
			// applies on cop and bbonemat
			b_bone_deform(pchan, pchan->bone, cop, NULL, (mat)?bbonemat:NULL);
		else
			Mat4MulVecfl(pchan->chan_mat, cop);
		
		vec[0]+=(cop[0]-co[0])*weight;
		vec[1]+=(cop[1]-co[1])*weight;
		vec[2]+=(cop[2]-co[2])*weight;

		if(mat)
			pchan_deform_mat_add(pchan, weight, bbonemat, mat);
	}
	else {
		if(pchan->bone->segments>1) {
			b_bone_deform(pchan, pchan->bone, cop, &bbonedq, NULL);
			DQuatAddWeighted(dq, &bbonedq, weight);
		}
		else
			DQuatAddWeighted(dq, pchan->dual_quat, weight);
	}

	(*contrib)+=weight;
}

void armature_deform_verts(Object *armOb, Object *target, DerivedMesh *dm,
                           float (*vertexCos)[3], float (*defMats)[3][3],
						   int numVerts, int deformflag, 
						   float (*prevCos)[3], const char *defgrp_name)
{
	bPoseChannel *pchan, **defnrToPC = NULL;
	MDeformVert *dverts = NULL;
	bDeformGroup *dg;
	DualQuat *dualquats= NULL;
	float obinv[4][4], premat[4][4], postmat[4][4];
	int use_envelope = deformflag & ARM_DEF_ENVELOPE;
	int use_quaternion = deformflag & ARM_DEF_QUATERNION;
	int bbone_rest_def = deformflag & ARM_DEF_B_BONE_REST;
	int invert_vgroup= deformflag & ARM_DEF_INVERT_VGROUP;
	int numGroups = 0;		/* safety for vertexgroup index overflow */
	int i, target_totvert = 0;	/* safety for vertexgroup overflow */
	int use_dverts = 0;
	int armature_def_nr = -1;
	int totchan;

	if(armOb == G.obedit) return;
	
	Mat4Invert(obinv, target->obmat);
	Mat4CpyMat4(premat, target->obmat);
	Mat4MulMat4(postmat, armOb->obmat, obinv);
	Mat4Invert(premat, postmat);

	/* bone defmats are already in the channels, chan_mat */
	
	/* initialize B_bone matrices and dual quaternions */
	if(use_quaternion) {
		totchan= BLI_countlist(&armOb->pose->chanbase);
		dualquats= MEM_callocN(sizeof(DualQuat)*totchan, "dualquats");
	}

	totchan= 0;
	for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
		if(!(pchan->bone->flag & BONE_NO_DEFORM)) {
			if(pchan->bone->segments > 1)
				pchan_b_bone_defmats(pchan, use_quaternion, bbone_rest_def);

			if(use_quaternion) {
				pchan->dual_quat= &dualquats[totchan++];
				Mat4ToDQuat(pchan->bone->arm_mat, pchan->chan_mat, pchan->dual_quat);
			}
		}
	}

	/* get the def_nr for the overall armature vertex group if present */
	for(i = 0, dg = target->defbase.first; dg; i++, dg = dg->next)
		if(defgrp_name && strcmp(defgrp_name, dg->name) == 0)
			armature_def_nr = i;

	/* get a vertex-deform-index to posechannel array */
	if(deformflag & ARM_DEF_VGROUP) {
		if(ELEM(target->type, OB_MESH, OB_LATTICE)) {
			numGroups = BLI_countlist(&target->defbase);
			
			if(target->type==OB_MESH) {
				Mesh *me= target->data;
				dverts = me->dvert;
				target_totvert = me->totvert;
			}
			else {
				Lattice *lt= target->data;
				dverts = lt->dvert;
				if(dverts)
					target_totvert = lt->pntsu*lt->pntsv*lt->pntsw;
			}
			/* if we have a DerivedMesh, only use dverts if it has them */
			if(dm)
				if(dm->getVertData(dm, 0, CD_MDEFORMVERT))
					use_dverts = 1;
				else use_dverts = 0;
			else if(dverts) use_dverts = 1;

			if(use_dverts) {
				defnrToPC = MEM_callocN(sizeof(*defnrToPC) * numGroups,
				                        "defnrToBone");
				for(i = 0, dg = target->defbase.first; dg;
				    i++, dg = dg->next) {
					defnrToPC[i] = get_pose_channel(armOb->pose, dg->name);
					/* exclude non-deforming bones */
					if(defnrToPC[i]) {
						if(defnrToPC[i]->bone->flag & BONE_NO_DEFORM)
							defnrToPC[i]= NULL;
					}
				}
			}
		}
	}

	for(i = 0; i < numVerts; i++) {
		MDeformVert *dvert;
		DualQuat sumdq, *dq = NULL;
		float *co, dco[3];
		float sumvec[3], summat[3][3];
		float *vec = NULL, (*smat)[3] = NULL;
		float contrib = 0.0f;
		float armature_weight = 1.0f;	/* default to 1 if no overall def group */
		float prevco_weight = 1.0f;		/* weight for optional cached vertexcos */
		int	  j;

		if(use_quaternion) {
			memset(&sumdq, 0, sizeof(DualQuat));
			dq= &sumdq;
		}
		else {
			sumvec[0] = sumvec[1] = sumvec[2] = 0.0f;
			vec= sumvec;

			if(defMats) {
				Mat3Clr((float*)summat);
				smat = summat;
			}
		}

		if(use_dverts || armature_def_nr >= 0) {
			if(dm) dvert = dm->getVertData(dm, i, CD_MDEFORMVERT);
			else if(dverts && i < target_totvert) dvert = dverts + i;
			else dvert = NULL;
		} else
			dvert = NULL;

		if(armature_def_nr >= 0 && dvert) {
			armature_weight = 0.0f; /* a def group was given, so default to 0 */
			for(j = 0; j < dvert->totweight; j++) {
				if(dvert->dw[j].def_nr == armature_def_nr) {
					armature_weight = dvert->dw[j].weight;
					break;
				}
			}
			/* hackish: the blending factor can be used for blending with prevCos too */
			if(prevCos) {
				if(invert_vgroup)
					prevco_weight= 1.0f-armature_weight;
				else
					prevco_weight= armature_weight;
				armature_weight= 1.0f;
			}
		}

		/* check if there's any  point in calculating for this vert */
		if(armature_weight == 0.0f) continue;
		
		/* get the coord we work on */
		co= prevCos?prevCos[i]:vertexCos[i];
		
		/* Apply the object's matrix */
		Mat4MulVecfl(premat, co);
		
		if(use_dverts && dvert && dvert->totweight) { // use weight groups ?
			int deformed = 0;
			
			for(j = 0; j < dvert->totweight; j++){
				int index = dvert->dw[j].def_nr;
				pchan = index < numGroups?defnrToPC[index]:NULL;
				if(pchan) {
					float weight = dvert->dw[j].weight;
					Bone *bone = pchan->bone;

					deformed = 1;
					
					if(bone && bone->flag & BONE_MULT_VG_ENV) {
						weight *= distfactor_to_bone(co, bone->arm_head,
						                             bone->arm_tail,
						                             bone->rad_head,
						                             bone->rad_tail,
						                             bone->dist);
					}
					pchan_bone_deform(pchan, weight, vec, dq, smat, co, &contrib);
				}
			}
			/* if there are vertexgroups but not groups with bones
			 * (like for softbody groups)
			 */
			if(deformed == 0 && use_envelope) {
				for(pchan = armOb->pose->chanbase.first; pchan;
				    pchan = pchan->next) {
					if(!(pchan->bone->flag & BONE_NO_DEFORM))
						contrib += dist_bone_deform(pchan, vec, dq, smat, co);
				}
			}
		}
		else if(use_envelope) {
			for(pchan = armOb->pose->chanbase.first; pchan;
			    pchan = pchan->next) {
				if(!(pchan->bone->flag & BONE_NO_DEFORM))
					contrib += dist_bone_deform(pchan, vec, dq, smat, co);
			}
		}

		/* actually should be EPSILON? weight values and contrib can be like 10e-39 small */
		if(contrib > 0.0001f) {
			if(use_quaternion) {
				DQuatNormalize(dq, contrib);

				if(armature_weight != 1.0f) {
					VECCOPY(dco, co);
					DQuatMulVecfl(dq, dco, (defMats)? summat: NULL);
					VecSubf(dco, dco, co);
					VecMulf(dco, armature_weight);
					VecAddf(co, co, dco);
				}
				else
					DQuatMulVecfl(dq, co, (defMats)? summat: NULL);

				smat = summat;
			}
			else {
				VecMulf(vec, armature_weight/contrib);
				VecAddf(co, vec, co);
			}

			if(defMats) {
				float pre[3][3], post[3][3], tmpmat[3][3];

				Mat3CpyMat4(pre, premat);
				Mat3CpyMat4(post, postmat);
				Mat3CpyMat3(tmpmat, defMats[i]);

				if(!use_quaternion) /* quaternion already is scale corrected */
					Mat3MulFloat((float*)smat, armature_weight/contrib);

				Mat3MulSerie(defMats[i], tmpmat, pre, smat, post,
					NULL, NULL, NULL, NULL);
			}
		}
		
		/* always, check above code */
		Mat4MulVecfl(postmat, co);
		
		
		/* interpolate with previous modifier position using weight group */
		if(prevCos) {
			float mw= 1.0f - prevco_weight;
			vertexCos[i][0]= prevco_weight*vertexCos[i][0] + mw*co[0];
			vertexCos[i][1]= prevco_weight*vertexCos[i][1] + mw*co[1];
			vertexCos[i][2]= prevco_weight*vertexCos[i][2] + mw*co[2];
		}
	}

	if(dualquats) MEM_freeN(dualquats);
	if(defnrToPC) MEM_freeN(defnrToPC);
	
	/* free B_bone matrices */
	for(pchan = armOb->pose->chanbase.first; pchan; pchan = pchan->next) {
		if(pchan->b_bone_mats) {
			MEM_freeN(pchan->b_bone_mats);
			pchan->b_bone_mats = NULL;
		}
		if(pchan->b_bone_dual_quats) {
			MEM_freeN(pchan->b_bone_dual_quats);
			pchan->b_bone_dual_quats = NULL;
		}

		pchan->dual_quat = NULL;
	}
}

/* ************ END Armature Deform ******************* */

void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed)
{
	Mat4CpyMat4(M_accumulatedMatrix, bone->arm_mat);
}

/* **************** Space to Space API ****************** */

/* Convert World-Space Matrix to Pose-Space Matrix */
void armature_mat_world_to_pose(Object *ob, float inmat[][4], float outmat[][4]) 
{
	float obmat[4][4];
	
	/* prevent crashes */
	if (ob==NULL) return;
	
	/* get inverse of (armature) object's matrix  */
	Mat4Invert(obmat, ob->obmat);
	
	/* multiply given matrix by object's-inverse to find pose-space matrix */
	Mat4MulMat4(outmat, obmat, inmat);
}

/* Convert Wolrd-Space Location to Pose-Space Location
 * NOTE: this cannot be used to convert to pose-space location of the supplied
 * 		pose-channel into its local space (i.e. 'visual'-keyframing) 
 */
void armature_loc_world_to_pose(Object *ob, float *inloc, float *outloc) 
{
	float xLocMat[4][4];
	float nLocMat[4][4];
	
	/* build matrix for location */
	Mat4One(xLocMat);
	VECCOPY(xLocMat[3], inloc);

	/* get bone-space cursor matrix and extract location */
	armature_mat_world_to_pose(ob, xLocMat, nLocMat);
	VECCOPY(outloc, nLocMat[3]);
}

/* Convert Pose-Space Matrix to Bone-Space Matrix 
 * NOTE: this cannot be used to convert to pose-space transforms of the supplied
 * 		pose-channel into its local space (i.e. 'visual'-keyframing)
 */
void armature_mat_pose_to_bone(bPoseChannel *pchan, float inmat[][4], float outmat[][4])
{
	float pc_trans[4][4], inv_trans[4][4];
	float pc_posemat[4][4], inv_posemat[4][4];
	
	/* paranoia: prevent crashes with no pose-channel supplied */
	if (pchan==NULL) return;
	
	/* get the inverse matrix of the pchan's transforms */
	LocQuatSizeToMat4(pc_trans, pchan->loc, pchan->quat, pchan->size);
	Mat4Invert(inv_trans, pc_trans);
	
	/* Remove the pchan's transforms from it's pose_mat.
	 * This should leave behind the effects of restpose + 
	 * parenting + constraints
	 */
	Mat4MulMat4(pc_posemat, inv_trans, pchan->pose_mat);
	
	/* get the inverse of the leftovers so that we can remove 
	 * that component from the supplied matrix
	 */
	Mat4Invert(inv_posemat, pc_posemat);
	
	/* get the new matrix */
	Mat4MulMat4(outmat, inmat, inv_posemat);
}

/* Convert Pose-Space Location to Bone-Space Location
 * NOTE: this cannot be used to convert to pose-space location of the supplied
 * 		pose-channel into its local space (i.e. 'visual'-keyframing) 
 */
void armature_loc_pose_to_bone(bPoseChannel *pchan, float *inloc, float *outloc) 
{
	float xLocMat[4][4];
	float nLocMat[4][4];
	
	/* build matrix for location */
	Mat4One(xLocMat);
	VECCOPY(xLocMat[3], inloc);

	/* get bone-space cursor matrix and extract location */
	armature_mat_pose_to_bone(pchan, xLocMat, nLocMat);
	VECCOPY(outloc, nLocMat[3]);
}

/* Remove rest-position effects from pose-transform for obtaining
 * 'visual' transformation of pose-channel. 
 * (used by the Visual-Keyframing stuff)
 */
void armature_mat_pose_to_delta(float delta_mat[][4], float pose_mat[][4], float arm_mat[][4])
{
 	float imat[4][4];
 
 	Mat4Invert(imat, arm_mat);
 	Mat4MulMat4(delta_mat, pose_mat, imat);
}


/* **************** The new & simple (but OK!) armature evaluation ********* */ 

/*  ****************** And how it works! ****************************************

  This is the bone transformation trick; they're hierarchical so each bone(b)
  is in the coord system of bone(b-1):

  arm_mat(b)= arm_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) 
  
  -> yoffs is just the y axis translation in parent's coord system
  -> d_root is the translation of the bone root, also in parent's coord system

  pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b)

  we then - in init deform - store the deform in chan_mat, such that:

  pose_mat(b)= arm_mat(b) * chan_mat(b)
  
  *************************************************************************** */
/*  Computes vector and roll based on a rotation. "mat" must
     contain only a rotation, and no scaling. */ 
void mat3_to_vec_roll(float mat[][3], float *vec, float *roll) 
{
    if (vec)
        VecCopyf(vec, mat[1]);

    if (roll) {
        float vecmat[3][3], vecmatinv[3][3], rollmat[3][3];

        vec_roll_to_mat3(mat[1], 0.0f, vecmat);
        Mat3Inv(vecmatinv, vecmat);
        Mat3MulMat3(rollmat, vecmatinv, mat);

        *roll= atan2(rollmat[2][0], rollmat[2][2]);
    }
}

/*	Calculates the rest matrix of a bone based
	On its vector and a roll around that vector */
void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
{
	float	nor[3], axis[3], target[3]={0,1,0};
	float	theta;
	float	rMatrix[3][3], bMatrix[3][3];
	
	VECCOPY (nor, vec);
	Normalize (nor);
	
	/*	Find Axis & Amount for bone matrix*/
	Crossf (axis,target,nor);

	if (Inpf(axis,axis) > 0.0000000000001) {
		/* if nor is *not* a multiple of target ... */
		Normalize (axis);
		
		theta= NormalizedVecAngle2(target, nor);
		
		/*	Make Bone matrix*/
		VecRotToMat3(axis, theta, bMatrix);
	}
	else {
		/* if nor is a multiple of target ... */
		float updown;
		
		/* point same direction, or opposite? */
		updown = ( Inpf (target,nor) > 0 ) ? 1.0 : -1.0;
		
		/* I think this should work ... */
		bMatrix[0][0]=updown; bMatrix[0][1]=0.0;    bMatrix[0][2]=0.0;
		bMatrix[1][0]=0.0;    bMatrix[1][1]=updown; bMatrix[1][2]=0.0;
		bMatrix[2][0]=0.0;    bMatrix[2][1]=0.0;    bMatrix[2][2]=1.0;
	}
	
	/*	Make Roll matrix*/
	VecRotToMat3(nor, roll, rMatrix);
	
	/*	Combine and output result*/
	Mat3MulMat3 (mat, rMatrix, bMatrix);
}


/* recursive part, calculates restposition of entire tree of children */
/* used by exiting editmode too */
void where_is_armature_bone(Bone *bone, Bone *prevbone)
{
	float vec[3];
	
	/* Bone Space */
	VecSubf (vec, bone->tail, bone->head);
	vec_roll_to_mat3(vec, bone->roll, bone->bone_mat);

	bone->length= VecLenf(bone->head, bone->tail);
	
	/* this is called on old file reading too... */
	if(bone->xwidth==0.0) {
		bone->xwidth= 0.1f;
		bone->zwidth= 0.1f;
		bone->segments= 1;
	}
	
	if(prevbone) {
		float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
		
		/* bone transform itself */
		Mat4CpyMat3(offs_bone, bone->bone_mat);
				
		/* The bone's root offset (is in the parent's coordinate system) */
		VECCOPY(offs_bone[3], bone->head);

		/* Get the length translation of parent (length along y axis) */
		offs_bone[3][1]+= prevbone->length;
		
		/* Compose the matrix for this bone  */
		Mat4MulMat4(bone->arm_mat, offs_bone, prevbone->arm_mat);
	}
	else {
		Mat4CpyMat3(bone->arm_mat, bone->bone_mat);
		VECCOPY(bone->arm_mat[3], bone->head);
	}
	
	/* head */
	VECCOPY(bone->arm_head, bone->arm_mat[3]);
	/* tail is in current local coord system */
	VECCOPY(vec, bone->arm_mat[1]);
	VecMulf(vec, bone->length);
	VecAddf(bone->arm_tail, bone->arm_head, vec);
	
	/* and the kiddies */
	prevbone= bone;
	for(bone= bone->childbase.first; bone; bone= bone->next) {
		where_is_armature_bone(bone, prevbone);
	}
}

/* updates vectors and matrices on rest-position level, only needed 
   after editing armature itself, now only on reading file */
void where_is_armature (bArmature *arm)
{
	Bone *bone;
	
	/* hierarchical from root to children */
	for(bone= arm->bonebase.first; bone; bone= bone->next) {
		where_is_armature_bone(bone, NULL);
	}
}

/* if bone layer is protected, copy the data from from->pose */
static void pose_proxy_synchronize(Object *ob, Object *from, int layer_protected)
{
	bPose *pose= ob->pose, *frompose= from->pose;
	bPoseChannel *pchan, *pchanp, pchanw;
	bConstraint *con;
	
	if(frompose==NULL) return;
	
	/* exception, armature local layer should be proxied too */
	if(pose->proxy_layer)
		((bArmature *)ob->data)->layer= pose->proxy_layer;
	
	/* clear all transformation values from library */
	rest_pose(frompose);
	
	pchan= pose->chanbase.first;
	for(; pchan; pchan= pchan->next) {
		if(pchan->bone->layer & layer_protected) {
			pchanp= get_pose_channel(frompose, pchan->name);
			
			/* copy posechannel to temp, but restore important pointers */
			pchanw= *pchanp;
			pchanw.prev= pchan->prev;
			pchanw.next= pchan->next;
			pchanw.parent= pchan->parent;
			pchanw.child= pchan->child;
			pchanw.path= NULL;
			
			/* constraints, set target ob pointer to own object */
			copy_constraints(&pchanw.constraints, &pchanp->constraints);
			
			for (con= pchanw.constraints.first; con; con= con->next) {
				bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
				ListBase targets = {NULL, NULL};
				bConstraintTarget *ct;
				
				if (cti && cti->get_constraint_targets) {
					cti->get_constraint_targets(con, &targets);
					
					for (ct= targets.first; ct; ct= ct->next) {
						if (ct->tar == from)
							ct->tar = ob;
					}
					
					if (cti->flush_constraint_targets)
						cti->flush_constraint_targets(con, &targets, 0);
				}
			}
			
			/* free stuff from current channel */
			if (pchan->path) MEM_freeN(pchan->path);
			free_constraints(&pchan->constraints);
			
			/* the final copy */
			*pchan= pchanw;
		}
	}
}

static int rebuild_pose_bone(bPose *pose, Bone *bone, bPoseChannel *parchan, int counter)
{
	bPoseChannel *pchan = verify_pose_channel (pose, bone->name);   // verify checks and/or adds

	pchan->bone= bone;
	pchan->parent= parchan;
	
	counter++;
	
	for(bone= bone->childbase.first; bone; bone= bone->next) {
		counter= rebuild_pose_bone(pose, bone, pchan, counter);
		/* for quick detecting of next bone in chain, only b-bone uses it now */
		if(bone->flag & BONE_CONNECTED)
			pchan->child= get_pose_channel(pose, bone->name);
	}
	
	return counter;
}

/* only after leave editmode, duplicating, validating older files, library syncing */
/* NOTE: pose->flag is set for it */
void armature_rebuild_pose(Object *ob, bArmature *arm)
{
	Bone *bone;
	bPose *pose;
	bPoseChannel *pchan, *next;
	int counter=0;
		
	/* only done here */
	if(ob->pose==NULL) ob->pose= MEM_callocN(sizeof(bPose), "new pose");
	pose= ob->pose;
	
	/* clear */
	for(pchan= pose->chanbase.first; pchan; pchan= pchan->next) {
		pchan->bone= NULL;
		pchan->child= NULL;
	}
	
	/* first step, check if all channels are there */
	for(bone= arm->bonebase.first; bone; bone= bone->next) {
		counter= rebuild_pose_bone(pose, bone, NULL, counter);
	}

	/* and a check for garbage */
	for(pchan= pose->chanbase.first; pchan; pchan= next) {
		next= pchan->next;
		if(pchan->bone==NULL) {
			if(pchan->path)
				MEM_freeN(pchan->path);
			free_constraints(&pchan->constraints);
			BLI_freelinkN(&pose->chanbase, pchan);
		}
	}
	// printf("rebuild pose %s, %d bones\n", ob->id.name, counter);
	
	/* synchronize protected layers with proxy */
	if(ob->proxy)
		pose_proxy_synchronize(ob, ob->proxy, arm->layer_protected);
	
	update_pose_constraint_flags(ob->pose); // for IK detection for example
	
	/* the sorting */
	if(counter>1)
		DAG_pose_sort(ob);
	
	ob->pose->flag &= ~POSE_RECALC;
}


/* ********************** THE IK SOLVER ******************* */



/* allocates PoseTree, and links that to root bone/channel */
/* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
{
	bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
	PoseTree *tree;
	PoseTarget *target;
	bConstraint *con;
	bKinematicConstraint *data;
	int a, segcount= 0, size, newsize, *oldparent, parent;
	
	/* find IK constraint, and validate it */
	for(con= pchan_tip->constraints.first; con; con= con->next) {
		if(con->type==CONSTRAINT_TYPE_KINEMATIC) break;
	}
	if(con==NULL) return;
	
	data=(bKinematicConstraint*)con->data;
	
	/* two types of targets */
	if(data->flag & CONSTRAINT_IK_AUTO);
	else {
		if(con->flag & CONSTRAINT_DISABLE) return;	/* checked in editconstraint.c */
		if(con->enforce == 0.0f) return;
		if(data->tar==NULL) return;
		if(data->tar->type==OB_ARMATURE && data->subtarget[0]==0) return;
	}
	
	/* exclude tip from chain? */
	if(!(data->flag & CONSTRAINT_IK_TIP))
		pchan_tip= pchan_tip->parent;
	
	/* Find the chain's root & count the segments needed */
	for (curchan = pchan_tip; curchan; curchan=curchan->parent){
		pchan_root = curchan;
		
		curchan->flag |= POSE_CHAIN;	// don't forget to clear this
		chanlist[segcount]=curchan;
		segcount++;
		
		if(segcount==data->rootbone || segcount>255) break; // 255 is weak
	}
	if (!segcount) return;

	/* setup the chain data */
	
	/* we make tree-IK, unless all existing targets are in this chain */
	for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
		for(target= tree->targets.first; target; target= target->next) {
			curchan= tree->pchan[target->tip];
			if(curchan->flag & POSE_CHAIN)
				curchan->flag &= ~POSE_CHAIN;
			else
				break;
		}
		if(target) break;
	}

	/* create a target */
	target= MEM_callocN(sizeof(PoseTarget), "posetarget");
	target->con= con;
	pchan_tip->flag &= ~POSE_CHAIN;

	if(tree==NULL) {
		/* make new tree */
		tree= MEM_callocN(sizeof(PoseTree), "posetree");

		tree->iterations= data->iterations;
		tree->totchannel= segcount;
		tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
		
		tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
		tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
		for(a=0; a<segcount; a++) {
			tree->pchan[a]= chanlist[segcount-a-1];
			tree->parent[a]= a-1;
		}
		target->tip= segcount-1;
		
		/* AND! link the tree to the root */
		BLI_addtail(&pchan_root->iktree, tree);
	}
	else {
		tree->iterations= MAX2(data->iterations, tree->iterations);
		tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);

		/* skip common pose channels and add remaining*/
		size= MIN2(segcount, tree->totchannel);
		for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
		parent= a-1;

		segcount= segcount-a;
		target->tip= tree->totchannel + segcount - 1;

		if (segcount > 0) {
			/* resize array */
			newsize= tree->totchannel + segcount;
			oldchan= tree->pchan;
			oldparent= tree->parent;

			tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
			tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
			memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
			memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
			MEM_freeN(oldchan);
			MEM_freeN(oldparent);

			/* add new pose channels at the end, in reverse order */
			for(a=0; a<segcount; a++) {
				tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
				tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
			}
			tree->parent[tree->totchannel]= parent;
			
			tree->totchannel= newsize;
		}

		/* move tree to end of list, for correct evaluation order */
		BLI_remlink(&pchan_root->iktree, tree);
		BLI_addtail(&pchan_root->iktree, tree);
	}

	/* add target to the tree */
	BLI_addtail(&tree->targets, target);
}

/* called from within the core where_is_pose loop, all animsystems and constraints
were executed & assigned. Now as last we do an IK pass */
static void execute_posetree(Object *ob, PoseTree *tree)
{
	float R_parmat[3][3];
	float iR_parmat[3][3];
	float R_bonemat[3][3];
	float goalrot[3][3], goalpos[3];
	float rootmat[4][4], imat[4][4];
	float goal[4][4], goalinv[4][4];
	float irest_basis[3][3], full_basis[3][3];
	float end_pose[4][4], world_pose[4][4];
	float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
	int a, flag, hasstretch=0;
	bPoseChannel *pchan;
	IK_Segment *seg, *parent, **iktree, *iktarget;
	IK_Solver *solver;
	PoseTarget *target;
	bKinematicConstraint *data, *poleangledata=NULL;
	Bone *bone;

	if (tree->totchannel == 0)
		return;

	iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");

	for(a=0; a<tree->totchannel; a++) {
		pchan= tree->pchan[a];
		bone= pchan->bone;

		/* set DoF flag */
		flag= 0;
		if((pchan->ikflag & BONE_IK_NO_XDOF) == 0)
			flag |= IK_XDOF;
		if((pchan->ikflag & BONE_IK_NO_YDOF) == 0)
			flag |= IK_YDOF;
		if((pchan->ikflag & BONE_IK_NO_ZDOF) == 0)
			flag |= IK_ZDOF;

		if(tree->stretch && (pchan->ikstretch > 0.0)) {
			flag |= IK_TRANS_YDOF;
			hasstretch = 1;
		}

		seg= iktree[a]= IK_CreateSegment(flag);

		/* find parent */
		if(a == 0)
			parent= NULL;
		else
			parent= iktree[tree->parent[a]];

		IK_SetParent(seg, parent);
	
		/* get the matrix that transforms from prevbone into this bone */
		Mat3CpyMat4(R_bonemat, pchan->pose_mat);

		/* gather transformations for this IK segment */

		if (pchan->parent)
			Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
		else
			Mat3One(R_parmat);

		/* bone offset */
		if (pchan->parent && (a > 0))
			VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
		else
			/* only root bone (a = 0) has no parent */
			start[0]= start[1]= start[2]= 0.0f;
		
		/* change length based on bone size */
		length= bone->length*VecLength(R_bonemat[1]);

		/* compute rest basis and its inverse */
		Mat3CpyMat3(rest_basis, bone->bone_mat);
		Mat3CpyMat3(irest_basis, bone->bone_mat);
		Mat3Transp(irest_basis);

		/* compute basis with rest_basis removed */
		Mat3Inv(iR_parmat, R_parmat);
		Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
		Mat3MulMat3(basis, irest_basis, full_basis);

		/* basis must be pure rotation */
		Mat3Ortho(basis);

		/* transform offset into local bone space */
		Mat3Ortho(iR_parmat);
		Mat3MulVecfl(iR_parmat, start);

		IK_SetTransform(seg, start, rest_basis, basis, length);

		if (pchan->ikflag & BONE_IK_XLIMIT)
			IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
		if (pchan->ikflag & BONE_IK_YLIMIT)
			IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
		if (pchan->ikflag & BONE_IK_ZLIMIT)
			IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);

		IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
		IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
		IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);

		if(tree->stretch && (pchan->ikstretch > 0.0)) {
			float ikstretch = pchan->ikstretch*pchan->ikstretch;
			IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.999));
			IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
		}
	}

	solver= IK_CreateSolver(iktree[0]);

	/* set solver goals */

	/* first set the goal inverse transform, assuming the root of tree was done ok! */
	pchan= tree->pchan[0];
	if (pchan->parent)
		/* transform goal by parent mat, so this rotation is not part of the
		   segment's basis. otherwise rotation limits do not work on the
		   local transform of the segment itself. */
		Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
	else
		Mat4One(rootmat);
	VECCOPY(rootmat[3], pchan->pose_head);
	
	Mat4MulMat4 (imat, rootmat, ob->obmat);
	Mat4Invert (goalinv, imat);
	
	for (target=tree->targets.first; target; target=target->next) {
		float polepos[3];
		int poleconstrain= 0;

		data= (bKinematicConstraint*)target->con->data;
		
		/* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
		 * strictly speaking, it is a posechannel)
		 */
		get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
		
		/* and set and transform goal */
		Mat4MulMat4(goal, rootmat, goalinv);
		
		VECCOPY(goalpos, goal[3]);
		Mat3CpyMat4(goalrot, goal);
		
		/* same for pole vector target */
		if(data->poletar) {
			get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);

			if(data->flag & CONSTRAINT_IK_SETANGLE) {
				/* don't solve IK when we are setting the pole angle */
				break;
			}
			else {
				Mat4MulMat4(goal, rootmat, goalinv);
				VECCOPY(polepos, goal[3]);
				poleconstrain= 1;

				if(data->flag & CONSTRAINT_IK_GETANGLE) {
					poleangledata= data;
					data->flag &= ~CONSTRAINT_IK_GETANGLE;
				}
			}
		}

		/* do we need blending? */
		if (target->con->enforce!=1.0) {
			float q1[4], q2[4], q[4];
			float fac= target->con->enforce;
			float mfac= 1.0-fac;
			
			pchan= tree->pchan[target->tip];
			
			/* end effector in world space */
			Mat4CpyMat4(end_pose, pchan->pose_mat);
			VECCOPY(end_pose[3], pchan->pose_tail);
			Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
			
			/* blend position */
			goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
			goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
			goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
			
			/* blend rotation */
			Mat3ToQuat(goalrot, q1);
			Mat4ToQuat(world_pose, q2);
			QuatInterpol(q, q1, q2, mfac);
			QuatToMat3(q, goalrot);
		}
		
		iktarget= iktree[target->tip];
		
		if(data->weight != 0.0) {
			if(poleconstrain)
				IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
					polepos, data->poleangle*M_PI/180, (poleangledata == data));
			IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
		}
		if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
			if((data->flag & CONSTRAINT_IK_AUTO)==0)
				IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
					data->orientweight);
	}

	/* solve */
	IK_Solve(solver, 0.0f, tree->iterations);

	if(poleangledata)
		poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180/M_PI;

	IK_FreeSolver(solver);

	/* gather basis changes */
	tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
	if(hasstretch)
		ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");

	for(a=0; a<tree->totchannel; a++) {
		IK_GetBasisChange(iktree[a], tree->basis_change[a]);

		if(hasstretch) {
			/* have to compensate for scaling received from parent */
			float parentstretch, stretch;

			pchan= tree->pchan[a];
			parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;

			if(tree->stretch && (pchan->ikstretch > 0.0)) {
				float trans[3], length;

				IK_GetTranslationChange(iktree[a], trans);
				length= pchan->bone->length*VecLength(pchan->pose_mat[1]);

				ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
			}
			else
				ikstretch[a] = 1.0;

			stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;

			VecMulf(tree->basis_change[a][0], stretch);
			VecMulf(tree->basis_change[a][1], stretch);
			VecMulf(tree->basis_change[a][2], stretch);

		}

		IK_FreeSegment(iktree[a]);
	}
	
	MEM_freeN(iktree);
	if(ikstretch) MEM_freeN(ikstretch);
}

void free_posetree(PoseTree *tree)
{
	BLI_freelistN(&tree->targets);
	if(tree->pchan) MEM_freeN(tree->pchan);
	if(tree->parent) MEM_freeN(tree->parent);
	if(tree->basis_change) MEM_freeN(tree->basis_change);
	MEM_freeN(tree);
}

/* ********************** THE POSE SOLVER ******************* */


/* loc/rot/size to mat4 */
/* used in constraint.c too */
void chan_calc_mat(bPoseChannel *chan)
{
	float smat[3][3];
	float rmat[3][3];
	float tmat[3][3];
	
	SizeToMat3(chan->size, smat);
	
	NormalQuat(chan->quat);

	QuatToMat3(chan->quat, rmat);
	
	Mat3MulMat3(tmat, rmat, smat);
	
	Mat4CpyMat3(chan->chan_mat, tmat);
	
	/* prevent action channels breaking chains */
	/* need to check for bone here, CONSTRAINT_TYPE_ACTION uses this call */
	if (chan->bone==NULL || !(chan->bone->flag & BONE_CONNECTED)) {
		VECCOPY(chan->chan_mat[3], chan->loc);
	}

}

/* transform from bone(b) to bone(b+1), store in chan_mat */
static void make_dmats(bPoseChannel *pchan)
{
	if (pchan->parent) {
		float iR_parmat[4][4];
		Mat4Invert(iR_parmat, pchan->parent->pose_mat);
		Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);	// delta mat
	}
	else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
}

/* applies IK matrix to pchan, IK is done separated */
/* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
/* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
{
	float vec[3], ikmat[4][4];
	
	Mat4CpyMat3(ikmat, ik_mat);
	
	if (pchan->parent)
		Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
	else 
		Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);

	/* calculate head */
	VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
	/* calculate tail */
	VECCOPY(vec, pchan->pose_mat[1]);
	VecMulf(vec, pchan->bone->length);
	VecAddf(pchan->pose_tail, pchan->pose_head, vec);

	pchan->flag |= POSE_DONE;
}

/* NLA strip modifiers */
static void do_strip_modifiers(Object *armob, Bone *bone, bPoseChannel *pchan)
{
	bActionModifier *amod;
	bActionStrip *strip, *strip2;
	float scene_cfra= G.scene->r.cfra;
	int do_modif;

	for (strip=armob->nlastrips.first; strip; strip=strip->next) {
		do_modif=0;
		
		if (scene_cfra>=strip->start && scene_cfra<=strip->end)
			do_modif=1;
		
		if ((scene_cfra > strip->end) && (strip->flag & ACTSTRIP_HOLDLASTFRAME)) {
			do_modif=1;
			
			/* if there are any other strips active, ignore modifiers for this strip - 
			 * 'hold' option should only hold action modifiers if there are 
			 * no other active strips */
			for (strip2=strip->next; strip2; strip2=strip2->next) {
				if (strip2 == strip) continue;
				
				if (scene_cfra>=strip2->start && scene_cfra<=strip2->end) {
					if (!(strip2->flag & ACTSTRIP_MUTE))
						do_modif=0;
				}
			}
			
			/* if there are any later, activated, strips with 'hold' set, they take precedence, 
			 * so ignore modifiers for this strip */
			for (strip2=strip->next; strip2; strip2=strip2->next) {
				if (scene_cfra < strip2->start) continue;
				if ((strip2->flag & ACTSTRIP_HOLDLASTFRAME) && !(strip2->flag & ACTSTRIP_MUTE)) {
					do_modif=0;
				}
			}
		}
		
		if (do_modif) {
			/* temporal solution to prevent 2 strips accumulating */
			if(scene_cfra==strip->end && strip->next && strip->next->start==scene_cfra)
				continue;
			
			for(amod= strip->modifiers.first; amod; amod= amod->next) {
				switch (amod->type) {
				case ACTSTRIP_MOD_DEFORM:
				{
					/* validate first */
					if(amod->ob && amod->ob->type==OB_CURVE && amod->channel[0]) {
						
						if( strcmp(pchan->name, amod->channel)==0 ) {
							float mat4[4][4], mat3[3][3];
							
							curve_deform_vector(amod->ob, armob, bone->arm_mat[3], pchan->pose_mat[3], mat3, amod->no_rot_axis);
							Mat4CpyMat4(mat4, pchan->pose_mat);
							Mat4MulMat34(pchan->pose_mat, mat3, mat4);
							
						}
					}
				}
					break;
				case ACTSTRIP_MOD_NOISE:	
				{
					if( strcmp(pchan->name, amod->channel)==0 ) {
						float nor[3], loc[3], ofs;
						float eul[3], size[3], eulo[3], sizeo[3];
						
						/* calculate turbulance */
						ofs = amod->turbul / 200.0f;
						
						/* make a copy of starting conditions */
						VECCOPY(loc, pchan->pose_mat[3]);
						Mat4ToEul(pchan->pose_mat, eul);
						Mat4ToSize(pchan->pose_mat, size);
						VECCOPY(eulo, eul);
						VECCOPY(sizeo, size);
						
						/* apply noise to each set of channels */
						if (amod->channels & 4) {
							/* for scaling */
							nor[0] = BLI_gNoise(amod->noisesize, size[0]+ofs, size[1], size[2], 0, 0) - ofs;
							nor[1] = BLI_gNoise(amod->noisesize, size[0], size[1]+ofs, size[2], 0, 0) - ofs;	
							nor[2] = BLI_gNoise(amod->noisesize, size[0], size[1], size[2]+ofs, 0, 0) - ofs;
							VecAddf(size, size, nor);
							
							if (sizeo[0] != 0)
								VecMulf(pchan->pose_mat[0], size[0] / sizeo[0]);
							if (sizeo[1] != 0)
								VecMulf(pchan->pose_mat[1], size[1] / sizeo[1]);
							if (sizeo[2] != 0)
								VecMulf(pchan->pose_mat[2], size[2] / sizeo[2]);
						}
						if (amod->channels & 2) {
							/* for rotation */
							nor[0] = BLI_gNoise(amod->noisesize, eul[0]+ofs, eul[1], eul[2], 0, 0) - ofs;
							nor[1] = BLI_gNoise(amod->noisesize, eul[0], eul[1]+ofs, eul[2], 0, 0) - ofs;	
							nor[2] = BLI_gNoise(amod->noisesize, eul[0], eul[1], eul[2]+ofs, 0, 0) - ofs;
							
							compatible_eul(nor, eulo);
							VecAddf(eul, eul, nor);
							compatible_eul(eul, eulo);
							
							LocEulSizeToMat4(pchan->pose_mat, loc, eul, size);
						}
						if (amod->channels & 1) {
							/* for location */
							nor[0] = BLI_gNoise(amod->noisesize, loc[0]+ofs, loc[1], loc[2], 0, 0) - ofs;
							nor[1] = BLI_gNoise(amod->noisesize, loc[0], loc[1]+ofs, loc[2], 0, 0) - ofs;	
							nor[2] = BLI_gNoise(amod->noisesize, loc[0], loc[1], loc[2]+ofs, 0, 0) - ofs;
							
							VecAddf(pchan->pose_mat[3], loc, nor);
						}
					}
				}
					break;
				}
			}
		}
	}
}


/* The main armature solver, does all constraints excluding IK */
/* pchan is validated, as having bone and parent pointer */
static void where_is_pose_bone(Object *ob, bPoseChannel *pchan, float ctime)
{
	Bone *bone, *parbone;
	bPoseChannel *parchan;
	float vec[3];
	
	/* set up variables for quicker access below */
	bone= pchan->bone;
	parbone= bone->parent;
	parchan= pchan->parent;
	
	/* this gives a chan_mat with actions (ipos) results */
	chan_calc_mat(pchan);
	
	/* construct the posemat based on PoseChannels, that we do before applying constraints */
	/* pose_mat(b)= pose_mat(b-1) * yoffs(b-1) * d_root(b) * bone_mat(b) * chan_mat(b) */
	
	if(parchan) {
		float offs_bone[4][4];  // yoffs(b-1) + root(b) + bonemat(b)
		
		/* bone transform itself */
		Mat4CpyMat3(offs_bone, bone->bone_mat);
		
		/* The bone's root offset (is in the parent's coordinate system) */
		VECCOPY(offs_bone[3], bone->head);
		
		/* Get the length translation of parent (length along y axis) */
		offs_bone[3][1]+= parbone->length;
		
		/* Compose the matrix for this bone  */
		if(bone->flag & BONE_HINGE) {	// uses restposition rotation, but actual position
			float tmat[4][4];
			
			/* the rotation of the parent restposition */
			Mat4CpyMat4(tmat, parbone->arm_mat);
			
			/* the location of actual parent transform */
			VECCOPY(tmat[3], offs_bone[3]);
			offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
			Mat4MulVecfl(parchan->pose_mat, tmat[3]);
			
			Mat4MulSerie(pchan->pose_mat, tmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
		}
		else if(bone->flag & BONE_NO_SCALE) {
			float orthmat[4][4], vec[3];
			
			/* get the official transform, but we only use the vector from it (optimize...) */
			Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
			VECCOPY(vec, pchan->pose_mat[3]);
			
			/* do this again, but with an ortho-parent matrix */
			Mat4CpyMat4(orthmat, parchan->pose_mat);
			Mat4Ortho(orthmat);
			Mat4MulSerie(pchan->pose_mat, orthmat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
			
			/* copy correct transform */
			VECCOPY(pchan->pose_mat[3], vec);
		}
		else 
			Mat4MulSerie(pchan->pose_mat, parchan->pose_mat, offs_bone, pchan->chan_mat, NULL, NULL, NULL, NULL, NULL);
	}
	else {
		Mat4MulMat4(pchan->pose_mat, pchan->chan_mat, bone->arm_mat);
		/* only rootbones get the cyclic offset */
		VecAddf(pchan->pose_mat[3], pchan->pose_mat[3], ob->pose->cyclic_offset);
	}
	
	/* do NLA strip modifiers - i.e. curve follow */
	do_strip_modifiers(ob, bone, pchan);
	
	/* Do constraints */
	if (pchan->constraints.first) {
		bConstraintOb *cob;
		
		/* local constraints */
		do_constraint_channels(&pchan->constraints, NULL, ctime, 0);
		
		/* make a copy of location of PoseChannel for later */
		VECCOPY(vec, pchan->pose_mat[3]);
		
		/* prepare PoseChannel for Constraint solving 
		 * - makes a copy of matrix, and creates temporary struct to use 
		 */
		cob= constraints_make_evalob(ob, pchan, CONSTRAINT_OBTYPE_BONE);
		
		/* Solve PoseChannel's Constraints */
		solve_constraints(&pchan->constraints, cob, ctime);	// ctime doesnt alter objects
		
		/* cleanup after Constraint Solving 
		 * - applies matrix back to pchan, and frees temporary struct used
		 */
		constraints_clear_evalob(cob);
		
		/* prevent constraints breaking a chain */
		if(pchan->bone->flag & BONE_CONNECTED) {
			VECCOPY(pchan->pose_mat[3], vec);
		}
	}
	
	/* calculate head */
	VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
	/* calculate tail */
	VECCOPY(vec, pchan->pose_mat[1]);
	VecMulf(vec, bone->length);
	VecAddf(pchan->pose_tail, pchan->pose_head, vec);
}

/* This only reads anim data from channels, and writes to channels */
/* This is the only function adding poses */
void where_is_pose (Object *ob)
{
	bArmature *arm;
	Bone *bone;
	bPoseChannel *pchan;
	float imat[4][4];
	float ctime= bsystem_time(ob, (float)G.scene->r.cfra, 0.0);	/* not accurate... */
	
	arm = get_armature(ob);
	
	if(arm==NULL) return;
	if(ob->pose==NULL || (ob->pose->flag & POSE_RECALC)) 
	   armature_rebuild_pose(ob, arm);
	
	/* In restposition we read the data from the bones */
	if(ob==G.obedit || (arm->flag & ARM_RESTPOS)) {
		
		for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
			bone= pchan->bone;
			if(bone) {
				Mat4CpyMat4(pchan->pose_mat, bone->arm_mat);
				VECCOPY(pchan->pose_head, bone->arm_head);
				VECCOPY(pchan->pose_tail, bone->arm_tail);
			}
		}
	}
	else {
		Mat4Invert(ob->imat, ob->obmat);	// imat is needed 

		/* 1. construct the PoseTrees, clear flags */
		for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
			pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
			if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
				initialize_posetree(ob, pchan);	// will attach it to root!
		}
		
		/* 2. the main loop, channels are already hierarchical sorted from root to children */
		for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
			
			/* 3. if we find an IK root, we handle it separated */
			if(pchan->iktree.first) {
				while(pchan->iktree.first) {
					PoseTree *tree= pchan->iktree.first;
					int a;
					
					/* 4. walk over the tree for regular solving */
					for(a=0; a<tree->totchannel; a++) {
						if(!(tree->pchan[a]->flag & POSE_DONE))	// successive trees can set the flag
							where_is_pose_bone(ob, tree->pchan[a], ctime);
					}
					/* 5. execute the IK solver */
					execute_posetree(ob, tree);
					
					/* 6. apply the differences to the channels, 
						  we need to calculate the original differences first */
					for(a=0; a<tree->totchannel; a++)
						make_dmats(tree->pchan[a]);
					
					for(a=0; a<tree->totchannel; a++)
						/* sets POSE_DONE */
						where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
					
					/* 7. and free */
					BLI_remlink(&pchan->iktree, tree);
					free_posetree(tree);
				}
			}
			else if(!(pchan->flag & POSE_DONE)) {
				where_is_pose_bone(ob, pchan, ctime);
			}
		}
	}
		
	/* calculating deform matrices */
	for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
		if(pchan->bone) {
			Mat4Invert(imat, pchan->bone->arm_mat);
			Mat4MulMat4(pchan->chan_mat, imat, pchan->pose_mat);
		}
	}
}