Java Code Examples for javax.vecmath.Matrix4d#set()

/**
 * Interpolate between two 4d matrixes, (end-start)*i + start where i=[0...1]
 * @param start start matrix
 * @param end end matrix
 * @param alpha double value in the range [0...1]
 * @param result where to store the resulting matrix
 * @return True if the operation succeeds.  False if the inputs are bad or the operation fails. 
 */
public static boolean interpolate(Matrix4d start,Matrix4d end,double alpha,Matrix4d result) {
	if(alpha<0 || alpha>1) return false;
	// spherical interpolation (slerp) between the two matrix orientations
	Quat4d qStart = new Quat4d();
	qStart.set(start);
	Quat4d qEnd = new Quat4d();
	qEnd.set(end);
	Quat4d qInter = new Quat4d();
	qInter.interpolate(qStart, qEnd, alpha);
	// linear interpolation between the two matrix translations
	Vector3d tStart = new Vector3d();
	start.get(tStart);
	Vector3d tEnd = new Vector3d();
	end.get(tEnd);
	Vector3d tInter = new Vector3d();
	tInter.interpolate(tStart, tEnd, alpha);
	// build the result matrix
	result.set(qInter);
	result.setTranslation(tInter);
	// report ok
	return true;
}
 

/**
 * Used by plotXY() and plotXZ()
 * @param x
 * @param y
 * @param z
 * @param u
 * @param v
 * @param w
 * @param out
 * @param robot
 * @throws IOException
 */
private void plot(double x,double y,double z,double u,double v,double w,BufferedWriter out,Sixi2 robot) throws IOException {
	DHKeyframe keyframe0 = robot.sim.getIKSolver().createDHKeyframe();
	Matrix4d m0 = new Matrix4d();
	
	keyframe0.fkValues[0]=x;
	keyframe0.fkValues[1]=y;
	keyframe0.fkValues[2]=z;
	keyframe0.fkValues[3]=u;
	keyframe0.fkValues[4]=v;
	keyframe0.fkValues[5]=w;
				
	// use forward kinematics to find the endMatrix of the pose
	robot.sim.setPoseFK(keyframe0);
	m0.set(robot.sim.endEffector.getPoseWorld());
	
	String message = StringHelper.formatDouble(m0.m03)+"\t"
					+StringHelper.formatDouble(m0.m13)+"\t"
					+StringHelper.formatDouble(m0.m23)+"\n";
  		out.write(message);
}
 

/**
 * normalize the 3x3 component of the mTarget matrix.  Do not affect position. 
 * @param mTarget the matrix that will be normalized.
 */
public static void normalize3(Matrix4d mTarget) {
	Matrix3d m3 = new Matrix3d();
	Vector3d v3 = new Vector3d();
	mTarget.get(v3);
	mTarget.get(m3);
	m3.normalize();
	mTarget.set(m3);
	mTarget.setTranslation(v3);
}
 

@Override
public void update(Observable o, Object arg) {
	Matrix4d m4 = new Matrix4d();
	Vector3d rDeg = rot.get();
	Vector3d rRad = new Vector3d(
			Math.toRadians(rDeg.x),
			Math.toRadians(rDeg.y),
			Math.toRadians(rDeg.z)); 
	Matrix3d m3 = MatrixHelper.eulerToMatrix(rRad);
	m4.set(m3);
	m4.setTranslation(pos.get());
	this.set(m4);
	
	super.update(o, arg);
}
 

/**
 * 
 * @param arg0 Vector3d of radian rotation values
 */
public void setRotation(Vector3d arg0) {
	Matrix4d m4 = new Matrix4d();
	Matrix3d m3 = MatrixHelper.eulerToMatrix(arg0);
	m4.set(m3);
	m4.setTranslation(getPosition());
	setPose(m4);
}
 

/**
 * Test {@link UnitQuaternions#relativeOrientation(Point3d[], Point3d[])} on
 * a real structure. Test recovering of the angle applied.
 *
 * @throws StructureException
 * @throws IOException
 */
