/* Facial Data Stabilizer
    
Script: FDS.mel
Author: Midori Kitagawa
Version: 1.0 
Last modified: Dec 2, 2008
Description: This script removes head movement from facial motion capture
	marker data to isolate the movements of facial features.

	Head motion, which we want to eliminate, is considered as rotation and
	translation of the object in the world space while the deformation of 
	facial features, that we want to keep, are considered as translation 
	of the object's vertices in the object (local) space.

	Markers in the Playback frame range are stabilized.
	Each selected marker is processed and keyed once per frame.
	Stabilized markers are placed in the group called FdsMarkerGroup.
	Stabilized face faces the positive z-axis.

Usage: 
1.	Select STABLE markers in the right temple area.
2.	Select STABLE markers in the left temple area that have the mirrored 
	positions of the right markers that have been selected.
3.	Select STABLE markers in the upper forehead area that are on or very 	
	close to the plane that divides the face into the right and left halves 
	AND/OR pairs of markers where each pair of markers have mirrored 
	positions in the right & left halves of the face.
4.	Select STABLE markers in the lower forehead area that are pairs of 
	markers where the markers of each pair have mirrored positions in the 
	right & left halves of the face.
5.	Select all the markers to be processed.
6.	Stabilize.

Note: STABLE markers are the ones do not move much by facial expressions 
or speech
	
*/

// define and opens a window
proc fds() {
	global int $fdsMaxNoOfMarkers = 200; // Max number of markers
	global int $fdsMaxNoOfStMarkers = 50; // Max number of stable markers per area
	global int $fdsNoOfRightMarkers, $fdsNoOfLeftMarkers;
	global int $fdsNoOfUpperMarkers, $fdsNoOfLowerMarkers, $fdsNoOfAllMarkers;
	global vector $fdsRightMarkers[50], $fdsLeftMarkers[50];
	global vector $fdsUpperMarkers[50], $fdsLowerMarkers[50], $fdsAllMarkers[200];	
	global string $fdsRightMarkersStr[50], $fdsLeftMarkersStr[50];
	global string $fdsUpperMarkersStr[50], $fdsLowerMarkersStr[50], $fdsAllMarkersStr[200];

	if (`window -query -exists FacialDataStabilizerUI`) {
		deleteUI FacialDataStabilizerUI;
		windowPref -remove FacialDataStabilizerUI;
	}
	
	window 
		-title "Facial Data Stabilizer"
		-w 200
		-resizeToFitChildren true
		-sizeable true
		FacialDataStabilizerUI;
		
			columnLayout -adjustableColumn true -rowSpacing 3 
					-columnOffset "both" 5 -bgc 0.6 0.6 0.9;
			
				button
					-h 24
					-l "Select stable right temple markers" 
					-c "fdsSelectRightMarkers"
					-ann "Press to excute.";

				button
					-h 24
					-l "Select stable left temple markers" 
					-c "fdsSelectLeftMarkers"
					-ann "Press to excute.";

				button
					-h 24
					-l "Select stable upper forehead markers" 
					-c "fdsSelectUpperMarkers"
					-ann "Press to excute.";

				button 
					-h 24
					-l "Select stable lower forehead markers"
					-c "fdsSelectLowerMarkers"
					-ann "Press to excute.";				

				button 
					-h 24
					-l "Select markers to be stabilized"
					-c "fdsSelectAllMarkers"
					-ann "Press to excute.";				

				button
					-h 24
					-l "Stabilize"
					-c "fdsCheckSelections"
					-ann "Make selections first. Press to excute.";			

	showWindow FacialDataStabilizerUI;
}

// define a button function
proc fdsSelectRightMarkers(){
	global int $fdsNoOfRightMarkers = 0;
	global int $fdsMaxNoOfStMarkers;
	global vector $fdsRightMarkers[];	
	global string $fdsRightMarkersStr[];

	$fdsRightMarkersStr = `ls -sl`;
	int $i = countMarkers($fdsRightMarkersStr);
	
	if ($i > $fdsMaxNoOfStMarkers) {
		error("Select only stable markers in the right temple area.\n");
	} else {
		print("Number of right temple markers = " + $i + "\n");
	}

	// if the number of the markers read in does not match the previous count, 
	// ptint error 
	if (readMarkerPos($fdsRightMarkersStr, $fdsRightMarkers)!= $i) {
		error("Some of the selection are not markers\n");
	} else {
		$fdsNoOfRightMarkers = $i;
	}
}

