Cycles: svn merge -r39870:r40266 https://svn.blender.org/svnroot/bf-blender/trunk...
[blender-staging.git] / source / blender / blenkernel / intern / constraint.c
index a3f1cb0cb0c9d41388841fb36d47de217a12662d..91091d3880f5774ec6232dd7746e01e52dc53dcf 100644 (file)
@@ -1,6 +1,4 @@
-/**
- * $Id$
- *
+/*
  * ***** BEGIN GPL LICENSE BLOCK *****
  *
  * This program is free software; you can redistribute it and/or
  * ***** END GPL LICENSE BLOCK *****
  */
 
+/** \file blender/blenkernel/intern/constraint.c
+ *  \ingroup bke
+ */
+
+
 #include <stdio.h> 
 #include <stddef.h>
 #include <string.h>
@@ -39,6 +42,7 @@
 #include "BLI_listbase.h"
 #include "BLI_math.h"
 #include "BLI_editVert.h"
+#include "BLI_utildefines.h"
 
 #include "DNA_armature_types.h"
 #include "DNA_constraint_types.h"
@@ -52,7 +56,7 @@
 #include "DNA_scene_types.h"
 #include "DNA_text_types.h"
 
-#include "BKE_utildefines.h"
+
 #include "BKE_action.h"
 #include "BKE_anim.h" /* for the curve calculation part */
 #include "BKE_armature.h"
@@ -70,7 +74,7 @@
 #include "BKE_shrinkwrap.h"
 #include "BKE_mesh.h"
 
-#ifndef DISABLE_PYTHON
+#ifdef WITH_PYTHON
 #include "BPY_extern.h"
 #endif
 
@@ -256,7 +260,7 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
                                                                
                                                        /* construct offs_bone the same way it is done in armature.c */
                                                        copy_m4_m3(offs_bone, pchan->bone->bone_mat);
-                                                       VECCOPY(offs_bone[3], pchan->bone->head);
+                                                       copy_v3_v3(offs_bone[3], pchan->bone->head);
                                                        offs_bone[3][1]+= pchan->bone->parent->length;
                                                        
                                                        if (pchan->bone->flag & BONE_HINGE) {
@@ -267,7 +271,7 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
                                                                copy_m4_m4(tmat, pchan->bone->parent->arm_mat);
                                                                
                                                                /* the location of actual parent transform */
-                                                               VECCOPY(tmat[3], offs_bone[3]);
+                                                               copy_v3_v3(tmat[3], offs_bone[3]);
                                                                offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
                                                                mul_m4_v3(pchan->parent->pose_mat, tmat[3]);
                                                                
@@ -309,7 +313,7 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
                                                
                                                /* construct offs_bone the same way it is done in armature.c */
                                                copy_m4_m3(offs_bone, pchan->bone->bone_mat);
-                                               VECCOPY(offs_bone[3], pchan->bone->head);
+                                               copy_v3_v3(offs_bone[3], pchan->bone->head);
                                                offs_bone[3][1]+= pchan->bone->parent->length;
                                                
                                                if (pchan->bone->flag & BONE_HINGE) {
@@ -320,8 +324,8 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
                                                        copy_m4_m4(tmat, pchan->bone->parent->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;
+                                                       copy_v3_v3(tmat[3], offs_bone[3]);
+                                                       zero_v3(offs_bone[3]);
                                                        mul_m4_v3(pchan->parent->pose_mat, tmat[3]);
                                                        
                                                        mul_m4_m4m4(diff_mat, offs_bone, tmat);
@@ -371,7 +375,7 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
        else {
                /* objects */
                if (from==CONSTRAINT_SPACE_WORLD && to==CONSTRAINT_SPACE_LOCAL) {
-                       /* check if object has a parent - otherwise this won't work */
+                       /* check if object has a parent */
                        if (ob->parent) {
                                /* 'subtract' parent's effects from owner */
                                mul_m4_m4m4(diff_mat, ob->parentinv, ob->parent->obmat);
@@ -379,6 +383,18 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
                                copy_m4_m4(tempmat, mat);
                                mul_m4_m4m4(mat, tempmat, imat);
                        }
+                       else {
+                               /* Local space in this case will have to be defined as local to the owner's 
+                                * transform-property-rotated axes. So subtract this rotation component.
+                                */
+                               object_to_mat4(ob, diff_mat);
+                               normalize_m4(diff_mat);
+                               zero_v3(diff_mat[3]);
+                               
+                               invert_m4_m4(imat, diff_mat);
+                               copy_m4_m4(tempmat, mat);
+                               mul_m4_m4m4(mat, tempmat, imat);
+                       }
                }
                else if (from==CONSTRAINT_SPACE_LOCAL && to==CONSTRAINT_SPACE_WORLD) {
                        /* check that object has a parent - otherwise this won't work */
@@ -388,6 +404,17 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
                                mul_m4_m4m4(diff_mat, ob->parentinv, ob->parent->obmat);
                                mul_m4_m4m4(mat, tempmat, diff_mat);
                        }
+                       else {
+                               /* Local space in this case will have to be defined as local to the owner's 
+                                * transform-property-rotated axes. So add back this rotation component.
+                                */
+                               object_to_mat4(ob, diff_mat);
+                               normalize_m4(diff_mat);
+                               zero_v3(diff_mat[3]);
+                               
+                               copy_m4_m4(tempmat, mat);
+                               mul_m4_m4m4(mat, tempmat, diff_mat);
+                       }
                }
        }
 }
@@ -395,12 +422,12 @@ void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4
 /* ------------ General Target Matrix Tools ---------- */
 
 /* function that sets the given matrix based on given vertex group in mesh */
-static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, float mat[][4])
+static void contarget_get_mesh_mat (Object *ob, const char *substring, float mat[][4])
 {
        DerivedMesh *dm = NULL;
        Mesh *me= ob->data;
        EditMesh *em = BKE_mesh_get_editmesh(me);
-       float vec[3] = {0.0f, 0.0f, 0.0f}, tvec[3];
+       float vec[3] = {0.0f, 0.0f, 0.0f};
        float normal[3] = {0.0f, 0.0f, 0.0f}, plane[3];
        float imat[3][3], tmat[3][3];
        int dgroup;
@@ -420,16 +447,9 @@ static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, f
                freeDM= 1;
        }
        else {
-               /* when not in EditMode, use the 'final' derived mesh 
-                *      - check if the custom data masks for derivedFinal mean that we can just use that
-                *        (this is more effficient + sufficient for most cases)
-                */
-               if (!(ob->lastDataMask & CD_MASK_MDEFORMVERT)) {
-                       dm = mesh_get_derived_final(scene, ob, CD_MASK_MDEFORMVERT);
-                       freeDM= 1;
-               }
-               else 
-                       dm = (DerivedMesh *)ob->derivedFinal;
+               /* when not in EditMode, use the 'final' derived mesh, depsgraph
+                * ensures we build with CD_MDEFORMVERT layer */
+               dm = (DerivedMesh *)ob->derivedFinal;
        }
        
        /* only continue if there's a valid DerivedMesh */
@@ -477,9 +497,9 @@ static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, f
                        mul_m3_v3(tmat, normal);
                        
                        normalize_v3(normal);
-                       VECCOPY(plane, tmat[1]);
+                       copy_v3_v3(plane, tmat[1]);
                        
-                       VECCOPY(tmat[2], normal);
+                       copy_v3_v3(tmat[2], normal);
                        cross_v3_v3v3(tmat[0], normal, plane);
                        cross_v3_v3v3(tmat[1], tmat[2], tmat[0]);
                        
@@ -488,8 +508,7 @@ static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, f
                        
                        
                        /* apply the average coordinate as the new location */
-                       mul_v3_m4v3(tvec, ob->obmat, vec);
-                       VECCOPY(mat[3], tvec);
+                       mul_v3_m4v3(mat[3], ob->obmat, vec);
                }
        }
        
@@ -501,7 +520,7 @@ static void contarget_get_mesh_mat (Scene *scene, Object *ob, char *substring, f
 }
 
 /* function that sets the given matrix based on given vertex group in lattice */
