diff options
Diffstat (limited to 'source/blender/blenkernel/intern/armature.c')
| -rw-r--r-- | source/blender/blenkernel/intern/armature.c | 1451 |
1 files changed, 1451 insertions, 0 deletions
diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c new file mode 100644 index 00000000000..86ce2faa07c --- /dev/null +++ b/source/blender/blenkernel/intern/armature.c @@ -0,0 +1,1451 @@ +/** + * $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. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * 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. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#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_mesh_types.h" +#include "DNA_armature_types.h" +#include "DNA_action_types.h" +#include "DNA_object_types.h" +#include "DNA_scene_types.h" +#include "DNA_view3d_types.h" +#include "DNA_constraint_types.h" + +#include "BKE_displist.h" +#include "BKE_global.h" +#include "BKE_main.h" +#include "BKE_library.h" +#include "BKE_blender.h" +#include "BKE_armature.h" +#include "BKE_action.h" +#include "BKE_constraint.h" +#include "BKE_object.h" +#include "BKE_object.h" +#include "BKE_deform.h" +#include "BKE_utildefines.h" + +#include "BIF_editdeform.h" + +#include "IK_solver.h" + +/* Function prototypes */ + +static void apply_pose_bonechildren (Bone* bone, bPose* pose, int doit); +static float dist_to_bone (float vec[3], float b1[3], float b2[3]); +static Bone *get_named_bone_bonechildren (Bone *bone, const char *name); +static Bone *get_indexed_bone_bonechildren (Bone *bone, int *index); +/*void make_bone_parent_matrix (Bone* bone);*/ +static void copy_bonechildren (Bone* newBone, Bone* oldBone); +static void calc_armature_deform_bonechildren (Bone *bone, float *vec, float *co, float *contrib, float obmat[][4]); +static int verify_boneptr_children (Bone *cBone, Bone *tBone); +static void where_is_bonelist_time (Object *ob, ListBase *base, float ctime); +static Bone *get_last_ik_bone (Bone *bone); +static void precalc_bonelist_posemats(ListBase *bonelist, float parlen); + +/* Globals */ +static float g_premat[4][4]; +static float g_postmat[4][4]; +static MDeformVert *g_dverts; +static ListBase *g_defbase; +static bArmature *g_defarm; + +/* Functions */ + +float get_bone_length (Bone *bone) +{ + float result[3]; + + VecSubf (result, bone->tail, bone->head); + return (float)sqrt(result[0]*result[0] + result[1]*result[1] + result[2]*result[2]); + +} + +void apply_bonemat(Bone *bone) +{ + float mat[3][3], imat[3][3], tmat[3][3]; + + if(!bone) + return; + + Mat3CpyMat4(mat, bone->obmat); + + VECCOPY(bone->loc, bone->obmat[3]); + + Mat3ToQuat(mat, bone->quat); + QuatToMat3(bone->quat, tmat); + + Mat3Inv(imat, tmat); + + Mat3MulMat3(tmat, imat, mat); + + bone->size[0]= tmat[0][0]; + bone->size[1]= tmat[1][1]; + bone->size[2]= tmat[2][2]; + +} + +void GB_build_mats (float parmat[][4], float obmat[][4], float premat[][4], float postmat[][4]) +{ + float obinv[4][4]; +#if 0 + Mat4Invert(obinv, obmat); + Mat4CpyMat4(premat, obmat); + Mat4MulMat4(postmat, parmat, obinv); + + Mat4Invert (postmat, premat); +#else + Mat4Invert(obinv, obmat); + Mat4CpyMat4(premat, obmat); + Mat4MulMat4(postmat, parmat, obinv); + + Mat4Invert (premat, postmat); +#endif +} + +void GB_init_armature_deform(ListBase *defbase, float premat[][4], float postmat[][4]) +{ + g_defbase = defbase; + Mat4CpyMat4 (g_premat, premat); + Mat4CpyMat4 (g_postmat, postmat); + +} + +void GB_validate_defgroups (Mesh *mesh, ListBase *defbase) +{ + /* Should only be called when the mesh or armature changes */ + int j, i; + MDeformVert *dvert; + + for (j=0; j<mesh->totvert; j++){ + dvert = mesh->dvert+j; + for (i=0; i<dvert->totweight; i++) + dvert->dw[i].data = ((bDeformGroup*)BLI_findlink (defbase, dvert->dw[i].def_nr))->data; + } +} + +void GB_calc_armature_deform (float *co, MDeformVert *dvert) +{ + float vec[3]={0, 0, 0}; + float contrib = 0; + int i; + Bone *bone; + + Mat4MulVecfl(g_premat, co); + + for (i=0; i<dvert->totweight; i++){ + bone = dvert->dw[i].data; + if (bone) calc_bone_deform (bone, dvert->dw[i].weight, vec, co, &contrib); + } + + if (contrib){ + vec[0]/=contrib; + vec[1]/=contrib; + vec[2]/=contrib; + } + + VecAddf (co, vec, co); + Mat4MulVecfl(g_postmat, co); +} + +static Bone *get_last_ik_bone (Bone *bone) +{ + Bone *curBone; + + for (curBone = bone->childbase.first; curBone; curBone=curBone->next){ + if (curBone->flag & BONE_IK_TOPARENT){ + return get_last_ik_bone (curBone); + } + } + + return bone; +} + +#if 0 +static Bone *get_first_ik_bone (Bone *bone) +{ + Bone *curBone; + + for (curBone = bone; curBone; curBone=curBone->parent){ + if (!bone->parent) + return curBone; + if (!bone->flag & BONE_IK_TOPARENT) + return curBone; + } + + return bone; +/* for (curBone = bone->childbase.first; curBone; curBone=curBone->next){ + if (curBone->flag & BONE_IK_TOPARENT){ + return get_last_ik_bone (curBone); + } + } +*/ + return bone; + +} +#endif + +void where_is_bone(Object *ob, Bone *bone) +{ + where_is_bone_time (ob, bone, G.scene->r.cfra); +} + +void where_is_bone_time (Object *ob, Bone *bone, float ctime) +{ + where_is_bone1_time (ob, get_last_ik_bone(bone), ctime); +} + +void rebuild_bone_parent_matrix (Bone *bone) +{ + if (!bone) + return; + + if (bone->parent) + rebuild_bone_parent_matrix(bone->parent); + + /* Get the parent inverse */ + if (bone->parent) + Mat4MulMat4(bone->parmat, bone->parent->obmat, bone->parent->parmat); + else + Mat4One (bone->parmat); + +} +void where_is_bone1_time (Object *ob, Bone *bone, float ctime) +/* Assumes the pose has already been retrieved from the action */ +/* Also assumes where_is_object has been called for owner */ +{ + bPose *pose; + bPoseChannel *chan; + bArmature *arm; + float imat[4][4]; + float totmat[4][4]; + Object conOb; + + pose = ob->pose; + if (!pose) + return; + + arm = get_armature(ob); + + /* Ensure there is achannel for this bone*/ + verify_pose_channel (pose, bone->name); + + /* Search the pose for a channel with the same name, and copy the + transformations from the channel into the bone */ + for (chan=pose->chanbase.first; chan; chan=chan->next){ + if (!strcmp (chan->name, bone->name)){ + +#if 1 /* If 1 attempt to use pose caching features */ + /* Bail out if we've been recalced recently */ + if (chan->flag & PCHAN_DONE){ + Mat4CpyMat4 (bone->obmat, chan->obmat); + if (bone->parent){ + if ((bone->flag & BONE_IK_TOPARENT)) + where_is_bone1_time (ob, bone->parent, ctime); + else + where_is_bone_time (ob, bone->parent, ctime); + } + return; + } + else + chan->flag |= PCHAN_DONE; +#endif + break; + } + } + +#if 1 + /* Ensure parents have been evaluated */ + if (bone->parent){ + if ((bone->flag & BONE_IK_TOPARENT)) + where_is_bone1_time (ob, bone->parent, ctime); + else + where_is_bone_time (ob, bone->parent, ctime); + } + + /* Build the parent matrix : Depreciated */ +// if (bone->parent) +// Mat4MulMat4(bone->parmat, bone->parent->obmat, bone->parent->parmat); +// else +// Mat4One (bone->parmat); +#endif + + if (arm){ + if ((arm->flag & ARM_RESTPOS) || ((G.obedit && (ob->data == G.obedit->data)))){ + Mat4One (bone->obmat); + Mat4One (chan->obmat); + return; + } + } + + if (bone->flag & BONE_IK_TOPARENT){ + bone->loc[0]=bone->loc[1]=bone->loc[2]=0.0F; + } + bone_to_mat4(bone, bone->obmat); + + /* Do constraints */ + // clear_workob(); + + memset(&conOb, 0, sizeof(Object)); + conOb.size[0]= conOb.size[1]= conOb.size[2]= 1.0; + + /* Collect the constraints from the pose */ + conOb.constraints.first = chan->constraints.first; + conOb.constraints.last = chan->constraints.last; + + /* Totmat takes bone's obmat to worldspace */ + + { + float parmat[4][4]; + float temp[4][4]; + + Mat4CpyMat4 (temp, bone->obmat); + Mat4One (bone->obmat); + get_objectspace_bone_matrix(bone, parmat, 1, 1); + Mat4CpyMat4 (bone->obmat, temp); + Mat4MulMat4 (totmat, parmat, ob->obmat); + } + + /* Build a workob to pass the bone to the constraint solver */ + conOb.data = ob->data; + conOb.type = ob->type; + conOb.parent = ob; + conOb.trackflag = ob->trackflag; + conOb.upflag = ob->upflag; + + VECCOPY(conOb.size, bone->size); + + Mat4MulMat4 (conOb.obmat, bone->obmat, totmat); + + /* Solve */ + solve_constraints (&conOb, TARGET_BONE, (void*)bone, ctime); + + { + float parmat[4][4]; + float temp[4][4]; + + Mat4CpyMat4 (temp, bone->obmat); + Mat4One (bone->obmat); + get_objectspace_bone_matrix(bone, parmat, 1, 1); + Mat4CpyMat4 (bone->obmat, temp); + Mat4MulMat4 (totmat, parmat, ob->obmat); + } + + VECCOPY(bone->size, conOb.size); + + /* Take out of worldspace */ + Mat4Invert (imat, totmat); + Mat4MulMat4 (bone->obmat, conOb.obmat, imat); + Mat4CpyMat4 (chan->obmat, bone->obmat); + +} + + +bArmature *get_armature(Object *ob) +{ + if(ob==NULL) return NULL; + if(ob->type==OB_ARMATURE) return ob->data; + else return NULL; +} + +void init_armature_deform(Object *parent, Object *ob) +{ + bArmature *arm; + bDeformGroup *dg; + Bone *curBone; + MDeformVert *dvert; + int totverts; + float obinv[4][4]; + int i, j; + + arm = get_armature(parent); + if (!arm) + return; + + if (ob) + where_is_object (ob); + +#if 1 + apply_pose_armature (arm, parent->pose, 1); /* Hopefully doit parameter can be set to 0 in future */ + where_is_armature (parent); +#else + apply_pose_armature (arm, parent->pose, 0); +#endif + + g_defbase = &ob->defbase; + g_defarm = arm; + + Mat4Invert(obinv, ob->obmat); + Mat4CpyMat4(g_premat, ob->obmat); + Mat4MulMat4(g_postmat, parent->obmat, obinv); + + Mat4Invert (g_premat, g_postmat); + + /* Store the deformation verts */ + if (ob->type==OB_MESH){ + g_dverts = ((Mesh*)ob->data)->dvert; + totverts = ((Mesh*)ob->data)->totvert; + } + else{ + g_dverts=NULL; + totverts=0; + } + + /* Precalc bone defmats */ + precalc_armature_posemats (arm); + + for (curBone=arm->bonebase.first; curBone; curBone=curBone->next){ + precalc_bone_defmat(curBone); + } + + /* Validate bone data in bDeformGroups */ + + for (dg=g_defbase->first; dg; dg=dg->next) + dg->data = (void*)get_named_bone(arm, dg->name); + + if (g_dverts){ + for (j=0; j<totverts; j++){ + dvert = g_dverts+j; + for (i=0; i<dvert->totweight; i++){ + bDeformGroup *fg; + fg = BLI_findlink (g_defbase, dvert->dw[i].def_nr); + + if (fg) + dvert->dw[i].data = fg->data; + else + dvert->dw[i].data = NULL; + } + } + } +} + +void get_bone_root_pos (Bone* bone, float vec[3], int posed) +{ + Bone *curBone; + float mat[4][4]; + + get_objectspace_bone_matrix(bone, mat, 1, posed); + VECCOPY (vec, mat[3]); + return; + + rebuild_bone_parent_matrix(bone); + if (posed){ + + get_objectspace_bone_matrix(bone, mat, 1, posed); + VECCOPY (vec, mat[3]); + } + else { + vec[0]=vec[1]=vec[2]=0.0F; + for (curBone=bone; curBone; curBone=curBone->parent){ + if (curBone==bone) + VecAddf (vec, vec, curBone->head); + else + VecAddf (vec, vec, curBone->tail); + } + } +} + +void get_bone_tip_pos (Bone* bone, float vec[3], int posed) +{ + Bone *curBone; + float mat[4][4], tmat[4][4], rmat[4][4], bmat[4][4], fmat[4][4]; + + get_objectspace_bone_matrix(bone, mat, 0, posed); + VECCOPY (vec, mat[3]); + return; + + rebuild_bone_parent_matrix(bone); + if (posed){ + + Mat4One (mat); + + for (curBone = bone; curBone; curBone=curBone->parent){ + Mat4One (bmat); + /* [BMAT] This bone's offset */ + VECCOPY (bmat[3], curBone->head); + if (curBone==bone){ + Mat4One (tmat); + VecSubf (tmat[3], curBone->tail, curBone->head); + Mat4MulMat4 (bmat, tmat, curBone->obmat); + VecAddf (bmat[3], bmat[3], curBone->head); + } + else + VecAddf (bmat[3], bmat[3], curBone->obmat[3]); // Test + + /* [RMAT] Parent's bone length = parent rotmat * bone length */ + if (curBone->parent){ + Mat4One (tmat); + VecSubf (tmat[3], curBone->parent->tail, curBone->parent->head); + Mat4MulMat4 (rmat, tmat, curBone->parent->obmat); + VecSubf (rmat[3], rmat[3], curBone->parent->obmat[3]); + } + else + Mat4One (rmat); + + Mat4MulSerie (fmat, rmat, bmat, mat, 0, 0, 0, 0, 0); + Mat4CpyMat4 (mat, fmat); + } + + VECCOPY (vec, mat[3]); + } + else{ + vec[0]=vec[1]=vec[2]=0.0F; + for (curBone=bone; curBone; curBone=curBone->parent){ + VecAddf (vec, vec, curBone->tail); + } + } +} + +int verify_boneptr (bArmature *arm, Bone *bone) +{ + /* Ensures that a given bone exists in an armature */ + Bone *curBone; + + if (!arm) + return 0; + + for (curBone=arm->bonebase.first; curBone; curBone=curBone->next){ + if (verify_boneptr_children (curBone, bone)) + return 1; + } + + return 0; +} + +static int verify_boneptr_children (Bone *cBone, Bone *tBone) +{ + Bone *curBone; + + if (cBone == tBone) + return 1; + + for (curBone=cBone->childbase.first; curBone; curBone=curBone->next){ + if (verify_boneptr_children (curBone, tBone)) + return 1; + } + return 0; +} + + +static float dist_to_bone (float vec[3], float b1[3], float b2[3]) +{ +/* float dist=0; */ + float bdelta[3]; + float pdelta[3]; + float hsqr, a, l; + + VecSubf (bdelta, b2, b1); + l = Normalise (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){ + //return 100000; + /* If we're past the end of the bone, do some weird field attenuation thing */ + return ((b1[0]-vec[0])*(b1[0]-vec[0]) +(b1[1]-vec[1])*(b1[1]-vec[1]) +(b1[2]-vec[2])*(b1[2]-vec[2])) ; + } + else if (a > l){ + //return 100000; + /* If we're past the end of the bone, do some weird field attenuation thing */ + return ((b2[0]-vec[0])*(b2[0]-vec[0]) +(b2[1]-vec[1])*(b2[1]-vec[1]) +(b2[2]-vec[2])*(b2[2]-vec[2])) ; + } + else { + return (hsqr - (a*a)); + } + + +} + +static void calc_armature_deform_bonechildren (Bone *bone, float *vec, float *co, float *contrib, float obmat[][4]) +{ + Bone *curBone; + float root[3]; + float tip[3]; + float dist, fac, ifac; + float cop[3]; + float bdsqr; + + + get_bone_root_pos (bone, root, 0); + get_bone_tip_pos (bone, tip, 0); + + bdsqr = bone->dist*bone->dist; + VECCOPY (cop, co); + + dist = dist_to_bone(cop, root, tip); + + if ((dist) <= bdsqr){ + fac = (dist)/bdsqr; + ifac = 1.0F-fac; + + ifac*=bone->weight; + + if (!vec) + (*contrib) +=ifac; + else{ + ifac*=(1.0F/(*contrib)); + + VECCOPY (cop, co); + + Mat4MulVecfl(bone->defmat, cop); + + VecSubf (cop, cop, co); // Make this a delta from the base position + cop[0]*=ifac; cop[1]*=ifac; cop[2]*=ifac; + VecAddf (vec, vec, cop); + + } + } + +// calc_bone_deform (bone, bone->weight, vec, co, contrib, obmat); + for (curBone = bone->childbase.first; curBone; curBone=curBone->next) + calc_armature_deform_bonechildren (curBone, vec, co, contrib, obmat); +} + +void precalc_bone_irestmat (Bone *bone) +{ + float restmat[4][4]; + + get_objectspace_bone_matrix(bone, restmat, 1, 0); + Mat4Invert (bone->irestmat, restmat); +} + +static void precalc_bonelist_posemats(ListBase *bonelist, float parlen) +{ + Bone *curBone; + float length; + float T_parlen[4][4]; + float T_root[4][4]; + float M_obmat[4][4]; + float R_bmat[4][4]; + float M_accumulatedMatrix[4][4]; + float delta[3]; + + for (curBone = bonelist->first; curBone; curBone=curBone->next){ + + /* Get the length translation (length along y axis) */ + length = get_bone_length(curBone); + + /* Get the bone's root offset (in the parent's coordinate system) */ + Mat4One (T_root); + VECCOPY (T_root[3], curBone->head); + + /* Compose the restmat */ + VecSubf(delta, curBone->tail, curBone->head); + make_boneMatrixvr(R_bmat, delta, curBone->roll); + + /* Retrieve the obmat (user transformation) */ + Mat4CpyMat4 (M_obmat, curBone->obmat); + + /* Compose the accumulated matrix (i.e. parent matrix * parent translation ) */ + if (curBone->parent){ + Mat4One (T_parlen); + T_parlen[3][1] = parlen; + Mat4MulMat4 (M_accumulatedMatrix, T_parlen, curBone->parent->posemat); + } + else + Mat4One (M_accumulatedMatrix); + + /* Compose the matrix for this bone */ + Mat4MulSerie (curBone->posemat, M_accumulatedMatrix, T_root, R_bmat, M_obmat, NULL, NULL, NULL, NULL); + + precalc_bonelist_posemats(&curBone->childbase, length); + } +} + +void precalc_armature_posemats (bArmature *arm) +{ + precalc_bonelist_posemats(&arm->bonebase, 0.0); +} + +void precalc_bone_defmat (Bone *bone) +{ + Bone *curBone; +#if 0 + float restmat[4][4]; + float posemat[4][4]; + float imat[4][4]; + + /* Store restmat and restmat inverse - Calculate once when leaving editmode */ + /* Store all bones' posemats - Do when applied */ + + /* EXPENSIVE! Don't do this! */ + get_objectspace_bone_matrix(bone, restmat, 1, 0); + get_objectspace_bone_matrix(bone, posemat, 1, 1); + Mat4Invert (imat, restmat); + Mat4MulMat4 (bone->defmat, imat, posemat); + /* /EXPENSIVE */ +#else + Mat4MulMat4 (bone->defmat, bone->irestmat, bone->posemat); +#endif + for (curBone = bone->childbase.first; curBone; curBone=curBone->next){ + precalc_bone_defmat(curBone); + } +} + +void calc_bone_deform (Bone *bone, float weight, float *vec, float *co, float *contrib) +{ + float cop[3]; + + if (!weight) + return; + + VECCOPY (cop, co); + + Mat4MulVecfl(bone->defmat, cop); + + vec[0]+=(cop[0]-co[0])*weight; + vec[1]+=(cop[1]-co[1])*weight; + vec[2]+=(cop[2]-co[2])*weight; + + (*contrib)+=weight; +} + +void calc_armature_deform (Object *ob, float *co, int index) +{ + bArmature *arm; + Bone *bone; + Bone *curBone; + float vec[3]; + float contrib=0; + int i; + MDeformVert *dvert = g_dverts+index; + + arm=g_defarm; + vec[0]=vec[1]=vec[2]=0; + + /* Apply the object's matrix */ + Mat4MulVecfl(g_premat, co); + + if (g_dverts){ + for (i=0; i<dvert->totweight; i++){ + bone = dvert->dw[i].data; + if (bone) calc_bone_deform (bone, dvert->dw[i].weight, vec, co, &contrib); + } + + if (contrib){ + vec[0]/=contrib; + vec[1]/=contrib; + vec[2]/=contrib; + } + VecAddf (co, vec, co); + Mat4MulVecfl(g_postmat, co); + return; + } + + + // Count the number of interested bones + for (curBone = arm->bonebase.first; curBone; curBone=curBone->next) + calc_armature_deform_bonechildren (curBone, NULL, co, &contrib, ob->obmat); + + // Do the deformation + for (curBone = arm->bonebase.first; curBone; curBone=curBone->next) + calc_armature_deform_bonechildren (curBone, vec, co, &contrib, ob->obmat); + + VecAddf (co, vec, co); + Mat4MulVecfl(g_postmat, co); +} + +void apply_pose_armature (bArmature* arm, bPose* pose, int doit) +{ + Bone *curBone; + for (curBone = arm->bonebase.first; curBone; curBone=curBone->next){ + apply_pose_bonechildren (curBone, pose, doit); + } +} + +void where_is_armature (Object *ob) +{ + where_is_object (ob); + where_is_armature_time(ob, (float)G.scene->r.cfra); +} + +void where_is_armature_time (Object *ob, float ctime) +{ + bArmature *arm; + + arm = get_armature(ob); + if (!arm) + return; + + where_is_bonelist_time (ob, &arm->bonebase, ctime); + +} + +static void where_is_bonelist_time (Object *ob, ListBase *base, float ctime) +{ + Bone *curBone; + + for (curBone=base->first; curBone; curBone=curBone->next){ + if (!curBone->childbase.first) + where_is_bone1_time (ob, curBone, ctime); + + where_is_bonelist_time(ob, &curBone->childbase, ctime); + } +} +static void apply_pose_bonechildren (Bone* bone, bPose* pose, int doit) +{ + Bone *curBone; + bPoseChannel *chan; + + if (!pose){ + + bone->dsize[0]=bone->dsize[1]=bone->dsize[2]=1.0F; + bone->size[0]=bone->size[1]=bone->size[2]=1.0F; + + bone->dquat[0]=bone->dquat[1]=bone->dquat[2]=bone->dquat[3]=0; + bone->quat[0]=bone->quat[1]=bone->quat[2]=bone->quat[3]=0.0F; + + bone->dloc[0]=bone->dloc[1]=bone->dloc[2]=0.0F; + bone->loc[0]=bone->loc[1]=bone->loc[2]=0.0F; + } + + // Ensure there is achannel for this bone + verify_pose_channel (pose, bone->name); + + // Search the pose for a channel with the same name + if (pose){ + for (chan=pose->chanbase.first; chan; chan=chan->next){ + if (!strcmp (chan->name, bone->name)){ + if (chan->flag & POSE_LOC) + memcpy (bone->loc, chan->loc, sizeof (bone->loc)); + if (chan->flag & POSE_SIZE) + memcpy (bone->size, chan->size, sizeof (bone->size)); + if (chan->flag & POSE_ROT) + memcpy (bone->quat, chan->quat, sizeof (bone->quat)); + + if (doit){ + bone_to_mat4(bone, bone->obmat); + } + else{ + Mat4CpyMat4 (bone->obmat, chan->obmat); + } + + + break; + } + } + } + + for (curBone = bone->childbase.first; curBone; curBone=curBone->next){ + apply_pose_bonechildren (curBone, pose, doit); + } +} + +void make_boneMatrixvr (float outmatrix[][4],float delta[3], float roll) +/* Calculates the rest matrix of a bone based + On its vector and a roll around that vector */ +{ + float nor[3],axis[3],target[3]={0,1,0}; + float theta; + float rMatrix[3][3], bMatrix[3][3], fMatrix[3][3]; + + VECCOPY (nor,delta); + Normalise (nor); + + /* Find Axis & Amount for bone matrix*/ + Crossf (axis,target,nor); + Normalise (axis); + theta=(float) acos (Inpf (target,nor)); + + /* Make Bone matrix*/ + VecRotToMat3(axis, theta, bMatrix); + + /* Make Roll matrix*/ + VecRotToMat3(nor, roll, rMatrix); + + /* Combine and output result*/ + Mat3MulMat3 (fMatrix,rMatrix,bMatrix); + Mat4CpyMat3 (outmatrix,fMatrix); +} + +void make_boneMatrix (float outmatrix[][4], Bone *bone) +/* Calculates the rest matrix of a bone based + On its vector and a roll around that vector */ +{ + float delta[3]; + float parmat[4][4], imat[4][4], obmat[4][4]; + + if (bone->parent){ + VecSubf (delta, bone->parent->tail, bone->parent->head); + make_boneMatrixvr(parmat, delta, bone->parent->roll); + } + else{ + Mat4One (parmat); + } + + Mat4Invert (imat, parmat); + + VecSubf (delta, bone->tail, bone->head); + make_boneMatrixvr(obmat, delta, bone->roll); + + Mat4MulMat4(outmatrix, obmat, imat); + +} + + +bArmature *add_armature() +{ + bArmature *arm; + + arm= alloc_libblock (&G.main->armature, ID_AR, "Armature"); + + if(arm) { + + + } + 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; +} + + +void bone_to_mat3(Bone *bone, float mat[][3]) /* no parent */ +{ + float smat[3][3]; + float rmat[3][3]; +/* float q1[4], vec[3];*/ + + /* size */ +/* if(bone->ipo) { + vec[0]= bone->size[0]+bone->dsize[0]; + vec[1]= bone->size[1]+bone->dsize[1]; + vec[2]= bone->size[2]+bone->dsize[2]; + SizeToMat3(vec, smat); + } + else +*/ { + SizeToMat3(bone->size, smat); + } + + /* rot */ + /*if(bone->flag & BONE_QUATROT) { + if(bone->ipo) { + QuatMul(q1, bone->quat, bone->dquat); + QuatToMat3(q1, rmat); + } + else + */ { + NormalQuat(bone->quat); + QuatToMat3(bone->quat, rmat); + } +/* } +*/ + Mat3MulMat3(mat, rmat, smat); +} + +void bone_to_mat4(Bone *bone, float mat[][4]) +{ + float tmat[3][3]; + + bone_to_mat3(bone, tmat); + + Mat4CpyMat3(mat, tmat); + + VECCOPY(mat[3], bone->loc); +// VecAddf(mat[3], mat[3], bone->loc); +/* if(bone->ipo) { + mat[3][0]+= bone->dloc[0]; + mat[3][1]+= bone->dloc[1]; + mat[3][2]+= bone->dloc[2]; + } +*/ +} + +Bone *get_indexed_bone (bArmature *arm, int index) +/* + Walk the list until the index is reached +*/ +{ + Bone *bone=NULL, *curBone; + int ref=index; + + if (!arm) + return NULL; + + for (curBone=arm->bonebase.first; curBone; curBone=curBone->next){ + bone = get_indexed_bone_bonechildren (curBone, &ref); + if (bone) + return bone; + } + + return bone; +} + +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; +} + +static Bone *get_indexed_bone_bonechildren (Bone *bone, int *index) +{ + Bone *curBone, *rbone; + + if (!*index) + return bone; + + (*index)--; + + for (curBone=bone->childbase.first; curBone; curBone=curBone->next){ + rbone=get_indexed_bone_bonechildren (curBone, index); + if (rbone) + return rbone; + } + + return NULL; +} + +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; +} + +void make_displists_by_armature (Object *ob) +{ + Base *base; + + if (ob){ + for (base= G.scene->base.first; base; base= base->next){ + if ((ob==base->object->parent) && (base->lay & G.scene->lay)) + if (base->object->partype==PARSKEL ) + makeDispList(base->object); + } + } +} + +void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed) +/* Gets matrix that transforms the bone to object space */ +/* This function is also used to compute the orientation of the bone for display */ +{ + Bone *curBone; + + Bone *bonelist[256]; + int bonecount=0, i; + + Mat4One (M_accumulatedMatrix); + + /* Build a list of bones from tip to root */ + for (curBone=bone; curBone; curBone=curBone->parent){ + bonelist[bonecount] = curBone; + bonecount++; + } + + /* Count through the inverted list (i.e. iterate from root to tip)*/ + for (i=0; i<bonecount; i++){ + float T_root[4][4]; + float T_len[4][4]; + float R_bmat[4][4]; + float M_obmat[4][4]; + float M_boneMatrix[4][4]; + float delta[3]; + + curBone = bonelist[bonecount-i-1]; + + /* Get the length translation (length along y axis) */ + Mat4One (T_len); + T_len[3][1] = get_bone_length(curBone); + + if ((curBone == bone) && (root)) + Mat4One (T_len); + + /* Get the bone's root offset (in the parent's coordinate system) */ + Mat4One (T_root); + VECCOPY (T_root[3], curBone->head); + + /* Compose the restmat */ + VecSubf(delta, curBone->tail, curBone->head); + make_boneMatrixvr(R_bmat, delta, curBone->roll); + + + /* Retrieve the obmat (user transformation) */ + if (posed) + Mat4CpyMat4 (M_obmat, curBone->obmat); + else + Mat4One (M_obmat); + + /* Compose the matrix for this bone */ +#if 0 + Mat4MulSerie (M_boneMatrix, M_accumulatedMatrix, T_root, M_obmat, R_bmat, T_len, NULL, NULL, NULL); +#else + Mat4MulSerie (M_boneMatrix, M_accumulatedMatrix, T_root, R_bmat, M_obmat, T_len, NULL, NULL, NULL); +#endif + Mat4CpyMat4 (M_accumulatedMatrix, M_boneMatrix); + } + + +} + +void solve_posechain (PoseChain *chain) +{ + float goal[3]; + int i; + Bone *curBone; + float M_obmat[4][4]; + float M_basischange[4][4]; + bPoseChannel *chan; + + if (!chain->solver) return; + + /** + * Transform the goal from worldspace + * to the coordinate system of the root + * of the chain. The matrix for this + * was computed when the chain was built + * in ik_chain_to_posechain + */ + + VECCOPY (goal, chain->goal); + Mat4MulVecfl (chain->goalinv, goal); + + /* Solve the chain */ + + IK_SolveChain(chain->solver, + goal, + chain->tolerance, + chain->iterations, + 0.1f, + chain->solver->segments); + + /* Copy the results back into the bones */ + for (i = chain->solver->num_segments-1, curBone=chain->target->parent; i>=0; i--, curBone=curBone->parent){ + + /* Retrieve the delta rotation from the solver */ + Mat4One(M_basischange); + Mat4CpyMat3(M_basischange, chain->solver->segments[i].basis_change); + + + /** + * Multiply the bone's usertransform by the + * basis change to get the new usertransform + */ + + Mat4CpyMat4 (M_obmat, curBone->obmat); + Mat4MulMat4 (curBone->obmat, M_basischange, M_obmat); + + /* Store the solve results on the childrens' channels */ + for (chan = chain->pose->chanbase.first; chan; chan=chan->next){ + if (!strcmp (chan->name, curBone->name)){ + Mat4CpyMat4 (chan->obmat, curBone->obmat); + break; + } + } + + } +} + +void free_posechain (PoseChain *chain) +{ + if (chain->solver) { + MEM_freeN (chain->solver->segments); + chain->solver->segments = NULL; + IK_FreeChain(chain->solver); + } + MEM_freeN (chain); +} + +PoseChain *ik_chain_to_posechain (Object *ob, Bone *bone) +{ + IK_Segment_Extern *segs; + PoseChain *chain = NULL; + Bone *curBone, *rootBone; + int segcount, curseg, icurseg; + float imat[4][4]; + Bone *bonelist[256]; + float rootmat[4][4]; + float bonespace[4][4]; + + /** + * Some interesting variables in this function: + * + * Bone->obmat Bone's user transformation; + * It is initialized in where_is_bone1_time + * + * rootmat Bone's coordinate system, computed by + * get_objectspace_bone_matrix. Takes all + * parents transformations into account. + */ + + + + /* Ensure that all of the bone parent matrices are correct */ + + /* Find the chain's root & count the segments needed */ + segcount = 0; + for (curBone = bone; curBone; curBone=curBone->parent){ + rootBone = curBone; + if (curBone!=bone){ + bonelist[segcount]=curBone; + segcount++; + } + if (!curBone->parent) + break; + else if (!(curBone->flag & BONE_IK_TOPARENT)) + break; + } + + if (!segcount) + return NULL; + + + /** + * Initialize a record to store information about the original bones + * This will be the return value for this function. + */ + + chain = MEM_callocN(sizeof(PoseChain), "posechain"); + chain->solver = IK_CreateChain(); + chain->target = bone; + chain->root = rootBone; + chain->pose = ob->pose; + + /* Allocate some IK segments */ + segs = MEM_callocN (sizeof(IK_Segment_Extern)*segcount, "iksegments"); + + + /** + * Remove the offset from the first bone in the chain and take the target to chainspace + */ + + + get_objectspace_bone_matrix(rootBone, bonespace, 1, 1); + Mat4One (rootmat); + VECCOPY (rootmat[3], bonespace[3]); + + /* Take the target to bonespace */ + Mat4MulMat4 (imat, rootmat, ob->obmat); + Mat4Invert (chain->goalinv, imat); + + + /** + * Build matrices from the root to the tip + * We count backwards through the bone list (which is sorted tip to root) + * and forwards through the ik_segment list + */ + + for (curseg = segcount-1; curseg>=0; curseg--){ + float M_basismat[4][4]; + float R_parmat[4][4]; + float iR_parmat[4][4]; + float R_bonemat[4][4]; + + /* Retrieve the corresponding bone for this segment */ + icurseg=segcount-curseg-1; + curBone = bonelist[curseg]; + + /* Get the basis matrix */ + Mat4One (R_parmat); + get_objectspace_bone_matrix(curBone, R_bonemat, 1, 1); + R_bonemat[3][0]=R_bonemat[3][1]=R_bonemat[3][2]=0.0F; + + if (curBone->parent && (curBone->flag & BONE_IK_TOPARENT)){ + get_objectspace_bone_matrix(curBone->parent, R_parmat, 1, 1); + R_parmat[3][0]=R_parmat[3][1]=R_parmat[3][2]=0.0F; + } + + Mat4Invert(iR_parmat, R_parmat); + Mat4MulMat4(M_basismat, R_bonemat, iR_parmat); + + /* Copy the matrix into the basis and transpose */ + Mat3CpyMat4(segs[icurseg].basis, M_basismat); + Mat3Transp(segs[icurseg].basis); + + /* Fill out the IK segment */ + segs[icurseg].length = get_bone_length(curBone); + + }; + + IK_LoadChain(chain->solver, segs, segcount); + return chain; +} + + + +void precalc_bonelist_irestmats (ListBase* bonelist) +{ + Bone *curbone; + + if (!bonelist) + return; + + for (curbone = bonelist->first; curbone; curbone=curbone->next){ + precalc_bone_irestmat(curbone); + precalc_bonelist_irestmats(&curbone->childbase); + } +} |