@Test
public void testRelativeOrientation() throws IOException,
		StructureException {

	// Get points from a structure.
	Structure pdb = StructureIO.getStructure("4hhb.A");
	Point3d[] cloud = Calc.atomsToPoints(StructureTools
			.getRepresentativeAtomArray(pdb));
	Point3d[] cloud2 = CalcPoint.clonePoint3dArray(cloud);

	// Test orientation angle equal to 0 at this point
	double angle = UnitQuaternions.orientationAngle(cloud, cloud2, false);
	assertEquals(angle, 0, 0.001);

	// Apply a 30 degree rotation to cloud
	AxisAngle4d axis = new AxisAngle4d(new Vector3d(1,1,1), Math.PI / 6);
	Matrix4d transform = new Matrix4d();
	transform.set(axis);

	CalcPoint.transform(transform, cloud);
	angle = UnitQuaternions.orientationAngle(cloud, cloud2, false);
	angle = Math.min(Math.abs(2 * Math.PI - angle), angle);

	// Test that angle was recovered
	assertEquals(angle, Math.PI / 6, 0.001);
}
 

public void setupModels(DHRobotEntity robot) {
	material = new MaterialEntity();
	float r=0.75f;
	float g=0.15f;
	float b=0.15f;
	material.setDiffuseColor(r,g,b,1);
	
	try {
		robot.links.get(0).setModelFilename("/Sixi/anchor.stl");
		robot.links.get(1).setModelFilename("/Sixi/shoulder.stl");
		robot.links.get(2).setModelFilename("/Sixi/bicep.stl");
		robot.links.get(3).setModelFilename("/Sixi/elbow.stl");
		robot.links.get(5).setModelFilename("/Sixi/forearm.stl");
		robot.links.get(6).setModelFilename("/Sixi/wrist.stl");
		robot.links.get(7).setModelFilename("/Sixi/hand.stl");

		for( DHLink link : robot.links ) link.setModelScale(0.1f);
		robot.links.get(1).getModel().adjustOrigin(new Vector3d(0, 0, -25));
		robot.links.get(2).getModel().adjustOrigin(new Vector3d(0, -5, -25));
		robot.links.get(2).getModel().adjustRotation(new Vector3d(-11.3,0,0));
		
		robot.links.get(5).getModel().adjustOrigin(new Vector3d(0, 0, -60));
		robot.links.get(6).getModel().adjustOrigin(new Vector3d(0, 0, -70));
		robot.links.get(7).getModel().adjustOrigin(new Vector3d(0, 0, -74));

		Matrix4d rot = new Matrix4d();
		Matrix4d rotX = new Matrix4d();
		Matrix4d rotY = new Matrix4d();
		Matrix4d rotZ = new Matrix4d();
		rot.setIdentity();
		rotX.rotX((float)Math.toRadians(90));
		rotY.rotY((float)Math.toRadians(0));
		rotZ.rotZ((float)Math.toRadians(0));
		rot.set(rotX);
		rot.mul(rotY);
		rot.mul(rotZ);
		Matrix4d pose = new Matrix4d(rot);
		Vector3d adjustPos = new Vector3d(0, 5, -50);
		pose.transform(adjustPos);
		
		robot.links.get(3).getModel().adjustOrigin(adjustPos);
		robot.links.get(3).getModel().adjustRotation(new Vector3d(90, 0, 0));
	} catch (Exception e) {
		// TODO Auto-generated catch block
		e.printStackTrace();
	}
}
 

public void setRotation(Matrix3d arg0) {
	Matrix4d m = new Matrix4d();
	m.set(arg0);
	m.setTranslation(getPosition());
	setPose(m);
}
 

public void getRotation(Matrix4d arg0) {
	arg0.set(pose);
	arg0.setTranslation(new Vector3d(0,0,0));
}
 

private boolean evaluatePermutation(List<Integer> permutation) {
	// permutate subunits
	for (int j = 0, n = subunits.getSubunitCount(); j < n; j++) {
		transformedCoords[j].set(originalCoords[permutation.get(j)]);
	}

	int fold = PermutationGroup.getOrder(permutation);

	// TODO implement this piece of code using at origin superposition
	Quat4d quat = UnitQuaternions.relativeOrientation(
			originalCoords, transformedCoords);
	AxisAngle4d axisAngle = new AxisAngle4d();
	Matrix4d transformation = new Matrix4d();

	transformation.set(quat);
	axisAngle.set(quat);

	Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
	if (axis.lengthSquared() < 1.0E-6) {
		axisAngle.x = 0;
		axisAngle.y = 0;
		axisAngle.z = 1;
		axisAngle.angle = 0;
	} else {
		axis.normalize();
		axisAngle.x = axis.x;
		axisAngle.y = axis.y;
		axisAngle.z = axis.z;
	}

	CalcPoint.transform(transformation, transformedCoords);

	double subunitRmsd = CalcPoint.rmsd(transformedCoords, originalCoords);

	if (subunitRmsd <parameters.getRmsdThreshold()) {
		// transform to original coordinate system
		combineWithTranslation(transformation);
		QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation);
		if (scores.getRmsd() < 0.0 || scores.getRmsd() > parameters.getRmsdThreshold()) {
			return false;
		}

		scores.setRmsdCenters(subunitRmsd);
		Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores);
		rotations.addRotation(symmetryOperation);
		return true;
	}
	return false;
}
 