// define a button function
proc fdsSelectLeftMarkers(){
	global int $fdsNoOfLeftMarkers = 0;
	global int $fdsMaxNoOfStMarkers;
	global vector $fdsLeftMarkers[];	
	global string $fdsLeftMarkersStr[];

	$fdsLeftMarkersStr = `ls -sl`;
	int $i = countMarkers($fdsLeftMarkersStr);
	
	if ($i > $fdsMaxNoOfStMarkers) {
		error("Select only stable markers in the left temple area.\n");
	} else {
		print("Number of left temple markers = " + $i + "\n");
	}

	// if the number of the markers read in does not match the previous count, 
	// ptint error 
	if (readMarkerPos($fdsLeftMarkersStr, $fdsLeftMarkers)!= $i) {
		error("Some of the selection are not markers\n");
	} else {
		$fdsNoOfLeftMarkers = $i;
	}
}

// define a button function
proc fdsSelectUpperMarkers(){
	global int $fdsNoOfUpperMarkers = 0;
	global int $fdsMaxNoOfStMarkers;
	global vector $fdsUpperMarkers[];	
	global string $fdsUpperMarkersStr[];

	$fdsUpperMarkersStr = `ls -sl`;
	int $i = countMarkers($fdsUpperMarkersStr);
	
	if ($i > $fdsMaxNoOfStMarkers) {
		error("Select only stable markers in the upper forehead area.\n");
	} else {
		print("Number of forehead markers = " + $i + "\n");
	}

	// if the number of the markers read in does not match the previous count, 
	// ptint error 
	if (readMarkerPos($fdsUpperMarkersStr, $fdsUpperMarkers)!= $i) {
		error("Some of the selection are not markers\n");
	} else {
		$fdsNoOfUpperMarkers = $i;
	}
}

// define a button function
proc fdsSelectLowerMarkers(){
	global int $fdsNoOfLowerMarkers = 0;
	global int $fdsMaxNoOfStMarkers;
	global vector $fdsLowerMarkers[];	
	global string $fdsLowerMarkersStr[];

	$fdsLowerMarkersStr = `ls -sl`;
	int $i = countMarkers($fdsLowerMarkersStr);
	
	if ($i > $fdsMaxNoOfStMarkers) {
		error("Select only stable markers in the lower forehead area.\n");
	} else {
		print("Number of lower forehead markers = " + $i + "\n");
	}

	// if the number of the markers read in does not match the previous count, 
	// ptint error 
	if (readMarkerPos($fdsLowerMarkersStr, $fdsLowerMarkers)!= $i) {
		error("Some of the selection are not markers\n");
	} else {
		$fdsNoOfLowerMarkers = $i;
	}
}

// define a button function
proc fdsSelectAllMarkers(){
	global int $fdsNoOfAllMarkers = 0;
	global int $fdsMaxNoOfMarkers;
	global vector $fdsAllMarkers[];	
	global string $fdsAllMarkersStr[];

	int $i;

	$fdsAllMarkersStr = `ls -sl`;
	int $i = countMarkers($fdsAllMarkersStr);
	
	if ($i > $fdsMaxNoOfMarkers) {
		error("In fds() increse the maximum number of markers\n");
	} else {
		print("Number of all markers = " + $i + "\n");
	}
	
	// if the number of the markers read in does not match the previous count, 
	// ptint error 
	if (readMarkerPos($fdsAllMarkersStr, $fdsAllMarkers)!= $i) {
		error("Some of the selection are not markers\n");
	} else {
		$fdsNoOfAllMarkers = $i;
	}
}

// return the number of elements in a string array
proc int countMarkers(string $selection[]) {
	int $i;

	// make sure there is at least one marker selected
	if (`size $selection` == 0) error "Nothing is selected.";
	
	// make sure all the objects are transforms
	for ($marker in $selection) {
		// print($marker + " (" + `objectType $marker` + ")\n");
     
		// if the shape node type is not a transform, print error
		if (`nodeType $marker` == "transform") {
			$i++;
		} else {
			error($marker + "[" + `nodeType $marker` + 
				"] is not a marker. Please select markers only.");		
		}
	}
	return $i;
}

