new quatToAffine() with any direction

src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java

@@ -36,6 +36,18 @@ public final class QuatUtils { // static class
      * t3 = (r0s3 - r1s2 + r2s1 + r3s0)
 	 */
 
+	public static String toString(
+			double [] quat_i,
+			boolean degrees) {
+		double [] quat=quat_i.clone();
+		double scale =normalizeInPlace(quat);
+		String fmt_rad = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.9f, dir=%12.9f, scale = %12.10f";
+		String fmt_deg = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.7f\u00B0, dir=%12.7f\u00B0, scale=%12.10f";
+		double s = degrees ? (180/Math.PI):1;
+		String fmt=degrees ? fmt_deg : fmt_rad;
+		return String.format(fmt, quat[0], quat[1],quat[2],quat[3], s*2*Math.acos(quat[0]), s*Math.atan2(quat[2],quat[1]), scale);
+	}
+	
 	/**
 	 * Multiply to quaternions
 	 * @param r first quaternion
@@ -44,8 +56,7 @@ public final class QuatUtils { // static class
 	 */
 	public static double [] multiply(
 			double [] r,
-			double [] s
-			) {
+			double [] s	) {
 		double [] t = {
 				 r[0]*s[0] - r[1]*s[1] - r[2]*s[2] - r[3]*s[3],
 			     r[0]*s[1] + r[1]*s[0] - r[2]*s[3] + r[3]*s[2],
@@ -54,6 +65,59 @@ public final class QuatUtils { // static class
 		return t;
 	}
 	
+	public static double [] multiplyScaled(
+			double [] r,
+			double [] s	) {
+		return scale(multiply(normalize(r),normalize(s)),norm(r)*norm(s));
+	}
+	
+	public static double [] invertScaled(double [] quat) {
+		return scale(invert(normalize(quat)),1.0/norm(quat));
+	}
+	
+/*
+	public static double [] divide( // pure rotation
+			double [] r,
+			double [] s	) {
+		return multiply(invert(r), s);
+	}	
+
+	public static double [] divideScaled(
+			double [] r,
+			double [] s	) {
+		return multiplyScaled(invertScaled(r), s);
+	}	
+
+	public static double [] divideScaled1(
+			double [] r,
+			double [] s	) {
+		return multiply(invertScaled(r), s);
+	}	
+
+ */
+	
+	public static double [] divide( // pure rotation
+			double [] r,
+			double [] s	) {
+		return multiply(r, invert(s));
+	}	
+
+	public static double [] divideScaled(
+			double [] r,
+			double [] s	) {
+		return multiplyScaled(r, invertScaled(s));
+	}	
+
+	public static double [] divideScaled1(
+			double [] r,
+			double [] s	) {
+		return multiply(r, invertScaled(s));
+	}	
+	
+	
+	
+	
+	
 	public static double norm(
 			double [] quat) {
 		return Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
@@ -199,6 +263,86 @@ public final class QuatUtils { // static class
 		return affine;
 	}
 	
+	public static double [][] quatToAffine(
+			double [] quat,
+			boolean   invert,
+			boolean   y_down_ccw){
+		/*
+The product of two quaternions:
+t = rs
+(t0, t1, t2, t3) = (r0, r1, r2, r3) * (s0, s1, s2, s3)
+
+t0 = (r0s0 - r1s1 - r2s2 - r3s3)
+t1 = (r0s1 + r1s0 - r2s3 + r3s2)
+t2 = (r0s2 + r1s3 + r2s0 - r3s1)
+t3 = (r0s3 - r1s2 + r2s1 + r3s0)
+p = (0, x, y, z)
+p'= inv(q) * p *q for active rotation (we'll need an inverse to get source pixel coordinates x,y
+
+point (z==0), starting with map coordinates
+
+p = (0, x, y, 0)
+
+pq0 = ( - r1s1 - r2s2 ) = (-x * q[1] - y * q[2])
+pq1 = ( + r1s0 - r2s3 ) = ( x * q[0] - y * q[3])
+pq2 = ( + r1s3 + r2s0 ) = ( x * q[3] + y * q[0])
+pq3 = ( - r1s2 + r2s1 ) = (-x * q[2] + y * q[1])
+
+~q=[q0,-q1,-q2,-q3]
+~q*p*q0 = (q0pq0 +  q1pq1 +  q2pq2 + -q3pq3)  = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
+~q*p*q1 = (q0pq1 + -q1pq0 - -q2pq3 + -q3pq2)  = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
+~q*p*q2 = (q0pq2 + -q1pq3 + -q2pq0 - -q3pq1)  = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
+~q*p*q3 = (q0pq3 - -q1pq2 + -q2pq1 + -q3pq0)  = (q0pq3 + q1pq2 - q2pq1 - q3pq0)
+
+
+~q*p*q0 = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
+~q*p*q1 = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
+~q*p*q2 = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
+~q*p*q3 = (q0pq3 + q1pq2 - q2pq1 - q3pq0)
+
+~q*p*q0 = q[0]*(-x * q[1] - y * q[2]) + q[1]*( x * q[0] - y * q[3]) + q[2]*( x * q[3] + y * q[0]) + q[3]*(-x * q[2] + y * q[1])
+~q*p*q1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
+~q*p*q2 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])
+~q*p*q3 = q[0]*(-x * q[2] + y * q[1]) + q[1]*( x * q[3] + y * q[0]) - q[2]*( x * q[0] - y * q[3]) - q[3]*(-x * q[1] - y * q[2])
+
+
+x1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
+y1 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])
+
+x1 = x* (q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) + y* (-q[0]*q[3] + q[1]*q[2] + q[2]*q[1] - q[3]*q[0])
+y1 = x* (q[0]*q[3] + q[1]*q[2] + q[2]*q[1] + q[3]*q[0]) + y* ( q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3])
+
+x1 = x* (q00 + q11 - q22 - q33) + y* (-q03 + q12 + q12 - q03)
+y1 = x* (q03 + q12 + q12 + q03) + y* ( q00 - q11 + q22 - q33)
+
+x1 = x* (q00 + q11 - q22 - q33) + y*2*(q12 - q03)
+y1 = x*2*(q03 + q12)            + y* ( q00 - q11 + q22 - q33)
+
+where:
+q00 = q[0]*q[0]
+q11 = q[1]*q[1]
+q22 = q[1]*q[1]
+q33 = q[1]*q[1]
+q03 = q[0]*q[3]
+q12 = q[1]*q[2]
+		 */
+		double scale = normQuat(quat);
+		double scale_y = scale * (y_down_ccw?-1:1); // invert y from quaternions to affines
+		double q00 = quat[0]*quat[0];
+		double q11 = quat[1]*quat[1];
+		double q22 = quat[1]*quat[1];
+		double q33 = quat[1]*quat[1];
+		double q03 = quat[0]*quat[3];
+		double q12 = quat[1]*quat[2];
+		
+		double [][] affine = {
+				{scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)},
+				{scale_y*2*(q03 + q12),         scale_y*(q00 - q11 + q22 - q33)}};
+		if (invert) {
+			affine = matInverse2x2(affine);
+		}
+		return affine;
+	}
 	/**
 	 * Restore quaternion (rotation+scale) from affine traqnsform (only [2][2] is used, ok to have [2][3] input
 	 * As there are 2 solutions, both are provided in the output. As the
@@ -231,7 +375,8 @@ public final class QuatUtils { // static class
 		double stilt = Math.sin(tiltAngle/2);
 		double [] q_plus =  {ctilt,  stilt*cmbeta,  stilt*smbeta, 0};
 		double [] q_minus = {ctilt, -stilt*cmbeta, -stilt*smbeta, 0};
-		double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
+///		double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
+		double [][] quats_pm = {scale(multiply(qrot,q_plus), scale),scale(multiply(qrot, q_minus), scale)};
 		return quats_pm;
 	}
 	
@@ -253,7 +398,7 @@ public final class QuatUtils { // static class
 		return m;
 	}
 	
-	public static double [] matMul (
+	public static double [] matMult (
 			double [][] m1,
 			double []   v){
 		if (m1[0].length != v.length) {
@@ -268,7 +413,14 @@ public final class QuatUtils { // static class
 		}
 		return v2;
 	}
-
+	public static double [][] matInverse2x2(
+			double [][] a) {
+//			throw new IllegalArgumentException("Only [2][2] arrays");
+		double idet = 1.0/(a[0][0] * a[1][1] - a[0][1] * a[1][0]);
+		return new double [][] {
+			{ idet*a[1][1], -idet*a[0][1]},
+			{-idet*a[1][0],  idet*a[0][0]}};
+	}
 	
 	
 	

src/main/java/com/elphel/imagej/orthomosaic/SingularValueDecomposition.java

@@ -43,6 +43,15 @@ public class SingularValueDecomposition {
 	public double getMinScale() {return Math.min(w1,w2);}
 	public double getMaxScale() {return Math.max(w1,w2);}
 	
+	public String toString(boolean degrees) {
+//System.out.println("svd_affine_pair=    ["+svd_affine_pair.scale+  ","+svd_affine_pair.getTiltAngle()+ ","+svd_affine_pair.gamma+ ","+svd_affine_pair.rot+
+// "] tilt="+(svd_affine_pair.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_affine_pair.gamma*180/Math.PI)+"\u00B0");
+		String fmt_rad = "scale=%10.8f,tilt= %10.7f, gamma=%10.7f, beta=%10.7f, rot=%10.7f, w1=%10.8f, w2=%10.8f, ratio=%10.8f";
+		String fmt_deg = "scale=%10.8f,tilt= %10.5f\u00B0, gamma=%10.5f\u00B0, beta=%10.5f\u00B0, rot=%10.5f\u00B0, w1=%10.8f, w2=%10.8f, ratio=%10.8f";
+		double s = degrees ? (180/Math.PI):1;
+		String fmt=degrees ? fmt_deg : fmt_rad;
+		return String.format(fmt, scale, s*getTiltAngle(), s*gamma, s*beta, s*rot, w1, w2, ratio);
+	}
 	
 	public double [][] getW(){
 		return new double [][] {{w1,0},{0,w1}};