private void solve() {
		initialize();
		Vector3d trans = new Vector3d(subunits.getCentroid());
		trans.negate();
		List<Point3d[]> traces = subunits.getTraces();

//		Point3d[] x = SuperPosition.clonePoint3dArray(traces.get(0));
//		SuperPosition.center(x);
//		Point3d[] y = SuperPosition.clonePoint3dArray(traces.get(1));
//		SuperPosition.center(y);

		Point3d[] x = CalcPoint.clonePoint3dArray(traces.get(0));
		CalcPoint.translate(trans, x);
		Point3d[] y = CalcPoint.clonePoint3dArray(traces.get(1));
		CalcPoint.translate(trans, y);

		// TODO implement this piece of code using at origin superposition
		Quat4d quat = UnitQuaternions.relativeOrientation(
				x, y);
		AxisAngle4d axisAngle = new AxisAngle4d();
		Matrix4d transformation = new Matrix4d();

		transformation.set(quat);
		axisAngle.set(quat);

		Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
		if (axis.lengthSquared() < 1.0E-6) {
			axisAngle.x = 0;
			axisAngle.y = 0;
			axisAngle.z = 1;
			axisAngle.angle = 0;
		} else {
			axis.normalize();
			axisAngle.x = axis.x;
			axisAngle.y = axis.y;
			axisAngle.z = axis.z;
		}

		CalcPoint.transform(transformation, y);

		// if rmsd or angle deviation is above threshold, stop
		double angleThresholdRadians = Math.toRadians(parameters.getAngleThreshold());
		double deltaAngle = Math.abs(Math.PI-axisAngle.angle);

		if (deltaAngle > angleThresholdRadians) {
			rotations.setC1(subunits.getSubunitCount());
			return;
		}

		// add unit operation
		addEOperation();

		// add C2 operation
		int fold = 2;
		combineWithTranslation(transformation);
		List<Integer> permutation = Arrays.asList(1,0);
		QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation);
		scores.setRmsdCenters(0.0); // rmsd for superposition of two subunits centers is zero by definition

		if (scores.getRmsd() > parameters.getRmsdThreshold() || deltaAngle > angleThresholdRadians) {
			rotations.setC1(subunits.getSubunitCount());
			return;
		}

		Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores);
		rotations.addRotation(symmetryOperation);
	}
 

private void solve() {
	initialize();

	int maxSymOps = subunits.getSubunitCount();

	boolean isSpherical = isSpherical();

	// for cases with icosahedral symmetry n cannot be higher than 60, should check for spherical symmetry here
	// isSpherical check added 08-04-11
	if (maxSymOps % 60 == 0 && isSpherical) {
		maxSymOps = 60;
	 }

	AxisAngle4d sphereAngle = new AxisAngle4d();
	Matrix4d transformation = new Matrix4d();

	int n = subunits.getSubunitCount();

	int sphereCount = SphereSampler.getSphereCount();

	List<Double> angles = getAngles();

	for (int i = 0; i < sphereCount; i++) {
		// Sampled orientation
		//TODO The SphereSampler samples 4D orientation space. We really
		// only need to sample 3D unit vectors, since we use limited
		// angles. -SB
		SphereSampler.getAxisAngle(i, sphereAngle);

		// Each valid rotation angle
		for (double angle : angles) {
			// apply rotation
			sphereAngle.angle = angle;
			transformation.set(sphereAngle);
			// Make sure matrix element m33 is 1.0. It's not on Linux.
			transformation.setElement(3, 3, 1.0);
			for (int j = 0; j < n; j++) {
				transformedCoords[j].set(originalCoords[j]);
				transformation.transform(transformedCoords[j]);
			}

			// get permutation of subunits and check validity/uniqueness
			List<Integer> permutation = getPermutation();
//              System.out.println("Rotation Solver: permutation: " + i + ": " + permutation);

			// check if novel
			if ( evaluatedPermutations.containsKey(permutation)) {
				continue; //either invalid or already added
			}

			Rotation newPermutation = isValidPermutation(permutation);
			if (newPermutation != null) {
				completeRotationGroup(newPermutation);
			}

			// check if all symmetry operations have been found.
			if (rotations.getOrder() >= maxSymOps) {
				return;
			}
		}
	}
}
 