// find a marker position and store a vector array
proc int readMarkerPos(string $selection[], vector $markerPos[]) {
	int $i = 0;

	// Read each marker position
	for ($marker in $selection) {
		$markerPos[$i] = `pointPosition -world $marker`;

		// debug
		// $pos = $markerPos[$i];
		// print ("marker[" + $i + "] = " + $pos + "\n");

		$i++;
	}
	return $i; // return the num of marker positions that were read
}

// if all the necessary selections have been made, call stabilizer(); 
// otherwise, print error
proc fdsCheckSelections() {
	global int $fdsNoOfRightMarkers, $fdsNoOfLeftMarkers;
	global int $fdsNoOfUpperMarkers, $fdsNoOfLowerMarkers, $fdsNoOfAllMarkers;

	if ($fdsNoOfRightMarkers == 0) {
		error("Select stable marker(s) in the right temple area.\n");
	} else if ($fdsNoOfLeftMarkers == 0) {
		error("Select stable marker(s) in the left temple area.\n");
	} else if ($fdsNoOfUpperMarkers == 0) {
		error("Select stable marker(s) in the forehead area.\n");
	} else if ($fdsNoOfLowerMarkers == 0) {
		error("Select any marker(s) in the chin or lower lip area.\n");
	} else if ($fdsNoOfAllMarkers == 0) {
		error("Select markers to be stabilized.\n");
	} else {
		stabilizer();
	}
}

