Object tracking: object with object solver constraint is now parented to scene's camera

Made Object Solver operator parent object to scene's camera. Behavior is pretty much
familiar to Child Of constraint -- it stores inverted transformation matrix which gives
constant offset in parent's space.
Current files would open incorrect, to make object aligned well again, just press
"Set Inverse" button in Object Solver constraint.
Fixed orientation operators so now they should work in all cases.
Also changed behavior of Set Origin operator which now sets origin to the median
point of all selected tracks/

10 files changed, 219 insertions, 41 deletions

diff --git a/source/blender/blenkernel/intern/constraint.c b/source/blender/blenkernel/intern/constraint.c
index ba1bfc8f303..1699cb05fde 100644
--- a/source/blender/blenkernel/intern/constraint.c
+++ b/source/blender/blenkernel/intern/constraint.c
@@ -825,7 +825,7 @@ static void childof_evaluate (bConstraint *con, bConstraintOb *cob, ListBase *ta
 {
 	bChildOfConstraint *data= con->data;
 	bConstraintTarget *ct= targets->first;
-	
+
 	/* only evaluate if there is a target */
 	if (VALID_CONS_TARGET(ct)) {
 		float parmat[4][4];
@@ -4183,20 +4183,21 @@ static void objectsolver_evaluate (bConstraint *con, bConstraintOb *cob, ListBas
 		object= BKE_tracking_named_object(tracking, data->object);
 
 		if(object) {
-			float mat[4][4], obmat[4][4], imat[4][4], cammat[4][4], camimat[4][4];
+			float mat[4][4], obmat[4][4], imat[4][4], cammat[4][4], camimat[4][4], parmat[4][4];
 
 			where_is_object_mat(scene, scene->camera, cammat);
 
 			BKE_tracking_get_interpolated_camera(tracking, object, scene->r.cfra, mat);
 
 			invert_m4_m4(camimat, cammat);
+			mul_m4_m4m4(parmat, data->invmat, cammat);
 
 			copy_m4_m4(cammat, scene->camera->obmat);
 			copy_m4_m4(obmat, cob->matrix);
 
 			invert_m4_m4(imat, mat);
 
-			mul_serie_m4(cob->matrix, cammat, imat, camimat, obmat, NULL, NULL, NULL, NULL);
+			mul_serie_m4(cob->matrix, cammat, imat, camimat, parmat, obmat, NULL, NULL, NULL);
 		}
 	}
 }
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c
index 37cb49fc17b..19aa86ee941 100644
--- a/source/blender/blenlib/intern/math_matrix.c
+++ b/source/blender/blenlib/intern/math_matrix.c
@@ -195,8 +195,14 @@ void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3])
 	m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 
 }
 
