// ===========================================================================
// Copyright 2018 Autodesk, Inc. All rights reserved.
//
// Use of this software is subject to the terms of the Autodesk license
// agreement provided at the time of installation or download, or which
// otherwise accompanies this software in either electronic or hard copy form.
// ===========================================================================
//
//
//  Creation Date:  July 17, 1998
//
//
//<doc>
//<name snap2to2>
//
//<synopsis>
//      snap2to2 ( string $objectList[] )
//
//<description>
//      This is a manual version of snap align action available in Maya.
//      Some of the component selection in Maya doesn't preserve ordering.
//      For example, if you select curve edit point 5, followed by curve
//      edit point 3, the list of selected items will be equivalent to that
//      one if you selected the two points in the reverse order.  Polygon
//      vertices and NURBS curve control points are another example of this.
//<p>
//      In the event of needing to use snap align function with one of these
//      point components, you will need to use this script as follows:
//<p>
//      Select 4 points in turn (2 on the object to move and 2 on the target
//      location).  Note the echoed "select -r" statement in the command
//      editor.
//<p>
//      Use snap2to2 with those for objects; for example, if your command
//      window showed:
//<pre>
//  select -r curve1.ep[3;
//  select -r curve1.ep[1];
//  select -r nurbsPlane1.ep[1][1];
//  select -r nurbsPlane1.ep[2][2];
//</pre>
//  you would type:
//
//<pre>
//  snap2to2 { "curve1.ep[3]", "curve1.ep[1]",
//             "nurbsPlane1.ep[1][1]", "nurbsPlane1.ep[2][2]" };
//</pre>
//
//<flags>
//      string   $objectList  list of points on objects to use for snapping
//<returns>
//		none
//
//<examples>
//      snap2to2 { "curve1.ep[3]", "curve1.ep[1]",
//                 "nurbsPlane1.ep[1][1]", "nurbsPlane1.ep[2][2]" };
//
//</doc>

proc xyzRotation( float $theta, float $axis[], float $rotation[] )
//
//	Description:
//		Given an angle for rotation (in radians) and an axis about which to 
//		do the rotation, return the rotation as XYZ values (in $rotation).
//
{
	// set up the xyzw quaternion values
	//
	$theta *= 0.5;
	float $w = cos($theta);
	float $factor = sin($theta);
	float $axisLen2 = dotProduct( $axis, $axis, 0 );
	if ( $axisLen2 != 1.0 && $axisLen2 != 0.0 ) $factor /= sqrt($axisLen2);
	float $x = $factor * $axis[0];
	float $y = $factor * $axis[1];
	float $z = $factor * $axis[2];

	// setup rotation in a matrix
	//
	float $matrix[];
	float $ww = $w*$w;
	float $xx = $x*$x;
	float $yy = $y*$y;
	float $zz = $z*$z;
	float $s = 2.0 / ($ww + $xx + $yy + $zz);
	float $xy = $x*$y;
	float $xz = $x*$z;
	float $yz = $y*$z;
	float $wx = $w*$x;
	float $wy = $w*$y;
	float $wz = $w*$z;
	$matrix[0] = 1.0 - $s * ($yy + $zz);
	$matrix[1] = $s * ($xy + $wz);
	$matrix[2] = $s * ($xz - $wy);
	$matrix[5] = 1.0 - $s * ($xx + $zz);
	$matrix[6] = $s * ($yz + $wx);
	$matrix[9] = $s * ($yz - $wx);
	$matrix[10] = 1.0 - $s * ($xx + $yy);

	// get x,y,z values for rotation
	//
	float $solution1[];
	float $solution2[];
	float $cosB = sqrt($matrix[0]*$matrix[0] + $matrix[1]*$matrix[1]);
	if ( $cosB > 1.0e-10 )
	{
		float $a, $b, $c;
		float $pi = 3.14159265;
 
		$solution1[0] = $a = atan2( $matrix[6], $matrix[10]);
		$solution1[1] = $b = atan2(-$matrix[2], $cosB);
		$solution1[2] = $c = atan2( $matrix[1], $matrix[0]);

		$solution2[0] = $a + (($a < $pi) ? $pi : -$pi);
		$solution2[1] = (($b > -$pi) ? $pi : -$pi) - $b;
		$solution2[2] = $c + (($c < $pi) ? $pi : -$pi);    

		if ( abs($solution2[0]) + abs($solution2[1]) + abs($solution2[2]) <
			abs($solution1[0]) + abs($solution1[1]) + abs($solution1[2]) )
		{
			$rotation = $solution2;
		}
		else 
		{
			$rotation = $solution1;
		}
	}
	else
	{
		$rotation[0] = atan2(-$matrix[9], $matrix[5]);
		$rotation[1] = atan2(-$matrix[2], $cosB);
		$rotation[2] = 0.0;
	}
}

