diff --git a/source/blender/blenkernel/BKE_armature.h b/source/blender/blenkernel/BKE_armature.h index 684e69e7694..5134e352223 100644 --- a/source/blender/blenkernel/BKE_armature.h +++ b/source/blender/blenkernel/BKE_armature.h @@ -95,6 +95,11 @@ void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[] void vec_roll_to_mat3(float *vec, float roll, float mat[][3]); void mat3_to_vec_roll(float mat[][3], float *vec, float *roll); +/* Common Conversions Between Co-ordinate Spaces */ +void armature_mat_world_to_pose(struct Object *ob, float inmat[][4], float outmat[][4]); +void armature_mat_pose_to_bone(struct bPoseChannel *pchan, float inmat[][4], float outmat[][4]); +void armature_loc_pose_to_bone(struct bPoseChannel *pchan, float *inloc, float *outloc); + /* Animation functions */ struct PoseTree *ik_tree_to_posetree(struct Object *ob, struct Bone *bone); void solve_posetree(PoseTree *tree); diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c index 58df56238b1..b10018a5e73 100644 --- a/source/blender/blenkernel/intern/armature.c +++ b/source/blender/blenkernel/intern/armature.c @@ -819,6 +819,66 @@ void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[] 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 Pose-Space Matrix to Bone-Space Matrix */ +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 */ +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]); +} + /* **************** The new & simple (but OK!) armature evaluation ********* */ diff --git a/source/blender/blenlib/BLI_arithb.h b/source/blender/blenlib/BLI_arithb.h index 5453c87e35c..d11c04291b9 100644 --- a/source/blender/blenlib/BLI_arithb.h +++ b/source/blender/blenlib/BLI_arithb.h @@ -949,6 +949,7 @@ Normalise2( ); void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3]); +void LocQuatSizeToMat4(float mat[][4], float loc[3], float quat[4], float size[3]); void tubemap(float x, float y, float z, float *u, float *v); void spheremap(float x, float y, float z, float *u, float *v); diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c index 102218d8b99..97d40fbb95b 100644 --- a/source/blender/blenlib/intern/arithb.c +++ b/source/blender/blenlib/intern/arithb.c @@ -3266,3 +3266,17 @@ void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3]) mat[3][1] = loc[1]; mat[3][2] = loc[2]; } + +/* make a 4x4 matrix out of 3 transform components */ +void LocQuatSizeToMat4(float mat[][4], float loc[3], float quat[4], float size[3]) +{ + float eul[3]; + + /* convert quaternion component to euler + * NOTE: not as good as using quat directly. Todo for later. + */ + QuatToEul(quat, eul); + + /* make into matrix using exisiting code */ + LocEulSizeToMat4(mat, loc, eul, size); +} diff --git a/source/blender/src/edit.c b/source/blender/src/edit.c index b915e1b6627..5e174e8fa47 100644 --- a/source/blender/src/edit.c +++ b/source/blender/src/edit.c @@ -1103,7 +1103,52 @@ void snap_sel_to_grid() if( ( ((base)->flag & SELECT) && ((base)->lay & G.vd->lay) && ((base)->object->id.lib==0))) { ob= base->object; if(ob->flag & OB_POSEMODE) { - ; // todo + bPoseChannel *pchan; + bArmature *arm= ob->data; + + for (pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) { + if(pchan->bone->flag & BONE_SELECTED) { + if(pchan->bone->layer & arm->layer) { + if (pchan->parent==NULL) { + float dLoc[3], oldLoc[3], nLoc[3]; + + /* get nearest grid point to snap to */ + VECCOPY(nLoc, pchan->pose_head); + vec[0]= gridf * (float)(floor(.5+ nLoc[0]/gridf)); + vec[1]= gridf * (float)(floor(.5+ nLoc[1]/gridf)); + vec[2]= gridf * (float)(floor(.5+ nLoc[2]/gridf)); + + /* adjust location */ + VecSubf(dLoc, vec, nLoc); + VECCOPY(oldLoc, pchan->loc); + VecAddf(pchan->loc, oldLoc, dLoc); + } + else if((pchan->bone->flag & BONE_CONNECTED)==0) { + float vecN[3], nLoc[3]; + float dLoc[3], oldLoc[3]; + + /* get nearest grid point to snap to */ + VECCOPY(nLoc, pchan->pose_mat[3]); + vec[0]= gridf * (float)(floor(.5+ nLoc[0]/gridf)); + vec[1]= gridf * (float)(floor(.5+ nLoc[1]/gridf)); + vec[2]= gridf * (float)(floor(.5+ nLoc[2]/gridf)); + + /* get bone-space location of grid point */ + armature_loc_pose_to_bone(pchan, vec, vecN); + + /* adjust location */ + VECCOPY(oldLoc, pchan->loc); + VecSubf(dLoc, vecN, oldLoc); + VecAddf(pchan->loc, oldLoc, dLoc); + } + /* if the bone has a parent and is connected to the parent, + * don't do anything - will break chain unless we do auto-ik. + */ + } + } + } + ob->pose->flag |= (POSE_LOCKED|POSE_DO_UNLOCK); + ob->recalc |= OB_RECALC_DATA; } else { ob->recalc |= OB_RECALC_OB; @@ -1196,10 +1241,26 @@ void snap_sel_to_curs() if(pchan->bone->flag & BONE_SELECTED) { if(pchan->bone->layer & arm->layer) { if(pchan->parent==NULL) { - /* this is wrong... lazy! */ - VECCOPY(pchan->loc, cursp); + float dLoc[3], oldLoc[3]; + + VecSubf(dLoc, cursp, pchan->pose_head); + VECCOPY(oldLoc, pchan->loc); + VecAddf(pchan->loc, oldLoc, dLoc); } - /* else todo... */ + else if((pchan->bone->flag & BONE_CONNECTED)==0) { + float curspn[3], dLoc[3], oldLoc[3]; + + /* get location of cursor in bone-space */ + armature_loc_pose_to_bone(pchan, cursp, curspn); + + /* calculate new position */ + VECCOPY(oldLoc, pchan->loc); + VecSubf(dLoc, curspn, oldLoc); + VecAddf(pchan->loc, oldLoc, dLoc); + } + /* if the bone has a parent and is connected to the parent, + * don't do anything - will break chain unless we do auto-ik. + */ } } } @@ -1459,10 +1520,29 @@ void snap_to_center() base= (G.scene->base.first); while(base) { if(((base)->flag & SELECT) && ((base)->lay & G.vd->lay) ) { - VECCOPY(vec, base->object->obmat[3]); - VecAddf(centroid, centroid, vec); - DO_MINMAX(vec, min, max); - count++; + ob= base->object; + if(ob->flag & OB_POSEMODE) { + bPoseChannel *pchan; + bArmature *arm= ob->data; + + for (pchan = ob->pose->chanbase.first; pchan; pchan=pchan->next) { + if(pchan->bone->flag & BONE_SELECTED) { + if(pchan->bone->layer & arm->layer) { + VECCOPY(vec, pchan->pose_mat[3]); + VecAddf(centroid, centroid, vec); + DO_MINMAX(vec, min, max); + count++; + } + } + } + } + else { + /* not armature bones (i.e. objects) */ + VECCOPY(vec, base->object->obmat[3]); + VecAddf(centroid, centroid, vec); + DO_MINMAX(vec, min, max); + count++; + } } base= base->next; } @@ -1514,7 +1594,37 @@ void snap_to_center() if( ( ((base)->flag & SELECT) && ((base)->lay & G.vd->lay) && ((base)->object->id.lib==0))) { ob= base->object; if(ob->flag & OB_POSEMODE) { - ; // todo + bPoseChannel *pchan; + bArmature *arm= ob->data; + + for (pchan = ob->pose->chanbase.first; pchan; pchan=pchan->next) { + if(pchan->bone->flag & BONE_SELECTED) { + if(pchan->bone->layer & arm->layer) { + if(pchan->parent==NULL) { + float dLoc[3], oldLoc[3]; + VecSubf(dLoc, snaploc, pchan->pose_head); + VECCOPY(oldLoc, pchan->loc); + VecAddf(pchan->loc, oldLoc, dLoc); + } + else if((pchan->bone->flag & BONE_CONNECTED)==0) { + float dLoc[3], oldLoc[3]; + + /* get location of cursor in bone-space */ + armature_loc_pose_to_bone(pchan, snaploc, vec); + + /* calculate new position */ + VECCOPY(oldLoc, pchan->loc); + VecSubf(dLoc, vec, oldLoc); + VecAddf(pchan->loc, oldLoc, dLoc); + } + /* if the bone has a parent and is connected to the parent, + * don't do anything - will break chain unless we do auto-ik. + */ + } + } + } + ob->pose->flag |= (POSE_LOCKED|POSE_DO_UNLOCK); + ob->recalc |= OB_RECALC_DATA; } else { ob->recalc |= OB_RECALC_OB;