-void mul_m4_m4m3(float (*m1)[4], float (*m3)[4], float (*m2)[3])
+void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3])
 {
+	float m2[3][3], m3[4][4];
+
+	/* copy so it works when m1 is the same pointer as m2 or m3 */
+	copy_m3_m3(m2, m2_);
+	copy_m4_m4(m3, m3_);
+
 	m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
 	m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
 	m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
diff --git a/source/blender/blenloader/intern/readfile.c b/source/blender/blenloader/intern/readfile.c
index c8130ba6f24..dbc8439e50d 100644
--- a/source/blender/blenloader/intern/readfile.c
+++ b/source/blender/blenloader/intern/readfile.c
@@ -12689,6 +12689,7 @@ static void do_versions(FileData *fd, Library *lib, Main *main)
 	/* put compatibility code here until next subversion bump */
 	{
 		MovieClip *clip;
+		Object *ob;
 
 		for (clip= main->movieclip.first; clip; clip= clip->id.next) {
 			MovieTracking *tracking= &clip->tracking;
@@ -12696,6 +12697,23 @@ static void do_versions(FileData *fd, Library *lib, Main *main)
 			if(tracking->objects.first == NULL)
 				BKE_tracking_new_object(tracking, "Camera");
 		}
+
+		for (ob= main->object.first; ob; ob= ob->id.next) {
+			bConstraint *con;
+			for (con= ob->constraints.first; con; con=con->next) {
+				bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
+
+				if(!cti)
+					continue;
+
+				if(cti->type==CONSTRAINT_TYPE_OBJECTSOLVER) {
+					bObjectSolverConstraint *data= (bObjectSolverConstraint *)con->data;
+
+					if(data->invmat[3][3]==0.0f)
+						unit_m4(data->invmat);
+				}
+			}
+		}
 	}
 
 	/* WATCH IT!!!: pointers from libdata have not been converted yet here! */
diff --git a/source/blender/editors/object/object_constraint.c b/source/blender/editors/object/object_constraint.c
index 7b9e809fffc..a4a33b866ff 100644
--- a/source/blender/editors/object/object_constraint.c
+++ b/source/blender/editors/object/object_constraint.c
@@ -697,25 +697,13 @@ void CONSTRAINT_OT_limitdistance_reset (wmOperatorType *ot)
 
 /* ------------- Child-Of Constraint ------------------ */
 
-/* ChildOf Constraint - set inverse callback */
-static int childof_set_inverse_exec (bContext *C, wmOperator *op)
+static void child_get_inverse_matrix (Scene *scene, Object *ob, bConstraint *con, float invmat[4][4])
 {
-	Scene *scene= CTX_data_scene(C);
-	Object *ob = ED_object_active_context(C);
-	bConstraint *con = edit_constraint_property_get(op, ob, CONSTRAINT_TYPE_CHILDOF);
-	bChildOfConstraint *data= (con) ? (bChildOfConstraint *)con->data : NULL;
 	bConstraint *lastcon = NULL;
 	bPoseChannel *pchan= NULL;
 	
-	/* despite 3 layers of checks, we may still not be able to find a constraint */
-	if (data == NULL) {
-		printf("DEBUG: Child-Of Set Inverse - object = '%s'\n", (ob)? ob->id.name+2 : "<None>");
-		BKE_report(op->reports, RPT_ERROR, "Couldn't find constraint data for Child-Of Set Inverse");
-		return OPERATOR_CANCELLED;
-	}
-	
 	/* nullify inverse matrix first */
-	unit_m4(data->invmat);
+	unit_m4(invmat);
 	
 	/* try to find a pose channel - assume that this is the constraint owner */
 	// TODO: get from context instead?
@@ -761,7 +749,7 @@ static int childof_set_inverse_exec (bContext *C, wmOperator *op)
 		 */
 		invert_m4_m4(imat, pchan->pose_mat);
 		mul_m4_m4m4(tmat, imat, pmat);
-		invert_m4_m4(data->invmat, tmat);
+		invert_m4_m4(invmat, tmat);
 		
 		/* 5. restore constraints */
 		pchan->constraints.last = lastcon;
@@ -783,9 +771,27 @@ static int childof_set_inverse_exec (bContext *C, wmOperator *op)
 		
 		/* use what_does_parent to find inverse - just like for normal parenting */
 		what_does_parent(scene, ob, &workob);
-		invert_m4_m4(data->invmat, workob.obmat);
+		invert_m4_m4(invmat, workob.obmat);
+	}
+}
+
+/* ChildOf Constraint - set inverse callback */
+static int childof_set_inverse_exec (bContext *C, wmOperator *op)
+{
+	Scene *scene= CTX_data_scene(C);
+	Object *ob = ED_object_active_context(C);
+	bConstraint *con = edit_constraint_property_get(op, ob, CONSTRAINT_TYPE_CHILDOF);
+	bChildOfConstraint *data= (con) ? (bChildOfConstraint *)con->data : NULL;
+
+	/* despite 3 layers of checks, we may still not be able to find a constraint */
+	if (data == NULL) {
+		printf("DEBUG: Child-Of Set Inverse - object = '%s'\n", (ob)? ob->id.name+2 : "<None>");
+		BKE_report(op->reports, RPT_ERROR, "Couldn't find constraint data for Child-Of Set Inverse");
+		return OPERATOR_CANCELLED;
 	}
 	
+	child_get_inverse_matrix(scene, ob, con, data->invmat);
+
 	WM_event_add_notifier(C, NC_OBJECT|ND_CONSTRAINT, ob);
 	
 	return OPERATOR_FINISHED;
@@ -859,6 +865,96 @@ void CONSTRAINT_OT_childof_clear_inverse (wmOperatorType *ot)
 	edit_constraint_properties(ot);
 }
 