proc string getObjectToMoveName( string $object )
//
//	Description:
//		Returns the transform name of the object with no components in the 
//		string e.g. curve1.u[0.5] will return curve1
//
{
	string $name;

	// get the string before any "." or "->" - this could be a shape name
	//
	string $buffer[];
	int $numTokens = `tokenize $object ".->" $buffer`;
	if ( $numTokens >= 1 ) $name = $buffer[0];

	return $name;
}

global proc snap2to2( string $objectList[] )
{
	// make sure there are 4 objects selected
	//
	int $total = size($objectList);
	if ( $total == 0 )
	{
		error((uiRes("m_snap2to2.kErrorNoPoints")) );
		return;
	}
	else if ( $total < 4 )
	{
		error((uiRes("m_snap2to2.kErrorNotEnoughPoints")) );
		return;
	}
	else if ( $total > 4 )
	{
		warning((uiRes("m_snap2to2.kWarningTooManyPoints")) );
	}

	string $obj1 = $objectList[$total-4];
	string $obj2 = $objectList[$total-3];
	string $obj3 = $objectList[$total-2];
	string $obj4 = $objectList[$total-1];

	// get the name of the object to move (with no component part). The first
	// 2 points should be on the same object?
	//
	string $objectToMove = getObjectToMoveName( $obj1 );
	string $tmpToMove = getObjectToMoveName( $obj2 );
	string $tmp1 = getObjectToMoveName( $obj3 );
	string $tmp2 = getObjectToMoveName( $obj4 );
	if ( $objectToMove == $tmp1 || $objectToMove == $tmp2 )
	{
		error((uiRes("m_snap2to2.kErrorSnapToSameObject")) );
		return;
	}
	else
	{
		string $fullPath1[] = `ls -l $objectToMove`;
		string $fullPath2[] = `ls -l $tmpToMove`;
		if ( strcmp($fullPath1[0], $fullPath2[0]) != 0 )
		{
			error((uiRes("m_snap2to2.kErrorNotSameObject")) );
			return;
		}
	}

	// get the world space position of each selected point object
	//
	float $pos1[] = `pointPosition $obj1`;
	float $pos2[] = `pointPosition $obj2`;
	float $pos3[] = `pointPosition $obj3`;
	float $pos4[] = `pointPosition $obj4`;

	if ( $pos1[0] == $pos2[0] && $pos1[1] == $pos2[1] && $pos1[2] == $pos2[2] )
	{
		error((uiRes("m_snap2to2.kErrorNonUniqueFirstPoints")) );
		return;
	}
	if ( $pos3[0] == $pos4[0] && $pos3[1] == $pos4[1] && $pos3[2] == $pos4[2] )
	{
		error((uiRes("m_snap2to2.kErrorNonUniqueSecondPoints")) );
		return;
	}

	// calculate the translation amount - the first point on each pair is the
	// point to use for translation
	//
	float $distance[];
	$distance[0] = $pos3[0] - $pos1[0];
	$distance[1] = $pos3[1] - $pos1[1];
	$distance[2] = $pos3[2] - $pos1[2];

 	// move the first object by that amount
	//
	evalEcho("move -r " + $distance[0] + " " + $distance[1] + " " + $distance[2] + " " + $objectToMove);

	// define the two vectors for each pair of points
	//
	float $axis1[];
	$axis1[0] = $pos2[0] - $pos1[0];
	$axis1[1] = $pos2[1] - $pos1[1];
	$axis1[2] = $pos2[2] - $pos1[2];
	float $axis2[];
	$axis2[0] = $pos4[0] - $pos3[0];
	$axis2[1] = $pos4[1] - $pos3[1];
	$axis2[2] = $pos4[2] - $pos3[2];

	// get the angle (in radians) between the two vectors and the axis of
	// rotation
	//
	float $dotProduct = dotProduct( $axis1, $axis2, 1 );
	$dotProduct = clamp(-1.0, 1.0, $dotProduct);
	float $angle = acos($dotProduct);
	float $crossProduct[] = crossProduct( $axis1, $axis2, 1, 1 );

 	// rotate the first object about the pivot point (the pivot is defined 
	// by the first point from the second pair of points  - i.e. point 3 from 
	// the inputs above)
	//
	float $rotation[];
	xyzRotation( $angle, $crossProduct, $rotation );
	evalEcho("rotate -r -p " + $pos3[0] + " " + $pos3[1] + " " + $pos3[2] + " " + $rotation[0] + "rad " + $rotation[1] + "rad " + $rotation[2] + "rad " + $objectToMove);
}