void setLocalConstraint(TransInfo *t, int mode, const char text[]) {
if (t->flag & T_EDIT) {
float obmat[3][3];
copy_m3_m4(obmat, t->scene->obedit->obmat);
normalize_m3(obmat);
setConstraint(t, obmat, mode, text);
}
else {
if (t->total == 1) {
setConstraint(t, t->data->axismtx, mode, text);
}
else {
strncpy(t->con.text + 1, text, 48);
copy_m3_m3(t->con.mtx, t->data->axismtx);
t->con.mode = mode;
getConstraintMatrix(t);
startConstraint(t);
t->con.drawExtra = drawObjectConstraint;
t->con.applyVec = applyObjectConstraintVec;
t->con.applySize = applyObjectConstraintSize;
t->con.applyRot = applyObjectConstraintRot;
t->redraw = 1;
}
}
}
void ED_armature_transform_bones(struct bArmature *arm, float mat[4][4])
{
EditBone *ebone;
float scale = mat4_to_scale(mat); /* store the scale of the matrix here to use on envelopes */
float mat3[3][3];
copy_m3_m4(mat3, mat);
normalize_m3(mat3);
/* Do the rotations */
for (ebone = arm->edbo->first; ebone; ebone = ebone->next) {
float tmat[3][3];
/* find the current bone's roll matrix */
ED_armature_ebone_to_mat3(ebone, tmat);
/* transform the roll matrix */
mul_m3_m3m3(tmat, mat3, tmat);
/* transform the bone */
mul_m4_v3(mat, ebone->head);
mul_m4_v3(mat, ebone->tail);
/* apply the transformed roll back */
mat3_to_vec_roll(tmat, NULL, &ebone->roll);
ebone->rad_head *= scale;
ebone->rad_tail *= scale;
ebone->dist *= scale;
/* we could be smarter and scale by the matrix along the x & z axis */
ebone->xwidth *= scale;
ebone->zwidth *= scale;
}
}
static bool mball_select_similar_rotation(MetaBall *mb, const float thresh)
{
const float thresh_rad = thresh * (float)M_PI_2;
MetaElem *ml;
bool changed = false;
for (ml = mb->editelems->first; ml; ml = ml->next) {
if (ml->flag & SELECT) {
MetaElem *ml_iter;
float ml_mat[3][3];
unit_m3(ml_mat);
mul_qt_v3(ml->quat, ml_mat[0]);
mul_qt_v3(ml->quat, ml_mat[1]);
mul_qt_v3(ml->quat, ml_mat[2]);
normalize_m3(ml_mat);
for (ml_iter = mb->editelems->first; ml_iter; ml_iter = ml_iter->next) {
if ((ml_iter->flag & SELECT) == 0) {
float ml_iter_mat[3][3];
unit_m3(ml_iter_mat);
mul_qt_v3(ml_iter->quat, ml_iter_mat[0]);
mul_qt_v3(ml_iter->quat, ml_iter_mat[1]);
mul_qt_v3(ml_iter->quat, ml_iter_mat[2]);
normalize_m3(ml_iter_mat);
if ((angle_normalized_v3v3(ml_mat[0], ml_iter_mat[0]) +
angle_normalized_v3v3(ml_mat[1], ml_iter_mat[1]) +
angle_normalized_v3v3(ml_mat[2], ml_iter_mat[2])) < thresh_rad)
{
ml_iter->flag |= SELECT;
changed = true;
}
}
}
}
}
return changed;
}
static void camera_frame_fit_data_init(
const Scene *scene, const Object *ob,
CameraParams *params, CameraViewFrameData *data)
{
float camera_rotmat_transposed_inversed[4][4];
unsigned int i;
/* setup parameters */
BKE_camera_params_init(params);
BKE_camera_params_from_object(params, ob);
/* compute matrix, viewplane, .. */
if (scene) {
BKE_camera_params_compute_viewplane(params, scene->r.xsch, scene->r.ysch, scene->r.xasp, scene->r.yasp);
}
else {
BKE_camera_params_compute_viewplane(params, 1, 1, 1.0f, 1.0f);
}
BKE_camera_params_compute_matrix(params);
/* initialize callback data */
copy_m3_m4(data->camera_rotmat, (float (*)[4])ob->obmat);
normalize_m3(data->camera_rotmat);
/* To transform a plane which is in its homogeneous representation (4d vector),
* we need the inverse of the transpose of the transform matrix... */
copy_m4_m3(camera_rotmat_transposed_inversed, data->camera_rotmat);
transpose_m4(camera_rotmat_transposed_inversed);
invert_m4(camera_rotmat_transposed_inversed);
/* Extract frustum planes from projection matrix. */
planes_from_projmat(params->winmat,
/* left right top bottom near far */
data->plane_tx[2], data->plane_tx[0], data->plane_tx[3], data->plane_tx[1], NULL, NULL);
/* Rotate planes and get normals from them */
for (i = 0; i < CAMERA_VIEWFRAME_NUM_PLANES; i++) {
mul_m4_v4(camera_rotmat_transposed_inversed, data->plane_tx[i]);
normalize_v3_v3(data->normal_tx[i], data->plane_tx[i]);
}
copy_v4_fl(data->dist_vals_sq, FLT_MAX);
data->tot = 0;
data->is_ortho = params->is_ortho;
if (params->is_ortho) {
/* we want (0, 0, -1) transformed by camera_rotmat, this is a quicker shortcut. */
negate_v3_v3(data->camera_no, data->camera_rotmat[2]);
data->dist_to_cam = FLT_MAX;
}
}
int mathutils_any_to_rotmat(float rmat[3][3], PyObject *value, const char *error_prefix)
{
if (EulerObject_Check(value)) {
if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) {
return -1;
}
else {
eulO_to_mat3(rmat, ((EulerObject *)value)->eul, ((EulerObject *)value)->order);
return 0;
}
}
else if (QuaternionObject_Check(value)) {
if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) {
return -1;
}
else {
float tquat[4];
normalize_qt_qt(tquat, ((QuaternionObject *)value)->quat);
quat_to_mat3(rmat, tquat);
return 0;
}
}
else if (MatrixObject_Check(value)) {
if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) {
return -1;
}
else if (((MatrixObject *)value)->num_row < 3 || ((MatrixObject *)value)->num_col < 3) {
PyErr_Format(
PyExc_ValueError, "%.200s: matrix must have minimum 3x3 dimensions", error_prefix);
return -1;
}
else {
matrix_as_3x3(rmat, (MatrixObject *)value);
normalize_m3(rmat);
return 0;
}
}
else {
PyErr_Format(PyExc_TypeError,
"%.200s: expected a Euler, Quaternion or Matrix type, "
"found %.200s",
error_prefix,
Py_TYPE(value)->tp_name);
return -1;
}
}
void ED_armature_apply_transform(Object *ob, float mat[4][4])
{
EditBone *ebone;
bArmature *arm = ob->data;
float scale = mat4_to_scale(mat); /* store the scale of the matrix here to use on envelopes */
float mat3[3][3];
copy_m3_m4(mat3, mat);
normalize_m3(mat3);
/* Put the armature into editmode */
ED_armature_to_edit(ob);
/* Do the rotations */
for (ebone = arm->edbo->first; ebone; ebone = ebone->next) {
float delta[3], tmat[3][3];
/* find the current bone's roll matrix */
sub_v3_v3v3(delta, ebone->tail, ebone->head);
vec_roll_to_mat3(delta, ebone->roll, tmat);
/* transform the roll matrix */
mul_m3_m3m3(tmat, mat3, tmat);
/* transform the bone */
mul_m4_v3(mat, ebone->head);
mul_m4_v3(mat, ebone->tail);
/* apply the transfiormed roll back */
mat3_to_vec_roll(tmat, NULL, &ebone->roll);
ebone->rad_head *= scale;
ebone->rad_tail *= scale;
ebone->dist *= scale;
/* we could be smarter and scale by the matrix along the x & z axis */
ebone->xwidth *= scale;
ebone->zwidth *= scale;
}
/* Turn the list into an armature */
ED_armature_from_edit(ob);
ED_armature_edit_free(ob);
}
/* Evaluate spline IK for a given bone */
static void splineik_evaluate_bone(tSplineIK_Tree *tree, Scene *scene, Object *ob, bPoseChannel *pchan,
int index, float ctime)
{
bSplineIKConstraint *ikData = tree->ikData;
float poseHead[3], poseTail[3], poseMat[4][4];
float splineVec[3], scaleFac, radius = 1.0f;
/* firstly, calculate the bone matrix the standard way, since this is needed for roll control */
BKE_pose_where_is_bone(scene, ob, pchan, ctime, 1);
copy_v3_v3(poseHead, pchan->pose_head);
copy_v3_v3(poseTail, pchan->pose_tail);
/* step 1: determine the positions for the endpoints of the bone */
{
float vec[4], dir[3], rad;
float tailBlendFac = 1.0f;
/* determine if the bone should still be affected by SplineIK */
if (tree->points[index + 1] >= 1.0f) {
/* spline doesn't affect the bone anymore, so done... */
pchan->flag |= POSE_DONE;
return;
}
else if ((tree->points[index] >= 1.0f) && (tree->points[index + 1] < 1.0f)) {
/* blending factor depends on the amount of the bone still left on the chain */
tailBlendFac = (1.0f - tree->points[index + 1]) / (tree->points[index] - tree->points[index + 1]);
}
/* tail endpoint */
if (where_on_path(ikData->tar, tree->points[index], vec, dir, NULL, &rad, NULL)) {
/* apply curve's object-mode transforms to the position
* unless the option to allow curve to be positioned elsewhere is activated (i.e. no root)
*/
if ((ikData->flag & CONSTRAINT_SPLINEIK_NO_ROOT) == 0)
mul_m4_v3(ikData->tar->obmat, vec);
/* convert the position to pose-space, then store it */
mul_m4_v3(ob->imat, vec);
interp_v3_v3v3(poseTail, pchan->pose_tail, vec, tailBlendFac);
/* set the new radius */
radius = rad;
}
/* head endpoint */
if (where_on_path(ikData->tar, tree->points[index + 1], vec, dir, NULL, &rad, NULL)) {
/* apply curve's object-mode transforms to the position
* unless the option to allow curve to be positioned elsewhere is activated (i.e. no root)
*/
if ((ikData->flag & CONSTRAINT_SPLINEIK_NO_ROOT) == 0)
mul_m4_v3(ikData->tar->obmat, vec);
/* store the position, and convert it to pose space */
mul_m4_v3(ob->imat, vec);
copy_v3_v3(poseHead, vec);
/* set the new radius (it should be the average value) */
radius = (radius + rad) / 2;
}
}
/* step 2: determine the implied transform from these endpoints
* - splineVec: the vector direction that the spline applies on the bone
* - scaleFac: the factor that the bone length is scaled by to get the desired amount
*/
sub_v3_v3v3(splineVec, poseTail, poseHead);
scaleFac = len_v3(splineVec) / pchan->bone->length;
/* step 3: compute the shortest rotation needed to map from the bone rotation to the current axis
* - this uses the same method as is used for the Damped Track Constraint (see the code there for details)
*/
{
float dmat[3][3], rmat[3][3], tmat[3][3];
float raxis[3], rangle;
/* compute the raw rotation matrix from the bone's current matrix by extracting only the
* orientation-relevant axes, and normalizing them
*/
copy_v3_v3(rmat[0], pchan->pose_mat[0]);
copy_v3_v3(rmat[1], pchan->pose_mat[1]);
copy_v3_v3(rmat[2], pchan->pose_mat[2]);
normalize_m3(rmat);
/* also, normalize the orientation imposed by the bone, now that we've extracted the scale factor */
normalize_v3(splineVec);
/* calculate smallest axis-angle rotation necessary for getting from the
* current orientation of the bone, to the spline-imposed direction
*/
cross_v3_v3v3(raxis, rmat[1], splineVec);
rangle = dot_v3v3(rmat[1], splineVec);
CLAMP(rangle, -1.0f, 1.0f);
rangle = acosf(rangle);
/* multiply the magnitude of the angle by the influence of the constraint to
* control the influence of the SplineIK effect
*/
rangle *= tree->con->enforce;
/* construct rotation matrix from the axis-angle rotation found above
* - this call takes care to make sure that the axis provided is a unit vector first
*/
axis_angle_to_mat3(dmat, raxis, rangle);
/* combine these rotations so that the y-axis of the bone is now aligned as the spline dictates,
* while still maintaining roll control from the existing bone animation
*/
mul_m3_m3m3(tmat, dmat, rmat); /* m1, m3, m2 */
normalize_m3(tmat); /* attempt to reduce shearing, though I doubt this'll really help too much now... */
copy_m4_m3(poseMat, tmat);
}
/* step 4: set the scaling factors for the axes */
{
/* only multiply the y-axis by the scaling factor to get nice volume-preservation */
mul_v3_fl(poseMat[1], scaleFac);
/* set the scaling factors of the x and z axes from... */
switch (ikData->xzScaleMode) {
case CONSTRAINT_SPLINEIK_XZS_ORIGINAL:
{
/* original scales get used */
float scale;
/* x-axis scale */
scale = len_v3(pchan->pose_mat[0]);
mul_v3_fl(poseMat[0], scale);
/* z-axis scale */
scale = len_v3(pchan->pose_mat[2]);
mul_v3_fl(poseMat[2], scale);
break;
}
case CONSTRAINT_SPLINEIK_XZS_INVERSE:
{
/* old 'volume preservation' method using the inverse scale */
float scale;
/* calculate volume preservation factor which is
* basically the inverse of the y-scaling factor
*/
if (fabsf(scaleFac) != 0.0f) {
scale = 1.0f / fabsf(scaleFac);
/* we need to clamp this within sensible values */
/* NOTE: these should be fine for now, but should get sanitised in future */
CLAMP(scale, 0.0001f, 100000.0f);
}
else
scale = 1.0f;
/* apply the scaling */
mul_v3_fl(poseMat[0], scale);
mul_v3_fl(poseMat[2], scale);
break;
}
case CONSTRAINT_SPLINEIK_XZS_VOLUMETRIC:
{
/* improved volume preservation based on the Stretch To constraint */
float final_scale;
/* as the basis for volume preservation, we use the inverse scale factor... */
if (fabsf(scaleFac) != 0.0f) {
/* NOTE: The method here is taken wholesale from the Stretch To constraint */
float bulge = powf(1.0f / fabsf(scaleFac), ikData->bulge);
if (bulge > 1.0f) {
if (ikData->flag & CONSTRAINT_SPLINEIK_USE_BULGE_MAX) {
float bulge_max = max_ff(ikData->bulge_max, 1.0f);
float hard = min_ff(bulge, bulge_max);
float range = bulge_max - 1.0f;
float scale = (range > 0.0f) ? 1.0f / range : 0.0f;
float soft = 1.0f + range * atanf((bulge - 1.0f) * scale) / (float)M_PI_2;
bulge = interpf(soft, hard, ikData->bulge_smooth);
}
}
if (bulge < 1.0f) {
if (ikData->flag & CONSTRAINT_SPLINEIK_USE_BULGE_MIN) {
float bulge_min = CLAMPIS(ikData->bulge_min, 0.0f, 1.0f);
float hard = max_ff(bulge, bulge_min);
float range = 1.0f - bulge_min;
float scale = (range > 0.0f) ? 1.0f / range : 0.0f;
float soft = 1.0f - range * atanf((1.0f - bulge) * scale) / (float)M_PI_2;
bulge = interpf(soft, hard, ikData->bulge_smooth);
}
}
/* compute scale factor for xz axes from this value */
final_scale = sqrtf(bulge);
}
else {
/* no scaling, so scale factor is simple */
final_scale = 1.0f;
}
/* apply the scaling (assuming normalised scale) */
mul_v3_fl(poseMat[0], final_scale);
mul_v3_fl(poseMat[2], final_scale);
break;
}
}
/* finally, multiply the x and z scaling by the radius of the curve too,
* to allow automatic scales to get tweaked still
*/
if ((ikData->flag & CONSTRAINT_SPLINEIK_NO_CURVERAD) == 0) {
mul_v3_fl(poseMat[0], radius);
mul_v3_fl(poseMat[2], radius);
}
}
/* step 5: set the location of the bone in the matrix */
if (ikData->flag & CONSTRAINT_SPLINEIK_NO_ROOT) {
/* when the 'no-root' option is affected, the chain can retain
* the shape but be moved elsewhere
*/
copy_v3_v3(poseHead, pchan->pose_head);
}
else if (tree->con->enforce < 1.0f) {
/* when the influence is too low
* - blend the positions for the 'root' bone
* - stick to the parent for any other
*/
if (pchan->parent) {
copy_v3_v3(poseHead, pchan->pose_head);
}
else {
/* FIXME: this introduces popping artifacts when we reach 0.0 */
interp_v3_v3v3(poseHead, pchan->pose_head, poseHead, tree->con->enforce);
}
}
copy_v3_v3(poseMat[3], poseHead);
/* finally, store the new transform */
copy_m4_m4(pchan->pose_mat, poseMat);
copy_v3_v3(pchan->pose_head, poseHead);
/* recalculate tail, as it's now outdated after the head gets adjusted above! */
BKE_pose_where_is_bone_tail(pchan);
/* done! */
pchan->flag |= POSE_DONE;
}
/* only valid for perspective cameras */
int camera_view_frame_fit_to_scene(Scene *scene, struct View3D *v3d, Object *camera_ob, float r_co[3])
{
float shift[2];
float plane_tx[4][3];
float rot_obmat[3][3];
const float zero[3]= {0,0,0};
CameraViewFrameData data_cb;
unsigned int i;
camera_view_frame(scene, camera_ob->data, data_cb.frame_tx);
copy_m3_m4(rot_obmat, camera_ob->obmat);
normalize_m3(rot_obmat);
for (i= 0; i < 4; i++) {
/* normalize so Z is always 1.0f*/
mul_v3_fl(data_cb.frame_tx[i], 1.0f/data_cb.frame_tx[i][2]);
}
/* get the shift back out of the frame */
shift[0]= (data_cb.frame_tx[0][0] +
data_cb.frame_tx[1][0] +
data_cb.frame_tx[2][0] +
data_cb.frame_tx[3][0]) / 4.0f;
shift[1]= (data_cb.frame_tx[0][1] +
data_cb.frame_tx[1][1] +
data_cb.frame_tx[2][1] +
data_cb.frame_tx[3][1]) / 4.0f;
for (i= 0; i < 4; i++) {
mul_m3_v3(rot_obmat, data_cb.frame_tx[i]);
}
for (i= 0; i < 4; i++) {
normal_tri_v3(data_cb.normal_tx[i],
zero, data_cb.frame_tx[i], data_cb.frame_tx[(i + 1) % 4]);
}
/* initialize callback data */
data_cb.dist_vals[0]=
data_cb.dist_vals[1]=
data_cb.dist_vals[2]=
data_cb.dist_vals[3]= FLT_MAX;
data_cb.tot= 0;
/* run callback on all visible points */
BKE_scene_foreach_display_point(scene, v3d, BA_SELECT,
camera_to_frame_view_cb, &data_cb);
if (data_cb.tot <= 1) {
return FALSE;
}
else {
float plane_isect_1[3], plane_isect_1_no[3], plane_isect_1_other[3];
float plane_isect_2[3], plane_isect_2_no[3], plane_isect_2_other[3];
float plane_isect_pt_1[3], plane_isect_pt_2[3];
/* apply the dist-from-plane's to the transformed plane points */
for (i= 0; i < 4; i++) {
mul_v3_v3fl(plane_tx[i], data_cb.normal_tx[i], data_cb.dist_vals[i]);
}
isect_plane_plane_v3(plane_isect_1, plane_isect_1_no,
plane_tx[0], data_cb.normal_tx[0],
plane_tx[2], data_cb.normal_tx[2]);
isect_plane_plane_v3(plane_isect_2, plane_isect_2_no,
plane_tx[1], data_cb.normal_tx[1],
plane_tx[3], data_cb.normal_tx[3]);
add_v3_v3v3(plane_isect_1_other, plane_isect_1, plane_isect_1_no);
add_v3_v3v3(plane_isect_2_other, plane_isect_2, plane_isect_2_no);
if (isect_line_line_v3(plane_isect_1, plane_isect_1_other,
plane_isect_2, plane_isect_2_other,
plane_isect_pt_1, plane_isect_pt_2) == 0)
{
return FALSE;
}
else {
float cam_plane_no[3]= {0.0f, 0.0f, -1.0f};
float plane_isect_delta[3];
float plane_isect_delta_len;
mul_m3_v3(rot_obmat, cam_plane_no);
sub_v3_v3v3(plane_isect_delta, plane_isect_pt_2, plane_isect_pt_1);
plane_isect_delta_len= len_v3(plane_isect_delta);
if (dot_v3v3(plane_isect_delta, cam_plane_no) > 0.0f) {
copy_v3_v3(r_co, plane_isect_pt_1);
/* offset shift */
normalize_v3(plane_isect_1_no);
madd_v3_v3fl(r_co, plane_isect_1_no, shift[1] * -plane_isect_delta_len);
}
else {
copy_v3_v3(r_co, plane_isect_pt_2);
/* offset shift */
normalize_v3(plane_isect_2_no);
madd_v3_v3fl(r_co, plane_isect_2_no, shift[0] * -plane_isect_delta_len);
}
return TRUE;
}
}
}
/* called from within the core BKE_pose_where_is loop, all animsystems and constraints
* were executed & assigned. Now as last we do an IK pass */
static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree)
{
float R_parmat[3][3], identity[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;
float resultinf = 0.0f;
int a, flag, hasstretch = 0, resultblend = 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) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
flag |= IK_XDOF;
if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
flag |= IK_YDOF;
if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
flag |= IK_ZDOF;
if (tree->stretch && (pchan->ikstretch > 0.0f)) {
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 */
copy_m3_m4(R_bonemat, pchan->pose_mat);
/* gather transformations for this IK segment */
if (pchan->parent)
copy_m3_m4(R_parmat, pchan->parent->pose_mat);
else
unit_m3(R_parmat);
/* bone offset */
if (pchan->parent && (a > 0))
sub_v3_v3v3(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 * len_v3(R_bonemat[1]);
/* compute rest basis and its inverse */
copy_m3_m3(rest_basis, bone->bone_mat);
copy_m3_m3(irest_basis, bone->bone_mat);
transpose_m3(irest_basis);
/* compute basis with rest_basis removed */
invert_m3_m3(iR_parmat, R_parmat);
mul_m3_m3m3(full_basis, iR_parmat, R_bonemat);
mul_m3_m3m3(basis, irest_basis, full_basis);
/* basis must be pure rotation */
normalize_m3(basis);
/* transform offset into local bone space */
normalize_m3(iR_parmat);
mul_m3_v3(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.0f)) {
const float ikstretch = pchan->ikstretch * pchan->ikstretch;
/* this function does its own clamping */
IK_SetStiffness(seg, IK_TRANS_Y, 1.0f - ikstretch);
IK_SetLimit(seg, IK_TRANS_Y, IK_STRETCH_STIFF_MIN, IK_STRETCH_STIFF_MAX);
}
}
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. */
copy_m4_m4(rootmat, pchan->parent->pose_mat);
/* However, we do not want to get (i.e. reverse) parent's scale, as it generates [#31008]
* kind of nasty bugs... */
normalize_m4(rootmat);
}
else
unit_m4(rootmat);
copy_v3_v3(rootmat[3], pchan->pose_head);
mul_m4_m4m4(imat, ob->obmat, rootmat);
invert_m4_m4(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)
*/
BKE_constraint_target_matrix_get(scene, target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
/* and set and transform goal */
mul_m4_m4m4(goal, goalinv, rootmat);
copy_v3_v3(goalpos, goal[3]);
copy_m3_m4(goalrot, goal);
normalize_m3(goalrot);
/* same for pole vector target */
if (data->poletar) {
BKE_constraint_target_matrix_get(scene, 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 {
mul_m4_m4m4(goal, goalinv, rootmat);
copy_v3_v3(polepos, goal[3]);
poleconstrain = 1;
/* for pole targets, we blend the result of the ik solver
* instead of the target position, otherwise we can't get
* a smooth transition */
resultblend = 1;
resultinf = target->con->enforce;
if (data->flag & CONSTRAINT_IK_GETANGLE) {
poleangledata = data;
data->flag &= ~CONSTRAINT_IK_GETANGLE;
}
}
}
/* do we need blending? */
if (!resultblend && target->con->enforce != 1.0f) {
float q1[4], q2[4], q[4];
float fac = target->con->enforce;
float mfac = 1.0f - fac;
pchan = tree->pchan[target->tip];
/* end effector in world space */
copy_m4_m4(end_pose, pchan->pose_mat);
copy_v3_v3(end_pose[3], pchan->pose_tail);
mul_serie_m4(world_pose, goalinv, ob->obmat, end_pose, NULL, NULL, NULL, NULL, NULL);
/* 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 */
mat3_to_quat(q1, goalrot);
mat4_to_quat(q2, world_pose);
interp_qt_qtqt(q, q1, q2, mfac);
quat_to_mat3(goalrot, q);
}
iktarget = iktree[target->tip];
if ((data->flag & CONSTRAINT_IK_POS) && data->weight != 0.0f) {
if (poleconstrain)
IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
polepos, data->poleangle, (poleangledata == data));
IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
}
if ((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0f))
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);
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.0f;
if (tree->stretch && (pchan->ikstretch > 0.0f)) {
float trans[3], length;
IK_GetTranslationChange(iktree[a], trans);
length = pchan->bone->length * len_v3(pchan->pose_mat[1]);
ikstretch[a] = (length == 0.0f) ? 1.0f : (trans[1] + length) / length;
}
else
ikstretch[a] = 1.0;
stretch = (parentstretch == 0.0f) ? 1.0f : ikstretch[a] / parentstretch;
mul_v3_fl(tree->basis_change[a][0], stretch);
mul_v3_fl(tree->basis_change[a][1], stretch);
mul_v3_fl(tree->basis_change[a][2], stretch);
}
if (resultblend && resultinf != 1.0f) {
unit_m3(identity);
blend_m3_m3m3(tree->basis_change[a], identity,
tree->basis_change[a], resultinf);
}
IK_FreeSegment(iktree[a]);
}
MEM_freeN(iktree);
if (ikstretch) MEM_freeN(ikstretch);
}
PyObject* BL_ArmatureChannel::py_attr_get_joint_rotation(void *self_v, const struct KX_PYATTRIBUTE_DEF *attrdef)
{
bPoseChannel* pchan = static_cast<bPoseChannel*>(self_v);
// decompose the pose matrix in euler rotation
float rest_mat[3][3];
float pose_mat[3][3];
float joint_mat[3][3];
float joints[3];
float norm;
double sa, ca;
// get rotation in armature space
copy_m3_m4(pose_mat, pchan->pose_mat);
normalize_m3(pose_mat);
if (pchan->parent) {
// bone has a parent, compute the rest pose of the bone taking actual pose of parent
mult_m3_m3m4(rest_mat, pchan->parent->pose_mat, pchan->bone->bone_mat);
normalize_m3(rest_mat);
} else {
// otherwise, the bone matrix in armature space is the rest pose
copy_m3_m4(rest_mat, pchan->bone->arm_mat);
}
// remove the rest pose to get the joint movement
transpose_m3(rest_mat);
mul_m3_m3m3(joint_mat, rest_mat, pose_mat);
joints[0] = joints[1] = joints[2] = 0.f;
// returns a 3 element list that gives corresponding joint
int flag = 0;
if (!(pchan->ikflag & BONE_IK_NO_XDOF))
flag |= 1;
if (!(pchan->ikflag & BONE_IK_NO_YDOF))
flag |= 2;
if (!(pchan->ikflag & BONE_IK_NO_ZDOF))
flag |= 4;
switch (flag) {
case 0: // fixed joint
break;
case 1: // X only
mat3_to_eulO( joints, EULER_ORDER_XYZ,joint_mat);
joints[1] = joints[2] = 0.f;
break;
case 2: // Y only
mat3_to_eulO( joints, EULER_ORDER_XYZ,joint_mat);
joints[0] = joints[2] = 0.f;
break;
case 3: // X+Y
mat3_to_eulO( joints, EULER_ORDER_ZYX,joint_mat);
joints[2] = 0.f;
break;
case 4: // Z only
mat3_to_eulO( joints, EULER_ORDER_XYZ,joint_mat);
joints[0] = joints[1] = 0.f;
break;
case 5: // X+Z
// decompose this as an equivalent rotation vector in X/Z plane
joints[0] = joint_mat[1][2];
joints[2] = -joint_mat[1][0];
norm = normalize_v3(joints);
if (norm < FLT_EPSILON) {
norm = (joint_mat[1][1] < 0.f) ? M_PI : 0.f;
} else {
norm = acos(joint_mat[1][1]);
}
mul_v3_fl(joints, norm);
break;
case 6: // Y+Z
mat3_to_eulO( joints, EULER_ORDER_XYZ,joint_mat);
joints[0] = 0.f;
break;
case 7: // X+Y+Z
// equivalent axis
joints[0] = (joint_mat[1][2]-joint_mat[2][1])*0.5f;
joints[1] = (joint_mat[2][0]-joint_mat[0][2])*0.5f;
joints[2] = (joint_mat[0][1]-joint_mat[1][0])*0.5f;
sa = len_v3(joints);
ca = (joint_mat[0][0]+joint_mat[1][1]+joint_mat[1][1]-1.0f)*0.5f;
if (sa > FLT_EPSILON) {
norm = atan2(sa,ca)/sa;
} else {
if (ca < 0.0) {
norm = M_PI;
mul_v3_fl(joints,0.f);
if (joint_mat[0][0] > 0.f) {
joints[0] = 1.0f;
} else if (joint_mat[1][1] > 0.f) {
joints[1] = 1.0f;
} else {
joints[2] = 1.0f;
}
} else {
norm = 0.0;
}
}
mul_v3_fl(joints,norm);
break;
}
return Vector_CreatePyObject(joints, 3, Py_NEW, NULL);
}