+/* ------------- Object Solver Constraint ------------------ */
+
+static int objectsolver_set_inverse_exec (bContext *C, wmOperator *op)
+{
+	Scene *scene= CTX_data_scene(C);
+	Object *ob = ED_object_active_context(C);
+	bConstraint *con = edit_constraint_property_get(op, ob, CONSTRAINT_TYPE_OBJECTSOLVER);
+	bObjectSolverConstraint *data= (con) ? (bObjectSolverConstraint *)con->data : NULL;
+
+	/* despite 3 layers of checks, we may still not be able to find a constraint */
+	if (data == NULL) {
+		printf("DEBUG: Child-Of Set Inverse - object = '%s'\n", (ob)? ob->id.name+2 : "<None>");
+		BKE_report(op->reports, RPT_ERROR, "Couldn't find constraint data for Child-Of Set Inverse");
+		return OPERATOR_CANCELLED;
+	}
+
+	child_get_inverse_matrix(scene, ob, con, data->invmat);
+
+	WM_event_add_notifier(C, NC_OBJECT|ND_CONSTRAINT, ob);
+
+	return OPERATOR_FINISHED;
+}
+
+static int objectsolver_set_inverse_invoke(bContext *C, wmOperator *op, wmEvent *UNUSED(event))
+{
+	if (edit_constraint_invoke_properties(C, op))
+		return objectsolver_set_inverse_exec(C, op);
+	else
+		return OPERATOR_CANCELLED;
+}
+
+void CONSTRAINT_OT_objectsolver_set_inverse (wmOperatorType *ot)
+{
+	/* identifiers */
+	ot->name= "Set Inverse";
+	ot->idname= "CONSTRAINT_OT_objectsolver_set_inverse";
+	ot->description= "Set inverse correction for ObjectSolver constraint";
+
+	ot->exec= objectsolver_set_inverse_exec;
+	ot->invoke= objectsolver_set_inverse_invoke;
+	ot->poll= edit_constraint_poll;
+
+	/* flags */
+	ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
+	edit_constraint_properties(ot);
+}
+
+static int objectsolver_clear_inverse_exec (bContext *C, wmOperator *op)
+{
+	Object *ob = ED_object_active_context(C);
+	bConstraint *con = edit_constraint_property_get(op, ob, CONSTRAINT_TYPE_OBJECTSOLVER);
+	bObjectSolverConstraint *data= (con) ? (bObjectSolverConstraint *)con->data : NULL;
+
+	if(data==NULL) {
+		BKE_report(op->reports, RPT_ERROR, "Childof constraint not found");
+		return OPERATOR_CANCELLED;
+	}
+
+	/* simply clear the matrix */
+	unit_m4(data->invmat);
+
+	WM_event_add_notifier(C, NC_OBJECT|ND_CONSTRAINT, ob);
+
+	return OPERATOR_FINISHED;
+}
+
+static int objectsolver_clear_inverse_invoke(bContext *C, wmOperator *op, wmEvent *UNUSED(event))
+{
+	if (edit_constraint_invoke_properties(C, op))
+		return objectsolver_clear_inverse_exec(C, op);
+	else
+		return OPERATOR_CANCELLED;
+}
+
+void CONSTRAINT_OT_objectsolver_clear_inverse (wmOperatorType *ot)
+{
+	/* identifiers */
+	ot->name= "Clear Inverse";
+	ot->idname= "CONSTRAINT_OT_objectsolver_clear_inverse";
+	ot->description= "Clear inverse correction for ObjectSolver constraint";
+
+	ot->exec= objectsolver_clear_inverse_exec;
+	ot->invoke= objectsolver_clear_inverse_invoke;
+	ot->poll= edit_constraint_poll;
+
+	/* flags */
+	ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
+	edit_constraint_properties(ot);
+}
+
 /***************************** BUTTONS ****************************/
 
 void ED_object_constraint_set_active(Object *ob, bConstraint *con)