/* main procedure
	A local coordinate system (local space) w.r.t. the world space is formed 
	as follows:
		x-axis is a vector from the averaged position of the stable markers
			in the right side of the face to that of the left side.
		y-axis is a vector from the averaged position of the stable markers 
			in the lower forehead area to that of the upper forehead area.
		z-axis is the cross product of the x- and y-axes.
		x-axis is recomputed as the cross product of the y- and z-axes to 
			ensure that x-, y-, and z-axes are perpendicular to each other.
		origin is the averaged position of the stable markers.

	We can find the coordinates of a point (marker) in the world space with 
	PA + local origin = P', where 
		P = [x, y, z] is the coordinates of a point in the local space
		A is a 3x3 matrix such that 
			the 1st row of A is the x-axis of the local space
			the 2nd row of A is the y-axis of the local space
			the 3rd row of A is the z-axis of the local space
		P' = [x', y', z'] is the coordinates of a point in the world space

	We know the marker positions in the world space but not in the local 
	space, i.e., P' is known and P is unknown.

	PA = P' - local origin -- local origin has moved to the right side
	PAB = (P' - local origin)B -- both sides are multiplied by B, inverted A
	PI = (P' - local origin)B -- AB is an identity matrix I
	P = (P' - local origin)B

	Thus, by finding a matrix B that is an inverse matrix of A, we can find the 
	marker positions in the local space.

	Since A's rows (the axes of the local space) are perpendicular to each other, 
	the determinant of A is never 0. A is never singular and A's inverse always
	exists.  So, the algebraic method for inverting a 3x3 matrix works fine here.  

	Let M be a metric A at the current frame when the script is executed.  PM gives
	us P", the coordinates of a marker in the world space.  Note that A is different
	for each frame but M stays the same.

	P" = PM = (P' - local origin)BM 
	
	Let N = BM then

	p" = (P' - local origin)N
*/
proc stabilizer() {
	global int $fdsMaxNoOfMarkers, $fdsNoOfAllMarkers;
	global vector $fdsAllMarkers[];
	global string $fdsAllMarkersStr[];

	global vector $xAxis, $yAxis, $zAxis; // vectors that define local space
	global vector $locOrig;		        // local origin

	int $i, $j, $start, $end, $current;
	float $newX, $newY, $newZ;
	string $groupName = "FdsMarkerGroup"; // group name for stabilized markers
	string $names[];				  // names of stabilized markers
	vector $origM, $pos;
	matrix $A[3][3], $B[3][3], $M[3][3], $N[3][3];

	// create a group for the stabilized markers
	if (`objExists $groupName` == 0) {
		group -world -empty -n $groupName;
	}

	// create locators for stabilized markers
	for ($i = 0; $i < $fdsNoOfAllMarkers; $i++) {
		// add "fds_" to marker name
		$names[$i] = ("fds_" + $fdsAllMarkersStr[$i]);

		if (`objExists $names[$i]` == 0) {
			spaceLocator -n $names[$i];
			parent $names[$i] $groupName;
		}
		$pos = $fdsAllMarkers[$i];
		setAttr ($names[$i] + ".translateX") ($pos.x);
		setAttr ($names[$i] + ".translateY") ($pos.y);
		setAttr ($names[$i] + ".translateZ") ($pos.z);
		setAttr ($names[$i] + ".scaleX") 0.25;
		setAttr ($names[$i] + ".scaleY") 0.25;
		setAttr ($names[$i] + ".scaleZ") 0.25;
	}

	// find start, end and current frames
	$start = `playbackOptions -query -minTime`;
	$end = `playbackOptions -query -maxTime`;
	$current = `currentTime -query`;
	print("Start = " + $start + ": End = " + $end + ": Current = " + $current + "\n");

	// find local axes and origin of M (A at the current frame)
	fdsFindLocalAxes();
	$origM = $locOrig;	
//	$origM.x = $locOrig.x;
//	$origM.y = $locOrig.y;
//	$origM.z = $locOrig.z;

	// form matrix M
	$M[0][0] = $xAxis.x; $M[0][1] = $xAxis.y; $M[0][2] = $xAxis.z;
	$M[1][0] = $yAxis.x; $M[1][1] = $yAxis.y; $M[1][2] = $yAxis.z;
	$M[2][0] = $zAxis.x; $M[2][1] = $zAxis.y; $M[2][2] = $zAxis.z;

	// process each frame
	for ($i = $start; $i < $end; $i++) {
		currentTime -edit ($i);
		
		// find local axes and origin
		fdsFindLocalAxes();

		// form matrix A
		$A[0][0] = $xAxis.x; $A[0][1] = $xAxis.y; $A[0][2] = $xAxis.z;
		$A[1][0] = $yAxis.x; $A[1][1] = $yAxis.y; $A[1][2] = $yAxis.z;
		$A[2][0] = $zAxis.x; $A[2][1] = $zAxis.y; $A[2][2] = $zAxis.z;
		// print($A);

		// determinant of A should be always 1.0 
		if (findDeterm3x3($A)< 0.0001) {
			error("Please re-select the markers\n");
		}

		// find matrix B, inverse matrix of A
		$B[0][0] = $A[1][1]*$A[2][2] - $A[1][2]*$A[2][1];
		$B[0][1] = $A[0][2]*$A[2][1] - $A[0][1]*$A[2][2];
		$B[0][2] = $A[0][1]*$A[1][2] - $A[0][2]*$A[1][1];
		$B[1][0] = $A[1][2]*$A[2][0] - $A[1][0]*$A[2][2];
		$B[1][1] = $A[0][0]*$A[2][2] - $A[0][2]*$A[2][0];
		$B[1][2] = $A[0][2]*$A[1][0] - $A[0][0]*$A[1][2];
		$B[2][0] = $A[1][0]*$A[2][1] - $A[1][1]*$A[2][0];
		$B[2][1] = $A[0][1]*$A[2][0] - $A[0][0]*$A[2][1];
		$B[2][2] = $A[0][0]*$A[1][1] - $A[0][1]*$A[1][0];
		// print($B);

		// A * B should be an identity matrix
		// matMulti3x3($A, $B, $N);

		// N = A * M
		matMulti3x3($B, $M, $N);
		// print($N);
		
		// find local coordinates
		for ($j = 0; $j < $fdsNoOfAllMarkers; $j++) {
			$pos = $fdsAllMarkers[$j] - $locOrig;
 			$newX = $pos.x*$N[0][0] + $pos.y*$N[1][0] + $pos.z*$N[2][0] + $origM.x;
 			$newY = $pos.x*$N[0][1] + $pos.y*$N[1][1] + $pos.z*$N[2][1] + $origM.y;
 			$newZ = $pos.x*$N[0][2] + $pos.y*$N[1][2] + $pos.z*$N[2][2] + $origM.z;
			setKeyframe -at "translateX" -v $newX $names[$j];
			setKeyframe -at "translateY" -v $newY $names[$j];
			setKeyframe -at "translateZ" -v $newZ $names[$j];
		}
	}
	
	// use spline to interpolate keys
	for ($i = 0; $i < $fdsNoOfAllMarkers; $i++) {
		keyTangent -an objects -itt spline -ott spline -time ($start + ":" + $end) $names[$i];
	}
}