-static void contarget_get_lattice_mat (Object *ob, char *substring, float mat[][4])
+static void contarget_get_lattice_mat (Object *ob, const char *substring, float mat[][4])
 {
        Lattice *lt= (Lattice *)ob->data;
        
@@ -554,12 +573,12 @@ static void contarget_get_lattice_mat (Object *ob, char *substring, float mat[][
        mul_v3_m4v3(tvec, ob->obmat, vec);
        
        /* copy new location to matrix */
-       VECCOPY(mat[3], tvec);
+       copy_v3_v3(mat[3], tvec);
 }
 
 /* generic function to get the appropriate matrix for most target cases */
 /* The cases where the target can be object data have not been implemented */
-static void constraint_target_to_mat4 (Scene *scene, Object *ob, char *substring, float mat[][4], short from, short to, float headtail)
+static void constraint_target_to_mat4 (Object *ob, const char *substring, float mat[][4], short from, short to, float headtail)
 {
        /*      Case OBJECT */
        if (!strlen(substring)) {
@@ -576,7 +595,7 @@ static void constraint_target_to_mat4 (Scene *scene, Object *ob, char *substring
         *              way as constraints can only really affect things on object/bone level.
         */
        else if (ob->type == OB_MESH) {
-               contarget_get_mesh_mat(scene, ob, substring, mat);
+               contarget_get_mesh_mat(ob, substring, mat);
                constraint_mat_convertspace(ob, NULL, mat, from, to);
        }
        else if (ob->type == OB_LATTICE) {
@@ -593,7 +612,7 @@ static void constraint_target_to_mat4 (Scene *scene, Object *ob, char *substring
                         * PoseChannel by the Armature Object's Matrix to get a worldspace
                         * matrix.
                         */
-                       if (headtail < 0.000001) {
+                       if (headtail < 0.000001f) {
                                /* skip length interpolation if set to head */
                                mul_m4_m4m4(mat, pchan->pose_mat, ob->obmat);
                        }
@@ -656,10 +675,10 @@ static bConstraintTypeInfo CTI_CONSTRNAME = {
 /* This function should be used for the get_target_matrix member of all 
  * constraints that are not picky about what happens to their target matrix.
  */
-static void default_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void default_get_tarmat (bConstraint *con, bConstraintOb *UNUSED(cob), bConstraintTarget *ct, float UNUSED(ctime))
 {
        if (VALID_CONS_TARGET(ct))
-               constraint_target_to_mat4(cob->scene, ct->tar, ct->subtarget, ct->matrix, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
+               constraint_target_to_mat4(ct->tar, ct->subtarget, ct->matrix, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
        else if (ct)
                unit_m4(ct->matrix);
 }
@@ -675,7 +694,7 @@ static void default_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstrain
                ct= MEM_callocN(sizeof(bConstraintTarget), "tempConstraintTarget"); \
                 \
                ct->tar= datatar; \
-               strcpy(ct->subtarget, datasubtarget); \
+               BLI_strncpy(ct->subtarget, datasubtarget, sizeof(ct->subtarget)); \
                ct->space= con->tarspace; \
                ct->flag= CONSTRAINT_TAR_TEMP; \
                 \
@@ -729,7 +748,7 @@ static void default_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstrain
                        bConstraintTarget *ctn = ct->next; \
                        if (nocopy == 0) { \
                                datatar= ct->tar; \
-                               strcpy(datasubtarget, ct->subtarget); \
+                               BLI_strncpy(datasubtarget, ct->subtarget, sizeof(datasubtarget)); \
                                con->tarspace= (char)ct->space; \
                        } \
                         \
@@ -811,50 +830,75 @@ static void childof_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
        
        /* only evaluate if there is a target */
        if (VALID_CONS_TARGET(ct)) {
-               float parmat[4][4], invmat[4][4], tempmat[4][4];
-               float loc[3], eul[3], size[3];
-               float loco[3], eulo[3], sizo[3];
-               
-               /* get offset (parent-inverse) matrix */
-               copy_m4_m4(invmat, data->invmat);
-               
-               /* extract components of both matrices */
-               VECCOPY(loc, ct->matrix[3]);
-               mat4_to_eulO( eul, ct->rotOrder,ct->matrix);
-               mat4_to_size( size,ct->matrix);
-               
-               VECCOPY(loco, invmat[3]);
-               mat4_to_eulO( eulo, cob->rotOrder,invmat);
-               mat4_to_size( sizo,invmat);
-               
-               /* disable channels not enabled */
-               if (!(data->flag & CHILDOF_LOCX)) loc[0]= loco[0]= 0.0f;
-               if (!(data->flag & CHILDOF_LOCY)) loc[1]= loco[1]= 0.0f;
-               if (!(data->flag & CHILDOF_LOCZ)) loc[2]= loco[2]= 0.0f;
-               if (!(data->flag & CHILDOF_ROTX)) eul[0]= eulo[0]= 0.0f;
-               if (!(data->flag & CHILDOF_ROTY)) eul[1]= eulo[1]= 0.0f;
-               if (!(data->flag & CHILDOF_ROTZ)) eul[2]= eulo[2]= 0.0f;
-               if (!(data->flag & CHILDOF_SIZEX)) size[0]= sizo[0]= 1.0f;
-               if (!(data->flag & CHILDOF_SIZEY)) size[1]= sizo[1]= 1.0f;
-               if (!(data->flag & CHILDOF_SIZEZ)) size[2]= sizo[2]= 1.0f;
-               
-               /* make new target mat and offset mat */
-               loc_eulO_size_to_mat4(ct->matrix, loc, eul, size, ct->rotOrder);
-               loc_eulO_size_to_mat4(invmat, loco, eulo, sizo, cob->rotOrder);
-               
-               /* multiply target (parent matrix) by offset (parent inverse) to get 
-                * the effect of the parent that will be exherted on the owner
-                */
-               mul_m4_m4m4(parmat, invmat, ct->matrix);
+               float parmat[4][4];
                
-               /* now multiply the parent matrix by the owner matrix to get the 
-                * the effect of this constraint (i.e.  owner is 'parented' to parent)
-                */
-               copy_m4_m4(tempmat, cob->matrix);
-               mul_m4_m4m4(cob->matrix, tempmat, parmat); 
+               /* simple matrix parenting */
+               if(data->flag == CHILDOF_ALL) {
+                       
+                       /* multiply target (parent matrix) by offset (parent inverse) to get 
+                        * the effect of the parent that will be exherted on the owner
+                        */
+                       mul_m4_m4m4(parmat, data->invmat, ct->matrix);
+                       
+                       /* now multiply the parent matrix by the owner matrix to get the 
+                        * the effect of this constraint (i.e.  owner is 'parented' to parent)
+                        */
+                       mul_m4_m4m4(cob->matrix, cob->matrix, parmat);
+               }
+               else {
+                       float invmat[4][4], tempmat[4][4];
+                       float loc[3], eul[3], size[3];
+                       float loco[3], eulo[3], sizo[3];
+                       
+                       /* get offset (parent-inverse) matrix */
+                       copy_m4_m4(invmat, data->invmat);
+                       
+                       /* extract components of both matrices */
+                       copy_v3_v3(loc, ct->matrix[3]);
+                       mat4_to_eulO(eul, ct->rotOrder, ct->matrix);
+                       mat4_to_size(size, ct->matrix);
+                       
+                       copy_v3_v3(loco, invmat[3]);
+                       mat4_to_eulO(eulo, cob->rotOrder, invmat);
+                       mat4_to_size(sizo, invmat);
+                       
+                       /* disable channels not enabled */
+                       if (!(data->flag & CHILDOF_LOCX)) loc[0]= loco[0]= 0.0f;
+                       if (!(data->flag & CHILDOF_LOCY)) loc[1]= loco[1]= 0.0f;
+                       if (!(data->flag & CHILDOF_LOCZ)) loc[2]= loco[2]= 0.0f;
+                       if (!(data->flag & CHILDOF_ROTX)) eul[0]= eulo[0]= 0.0f;
+                       if (!(data->flag & CHILDOF_ROTY)) eul[1]= eulo[1]= 0.0f;
+                       if (!(data->flag & CHILDOF_ROTZ)) eul[2]= eulo[2]= 0.0f;
+                       if (!(data->flag & CHILDOF_SIZEX)) size[0]= sizo[0]= 1.0f;
+                       if (!(data->flag & CHILDOF_SIZEY)) size[1]= sizo[1]= 1.0f;
+                       if (!(data->flag & CHILDOF_SIZEZ)) size[2]= sizo[2]= 1.0f;
+                       
+                       /* make new target mat and offset mat */
+                       loc_eulO_size_to_mat4(ct->matrix, loc, eul, size, ct->rotOrder);
+                       loc_eulO_size_to_mat4(invmat, loco, eulo, sizo, cob->rotOrder);
+                       
+                       /* multiply target (parent matrix) by offset (parent inverse) to get 
+                        * the effect of the parent that will be exherted on the owner
+                        */
+                       mul_m4_m4m4(parmat, invmat, ct->matrix);
+                       
+                       /* now multiply the parent matrix by the owner matrix to get the 
+                        * the effect of this constraint (i.e.  owner is 'parented' to parent)
+                        */
+                       copy_m4_m4(tempmat, cob->matrix);
+                       mul_m4_m4m4(cob->matrix, tempmat, parmat);
+
+                       /* without this, changes to scale and rotation can change location
+                        * of a parentless bone or a disconnected bone. Even though its set
+                        * to zero above. */
+                       if (!(data->flag & CHILDOF_LOCX)) cob->matrix[3][0]= tempmat[3][0];
+                       if (!(data->flag & CHILDOF_LOCY)) cob->matrix[3][1]= tempmat[3][1];
+                       if (!(data->flag & CHILDOF_LOCZ)) cob->matrix[3][2]= tempmat[3][2];     
+               }
        }
 }
 
+/* XXX note, con->flag should be CONSTRAINT_SPACEONCE for bone-childof, patched in readfile.c */
 static bConstraintTypeInfo CTI_CHILDOF = {
        CONSTRAINT_TYPE_CHILDOF, /* type */
        sizeof(bChildOfConstraint), /* size */
@@ -940,12 +984,11 @@ static void vectomat (float *vec, float *target_up, short axis, short upflag, sh
        float right[3];
        float neg = -1;
        int right_index;
-       
-       copy_v3_v3(n, vec);
-       if (normalize_v3(n) == 0.0) { 
-               n[0] = 0.0;
-               n[1] = 0.0;
-               n[2] = 1.0;
+
+       if (normalize_v3_v3(n, vec) == 0.0f) {
+               n[0] = 0.0f;
+               n[1] = 0.0f;
+               n[2] = 1.0f;
        }
        if (axis > 2) axis -= 3;
        else negate_v3(n);
@@ -953,9 +996,7 @@ static void vectomat (float *vec, float *target_up, short axis, short upflag, sh
        /* n specifies the transformation of the track axis */
        if (flags & TARGET_Z_UP) { 
                /* target Z axis is the global up axis */
-               u[0] = target_up[0];
-               u[1] = target_up[1];
-               u[2] = target_up[2];
+               copy_v3_v3(u, target_up);
        }
        else { 
                /* world Z axis is the global up axis */
@@ -969,10 +1010,10 @@ static void vectomat (float *vec, float *target_up, short axis, short upflag, sh
        sub_v3_v3v3(proj, u, proj); /* then onto the plane */
        /* proj specifies the transformation of the up axis */
 
-       if (normalize_v3(proj) == 0.0) { /* degenerate projection */
-               proj[0] = 0.0;
-               proj[1] = 1.0;
-               proj[2] = 0.0;
+       if (normalize_v3(proj) == 0.0f) { /* degenerate projection */
+               proj[0] = 0.0f;
+               proj[1] = 1.0f;
+               proj[2] = 0.0f;
        }
 
        /* Normalized cross product of n and proj specifies transformation of the right axis */
@@ -988,20 +1029,13 @@ static void vectomat (float *vec, float *target_up, short axis, short upflag, sh
                m[right_index][1] = neg * right[1];
                m[right_index][2] = neg * right[2];
                
-               m[upflag][0] = proj[0];
-               m[upflag][1] = proj[1];
-               m[upflag][2] = proj[2];
+               copy_v3_v3(m[upflag], proj);
                
-               m[axis][0] = n[0];
-               m[axis][1] = n[1];
-               m[axis][2] = n[2];
+               copy_v3_v3(m[axis], n);
        }
        /* identity matrix - don't do anything if the two axes are the same */
        else {
-               m[0][0]= m[1][1]= m[2][2]= 1.0;
-               m[0][1]= m[0][2]= 0.0;
-               m[1][0]= m[1][2]= 0.0;
-               m[2][0]= m[2][1]= 0.0;
+               unit_m3(m);
        }
 }
 
@@ -1017,7 +1051,7 @@ static void trackto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
                float tmat[4][4];
                
                /* Get size property, since ob->size is only the object's own relative size, not its global one */
-               mat4_to_size( size,cob->matrix);
+               mat4_to_size(size, cob->matrix);
                
                /* Clear the object's rotation */       
                cob->matrix[0][0]=size[0];
@@ -1111,12 +1145,12 @@ static void kinematic_flush_tars (bConstraint *con, ListBase *list, short nocopy
        }
 }
 
-static void kinematic_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void kinematic_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float UNUSED(ctime))
 {
        bKinematicConstraint *data= con->data;
        
        if (VALID_CONS_TARGET(ct)) 
-               constraint_target_to_mat4(cob->scene, ct->tar, ct->subtarget, ct->matrix, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
+               constraint_target_to_mat4(ct->tar, ct->subtarget, ct->matrix, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
        else if (ct) {
                if (data->flag & CONSTRAINT_IK_AUTO) {
                        Object *ob= cob->ob;
@@ -1127,10 +1161,9 @@ static void kinematic_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstra
                        else {
                                float vec[3];
                                /* move grabtarget into world space */
-                               VECCOPY(vec, data->grabtarget);
-                               mul_m4_v3(ob->obmat, vec);
+                               mul_v3_m4v3(vec, ob->obmat, data->grabtarget);
                                copy_m4_m4(ct->matrix, ob->obmat);
-                               VECCOPY(ct->matrix[3], vec);
+                               copy_v3_v3(ct->matrix[3], vec);
                        }
                }
                else
@@ -1200,19 +1233,18 @@ static void followpath_flush_tars (bConstraint *con, ListBase *list, short nocop
        }
 }
 
-static void followpath_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void followpath_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float UNUSED(ctime))
 {
        bFollowPathConstraint *data= con->data;
        
        if (VALID_CONS_TARGET(ct)) {
                Curve *cu= ct->tar->data;
-               float q[4], vec[4], dir[3], quat[4], radius, x1;
-               float totmat[4][4];
+               float vec[4], dir[3], radius;
+               float totmat[4][4]= MAT4_UNITY;
                float curvetime;
-               
-               unit_m4(totmat);
+
                unit_m4(ct->matrix);
-               
+
                /* note: when creating constraints that follow path, the curve gets the CU_PATH set now,
                 *              currently for paths to work it needs to go through the bevlist/displist system (ton) 
                 */
@@ -1222,7 +1254,8 @@ static void followpath_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
                        makeDispListCurveTypes(cob->scene, ct->tar, 0);
                
                if (cu->path && cu->path->data) {
-                       if ((data->followflag & FOLLOWPATH_STATIC) == 0) { 
+                       float quat[4];
+                       if ((data->followflag & FOLLOWPATH_STATIC) == 0) {
                                /* animated position along curve depending on time */
                                if (cob->scene)
                                        curvetime= bsystem_time(cob->scene, ct->tar, cu->ctime, 0.0) - data->offset;
@@ -1236,15 +1269,17 @@ static void followpath_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
                                 * factor, which then gets clamped to lie within 0.0 - 1.0 range
                                 */
                                curvetime /= cu->pathlen;
-                               CLAMP(curvetime, 0.0, 1.0);
+                               CLAMP(curvetime, 0.0f, 1.0f);
                        }
                        else {
                                /* fixed position along curve */
                                curvetime= data->offset_fac;
                        }
                        
-                       if ( where_on_path(ct->tar, curvetime, vec, dir, NULL, &radius, NULL) ) {
+                       if ( where_on_path(ct->tar, curvetime, vec, dir, (data->followflag & FOLLOWPATH_FOLLOW) ? quat : NULL, &radius, NULL) ) { /* quat_pt is quat or NULL*/
                                if (data->followflag & FOLLOWPATH_FOLLOW) {
+#if 0
+                                       float x1, q[4];
                                        vec_to_quat(quat, dir, (short)data->trackflag, (short)data->upflag);
                                        
                                        normalize_v3(dir);
@@ -1254,10 +1289,13 @@ static void followpath_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
                                        q[2]= -x1*dir[1];
                                        q[3]= -x1*dir[2];
                                        mul_qt_qtqt(quat, q, quat);
-                                       
+#else
+                                       quat_apply_track(quat, data->trackflag, data->upflag);
+#endif
+
                                        quat_to_mat4(totmat, quat);
                                }
-                               
+
                                if (data->followflag & FOLLOWPATH_RADIUS) {
                                        float tmat[4][4], rmat[4][4];
                                        scale_m4_fl(tmat, radius);
@@ -1265,7 +1303,7 @@ static void followpath_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
                                        copy_m4_m4(totmat, rmat);
                                }
                                
-                               VECCOPY(totmat[3], vec);
+                               copy_v3_v3(totmat[3], vec);
                                
                                mul_serie_m4(ct->matrix, ct->tar->obmat, totmat, NULL, NULL, NULL, NULL, NULL, NULL);
                        }
@@ -1329,7 +1367,7 @@ static bConstraintTypeInfo CTI_FOLLOWPATH = {
 /* --------- Limit Location --------- */
 
 
-static void loclimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+static void loclimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *UNUSED(targets))
 {
        bLocLimitConstraint *data = con->data;
        
@@ -1377,18 +1415,18 @@ static bConstraintTypeInfo CTI_LOCLIMIT = {
 
 /* -------- Limit Rotation --------- */
 
-static void rotlimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+static void rotlimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *UNUSED(targets))
 {
        bRotLimitConstraint *data = con->data;
        float loc[3];
        float eul[3];
        float size[3];
        
-       VECCOPY(loc, cob->matrix[3]);
-       mat4_to_size( size,cob->matrix);
-       
-       mat4_to_eulO( eul, cob->rotOrder,cob->matrix);
-       
+       copy_v3_v3(loc, cob->matrix[3]);
+       mat4_to_size(size, cob->matrix);
+
+       mat4_to_eulO(eul, cob->rotOrder, cob->matrix);
+
        /* constraint data uses radians internally */
        
        /* limiting of euler values... */
@@ -1436,7 +1474,7 @@ static bConstraintTypeInfo CTI_ROTLIMIT = {
 /* --------- Limit Scaling --------- */
 
 
-static void sizelimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+static void sizelimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *UNUSED(targets))
 {
        bSizeLimitConstraint *data = con->data;
        float obsize[3], size[3];
@@ -1545,7 +1583,7 @@ static void loclike_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
                float offset[3] = {0.0f, 0.0f, 0.0f};
                
                if (data->flag & LOCLIKE_OFFSET)
-                       VECCOPY(offset, cob->matrix[3]);
+                       copy_v3_v3(offset, cob->matrix[3]);
                        
                if (data->flag & LOCLIKE_X) {
                        cob->matrix[3][0] = ct->matrix[3][0];
@@ -1637,12 +1675,13 @@ static void rotlike_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
                float   eul[3], obeul[3];
                float   size[3];
                
-               VECCOPY(loc, cob->matrix[3]);
-               mat4_to_size( size,cob->matrix);
+               copy_v3_v3(loc, cob->matrix[3]);
+               mat4_to_size(size, cob->matrix);
                
                /* to allow compatible rotations, must get both rotations in the order of the owner... */
-               mat4_to_eulO( eul, cob->rotOrder,ct->matrix);
-               mat4_to_eulO( obeul, cob->rotOrder,cob->matrix);
+               mat4_to_eulO(obeul, cob->rotOrder, cob->matrix);
+               /* we must get compatible eulers from the beginning because some of them can be modified below (see bug #21875) */
+               mat4_to_compatible_eulO(eul, obeul, cob->rotOrder, ct->matrix);
                
                if ((data->flag & ROTLIKE_X)==0)
                        eul[0] = obeul[0];
@@ -1674,6 +1713,7 @@ static void rotlike_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
                                eul[2] *= -1;
                }
                
+               /* good to make eulers compatible again, since we don't know how much they were changed above */
                compatible_eul(eul, obeul);
                loc_eulO_size_to_mat4(cob->matrix, loc, eul, size, cob->rotOrder);
        }
@@ -1828,7 +1868,7 @@ static void translike_flush_tars (bConstraint *con, ListBase *list, short nocopy
        }
 }
 
-static void translike_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+static void translike_evaluate (bConstraint *UNUSED(con), bConstraintOb *cob, ListBase *targets)
 {
        bConstraintTarget *ct= targets->first;
        
@@ -1864,33 +1904,33 @@ static void samevolume_new_data (void *cdata)
        data->volume = 1.0f;
 }
 
-static void samevolume_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+static void samevolume_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *UNUSED(targets))
 {
        bSameVolumeConstraint *data= con->data;
 
+       float volume = data->volume;
+       float fac = 1.0f;
        float obsize[3];
-       float volume=data->volume;
 
        mat4_to_size(obsize, cob->matrix);
-
+       
+       /* calculate normalising scale factor for non-essential values */
+       if (obsize[data->flag] != 0) 
+               fac = sqrtf(volume / obsize[data->flag]) / obsize[data->flag];
+       
+       /* apply scaling factor to the channels not being kept */
        switch (data->flag) {
                case SAMEVOL_X:
-                       if (obsize[0]!=0) {
-                               mul_v3_fl(cob->matrix[1], sqrt(volume/obsize[0])/obsize[0]);
-                               mul_v3_fl(cob->matrix[2], sqrt(volume/obsize[0])/obsize[0]);
-                       }
+                       mul_v3_fl(cob->matrix[1], fac);
+                       mul_v3_fl(cob->matrix[2], fac);
                        break;
                case SAMEVOL_Y:
-                       if (obsize[1]!=0) {
-                               mul_v3_fl(cob->matrix[0], sqrt(volume/obsize[1])/obsize[1]);
-                               mul_v3_fl(cob->matrix[2], sqrt(volume/obsize[1])/obsize[1]);
-                       }
+                       mul_v3_fl(cob->matrix[0], fac);
+                       mul_v3_fl(cob->matrix[2], fac);
                        break;
                case SAMEVOL_Z:
-                       if (obsize[2]!=0) {
-                               mul_v3_fl(cob->matrix[0], sqrt(volume/obsize[2])/obsize[2]);
-                               mul_v3_fl(cob->matrix[1], sqrt(volume/obsize[2])/obsize[2]);
-                       }
+                       mul_v3_fl(cob->matrix[0], fac);
+                       mul_v3_fl(cob->matrix[1], fac);
                        break;
        }
 }
@@ -1978,9 +2018,9 @@ static void pycon_id_looper (bConstraint *con, ConstraintIDFunc func, void *user
 }
 
 /* Whether this approach is maintained remains to be seen (aligorith) */
-static void pycon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void pycon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float UNUSED(ctime))
 {
-#ifndef DISABLE_PYTHON
+#ifdef WITH_PYTHON
        bPythonConstraint *data= con->data;
 #endif
 
@@ -1997,10 +2037,10 @@ static void pycon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintT
                /* firstly calculate the matrix the normal way, then let the py-function override
                 * this matrix if it needs to do so
                 */
-               constraint_target_to_mat4(cob->scene, ct->tar, ct->subtarget, ct->matrix, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
+               constraint_target_to_mat4(ct->tar, ct->subtarget, ct->matrix, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
                
                /* only execute target calculation if allowed */
-#ifndef DISABLE_PYTHON
+#ifdef WITH_PYTHON
                if (G.f & G_SCRIPT_AUTOEXEC)
                        BPY_pyconstraint_target(data, ct);
 #endif
@@ -2011,7 +2051,8 @@ static void pycon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintT
 
 static void pycon_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
 {
-#ifdef DISABLE_PYTHON
+#ifndef WITH_PYTHON
+       (void)con; (void)cob; (void)targets; /* unused */
        return;
 #else
        bPythonConstraint *data= con->data;
@@ -2029,8 +2070,8 @@ static void pycon_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targ
 #endif
        
        /* Now, run the actual 'constraint' function, which should only access the matrices */
-       BPY_pyconstraint_eval(data, cob, targets);
-#endif /* DISABLE_PYTHON */
+       BPY_pyconstraint_exec(data, cob, targets);
+#endif /* WITH_PYTHON */
 }
 
 static bConstraintTypeInfo CTI_PYTHON = {
@@ -2102,9 +2143,8 @@ static void actcon_flush_tars (bConstraint *con, ListBase *list, short nocopy)
        }
 }
 
-static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float UNUSED(ctime))
 {
-       extern void chan_calc_mat(bPoseChannel *chan);
        bActionConstraint *data = con->data;
        
        if (VALID_CONS_TARGET(ct)) {
@@ -2116,7 +2156,7 @@ static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraint
                unit_m4(ct->matrix);
                
                /* get the transform matrix of the target */
-               constraint_target_to_mat4(cob->scene, ct->tar, ct->subtarget, tempmat, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
+               constraint_target_to_mat4(ct->tar, ct->subtarget, tempmat, CONSTRAINT_SPACE_WORLD, ct->space, con->headtail);
                
                /* determine where in transform range target is */
                /* data->type is mapped as follows for backwards compatability:
@@ -2127,9 +2167,7 @@ static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraint
                if (data->type < 10) {
                        /* extract rotation (is in whatever space target should be in) */
                        mat4_to_eul(vec, tempmat);
-                       vec[0] *= (float)(180.0/M_PI);
-                       vec[1] *= (float)(180.0/M_PI);
-                       vec[2] *= (float)(180.0/M_PI);
+                       mul_v3_fl(vec, (float)(180.0/M_PI)); /* rad -> deg */
                        axis= data->type;
                }
                else if (data->type < 20) {
@@ -2139,7 +2177,7 @@ static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraint
                }
                else {
                        /* extract location */
-                       VECCOPY(vec, tempmat[3]);
+                       copy_v3_v3(vec, tempmat[3]);
                        axis= data->type - 20;
                }
                
@@ -2169,10 +2207,10 @@ static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraint
                        tchan->rotmode= pchan->rotmode;
                        
                        /* evaluate action using workob (it will only set the PoseChannel in question) */
-                       what_does_obaction(cob->scene, cob->ob, &workob, pose, data->act, pchan->name, t);
+                       what_does_obaction(cob->ob, &workob, pose, data->act, pchan->name, t);
                        
                        /* convert animation to matrices for use here */
-                       chan_calc_mat(tchan);
+                       pchan_calc_mat(tchan);
                        copy_m4_m4(ct->matrix, tchan->chan_mat);
                        
                        /* Clean up */
@@ -2183,7 +2221,7 @@ static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraint
                        
                        /* evaluate using workob */
                        // FIXME: we don't have any consistent standards on limiting effects on object...
-                       what_does_obaction(cob->scene, cob->ob, &workob, NULL, data->act, NULL, t);
+                       what_does_obaction(cob->ob, &workob, NULL, data->act, NULL, t);
                        object_to_mat4(&workob, ct->matrix);
                }
                else {
@@ -2193,7 +2231,7 @@ static void actcon_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraint
        }
 }
 
-static void actcon_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+static void actcon_evaluate (bConstraint *UNUSED(con), bConstraintOb *cob, ListBase *targets)
 {
        bConstraintTarget *ct= targets->first;
        
@@ -2295,10 +2333,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        normalize_v3(totmat[1]);
                                        
                                        /* the x axis is fixed */
-                                       totmat[0][0] = cob->matrix[0][0];
-                                       totmat[0][1] = cob->matrix[0][1];
-                                       totmat[0][2] = cob->matrix[0][2];
-                                       normalize_v3(totmat[0]);
+                                       normalize_v3_v3(totmat[0], cob->matrix[0]);
                                        
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
@@ -2312,10 +2347,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        normalize_v3(totmat[2]);
                                        
                                        /* the x axis is fixed */
-                                       totmat[0][0] = cob->matrix[0][0];
-                                       totmat[0][1] = cob->matrix[0][1];
-                                       totmat[0][2] = cob->matrix[0][2];
-                                       normalize_v3(totmat[0]);
+                                       normalize_v3_v3(totmat[0], cob->matrix[0]);
                                        
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2330,10 +2362,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        negate_v3(totmat[1]);
                                        
                                        /* the x axis is fixed */
-                                       totmat[0][0] = cob->matrix[0][0];
-                                       totmat[0][1] = cob->matrix[0][1];
-                                       totmat[0][2] = cob->matrix[0][2];
-                                       normalize_v3(totmat[0]);
+                                       normalize_v3_v3(totmat[0], cob->matrix[0]);
                                        
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
@@ -2348,10 +2377,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        negate_v3(totmat[2]);
                                                
                                        /* the x axis is fixed */
-                                       totmat[0][0] = cob->matrix[0][0];
-                                       totmat[0][1] = cob->matrix[0][1];
-                                       totmat[0][2] = cob->matrix[0][2];
-                                       normalize_v3(totmat[0]);
+                                       normalize_v3_v3(totmat[0], cob->matrix[0]);
                                                
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2359,9 +2385,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        break;
                                default:
                                {
-                                       totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-                                       totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-                                       totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+                                       unit_m3(totmat);
                                }
                                        break;
                        }
@@ -2378,11 +2402,8 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        normalize_v3(totmat[0]);
                                        
                                        /* the y axis is fixed */
-                                       totmat[1][0] = cob->matrix[1][0];
-                                       totmat[1][1] = cob->matrix[1][1];
-                                       totmat[1][2] = cob->matrix[1][2];
-                                       normalize_v3(totmat[1]);
-                                       
+                                       normalize_v3_v3(totmat[1], cob->matrix[1]);
+
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
                                }
@@ -2395,10 +2416,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        normalize_v3(totmat[2]);
                                        
                                        /* the y axis is fixed */
-                                       totmat[1][0] = cob->matrix[1][0];
-                                       totmat[1][1] = cob->matrix[1][1];
-                                       totmat[1][2] = cob->matrix[1][2];
-                                       normalize_v3(totmat[1]);
+                                       normalize_v3_v3(totmat[1], cob->matrix[1]);
                                        
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2413,10 +2431,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        negate_v3(totmat[0]);
                                        
                                        /* the y axis is fixed */
-                                       totmat[1][0] = cob->matrix[1][0];
-                                       totmat[1][1] = cob->matrix[1][1];
-                                       totmat[1][2] = cob->matrix[1][2];
-                                       normalize_v3(totmat[1]);
+                                       normalize_v3_v3(totmat[1], cob->matrix[1]);
                                        
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[2], totmat[0], totmat[1]);
@@ -2431,10 +2446,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        negate_v3(totmat[2]);
                                        
                                        /* the y axis is fixed */
-                                       totmat[1][0] = cob->matrix[1][0];
-                                       totmat[1][1] = cob->matrix[1][1];
-                                       totmat[1][2] = cob->matrix[1][2];
-                                       normalize_v3(totmat[1]);
+                                       normalize_v3_v3(totmat[1], cob->matrix[1]);
                                        
                                        /* the z axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2442,9 +2454,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        break;
                                default:
                                {
-                                       totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-                                       totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-                                       totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+                                       unit_m3(totmat);
                                }
                                        break;
                        }
@@ -2461,10 +2471,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        normalize_v3(totmat[0]);
                                        
                                        /* the z axis is fixed */
-                                       totmat[2][0] = cob->matrix[2][0];
-                                       totmat[2][1] = cob->matrix[2][1];
-                                       totmat[2][2] = cob->matrix[2][2];
-                                       normalize_v3(totmat[2]);
+                                       normalize_v3_v3(totmat[2], cob->matrix[2]);
                                        
                                        /* the x axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2478,10 +2485,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        normalize_v3(totmat[1]);
                                        
                                        /* the z axis is fixed */
-                                       totmat[2][0] = cob->matrix[2][0];
-                                       totmat[2][1] = cob->matrix[2][1];
-                                       totmat[2][2] = cob->matrix[2][2];
-                                       normalize_v3(totmat[2]);
+                                       normalize_v3_v3(totmat[2], cob->matrix[2]);
                                                
                                        /* the x axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2496,10 +2500,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        negate_v3(totmat[0]);
                                        
                                        /* the z axis is fixed */
-                                       totmat[2][0] = cob->matrix[2][0];
-                                       totmat[2][1] = cob->matrix[2][1];
-                                       totmat[2][2] = cob->matrix[2][2];
-                                       normalize_v3(totmat[2]);
+                                       normalize_v3_v3(totmat[2], cob->matrix[2]);
                                        
                                        /* the x axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[1], totmat[2], totmat[0]);
@@ -2514,10 +2515,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        negate_v3(totmat[1]);
                                        
                                        /* the z axis is fixed */
-                                       totmat[2][0] = cob->matrix[2][0];
-                                       totmat[2][1] = cob->matrix[2][1];
-                                       totmat[2][2] = cob->matrix[2][2];
-                                       normalize_v3(totmat[2]);
+                                       normalize_v3_v3(totmat[2], cob->matrix[2]);
                                                
                                        /* the x axis gets mapped onto a third orthogonal vector */
                                        cross_v3_v3v3(totmat[0], totmat[1], totmat[2]);
@@ -2525,9 +2523,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        break;
                                default:
                                {
-                                       totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-                                       totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-                                       totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+                                       unit_m3(totmat);
                                }
                                        break;
                        }
@@ -2535,19 +2531,13 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                        break;
                default:
                {
-                       totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-                       totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-                       totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+                       unit_m3(totmat);
                }
                        break;
                }
                /* Block to keep matrix heading */
-               tmpmat[0][0] = cob->matrix[0][0];tmpmat[0][1] = cob->matrix[0][1];tmpmat[0][2] = cob->matrix[0][2];
-               tmpmat[1][0] = cob->matrix[1][0];tmpmat[1][1] = cob->matrix[1][1];tmpmat[1][2] = cob->matrix[1][2];
-               tmpmat[2][0] = cob->matrix[2][0];tmpmat[2][1] = cob->matrix[2][1];tmpmat[2][2] = cob->matrix[2][2];
-               normalize_v3(tmpmat[0]);
-               normalize_v3(tmpmat[1]);
-               normalize_v3(tmpmat[2]);
+               copy_m3_m4(tmpmat, cob->matrix);
+               normalize_m3(tmpmat);
                invert_m3_m3(invmat, tmpmat);
                mul_m3_m3m3(tmpmat, totmat, invmat);
                totmat[0][0] = tmpmat[0][0];totmat[0][1] = tmpmat[0][1];totmat[0][2] = tmpmat[0][2];
@@ -2560,9 +2550,7 @@ static void locktrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                                totmat[1][0],totmat[1][1],totmat[1][2],
                                                totmat[2][0],totmat[2][1],totmat[2][2]);
                if (mdet==0) {
-                       totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
-                       totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
-                       totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
+                       unit_m3(totmat);
                }
                
                /* apply out transformaton to the object */
@@ -2651,7 +2639,7 @@ static void distlimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                        /* if inside, then move to surface */
                        if (dist <= data->dist) {
                                clamp_surf= 1;
-                               sfac= data->dist / dist;
+                               if (dist != 0.0f) sfac= data->dist / dist;
                        }
                        /* if soft-distance is enabled, start fading once owner is dist+softdist from the target */
                        else if (data->flag & LIMITDIST_USESOFT) {
@@ -2664,23 +2652,23 @@ static void distlimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                        /* if outside, then move to surface */
                        if (dist >= data->dist) {
                                clamp_surf= 1;
-                               sfac= data->dist / dist;
+                               if (dist != 0.0f) sfac= data->dist / dist;
                        }
                        /* if soft-distance is enabled, start fading once owner is dist-soft from the target */
                        else if (data->flag & LIMITDIST_USESOFT) {
                                // FIXME: there's a problem with "jumping" when this kicks in
                                if (dist >= (data->dist - data->soft)) {
-                                       sfac = (float)( data->soft*(1.0 - exp(-(dist - data->dist)/data->soft)) + data->dist );
-                                       sfac /= dist;
+                                       sfac = (float)( data->soft*(1.0f - expf(-(dist - data->dist)/data->soft)) + data->dist );
+                                       if (dist != 0.0f) sfac /= dist;
                                        
                                        clamp_surf= 1;
                                }
                        }
                }
                else {
-                       if (IS_EQ(dist, data->dist)==0) {
+                       if (IS_EQF(dist, data->dist)==0) {
                                clamp_surf= 1;
-                               sfac= data->dist / dist;
+                               if (dist != 0.0f) sfac= data->dist / dist;
                        }
                }
                
@@ -2690,7 +2678,7 @@ static void distlimit_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                        interp_v3_v3v3(dvec, ct->matrix[3], cob->matrix[3], sfac);
                        
                        /* copy new vector onto owner */
-                       VECCOPY(cob->matrix[3], dvec);
+                       copy_v3_v3(cob->matrix[3], dvec);
                }
        }
 }
@@ -2770,19 +2758,13 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                float dist;
                
                /* store scaling before destroying obmat */
-               mat4_to_size( size,cob->matrix);
+               mat4_to_size(size, cob->matrix);
                
                /* store X orientation before destroying obmat */
-               xx[0] = cob->matrix[0][0];
-               xx[1] = cob->matrix[0][1];
-               xx[2] = cob->matrix[0][2];
-               normalize_v3(xx);
+               normalize_v3_v3(xx, cob->matrix[0]);
                
                /* store Z orientation before destroying obmat */
-               zz[0] = cob->matrix[2][0];
-               zz[1] = cob->matrix[2][1];
-               zz[2] = cob->matrix[2][2];
-               normalize_v3(zz);
+               normalize_v3_v3(zz, cob->matrix[2]);
                
                sub_v3_v3v3(vec, cob->matrix[3], ct->matrix[3]);
                vec[0] /= size[0];
@@ -2837,9 +2819,7 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                normalize_v3(vec);
                
                /* new Y aligns  object target connection*/
-               totmat[1][0] = -vec[0];
-               totmat[1][1] = -vec[1];
-               totmat[1][2] = -vec[2];
+               negate_v3_v3(totmat[1], vec);
                switch (data->plane) {
                case PLANE_X:
                        /* build new Z vector */
@@ -2848,16 +2828,11 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                        normalize_v3(orth);
                        
                        /* new Z*/
-                       totmat[2][0] = orth[0];
-                       totmat[2][1] = orth[1];
-                       totmat[2][2] = orth[2];
+                       copy_v3_v3(totmat[2], orth);
                        
                        /* we decided to keep X plane*/
                        cross_v3_v3v3(xx, orth, vec);
-                       normalize_v3(xx);
-                       totmat[0][0] = xx[0];
-                       totmat[0][1] = xx[1];
-                       totmat[0][2] = xx[2];
+                       normalize_v3_v3(totmat[0], xx);
                        break;
                case PLANE_Z:
                        /* build new X vector */
@@ -2866,16 +2841,11 @@ static void stretchto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                        normalize_v3(orth);
                        
                        /* new X */
-                       totmat[0][0] = -orth[0];
-                       totmat[0][1] = -orth[1];
-                       totmat[0][2] = -orth[2];
+                       negate_v3_v3(totmat[0], orth);
                        
                        /* we decided to keep Z */
                        cross_v3_v3v3(zz, orth, vec);
-                       normalize_v3(zz);
-                       totmat[2][0] = zz[0];
-                       totmat[2][1] = zz[1];
-                       totmat[2][2] = zz[2];
+                       normalize_v3_v3(totmat[2], zz);
                        break;
                } /* switch (data->plane) */
                
@@ -3007,10 +2977,10 @@ static void minmax_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *tar
                        obmat[3][index] = tarmat[3][index] + data->offset;
                        if (data->flag & MINMAX_STICKY) {
                                if (data->flag & MINMAX_STUCK) {
-                                       VECCOPY(obmat[3], data->cache);
+                                       copy_v3_v3(obmat[3], data->cache);
                                } 
                                else {
-                                       VECCOPY(data->cache, obmat[3]);
+                                       copy_v3_v3(data->cache, obmat[3]);
                                        data->flag |= MINMAX_STUCK;
                                }
                        }
@@ -3020,7 +2990,7 @@ static void minmax_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *tar
                                copy_m4_m4(cob->matrix, tmat);
                        } 
                        else {                  
-                               VECCOPY(cob->matrix[3], obmat[3]);
+                               copy_v3_v3(cob->matrix[3], obmat[3]);
                        }
                } 
                else {
@@ -3061,6 +3031,7 @@ static void rbj_id_looper (bConstraint *con, ConstraintIDFunc func, void *userda
        
        /* target only */
        func(con, (ID**)&data->tar, userdata);
+       func(con, (ID**)&data->child, userdata);
 }
 
 static int rbj_get_tars (bConstraint *con, ListBase *list)
@@ -3092,7 +3063,7 @@ static void rbj_flush_tars (bConstraint *con, ListBase *list, short nocopy)
 static bConstraintTypeInfo CTI_RIGIDBODYJOINT = {
        CONSTRAINT_TYPE_RIGIDBODYJOINT, /* type */
        sizeof(bRigidBodyJointConstraint), /* size */
-       "RigidBody Joint", /* name */
+       "Rigid Body Joint", /* name */
        "bRigidBodyJointConstraint", /* struct name */
        NULL, /* free data */
        NULL, /* relink data */
@@ -3141,7 +3112,7 @@ static void clampto_flush_tars (bConstraint *con, ListBase *list, short nocopy)
        }
 }
 
-static void clampto_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void clampto_get_tarmat (bConstraint *UNUSED(con), bConstraintOb *cob, bConstraintTarget *ct, float UNUSED(ctime))
 {
        if (VALID_CONS_TARGET(ct)) {
                Curve *cu= ct->tar->data;
@@ -3170,12 +3141,12 @@ static void clampto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
        /* only evaluate if there is a target and it is a curve */
        if (VALID_CONS_TARGET(ct) && (ct->tar->type == OB_CURVE)) {
                Curve *cu= data->tar->data;
-               float obmat[4][4], targetMatrix[4][4], ownLoc[3];
+               float obmat[4][4], ownLoc[3];
                float curveMin[3], curveMax[3];
+               float targetMatrix[4][4]= MAT4_UNITY;
                
                copy_m4_m4(obmat, cob->matrix);
-               unit_m4(targetMatrix);
-               VECCOPY(ownLoc, obmat[3]);
+               copy_v3_v3(ownLoc, obmat[3]);
                
                INIT_MINMAX(curveMin, curveMax)
                minmax_object(ct->tar, curveMin, curveMax);
@@ -3264,14 +3235,14 @@ static void clampto_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
                        /* 3. position on curve */
                        if (where_on_path(ct->tar, curvetime, vec, dir, NULL, NULL, NULL) ) {
                                unit_m4(totmat);
-                               VECCOPY(totmat[3], vec);
+                               copy_v3_v3(totmat[3], vec);
                                
                                mul_serie_m4(targetMatrix, ct->tar->obmat, totmat, NULL, NULL, NULL, NULL, NULL, NULL);
                        }
                }
                
                /* obtain final object position */
-               VECCOPY(cob->matrix[3], targetMatrix[3]);
+               copy_v3_v3(cob->matrix[3], targetMatrix[3]);
        }
 }
 
@@ -3353,9 +3324,8 @@ static void transform_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                mat4_to_size( dvec,ct->matrix);
                                break;
                        case 1: /* rotation (convert to degrees first) */
-                               mat4_to_eulO( dvec, cob->rotOrder,ct->matrix);
-                               for (i=0; i<3; i++)
-                                       dvec[i] = (float)(dvec[i] / M_PI * 180);
+                               mat4_to_eulO(dvec, cob->rotOrder, ct->matrix);
+                               mul_v3_fl(dvec, (float)(180.0/M_PI)); /* rad -> deg */
                                break;
                        default: /* location */
                                copy_v3_v3(dvec, ct->matrix[3]);
@@ -3363,9 +3333,9 @@ static void transform_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                }
                
                /* extract components of owner's matrix */
-               VECCOPY(loc, cob->matrix[3]);
-               mat4_to_eulO( eul, cob->rotOrder,cob->matrix);
-               mat4_to_size( size,cob->matrix);        
+               copy_v3_v3(loc, cob->matrix[3]);
+               mat4_to_eulO(eul, cob->rotOrder, cob->matrix);
+               mat4_to_size(size, cob->matrix);        
                
                /* determine where in range current transforms lie */
                if (data->expo) {
@@ -3405,7 +3375,7 @@ static void transform_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                                        eul[i]= tmin + (sval[(int)data->map[i]] * (tmax - tmin)); 
                                        
                                        /* now convert final value back to radians */
-                                       eul[i] = (float)(eul[i] / 180 * M_PI);
+                                       eul[i] = DEG2RADF(eul[i]);
                                }
                                break;
                        default: /* location */
@@ -3475,7 +3445,7 @@ static void shrinkwrap_flush_tars (bConstraint *con, ListBase *list, short nocop
 }
 
 
-static void shrinkwrap_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void shrinkwrap_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float UNUSED(ctime))
 {
        bShrinkwrapConstraint *scon = (bShrinkwrapConstraint *) con->data;
        
@@ -3485,107 +3455,108 @@ static void shrinkwrap_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstr
                float co[3] = {0.0f, 0.0f, 0.0f};
                float no[3] = {0.0f, 0.0f, 0.0f};
                float dist;
-
+               
                SpaceTransform transform;
-               DerivedMesh *target = object_get_derived_final(cob->scene, ct->tar, CD_MASK_BAREMESH);
+               DerivedMesh *target = object_get_derived_final(ct->tar);
                BVHTreeRayHit hit;
                BVHTreeNearest nearest;
-
-               BVHTreeFromMesh treeData;
-               memset( &treeData, 0, sizeof(treeData) );
-
+               
+               BVHTreeFromMesh treeData= {NULL};
+               
                nearest.index = -1;
                nearest.dist = FLT_MAX;
-
+               
                hit.index = -1;
                hit.dist = 100000.0f;  //TODO should use FLT_MAX.. but normal projection doenst yet supports it
-
+               
                unit_m4(ct->matrix);
-
+               
                if(target != NULL)
                {
                        space_transform_from_matrixs(&transform, cob->matrix, ct->tar->obmat);
-
+                       
                        switch(scon->shrinkType)
                        {
                                case MOD_SHRINKWRAP_NEAREST_SURFACE:
                                case MOD_SHRINKWRAP_NEAREST_VERTEX:
-
+                                       
                                        if(scon->shrinkType == MOD_SHRINKWRAP_NEAREST_VERTEX)
                                                bvhtree_from_mesh_verts(&treeData, target, 0.0, 2, 6);
                                        else
                                                bvhtree_from_mesh_faces(&treeData, target, 0.0, 2, 6);
-
+                                       
                                        if(treeData.tree == NULL)
                                        {
                                                fail = TRUE;
                                                break;
                                        }
-
+                                       
                                        space_transform_apply(&transform, co);
-
+                                       
                                        BLI_bvhtree_find_nearest(treeData.tree, co, &nearest, treeData.nearest_callback, &treeData);
                                        
                                        dist = len_v3v3(co, nearest.co);
-                                       interp_v3_v3v3(co, co, nearest.co, (dist - scon->dist)/dist);   /* linear interpolation */
+                                       if(dist != 0.0f) {
+                                               interp_v3_v3v3(co, co, nearest.co, (dist - scon->dist)/dist);   /* linear interpolation */
+                                       }
                                        space_transform_invert(&transform, co);
                                break;
-
+                               
                                case MOD_SHRINKWRAP_PROJECT:
                                        if(scon->projAxis & MOD_SHRINKWRAP_PROJECT_OVER_X_AXIS) no[0] = 1.0f;
                                        if(scon->projAxis & MOD_SHRINKWRAP_PROJECT_OVER_Y_AXIS) no[1] = 1.0f;
                                        if(scon->projAxis & MOD_SHRINKWRAP_PROJECT_OVER_Z_AXIS) no[2] = 1.0f;
-
+                                       
                                        if(INPR(no,no) < FLT_EPSILON)
                                        {
                                                fail = TRUE;
                                                break;
                                        }
-
+                                       
                                        normalize_v3(no);
-
-
+                                       
+                                       
                                        bvhtree_from_mesh_faces(&treeData, target, scon->dist, 4, 6);
                                        if(treeData.tree == NULL)
                                        {
                                                fail = TRUE;
                                                break;
                                        }
-
+                                       
                                        if(normal_projection_project_vertex(0, co, no, &transform, treeData.tree, &hit, treeData.raycast_callback, &treeData) == FALSE)
                                        {
                                                fail = TRUE;
                                                break;
                                        }
-                                       VECCOPY(co, hit.co);
+                                       copy_v3_v3(co, hit.co);
                                break;
                        }
-
+                       
                        free_bvhtree_from_mesh(&treeData);
-
+                       
                        target->release(target);
-
+                       
                        if(fail == TRUE)
                        {
                                /* Don't move the point */
                                co[0] = co[1] = co[2] = 0.0f;
                        }
-
+                       
                        /* co is in local object coordinates, change it to global and update target position */
                        mul_m4_v3(cob->matrix, co);
-                       VECCOPY(ct->matrix[3], co);
+                       copy_v3_v3(ct->matrix[3], co);
                }
        }
 }
 
-static void shrinkwrap_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+static void shrinkwrap_evaluate (bConstraint *UNUSED(con), bConstraintOb *cob, ListBase *targets)
 {
        bConstraintTarget *ct= targets->first;
        
        /* only evaluate if there is a target */
        if (VALID_CONS_TARGET(ct))
        {
-               VECCOPY(cob->matrix[3], ct->matrix[3]);
+               copy_v3_v3(cob->matrix[3], ct->matrix[3]);
        }
 }
 
@@ -3669,23 +3640,23 @@ static void damptrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                 *      - the normalisation step at the end should take care of any unwanted scaling
                 *        left over in the 3x3 matrix we used
                 */
-               VECCOPY(obvec, track_dir_vecs[data->trackflag]);
+               copy_v3_v3(obvec, track_dir_vecs[data->trackflag]);
                mul_mat3_m4_v3(cob->matrix, obvec);
                
                if (normalize_v3(obvec) == 0.0f) {
                        /* exceptional case - just use the track vector as appropriate */
-                       VECCOPY(obvec, track_dir_vecs[data->trackflag]);
+                       copy_v3_v3(obvec, track_dir_vecs[data->trackflag]);
                }
                
                /* find the (unit) direction vector going from the owner to the target */
-               VECCOPY(obloc, cob->matrix[3]);
+               copy_v3_v3(obloc, cob->matrix[3]);
                sub_v3_v3v3(tarvec, ct->matrix[3], obloc);
                
                if (normalize_v3(tarvec) == 0.0f) {
                        /* the target is sitting on the owner, so just make them use the same direction vectors */
                        // FIXME: or would it be better to use the pure direction vector?
-                       VECCOPY(tarvec, obvec);
-                       //VECCOPY(tarvec, track_dir_vecs[data->trackflag]);
+                       copy_v3_v3(tarvec, obvec);
+                       //copy_v3_v3(tarvec, track_dir_vecs[data->trackflag]);
                }
                
                /* determine the axis-angle rotation, which represents the smallest possible rotation
@@ -3704,7 +3675,7 @@ static void damptrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                /* 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( rmat,raxis, rangle);
+               axis_angle_to_mat3(rmat, raxis, rangle);
                
                /* rotate the owner in the way defined by this rotation matrix, then reapply the location since
                 * we may have destroyed that in the process of multiplying the matrix
@@ -3713,7 +3684,7 @@ static void damptrack_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *
                mul_m4_m3m4(tmat, rmat, cob->matrix); // m1, m3, m2
                
                copy_m4_m4(cob->matrix, tmat);
-               VECCOPY(cob->matrix[3], obloc);
+               copy_v3_v3(cob->matrix[3], obloc);
        }
 }
 
@@ -3794,7 +3765,7 @@ static void splineik_flush_tars (bConstraint *con, ListBase *list, short nocopy)
        }
 }
 
-static void splineik_get_tarmat (bConstraint *con, bConstraintOb *cob, bConstraintTarget *ct, float ctime)
+static void splineik_get_tarmat (bConstraint *UNUSED(con), bConstraintOb *cob, bConstraintTarget *ct, float UNUSED(ctime))
 {
        if (VALID_CONS_TARGET(ct)) {
                Curve *cu= ct->tar->data;
@@ -3831,6 +3802,131 @@ static bConstraintTypeInfo CTI_SPLINEIK = {
        NULL /* evaluate - solved as separate loop */
 };
 
+/* ----------- Pivot ------------- */
+
+static void pivotcon_id_looper (bConstraint *con, ConstraintIDFunc func, void *userdata)
+{
+       bPivotConstraint *data= con->data;
+       
+       /* target only */
+       func(con, (ID**)&data->tar, userdata);
+}
+
+static int pivotcon_get_tars (bConstraint *con, ListBase *list)
+{
+       if (con && list) {
+               bPivotConstraint *data= con->data;
+               bConstraintTarget *ct;
+               
+               /* standard target-getting macro for single-target constraints */
+               SINGLETARGET_GET_TARS(con, data->tar, data->subtarget, ct, list)
+               
+               return 1;
+       }
+       
+       return 0;
+}
+
+static void pivotcon_flush_tars (bConstraint *con, ListBase *list, short nocopy)
+{
+       if (con && list) {
+               bPivotConstraint *data= con->data;
+               bConstraintTarget *ct= list->first;
+               
+               /* the following macro is used for all standard single-target constraints */
+               SINGLETARGET_FLUSH_TARS(con, data->tar, data->subtarget, ct, list, nocopy)
+       }
+}
+
+static void pivotcon_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *targets)
+{
+       bPivotConstraint *data= con->data;
+       bConstraintTarget *ct= targets->first;
+       
+       float pivot[3], vec[3];
+       float rotMat[3][3];
+
+       /* pivot correction */
+       float axis[3], angle;
+       
+       /* firstly, check if pivoting should take place based on the current rotation */
+       if (data->rotAxis != PIVOTCON_AXIS_NONE) {
+               float rot[3];
+               
+               /* extract euler-rotation of target */
+               mat4_to_eulO(rot, cob->rotOrder, cob->matrix);
+               
+               /* check which range might be violated */
+               if (data->rotAxis < PIVOTCON_AXIS_X) {
+                       /* negative rotations (data->rotAxis = 0 -> 2) */
+                       if (rot[data->rotAxis] > 0.0f)
+                               return;
+               }
+               else {
+                       /* positive rotations (data->rotAxis = 3 -> 5 */
+                       if (rot[data->rotAxis - PIVOTCON_AXIS_X] < 0.0f)
+                               return;
+               }
+       }
+       
+       /* find the pivot-point to use  */
+       if (VALID_CONS_TARGET(ct)) {
+               /* apply offset to target location */
+               add_v3_v3v3(pivot, ct->matrix[3], data->offset);
+       }
+       else {
+               /* no targets to worry about... */
+               if ((data->flag & PIVOTCON_FLAG_OFFSET_ABS) == 0) {
+                       /* offset is relative to owner */
+                       add_v3_v3v3(pivot, cob->matrix[3], data->offset);
+               }
+               else {
+                       /* directly use the 'offset' specified as an absolute position instead */
+                       copy_v3_v3(pivot, data->offset);
+               }
+       }
+       
+       /* get rotation matrix representing the rotation of the owner */
+       // TODO: perhaps we might want to include scaling based on the pivot too?
+       copy_m3_m4(rotMat, cob->matrix);
+       normalize_m3(rotMat);
+
+
+       /* correct the pivot by the rotation axis otherwise the pivot translates when it shouldnt */
+       mat3_to_axis_angle(axis, &angle, rotMat);
+       if(angle) {
+               float dvec[3];
+               sub_v3_v3v3(vec, pivot, cob->matrix[3]);
+               project_v3_v3v3(dvec, vec, axis);
+               sub_v3_v3(pivot, dvec);
+       }
+
+       /* perform the pivoting... */
+               /* 1. take the vector from owner to the pivot */
+       sub_v3_v3v3(vec, cob->matrix[3], pivot);
+               /* 2. rotate this vector by the rotation of the object... */
+       mul_m3_v3(rotMat, vec);
+               /* 3. make the rotation in terms of the pivot now */
+       add_v3_v3v3(cob->matrix[3], pivot, vec);
+}
+
+
+static bConstraintTypeInfo CTI_PIVOT = {
+       CONSTRAINT_TYPE_PIVOT, /* type */
+       sizeof(bPivotConstraint), /* size */
+       "Pivot", /* name */
+       "bPivotConstraint", /* struct name */
+       NULL, /* free data */
+       NULL, /* relink data */
+       pivotcon_id_looper, /* id looper */
+       NULL, /* copy data */
+       NULL, /* new data */ // XXX: might be needed to get 'normal' pivot behaviour...
+       pivotcon_get_tars, /* get constraint targets */
+       pivotcon_flush_tars, /* flush constraint targets */
+       default_get_tarmat, /* get target matrix */
+       pivotcon_evaluate /* evaluate */
+};
+
 /* ************************* Constraints Type-Info *************************** */
 /* All of the constraints api functions use bConstraintTypeInfo structs to carry out
  * and operations that involve constraint specific code.
@@ -3841,7 +3937,7 @@ static bConstraintTypeInfo *constraintsTypeInfo[NUM_CONSTRAINT_TYPES];
 static short CTI_INIT= 1; /* when non-zero, the list needs to be updated */
 
 /* This function only gets called when CTI_INIT is non-zero */
-static void constraints_init_typeinfo () {
+static void constraints_init_typeinfo (void) {
        constraintsTypeInfo[0]=  NULL;                                  /* 'Null' Constraint */
        constraintsTypeInfo[1]=  &CTI_CHILDOF;                  /* ChildOf Constraint */
        constraintsTypeInfo[2]=  &CTI_TRACKTO;                  /* TrackTo Constraint */
@@ -3867,6 +3963,7 @@ static void constraints_init_typeinfo () {
        constraintsTypeInfo[22]= &CTI_SPLINEIK;                 /* Spline IK Constraint */
        constraintsTypeInfo[23]= &CTI_TRANSLIKE;                /* Copy Transforms Constraint */
        constraintsTypeInfo[24]= &CTI_SAMEVOL;                  /* Maintain Volume Constraint */
+       constraintsTypeInfo[25]= &CTI_PIVOT;                    /* Pivot Constraint */
 }
 
 /* This function should be used for getting the appropriate type-info when only
@@ -3913,7 +4010,9 @@ bConstraintTypeInfo *constraint_get_typeinfo (bConstraint *con)
  
 /* ---------- Data Management ------- */
 
-/* Free data of a specific constraint if it has any info */
+/* Free data of a specific constraint if it has any info.
+ * be sure to run BIK_clear_data() when freeing an IK constraint,
+ * unless DAG_scene_sort is called. */
 void free_constraint_data (bConstraint *con)
 {
        if (con->data) {
@@ -3954,17 +4053,6 @@ int remove_constraint (ListBase *list, bConstraint *con)
                return 0;
 }
 
-/* Remove the nth constraint from the given constraint stack */
-int remove_constraint_index (ListBase *list, int index)
-{
-       bConstraint *con= BLI_findlink(list, index);
-       
-       if (con)
-               return remove_constraint(list, con);
-       else 
-               return 0;
-}
-
 /* Remove all the constraints of the specified type from the given constraint stack */
 void remove_constraints_type (ListBase *list, short type, short last_only)
 {
@@ -4054,6 +4142,21 @@ static bConstraint *add_new_constraint (Object *ob, bPoseChannel *pchan, const c
                constraints_set_active(list, con);
        }
        
+       /* set type+owner specific immutable settings */
+       // TODO: does action constraint need anything here - i.e. spaceonce?
+       switch (type) {
+               case CONSTRAINT_TYPE_CHILDOF:
+               {
+                       /* if this constraint is being added to a posechannel, make sure
+                        * the constraint gets evaluated in pose-space */
+                       if (pchan) {
+                               con->ownspace = CONSTRAINT_SPACE_POSE;
+                               con->flag |= CONSTRAINT_SPACEONCE;
+                       }
+               }
+                       break;
+       }
+       
        return con;
 }
 
@@ -4124,7 +4227,7 @@ void id_loop_constraints (ListBase *conlist, ConstraintIDFunc func, void *userda
 /* ......... */
 
 /* helper for copy_constraints(), to be used for making sure that ID's are valid */
-static void con_extern_cb(bConstraint *con, ID **idpoin, void *userdata)
+static void con_extern_cb(bConstraint *UNUSED(con), ID **idpoin, void *UNUSED(userData))
 {
        if (*idpoin && (*idpoin)->lib)
                id_lib_extern(*idpoin);
@@ -4149,9 +4252,9 @@ void copy_constraints (ListBase *dst, const ListBase *src, int do_extern)
                        /* perform custom copying operations if needed */
                        if (cti->copy_data)
                                cti->copy_data(con, srccon);
-
+                       
                        /* for proxies we dont want to make extern */
-                       if(do_extern) {
+                       if (do_extern) {
                                /* go over used ID-links for this constraint to ensure that they are valid for proxies */
                                if (cti->id_looper)
                                        cti->id_looper(con, con_extern_cb, NULL);
@@ -4315,6 +4418,34 @@ void get_constraint_target_matrix (struct Scene *scene, bConstraint *con, int n,
                unit_m4(mat);
        }
 }
+
+/* Get the list of targets required for solving a constraint */
+void get_constraint_targets_for_solving (bConstraint *con, bConstraintOb *cob, ListBase *targets, float ctime)
+{
+       bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
+       
+       if (cti && cti->get_constraint_targets) {
+               bConstraintTarget *ct;
+               
+               /* get targets 
+                *      - constraints should use ct->matrix, not directly accessing values
+                *      - ct->matrix members have not yet been calculated here! 
+                */
+               cti->get_constraint_targets(con, targets);
+               
+               /* set matrices 
+                *      - calculate if possible, otherwise just initialise as identity matrix 
+                */
+               if (cti->get_target_matrix) {
+                       for (ct= targets->first; ct; ct= ct->next) 
+                               cti->get_target_matrix(con, cob, ct, ctime);
+               }
+               else {
+                       for (ct= targets->first; ct; ct= ct->next)
+                               unit_m4(ct->matrix);
+               }
+       }
+}
  
 /* ---------- Evaluation ----------- */
 
@@ -4327,8 +4458,7 @@ void get_constraint_target_matrix (struct Scene *scene, bConstraint *con, int n,
 void solve_constraints (ListBase *conlist, bConstraintOb *cob, float ctime)
 {
        bConstraint *con;
-       float solution[4][4], delta[4][4];
-       float oldmat[4][4], imat[4][4];
+       float oldmat[4][4];
        float enf;
 
        /* check that there is a valid constraint object to evaluate */
@@ -4353,34 +4483,16 @@ void solve_constraints (ListBase *conlist, bConstraintOb *cob, float ctime)
                 */
                enf = con->enforce;
                
+               /* make copy of worldspace matrix pre-constraint for use with blending later */
+               copy_m4_m4(oldmat, cob->matrix);
+               
                /* move owner matrix into right space */
                constraint_mat_convertspace(cob->ob, cob->pchan, cob->matrix, CONSTRAINT_SPACE_WORLD, con->ownspace);
-               copy_m4_m4(oldmat, cob->matrix);
                
                /* prepare targets for constraint solving */
-               if (cti->get_constraint_targets) {
-                       bConstraintTarget *ct;
-                       
-                       /* get targets 
-                        *      - constraints should use ct->matrix, not directly accessing values
-                        *      - ct->matrix members have not yet been calculated here! 
-                        */
-                       cti->get_constraint_targets(con, &targets);
-                       
-                       /* set matrices 
-                        *      - calculate if possible, otherwise just initialise as identity matrix 
-                        */
-                       if (cti->get_target_matrix) {
-                               for (ct= targets.first; ct; ct= ct->next) 
-                                       cti->get_target_matrix(con, cob, ct, ctime);
-                       }
-                       else {
-                               for (ct= targets.first; ct; ct= ct->next)
-                                       unit_m4(ct->matrix);
-                       }
-               }
+               get_constraint_targets_for_solving(con, cob, &targets, ctime);
                
-               /* Solve the constraint */
+               /* Solve the constraint and put result in cob->matrix */
                cti->evaluate_constraint(con, cob, &targets);
                
                /* clear targets after use 
@@ -4391,26 +4503,20 @@ void solve_constraints (ListBase *conlist, bConstraintOb *cob, float ctime)
                        cti->flush_constraint_targets(con, &targets, 1);
                }
                
-               /* Interpolate the enforcement, to blend result of constraint into final owner transform */
-               /* 1. Remove effects of original matrix from constraint solution ==> delta */
-               invert_m4_m4(imat, oldmat);
-               copy_m4_m4(solution, cob->matrix);
-               mul_m4_m4m4(delta, solution, imat);
-               
-               /* 2. If constraint influence is not full strength, then interpolate
-                *      identity_matrix --> delta_matrix to get the effect the constraint actually exerts
-                */
-               if (enf < 1.0) {
-                       float identity[4][4];
-                       unit_m4(identity);
-                       blend_m4_m4m4(delta, identity, delta, enf);
-               }
-               
-               /* 3. Now multiply the delta by the matrix in use before the evaluation */
-               mul_m4_m4m4(cob->matrix, delta, oldmat);
-               
                /* move owner back into worldspace for next constraint/other business */
                if ((con->flag & CONSTRAINT_SPACEONCE) == 0) 
                        constraint_mat_convertspace(cob->ob, cob->pchan, cob->matrix, con->ownspace, CONSTRAINT_SPACE_WORLD);
+                       
+               /* Interpolate the enforcement, to blend result of constraint into final owner transform 
+                *      - all this happens in worldspace to prevent any weirdness creeping in ([#26014] and [#25725]),
+                *        since some constraints may not convert the solution back to the input space before blending
+                *        but all are guaranteed to end up in good "worldspace" result
+                */
+               /* Note: all kind of stuff here before (caused trouble), much easier to just interpolate, or did I miss something? -jahka */
+               if (enf < 1.0f) {
+                       float solution[4][4];
+                       copy_m4_m4(solution, cob->matrix);
+                       blend_m4_m4m4(cob->matrix, oldmat, solution, enf);
+               }
        }
 }