diff --git a/source/blender/editors/object/object_intern.h b/source/blender/editors/object/object_intern.h
index 185d8d43765..d4f68b8816e 100644
--- a/source/blender/editors/object/object_intern.h
+++ b/source/blender/editors/object/object_intern.h
@@ -185,6 +185,8 @@ void CONSTRAINT_OT_stretchto_reset(struct wmOperatorType *ot);
 void CONSTRAINT_OT_limitdistance_reset(struct wmOperatorType *ot);
 void CONSTRAINT_OT_childof_set_inverse(struct wmOperatorType *ot);
 void CONSTRAINT_OT_childof_clear_inverse(struct wmOperatorType *ot);
+void CONSTRAINT_OT_objectsolver_set_inverse(struct wmOperatorType *ot);
+void CONSTRAINT_OT_objectsolver_clear_inverse (struct wmOperatorType *ot);
 
 /* object_vgroup.c */
 void OBJECT_OT_vertex_group_add(struct wmOperatorType *ot);
diff --git a/source/blender/editors/object/object_ops.c b/source/blender/editors/object/object_ops.c
index 3b4a5ed976e..4f598ce4417 100644
--- a/source/blender/editors/object/object_ops.c
+++ b/source/blender/editors/object/object_ops.c
@@ -161,6 +161,8 @@ void ED_operatortypes_object(void)
 	WM_operatortype_append(CONSTRAINT_OT_limitdistance_reset);
 	WM_operatortype_append(CONSTRAINT_OT_childof_set_inverse);
 	WM_operatortype_append(CONSTRAINT_OT_childof_clear_inverse);
+	WM_operatortype_append(CONSTRAINT_OT_objectsolver_set_inverse);
+	WM_operatortype_append(CONSTRAINT_OT_objectsolver_clear_inverse);
 
 	WM_operatortype_append(OBJECT_OT_vertex_group_add);
 	WM_operatortype_append(OBJECT_OT_vertex_group_remove);
diff --git a/source/blender/editors/space_clip/tracking_ops.c b/source/blender/editors/space_clip/tracking_ops.c
index 59631e3a181..81741ffab31 100644
--- a/source/blender/editors/space_clip/tracking_ops.c
+++ b/source/blender/editors/space_clip/tracking_ops.c
@@ -32,6 +32,7 @@
 #include "MEM_guardedalloc.h"
 
 #include "DNA_camera_types.h"
+#include "DNA_constraint_types.h"
 #include "DNA_gpencil_types.h"
 #include "DNA_movieclip_types.h"
 #include "DNA_object_types.h"	/* SELECT */
@@ -45,6 +46,7 @@
 
 #include "BKE_main.h"
 #include "BKE_context.h"
+#include "BKE_constraint.h"
 #include "BKE_movieclip.h"
 #include "BKE_tracking.h"
 #include "BKE_global.h"
@@ -1938,6 +1940,29 @@ static int count_selected_bundles(bContext *C)
 	return tot;
 }
 
