Rodrigues

- More support for 3-dof encoding

change.txt

@@ -1,5 +1,12 @@
 YEAR-MONTH-DAY
 
+---------------------------------------------
+Date    : 2024-XXX-YY
+Version : 0.27.2
+
+- ConvertRotation3D
+  * Supports rodrigues encoded as a vector
+
 ---------------------------------------------
 Date    : 2024-May-14
 Version : 0.27.1

main/src/georegression/geometry/ConvertRotation3D_F64.java

@@ -52,7 +52,51 @@ public static DMatrixRMaj rodriguesToMatrix( Rodrigues_F64 rodrigues, @Nullable
 	}
 
 	/**
-	 * Converts axis angle ({@link Rodrigues_F64}) into a rotation matrix with out needing to declare a storage
+	 * Converts axis angle ({@link Rodrigues_F64}) into a rotation matrix without needing to declare a storage
+	 * variable.
+	 *
+	 * @see Rodrigues_F64#setParamVector(double, double, double)
+	 *
+	 * @param x x-component of rotation vector
+	 * @param y y-component of rotation vector
+	 * @param z z-component of rotation vector
+	 * @param R (Optional) storage for 3x3 rotation matrix. If null one will be declared internally.
+	 * @return Rotation matrix.
+	 */
+	public static DMatrixRMaj rodriguesToMatrix( double x, double y, double z, @Nullable DMatrixRMaj R ) {
+		// Convert the rotation axis with magnitude vector into an angle and normalized vector
+
+		// scale the inputs so that their inputs are close to 1
+		double ax = Math.abs(x);
+		double ay = Math.abs(y);
+		double az = Math.abs(z);
+
+		double max = Math.max(ax, ay);
+		max = Math.max(max, az);
+
+		double axisX, axisY, axisZ, theta;
+
+		// Catch pathological case where there is no rotation
+		if (max == 0) {
+			// when there is no rotation the selected axis of rotation is arbitrary
+			axisX = 1;
+			axisY = axisZ = theta = 0;
+		} else {
+			x /= max;
+			y /= max;
+			z /= max;
+			theta = Math.sqrt(x*x + y*y + z*z);
+			axisX = x/theta;
+			axisY = y/theta;
+			axisZ = z/theta;
+			theta *= max;
+		}
+
+		return rodriguesToMatrix(axisX, axisY, axisZ, theta, R);
+	}
+
+	/**
+	 * Converts axis angle ({@link Rodrigues_F64}) into a rotation matrix without needing to declare a storage
 	 * variable.
 	 *
 	 * @param axisX x-component of normalized rotation vector

main/src/georegression/struct/so/Rodrigues_F64.java

@@ -71,6 +71,10 @@ public Rodrigues_F64( double theta, double x, double y, double z ) {
 		unitAxisRotation.normalize();
 	}
 
+	public Rodrigues_F64( double x, double y, double z ) {
+		setParamVector(x, y, z);
+	}
+
 	public Rodrigues_F64 setTo( Rodrigues_F64 src ) {
 		this.theta = src.theta;
 		this.unitAxisRotation.setTo(src.unitAxisRotation);
@@ -92,8 +96,9 @@ public void setUnitAxisRotation( Vector3D_F64 unitAxisRotation ) {
 	 * @param x x-component of 3 vector
 	 * @param y y-component of 3 vector
 	 * @param z z-component of 3 vector
+	 * @return Reference to this so that commands can be chained.
 	 */
-	public void setParamVector( double x, double y, double z ) {
+	public Rodrigues_F64 setParamVector( double x, double y, double z ) {
 		double ax = Math.abs(x);
 		double ay = Math.abs(y);
 		double az = Math.abs(z);
@@ -114,6 +119,7 @@ public void setParamVector( double x, double y, double z ) {
 			unitAxisRotation.z = z/theta;
 			theta *= max;
 		}
+		return this;
 	}
 
 	@Override

main/test/georegression/geometry/TestConvertRotation3D_F64.java

@@ -46,7 +46,7 @@ public class TestConvertRotation3D_F64 {
 	Random rand = new Random( 234234 );
 
 	@Test
-	void rodriguesToMatrix() {
+	void rodrigues4ToMatrix() {
 		DMatrixRMaj rotZ = ConvertRotation3D_F64.rotZ( 0.5, null );
 
 		Rodrigues_F64 r = new Rodrigues_F64( 0.5, 0, 0, 1 );
@@ -56,6 +56,16 @@ void rodriguesToMatrix() {
 		assertTrue( MatrixFeatures_DDRM.isIdentical( rotZ, rod, GrlConstants.TEST_F64) );
 	}
 
+	@Test
+	void rodrigues3ToMatrix() {
+		double x = 0.5, y = -1.1, z = 1.4;
+
+		DMatrixRMaj expected = ConvertRotation3D_F64.rodriguesToMatrix( new Rodrigues_F64(x,y,z), null );
+		DMatrixRMaj found = ConvertRotation3D_F64.rodriguesToMatrix(x,y,z, null);
+
+		assertTrue( MatrixFeatures_DDRM.isIdentical( expected, found, GrlConstants.TEST_F64) );
+	}
+
 	@Test
 	void rodriguesToEuler() throws InvocationTargetException, IllegalAccessException {
 		somethingToEulerTest("rodriguesToEuler",rand);