/**
 * Superimpose subunits based on the given permutation. Then check whether
 * the superposition passes RMSD thresholds and create a Rotation to
 * represent it if so.
 * @param permutation A list specifying which subunits should be aligned by the current transformation
 * @return A Rotation representing the permutation, or null if the superposition did not meet thresholds.
 */
private Rotation superimposePermutation(List<Integer> permutation) {
	// permutate subunits
	for (int j = 0, n = subunits.getSubunitCount(); j < n; j++) {
		transformedCoords[j].set(originalCoords[permutation.get(j)]);
	}

	int fold = PermutationGroup.getOrder(permutation);

	// get optimal transformation and axisangle by subunit superposition
	// TODO implement this piece of code using at origin superposition
	Quat4d quat = UnitQuaternions.relativeOrientation(
			originalCoords, transformedCoords);
	AxisAngle4d axisAngle = new AxisAngle4d();
	Matrix4d transformation = new Matrix4d();

	transformation.set(quat);
	axisAngle.set(quat);

	Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
	if (axis.lengthSquared() < 1.0E-6) {
		axisAngle.x = 0;
		axisAngle.y = 0;
		axisAngle.z = 1;
		axisAngle.angle = 0;
	} else {
		axis.normalize();
		axisAngle.x = axis.x;
		axisAngle.y = axis.y;
		axisAngle.z = axis.z;
	}

	CalcPoint.transform(transformation, transformedCoords);
	double subunitRmsd = CalcPoint.rmsd(transformedCoords, originalCoords);

	if (subunitRmsd < parameters.getRmsdThreshold()) {
		combineWithTranslation(transformation);

		// evaluate superposition of CA traces
		QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation);
		if (scores.getRmsd() < 0.0 || scores.getRmsd() > parameters.getRmsdThreshold()) {
			return null;
		}

		scores.setRmsdCenters(subunitRmsd);
		Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores);
		return symmetryOperation;
	}
	return null;
}
 

/**
 * Generate two clouds of random points of different sizes to test
 * correctness and performance of superposition algorithms.
 *
 * @throws StructureException
 */
@Before
public void setUp() throws StructureException {

	cloud1 = new ArrayList<Point3d[]>(5);
	cloud2 = new ArrayList<Point3d[]>(5);

	Random rnd = new Random(0);

	transform = new Matrix4d();
	transform.set(rotAxis);
	transform.setTranslation(translation);

	List<Integer> sizes = Arrays.asList(5, 50, 500, 5000, 50000, 500000);

	for (Integer size : sizes) {

		Point3d[] c1 = new Point3d[size];
		Point3d[] c2 = new Point3d[size];

		for (int p = 0; p < size; p++) {

			Point3d a = new Point3d(rnd.nextInt(100), rnd.nextInt(50),
					rnd.nextInt(150));
			c1[p] = a;

			// Add some noise
			Point3d b = new Point3d(a.x + rnd.nextDouble(), a.y
					+ rnd.nextDouble(), a.z + rnd.nextDouble());
			c2[p] = b;
		}

		CalcPoint.center(c1);
		CalcPoint.center(c2);

		CalcPoint.transform(transform, c1);

		cloud1.add(c1);
		cloud2.add(c2);

		Point3d centroid1 = CalcPoint. centroid(c1);
		Point3d centroid2 = CalcPoint. centroid(c2);
		LOGGER.debug("Centroid c1 (size %d): (%.2f, %.2f, %.2f)\n", size, centroid1.x, centroid1.y, centroid1.z);
		LOGGER.debug("Centroid c2 (size %d): (%.2f, %.2f, %.2f)\n", size, centroid2.x, centroid2.y, centroid2.z);
	}

}
 

/**
 * Test {@link UnitQuaternions#orientation(javax.vecmath.Point3d[])}.
 * <p>
 * Tests the identity orientation, orientation around one coordinate axis
 * and orientation around a non-coordinate axis.
 *
 * @throws StructureException
 * @throws IOException
 */