// find local axes and origin
proc fdsFindLocalAxes() {
	global int $fdsNoOfRightMarkers, $fdsNoOfLeftMarkers;
	global int $fdsNoOfUpperMarkers, $fdsNoOfLowerMarkers, $fdsNoOfAllMarkers;
	global string $fdsRightMarkersStr[], $fdsLeftMarkersStr[];
	global string $fdsUpperMarkersStr[], $fdsLowerMarkersStr[], $fdsAllMarkersStr[];
	global vector $locOrig, $xAxis, $yAxis, $zAxis;
	global vector $fdsRightMarkers[], $fdsLeftMarkers[];
	global vector $fdsUpperMarkers[], $fdsLowerMarkers[], $fdsAllMarkers[];	

	int $i;
	vector $right, $left, $upper, $lower, $origin;

	// read marker positions at the current frame
	readMarkerPos($fdsRightMarkersStr, $fdsRightMarkers);
	readMarkerPos($fdsLeftMarkersStr, $fdsLeftMarkers);
	readMarkerPos($fdsUpperMarkersStr, $fdsUpperMarkers);
	readMarkerPos($fdsLowerMarkersStr, $fdsLowerMarkers);
	readMarkerPos($fdsAllMarkersStr, $fdsAllMarkers);

	// average markers in each area
	$right = aveVectors($fdsRightMarkers, $fdsNoOfRightMarkers);
	$left = aveVectors($fdsLeftMarkers, $fdsNoOfLeftMarkers);
	$upper = aveVectors($fdsUpperMarkers, $fdsNoOfUpperMarkers);
	$lower = aveVectors($fdsLowerMarkers, $fdsNoOfLowerMarkers);

	// average all stable markers to determine local origin
	$locOrig = ($right + $left + $upper + $lower) / 4.0;

	// find x-, y, and z-axes
	$xAxis = unit($left - $right);
	$yAxis = unit($upper - $lower);
	$zAxis = unit(cross($xAxis, $yAxis));
	$xAxis = unit(cross($yAxis, $zAxis)); // ensure x, y, and z are perpendicular 

	// debug 
	// print("right = " + $right + "\n");
	// print("left = " + $left + "\n");
 	// print("upper = " + $upper + "\n");
	// print("lower = " + $lower + "\n");
	// print("locOrig = " + $locOrig + "\n");
	// print("xAxis = " + $xAxis + "\n");
	// print("yAxis = " + $yAxis + "\n");
	// print("zAxis = " + $zAxis + "\n");
}

// return the averaged vector
proc vector aveVectors(vector $vec[], int $n) {
	int $i;
	vector $v = <<0, 0, 0>>;

	for ($i = 0; $i < $n; $i++) {
		$v += $vec[$i];
		// print("v = " + $v + "\n");
	}
	return($v /= $n);
}

// return the determinant of a 3 x 3 matrix
proc float findDeterm3x3(matrix $a[][]) {
	return($a[0][0]*$a[1][1]*$a[2][2] + $a[1][0]*$a[2][1]*$a[0][2] 
		+ $a[2][0]*$a[0][1]*$a[1][2] - $a[0][0]*$a[2][1]*$a[1][2] 
		- $a[2][0]*$a[1][1]*$a[0][2] - $a[1][0]*$a[0][1]*$a[2][2]);
}

// multiply 3x3 matrices a and b 
// if matrices A and B are inverse of each other, AB = C is an identity matrix
proc matMulti3x3(matrix $a[][], matrix $b[][], matrix $c[][]) {
	int $i, $j, $k;

	for ($i = 0; $i < 3; $i++) {
		for ($j = 0; $j < 3; $j++) {
			$c[$i][$j] = 0.0;
			for ($k = 0; $k < 3; $k++) {
				$c[$i][$j] = $c[$i][$j] + $a[$i][$k] * $b[$k][$j];
			}
		}
	}
}