+static void object_solver_inverted_matrix(Scene *scene, Object *ob, float invmat[4][4])
+{
+	Object *cam= scene->camera;
+	bConstraint *con;
+
+	where_is_object_mat(scene, cam, invmat);
+
+	for (con= ob->constraints.first; con; con=con->next) {
+		bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
+
+		if(!cti)
+			continue;
+
+		if(cti->type==CONSTRAINT_TYPE_OBJECTSOLVER) {
+			bObjectSolverConstraint *data= (bObjectSolverConstraint *)con->data;
+
+			mul_m4_m4m4(invmat, data->invmat, invmat);
+		}
+	}
+
+	invert_m4(invmat);
+}
+
 static int set_origin_exec(bContext *C, wmOperator *op)
 {
 	SpaceClip *sc= CTX_wm_space_clip(C);
@@ -1948,10 +1973,11 @@ static int set_origin_exec(bContext *C, wmOperator *op)
 	Scene *scene= CTX_data_scene(C);
 	Object *object;
 	ListBase *tracksbase;
-	float mat[4][4], vec[3];
+	float mat[4][4], vec[3], median[3];
+	int selected_count= count_selected_bundles(C);
 
-	if(count_selected_bundles(C)!=1) {
-		BKE_report(op->reports, RPT_ERROR, "Track with bundle should be selected to define origin position");
+	if(selected_count==0) {
+		BKE_report(op->reports, RPT_ERROR, "At least one track with bundle should be selected to define origin position");
 
 		return OPERATOR_CANCELLED;
 	}
@@ -1969,21 +1995,28 @@ static int set_origin_exec(bContext *C, wmOperator *op)
 	tracksbase= BKE_tracking_object_tracks(tracking, tracking_object);
 
 	track= tracksbase->first;
+	zero_v3(median);
 	while(track) {
-		if(TRACK_VIEW_SELECTED(sc, track))
-			break;
+		if(TRACK_VIEW_SELECTED(sc, track) && (track->flag&TRACK_HAS_BUNDLE)) {
+			add_v3_v3(median, track->bundle_pos);
+		}
 
 		track= track->next;
 	}
+	mul_v3_fl(median, 1.0f/selected_count);
 
 	BKE_get_tracking_mat(scene, NULL, mat);
 
-	mul_v3_m4v3(vec, mat, track->bundle_pos);
+	mul_v3_m4v3(vec, mat, median);
 
-	if(tracking_object->flag&TRACKING_OBJECT_CAMERA)
+	if(tracking_object->flag&TRACKING_OBJECT_CAMERA) {
 		sub_v3_v3(object->loc, vec);
-	else
+	}
+	else {
+		object_solver_inverted_matrix(scene, object, mat);
+		mul_v3_m4v3(vec, mat, vec);
 		copy_v3_v3(object->loc, vec);
+	}
 
 	DAG_id_tag_update(&clip->id, 0);
 	DAG_id_tag_update(&object->id, OB_RECALC_OB);
@@ -2007,6 +2040,9 @@ void CLIP_OT_set_origin(wmOperatorType *ot)
 
 	/* flags */
 	ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
+
+	/* properties */
+	RNA_def_boolean(ot->srna, "use_median", 0, "Use Median", "Set origin to median point of selected bundles");
 }
 
 /********************** set floor operator *********************/
@@ -2027,6 +2063,9 @@ static void set_axis(Scene *scene,  Object *ob, MovieTrackingObject *tracking_ob
 	if(!is_camera) {
 		float imat[4][4];
 
+		object_solver_inverted_matrix(scene, ob, imat);
+		mul_v3_m4v3(vec, imat, vec);
+
 		invert_m4_m4(imat, obmat);
 		mul_v3_m4v3(dvec, imat, vec);
 
@@ -2094,23 +2133,22 @@ static void set_axis(Scene *scene,  Object *ob, MovieTrackingObject *tracking_ob
 		mul_m4_m4m4(mat, obmat, mat);
 	}
 	else {
-		float lmat[4][4], ilmat[4][4], m[4][4];
-
-		unit_m4(lmat);
-		copy_v3_v3(lmat[3], obmat[3]);
-		invert_m4_m4(ilmat, lmat);
-
 		if(!flip) {
-			float rmat[3][3], tmat[4][4];
+			float lmat[4][4], ilmat[4][4], rmat[3][3];
 
 			object_rot_to_mat3(ob, rmat);
-			copy_m4_m3(tmat, rmat);
-			invert_m4(tmat);
+			invert_m3(rmat);
+			mul_m4_m4m3(mat, mat, rmat);
 
-			mul_m4_m4m4(mat, mat, tmat);
-		}
+			unit_m4(lmat);
+			copy_v3_v3(lmat[3], obmat[3]);
+			invert_m4_m4(ilmat, lmat);
 
-		mul_serie_m4(mat, lmat, mat, ilmat, obmat, NULL, NULL, NULL, NULL);
+			mul_serie_m4(mat, lmat, mat, ilmat, obmat, NULL, NULL, NULL, NULL);
+		}
+		else {
+			mul_m4_m4m4(mat, mat, obmat);
+		}
 	}
 
 	object_apply_mat4(ob, mat, 0, 0);
diff --git a/source/blender/editors/space_view3d/drawobject.c b/source/blender/editors/space_view3d/drawobject.c
index cf210e67bdf..c6c42c64ca6 100644
--- a/source/blender/editors/space_view3d/drawobject.c
+++ b/source/blender/editors/space_view3d/drawobject.c
@@ -6820,7 +6820,20 @@ void draw_object(Scene *scene, ARegion *ar, View3D *v3d, Base *base, int flag)
 				ListBase targets = {NULL, NULL};
 				bConstraintTarget *ct;
 				
-				if ((curcon->flag & CONSTRAINT_EXPAND) && (cti) && (cti->get_constraint_targets)) {
+				if(cti->type==CONSTRAINT_TYPE_OBJECTSOLVER) {
+					/* special case for object solver constraint because it doesn't fill
+					   constraint targets properly (design limitation -- scene is needed for
+					   it's target but it can't be accessed from get_targets callvack) */
+					if(scene->camera) {
+						setlinestyle(3);
+						glBegin(GL_LINES);
+						glVertex3fv(scene->camera->obmat[3]);
+						glVertex3fv(ob->obmat[3]);
+						glEnd();
+						setlinestyle(0);
+					}
+				}
+				else if ((curcon->flag & CONSTRAINT_EXPAND) && (cti) && (cti->get_constraint_targets)) {
 					cti->get_constraint_targets(curcon, &targets);
 					
 					for (ct= targets.first; ct; ct= ct->next) {
diff --git a/source/blender/editors/transform/transform_conversions.c b/source/blender/editors/transform/transform_conversions.c
index 264ca803135..98ad5363c39 100644
--- a/source/blender/editors/transform/transform_conversions.c
+++ b/source/blender/editors/transform/transform_conversions.c
@@ -4196,6 +4196,7 @@ static short constraints_list_needinv(TransInfo *t, ListBase *list)
 				if (con->type == CONSTRAINT_TYPE_CHILDOF) return 1;
 				if (con->type == CONSTRAINT_TYPE_FOLLOWPATH) return 1;
 				if (con->type == CONSTRAINT_TYPE_CLAMPTO) return 1;
+				if (con->type == CONSTRAINT_TYPE_OBJECTSOLVER) return 1;
 				
 					/* constraints that require this only under special conditions */
 				if (con->type == CONSTRAINT_TYPE_ROTLIKE) {
diff --git a/source/blender/makesdna/DNA_constraint_types.h b/source/blender/makesdna/DNA_constraint_types.h
index 10c4153702d..5403bf26da0 100644
--- a/source/blender/makesdna/DNA_constraint_types.h
+++ b/source/blender/makesdna/DNA_constraint_types.h
@@ -426,6 +426,7 @@ typedef struct bObjectSolverConstraint {
 	struct MovieClip	*clip;
 	int		flag, pad;
 	char		object[24];
+	float		invmat[4][4];	/* parent-inverse matrix to use */
 } bObjectSolverConstraint;
 
 /* ------------------------------------------ */