@Test
public void testOrientation() throws IOException, StructureException {

	// Get points from a structure. It is difficult to generate points
	// with no bias in their distribution (too uniform, ie).
	Structure pdb = StructureIO.getStructure("4hhb.A");
	Point3d[] cloud = Calc.atomsToPoints(StructureTools
			.getRepresentativeAtomArray(pdb));

	// Center the cloud at the origin
	CalcPoint.center(cloud);

	// Orient its principal axes to the coordinate axis
	Quat4d orientation = UnitQuaternions.orientation(cloud);
	Matrix4d transform = new Matrix4d();
	transform.set(orientation);
	transform.invert();
	CalcPoint.transform(transform, cloud);

	// The orientation found now should be 0 (it has been re-oriented)
	orientation = UnitQuaternions.orientation(cloud);
	AxisAngle4d axis = new AxisAngle4d();
	axis.set(orientation);

	// No significant rotation
	assertEquals(orientation.x, 0.0, 0.01);
	assertEquals(orientation.y, 0.0, 0.01);
	assertEquals(orientation.z, 0.0, 0.01);
	assertEquals(axis.angle, 0.0, 0.01);

	// Now try to recover an orientation
	Quat4d quat = new Quat4d(0.418, 0.606, 0.303, 0.606);

	Matrix4d mat = new Matrix4d();
	mat.set(quat);

	CalcPoint.transform(mat, cloud);

	orientation = UnitQuaternions.orientation(cloud);

	// Test recovering the quaternion (q and -q same rotation)
	assertEquals(Math.abs(orientation.x), quat.x, 0.01);
	assertEquals(Math.abs(orientation.y), quat.y, 0.01);
	assertEquals(Math.abs(orientation.z), quat.z, 0.01);
	assertEquals(Math.abs(orientation.w), quat.w, 0.01);
}
 

/**
 * Test case proposed by Peter Rose from his observations about quaternary
 * symmetry artifacts with the QCP algorithm.
 */
@Test
public void testSymmetryQCP() {

	// Generate an array of points with symmetry
	Point3d[] set1 = new Point3d[16];
	set1[0] = new Point3d(14.065934, 47.068832, -32.895836);
	set1[1] = new Point3d(-14.065934, -47.068832, -32.895836);
	set1[2] = new Point3d(-47.068832, 14.065934, -32.895836);
	set1[3] = new Point3d(47.068832, -14.065934, -32.895836);
	set1[4] = new Point3d(-14.065934, 47.068832, 32.895836);
	set1[5] = new Point3d(14.065934, -47.068832, 32.895836);
	set1[6] = new Point3d(47.068832, 14.065934, 32.895836);
	set1[7] = new Point3d(-47.068832, -14.065934, 32.895836);
	set1[8] = new Point3d(43.813946, 22.748293, -32.14434);
	set1[9] = new Point3d(-43.813946, -22.748293, -32.14434);
	set1[10] = new Point3d(-22.748293, 43.813946, -32.14434);
	set1[11] = new Point3d(22.748293, -43.813946, -32.14434);
	set1[12] = new Point3d(-43.813946, 22.748293, 32.14434);
	set1[13] = new Point3d(43.813946, -22.748293, 32.14434);
	set1[14] = new Point3d(22.748293, 43.813946, 32.14434);
	set1[15] = new Point3d(-22.748293, -43.813946, 32.14434);

	Point3d[] set2 = CalcPoint.clonePoint3dArray(set1);

	// Use a random transformation to set2
	AxisAngle4d rotAxis = new AxisAngle4d(0.440, 0.302, 0.845, 1.570);
	Vector3d translation = new Vector3d(0.345, 2.453, 5.324);
	Matrix4d transform = new Matrix4d();
	transform.set(rotAxis);
	transform.setTranslation(translation);
	CalcPoint.transform(transform, set2);

	// Use Quaternion superposition to obtain the RMSD
	SuperPosition algorithm = new SuperPositionQuat(false);
	long quatStart = System.nanoTime();
	double quatrmsd = algorithm.getRmsd(set1, set2);
	long quatTime = (System.nanoTime() - quatStart) / 1000;

	// Use QCP algorithm to get the RMSD
	algorithm = new SuperPositionQCP(false);
	long qcpStart = System.nanoTime();
	double qcprmsd = algorithm.getRmsd(set1, set2);
	long qcpTime = (System.nanoTime() - qcpStart) / 1000;

	LOGGER.info(String.format("RMSD Symmetry: Quat time: %d us" + ", QCP time: %d us", quatTime, qcpTime));

	// Check that the returned RMSDs are equal
	assertEquals(quatrmsd, qcprmsd, 0.001);

}